Add Jacobian DLS IK script documentation

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`.

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: