Skip to content

Polish sim code #42

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
wilsonchenghy merged 1 commit into from
Mar 25, 2026
Merged

Polish sim code #42

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
154 changes: 19 additions & 135 deletions autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,24 +1,21 @@
"""
Standalone gradient-based IK for arm_assembly (6-DOF arm + 15-DOF hand).
Solves for joint angles so that 5 fingertips reach desired world positions
using damped least-squares (no dependencies on other scripts in this repo).
using damped least-squares

Joint 0 (shoulder_flexion_extension) issue:
MuJoCo's URDF importer can set continuous joints to range [0,0], which would
lock joint 0 at zero. We bypass that in clip_qpos_to_limits(): when
lower == upper we skip clipping so the IK can move that joint. See
diagnose_jacobian_joint0() and print_joint_limits_urdf() to inspect.
Here we found that MuJoCo's URDF importer can set continuous joints to range [0,0], which would
lock joint 0 at zero. We bypass that in clip_to_joint_limits(): when
lower == upper we skip clipping so the IK can move that joint.
"""
import os
import mujoco
import numpy as np
from pathlib import Path

# Path to URDF (script next to arm_assembly folder or repo root)
_SCRIPT_DIR = Path(__file__).resolve().parent
URDF_PATH = _SCRIPT_DIR / "arm_assembly.urdf"

# Fingertip body names (distal link of each finger in arm_assembly.urdf)
FINGERTIP_BODIES = [
"IP_THUMB_v1_1", # thumb
"DIP_INDEX_v1_1", # index
Expand All @@ -27,19 +24,16 @@
"DIP_PINKY_v1_1", # pinky
]

# First N DOFs are arm (shoulder, elbow, wrist); rest are hand. Used for weighted IK.
NUM_ARM_DOFS = 6


def load_model(urdf_path=None):
path = Path(urdf_path or URDF_PATH).resolve()
if not path.exists():
raise FileNotFoundError(f"URDF not found: {path}")

mesh_dir = path.parent / "meshes"
if not mesh_dir.is_dir():
raise FileNotFoundError(f"Mesh directory not found: {mesh_dir}")
# MuJoCo's URDF importer keeps only mesh basename and opens from cwd.
# Use mesh filenames without "meshes/" and chdir to meshes so they are found.

xml = path.read_text()
xml = xml.replace('filename="meshes/', 'filename="')
old_cwd = os.getcwd()
Expand All @@ -50,57 +44,25 @@ def load_model(urdf_path=None):
os.chdir(old_cwd)


def body_name_to_id(model: mujoco.MjModel, name: str) -> int:
try:
return model.body(name).id
except KeyError:
# List bodies for debugging
names = [model.body(i).name for i in range(model.nbody)]
raise KeyError(f"Body '{name}' not in model. Available: {names}")


def print_joint_limits_urdf(model: mujoco.MjModel) -> None:
"""Print joint position limits from the MuJoCo model (from URDF).
Use this to compare with USD joint limits (e.g. in Isaac Sim).
"""
print("[joint limits] From URDF (MuJoCo model.jnt_range):")
for i in range(model.njnt):
jnt_type = model.jnt_type[i]
if jnt_type not in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE):
continue
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) or f"joint_{i}"
qposadr = model.jnt_qposadr[i]
if qposadr < 0:
continue
lo, hi = model.jnt_range[i, 0], model.jnt_range[i, 1]
finite = np.isfinite(lo) or np.isfinite(hi)
marker = " <-- joint 0 (shoulder_flexion)" if qposadr == 0 else ""
if finite:
print(f" [qpos {qposadr}] {name}: lower={lo:.4f}, upper={hi:.4f}{marker}")
if qposadr == 0 and lo == hi:
print(" ^ LOCKED at one value (lower=upper). MuJoCo URDF import may have set continuous joints to [0,0]. Fix URDF or USD so this joint has a real range.")
else:
print(f" [qpos {qposadr}] {name}: (no limits / continuous){marker}")

def clip_to_joint_limits(model: mujoco.MjModel, data: mujoco.MjData) -> None:
"""Clamp each hinge/slide joint's scalar position to ``model.jnt_range``.

def clip_qpos_to_limits(model: mujoco.MjModel, data: mujoco.MjData) -> None:
"""Clip data.qpos to joint limits (only for limited joints).
Bypass: if lower == upper (e.g. MuJoCo imported continuous as [0,0]), skip clipping
so the IK can move that joint instead of locking it.
"""
for i in range(model.njnt):
jnt_type = model.jnt_type[i]
if jnt_type not in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE):
continue
qposadr = model.jnt_qposadr[i]
if qposadr < 0:
pos_idx = model.jnt_qposadr[i]
if pos_idx < 0:
continue
lo = model.jnt_range[i, 0]
hi = model.jnt_range[i, 1]
if lo == hi:
continue # bypass: treat as unlimited (e.g. continuous imported as [0,0])
continue # bypass: treat as unlimited revolute joint
if np.isfinite(lo) or np.isfinite(hi):
data.qpos[qposadr] = np.clip(data.qpos[qposadr], lo, hi)
data.qpos[pos_idx] = np.clip(data.qpos[pos_idx], lo, hi)


def solve_fingertip_ik(
Expand All @@ -113,34 +75,20 @@ def solve_fingertip_ik(
max_iter: int = 200,
tol: float = 1e-4,
clip_limits: bool = True,
arm_dofs: int = NUM_ARM_DOFS,
finger_weight: float = 4.0,
joint0_weight: float = 0.4,
) -> tuple[np.ndarray, bool, float]:
"""
Run damped least-squares IK so that each fingertip reaches its target.

Weighted regularization: arm DOFs (first arm_dofs) are penalized less than
finger DOFs (finger_weight > 1), so the solver prefers moving the arm to reach.
joint0_weight < 1 (default 0.4) further prefers joint 0 (shoulder_flexion) when
error has a Y/Z component, so the arm uses it to reduce large Y (or Y+Z) error.

Joint 0 rotates about X so it cannot help with pure X error; for Y/Z error it can.
targets: dict mapping body name -> (3,) world position.
Returns: (qpos, converged, final_max_error).
Returns: (generalized_positions, converged, final_max_error) — a copy of ``data.qpos``.
"""
# Build list of (body_name, target_pos) in consistent order
tip_names = [b for b in FINGERTIP_BODIES if b in targets]
if not tip_names:
raise ValueError("targets must contain at least one of %s" % FINGERTIP_BODIES)
target_list = [targets[b] for b in tip_names]

# Diagonal regularization: joint 0 preferred when < 1, other arm 1, fingers finger_weight.
nv = model.nv
arm_dofs = min(arm_dofs, nv)
W = np.ones(nv, dtype=np.float64)
W[0] = max(1e-3, float(joint0_weight))
W[arm_dofs:] = finger_weight

for _ in range(max_iter):
mujoco.mj_forward(model, data)
Expand All @@ -149,7 +97,7 @@ def solve_fingertip_ik(
err_list = []

for body_name, target in zip(tip_names, target_list):
body_id = body_name_to_id(model, body_name)
body_id = model.body(body_name).id
pos = data.xpos[body_id].copy()
err = target - pos
jacp = np.zeros((3, model.nv))
Expand All @@ -165,96 +113,32 @@ def solve_fingertip_ik(
if max_err < tol:
return data.qpos.copy(), True, float(max_err)

# Weighted damped least squares
A = J.T @ J + damping * np.diag(W)
A = J.T @ J + damping * np.eye(nv)
rhs = J.T @ err
dq = np.linalg.solve(A, rhs)

# All joints in this model are 1-DOF; qpos layout matches dofs.
data.qpos[:nv] += step * dq

if clip_limits:
clip_qpos_to_limits(model, data)
clip_to_joint_limits(model, data)

return data.qpos.copy(), False, float(max_err)


def get_fingertip_positions(model: mujoco.MjModel, data: mujoco.MjData) -> dict[str, np.ndarray]:
"""Return current world positions of all 5 fingertips."""
mujoco.mj_forward(model, data)
out = {}
for name in FINGERTIP_BODIES:
try:
bid = body_name_to_id(model, name)
out[name] = data.xpos[bid].copy()
except KeyError:
pass
id = model.body(name).id
out[name] = data.xpos[id].copy()
return out


def diagnose_jacobian_joint0(
model: mujoco.MjModel,
data: mujoco.MjData,
err: np.ndarray | None = None,
) -> None:
"""
At current qpos, build the 15x21 task Jacobian J and explain why joint 0
(shoulder_flexion_extension) often gets dq[0] ≈ 0. Joint 0 rotates about X,
so J's column 0 has zero X components; pure-X position error contributes
nothing to (J.T @ err)[0].

If err is provided (15-D task error: [ex,ey,ez] x 5 fingertips), also prints
error by axis and (J.T @ err) for first 6 joints so you can see if joint 0
gets a non-zero rhs when error is in Y.
"""
mujoco.mj_forward(model, data)
J_list = []
for name in FINGERTIP_BODIES:
bid = body_name_to_id(model, name)
jacp = np.zeros((3, model.nv))
jacr = np.zeros((3, model.nv))
mujoco.mj_jacBody(model, data, jacp, jacr, bid)
J_list.append(jacp)
J = np.vstack(J_list)
j0 = J[:, 0]
x_rows = [0, 3, 6, 9, 12]
j0_x = j0[x_rows]
j0_yz = np.delete(j0, x_rows)
print("Joint 0 (shoulder_flexion_extension) Jacobian column (15-D task):")
print(" ||J[:,0]|| =", np.linalg.norm(j0))
print(" X components (rows 0,3,6,9,12):", j0_x, " -> should be ~0 (joint rotates about X)")
print(" Y,Z components norm:", np.linalg.norm(j0_yz))
err_x = np.zeros(15)
err_x[0::3] = 1.0
rhs0_x = (J.T @ err_x)[0]
print(" (J.T @ err_pure_X)[0] =", rhs0_x, " -> 0 means joint 0 gets no update for forward (X) reach")
err_y = np.zeros(15)
err_y[1::3] = 1.0
rhs0_y = (J.T @ err_y)[0]
rhs1_y = (J.T @ err_y)[1]
print(" (J.T @ err_pure_Y)[0] =", rhs0_y, " [1] =", rhs1_y, " -> joint 0 can help for Y error")
print(" So dq[0] stays 0 when error is mostly in world X.")

if err is not None and err.size == 15:
rhs = J.T @ err
err_x_sum = np.sum(np.abs(err[0::3]))
err_y_sum = np.sum(np.abs(err[1::3]))
err_z_sum = np.sum(np.abs(err[2::3]))
print(" --- With current task error ---")
print(" |error| by axis (sum over 5 tips): X =", err_x_sum, " Y =", err_y_sum, " Z =", err_z_sum)
print(" (J.T @ err)[0:6] (arm joints):", rhs[:6])
print(" -> If Y error is large but rhs[0] ~ 0, other joints are covering Y; if rhs[0] != 0, joint 0 could step.")


def main():
model = load_model()
data = mujoco.MjData(model)

# --- Joint limits from URDF (MuJoCo) ---
print("========== Joint limits (URDF / MuJoCo) ==========")
print_joint_limits_urdf(model)
print()


data.qpos[:] = 0.0
mujoco.mj_forward(model, data)
default_positions = get_fingertip_positions(model, data)
Expand Down
Loading
Loading