Arm Kinematics & IK
Bimanual Arm Inverse Kinematics
Jacobian-based iterative IK for the dual-arm setup, driving both end effectors to target poses in simulation.
6 DOF Arm + 15 DOF Hand Jacobian Iterative Based IK Controller
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 (where , 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:
-
mj_forward(model, data)— compute forward kinematics with mujoco function -
Build error — for each fingertip that has a target, compute its error,
err = target - pos; concatenate into one vector.For fingertip :
Stacking fingertips:
-
Convergence check — if , exit early successfully.
-
Build Jacobian
J— for each such fingertip,mj_jacBody→ translation Jacobianjacp;vstackintoJ.Translation-only linearization for fingertip :
Stacking:
-
Damped least squares — solve for a joint increment
dqusing damped least squares withdamping.Compute that minimizes (formalizing the optimization problem)
The minimizer satisfies the normal equations (The actual equation that we need to solve)
-
Update —
qpos[:nv] += step * dq(i.e. ,step). -
Joint limits — if
clip_limits, runclip_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.
Arm Kinematics: DH Parameters & Workspace
Derived from the wato_arm_v2 (armWithStand) URDF — the 6-revolute-joint arm chain plus its 2-DOF parallel gripper (right arm shown; left is a mirror image).
Denavit-Hartenberg parameters
Every consecutive joint-axis pair in this design intersects exactly (common-normal length for all six joints) with a twist between axes 1–6, so the chain reduces to pure link offsets along each joint's own axis, with the joint variable:
| Joint | (mm) | (deg) | (mm) | Axis (local) | |
|---|---|---|---|---|---|
| 1 | 0 | — | −279.3 | Y | |
| 2 | 0 | 90 | −7.0 | X | |
| 3 | 0 | 90 | 300.9 | −Z | |
| 4 | 0 | 90 | 2.3 | −Y | |
| 5 | 0 | 90 | −235.5 | Z | |
| 6 | 0 | 90 | 7.0 | −Y |
(upper arm) and (forearm) dominate reach; , , are small wrist/shoulder eccentricities from the CAD design. The gripper (joint7/joint8, prismatic along the wrist's Y) rides on link 6 as the end effector and isn't part of the DH chain above.
Joint limits are not yet defined in the exported URDF (placeholder 0..0 on every joint) — the numbers below ignore joint limits and self-collision.
Workspace
Forward kinematics through the chain above, sampled and refined (Nelder-Mead) over the full joint range:
| Quantity | Value |
|---|---|
| Max reach (fully outstretched) | ≈ 934 mm from base_link |
| Min reach (fully folded) | ≈ 0 mm (arm can fold onto itself — no limits enforced yet) |
| Reach at zero configuration | 698 mm |
Because real joint limits aren't set yet, this is the theoretical actuation envelope, not the usable collision-free workspace — narrowing it down requires the actual limits plus a self-collision check once those are added to the URDF.