Skip to main content

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 q∈Rnv\mathbf{q} \in \mathbb{R}^{n_v} (where nv=21n_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 e\mathbf{e} — for each fingertip that has a target, compute its error, err = target - pos; concatenate into one vector.

    For fingertip ii:

    ei=pi∗−pi(q)∈R3.\mathbf{e}_i = \mathbf{p}_i^{\ast} - \mathbf{p}_i(\mathbf{q}) \in \mathbb{R}^3.

    Stacking mm fingertips:

    e=[e1⋮em]∈R3m.\mathbf{e} = \begin{bmatrix} \mathbf{e}_1 \\ \vdots \\ \mathbf{e}_m \end{bmatrix} \in \mathbb{R}^{3m}.
  3. Convergence check — if max⁡∣e∣<tol\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 ii:

    dpi≈Ji dq,Ji∈R3×nv.\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:

    J=[J1⋮Jm]∈R3m×nv.\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 Δq\Delta\mathbf{q} that minimizes (formalizing the optimization problem)

    min⁡Δq  ∥J Δq−e∥22+λ∥Δq∥22.\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)

    (JTJ+λI) Δq=JTe.\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. q←q+α Δq\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.

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 ai=0a_i = 0 for all six joints) with a 90°90° twist between axes 1–6, so the chain reduces to pure link offsets did_i along each joint's own axis, with θi\theta_i the joint variable:

Jointaia_i (mm)αi\alpha_i (deg)did_i (mm)θi\theta_iAxis (local)
10—−279.3θ1\theta_1Y
2090−7.0θ2\theta_2X
3090300.9θ3\theta_3−Z
40902.3θ4\theta_4−Y
5090−235.5θ5\theta_5Z
60907.0θ6\theta_6−Y

d3d_3 (upper arm) and d5d_5 (forearm) dominate reach; d2d_2, d4d_4, d6d_6 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:

QuantityValue
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 configuration698 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.