From 0adee67257b9c3f66447b4949bdaf9cbf100e59f Mon Sep 17 00:00:00 2001 From: wilsonchenghy Date: Wed, 25 Mar 2026 22:33:20 -0400 Subject: [PATCH] add-documentation-for-ik --- .../Humanoid_Wato/arm_assembly/IK.md | 73 +++++++++++++++++++ .../arm_assembly/fingertip_ik.py | 37 +++------- 2 files changed, 84 insertions(+), 26 deletions(-) create mode 100644 autonomy/simulation/Humanoid_Wato/arm_assembly/IK.md diff --git a/autonomy/simulation/Humanoid_Wato/arm_assembly/IK.md b/autonomy/simulation/Humanoid_Wato/arm_assembly/IK.md new file mode 100644 index 0000000..a0fec3c --- /dev/null +++ b/autonomy/simulation/Humanoid_Wato/arm_assembly/IK.md @@ -0,0 +1,73 @@ +# Inverse kinematics: `fingertip_ik.py` + +We implemented **iterative Jacobian IK with damped least squares (DLS)** to take in the 5 target fingertip position and outputting the joint angle that will make it happen. It utilize the Jacobians and body positions from MuJoCo for computation, and Isaac Sim for visualization. + +## Algorithm + +Joint Position is $\mathbf{q} \in \mathbb{R}^{n_v}$ (where $n_v = 21$, representing the 21 DOF, specfically `model.nv` is the dimension of the joint velocity). Targets are world positions for the five fingertip bodies (`targets` dict). **Only fingertip positions** are constrained (`jacp` from `mj_jacBody`; `jacr`, the fingertip orientaton, is unused). + +### Iteration loop + +Repeat up to `max_iter` times / return early if error lower than `tol` tolerance: + +1. **`mj_forward(model, data)`** — compute forward kinematics with mujoco function +2. **Build error $\mathbf{e}$** — for each fingertip that has a target, compute its error, `err = target - pos`; concatenate into one vector. + + For fingertip $i$: + + $$ + \mathbf{e}_i = \mathbf{p}_i^{\ast} - \mathbf{p}_i(\mathbf{q}) \in \mathbb{R}^3. + $$ + + Stacking $m$ fingertips: + + $$ + \mathbf{e} = + \begin{bmatrix} + \mathbf{e}_1 \\ \vdots \\ \mathbf{e}_m + \end{bmatrix} + \in \mathbb{R}^{3m}. + $$ +3. **Convergence check** — if $\max |\mathbf{e}| < \text{tol}$, exit early successfully. +4. **Build Jacobian `J`** — for each such fingertip, `mj_jacBody` → translation Jacobian `jacp`; `vstack` into `J`. + + Translation-only linearization for fingertip $i$: + + $$ + \mathrm{d}\mathbf{p}_i \approx \mathbf{J}_i\,\mathrm{d}\mathbf{q}, + \qquad + \mathbf{J}_i \in \mathbb{R}^{3 \times n_v}. + $$ + + Stacking: + + $$ + \mathbf{J} = + \begin{bmatrix} + \mathbf{J}_1 \\ \vdots \\ \mathbf{J}_m + \end{bmatrix} + \in \mathbb{R}^{3m \times n_v}. + $$ + +5. **Damped least squares** — solve for a joint increment `dq` using damped least squares with $\lambda =$ `damping`. + + Compute $\Delta\mathbf{q}$ that minimizes (**formalizing the optimization problem**) + + $$ + \min_{\Delta\mathbf{q}} \; + \bigl\|\mathbf{J}\,\Delta\mathbf{q} - \mathbf{e}\bigr\|_2^2 + + \lambda\bigl\|\Delta\mathbf{q}\bigr\|_2^2. + $$ + + The minimizer satisfies the normal equations (The **actual equation that we need to solve**) + + $$ + \bigl(\mathbf{J}^{\mathsf T}\mathbf{J} + \lambda \mathbf{I}\bigr)\,\Delta\mathbf{q} + = \mathbf{J}^{\mathsf T}\mathbf{e}. + $$ +6. **Update** — `qpos[:nv] += step * dq` (i.e. $\mathbf{q} \leftarrow \mathbf{q} + \alpha\,\Delta\mathbf{q}$, $\alpha =$ `step`). +7. **Joint limits** — if `clip_limits`, run `clip_to_joint_limits`, enforce joint limit + +**Return value**: new joint pos, boolean on whether it successfully converge with error under `tol`, max error + +**Defaults:** `damping=1e-4`, `step=0.5`, `max_iter=200`, `tol=1e-4`, `clip_limits=True`. \ No newline at end of file diff --git a/autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py b/autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py index 4b672f0..4768fbf 100644 --- a/autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py +++ b/autonomy/simulation/Humanoid_Wato/arm_assembly/fingertip_ik.py @@ -1,12 +1,7 @@ """ -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 - -Joint 0 (shoulder_flexion_extension) issue: - 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. +IK for the 21 DOF humanoid arm-hand, with iterative jacobian least damped square method + +Details in IK.md """ import os import mujoco @@ -45,22 +40,21 @@ def load_model(urdf_path=None): def clip_to_joint_limits(model: mujoco.MjModel, data: mujoco.MjData) -> None: - """Clamp each hinge/slide joint's scalar position to ``model.jnt_range``. + """Clamp joint value to be within joint limit - 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. + MuJoCo's URDF importer can set continuous joints to range [0,0] which would + lock joint 0 at zero, here we bypass that by making if lower == upper, + it doesn't enforce clipping """ 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 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 revolute joint + continue if np.isfinite(lo) or np.isfinite(hi): data.qpos[pos_idx] = np.clip(data.qpos[pos_idx], lo, hi) @@ -76,32 +70,24 @@ def solve_fingertip_ik( tol: float = 1e-4, clip_limits: bool = True, ) -> tuple[np.ndarray, bool, float]: - """ - Run damped least-squares IK so that each fingertip reaches its target. - - targets: dict mapping body name -> (3,) world position. - 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] - nv = model.nv - for _ in range(max_iter): mujoco.mj_forward(model, data) J_list = [] err_list = [] + nv = model.nv for body_name, target in zip(tip_names, target_list): body_id = model.body(body_name).id pos = data.xpos[body_id].copy() err = target - pos - jacp = np.zeros((3, model.nv)) - jacr = np.zeros((3, model.nv)) + jacp = np.zeros((3, nv)) + jacr = np.zeros((3, nv)) mujoco.mj_jacBody(model, data, jacp, jacr, body_id) J_list.append(jacp) err_list.append(err) @@ -117,7 +103,6 @@ def solve_fingertip_ik( 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: