how a robot arm goes from knowing nothing about its own body to smoothly tracing an ∞-shape, and eventually, learning to reach on its own.

Where is my hand, and where do I want it to go?

Before a robot can move toward anything, it needs to answer a simple question: given my current joint angles, where is my end-effector in space?

That's Forward Kinematics (FK): walking the chain of joints outward to get a 3D position. In MuJoCo this is just:

mujoco.mj_kinematics(model, data)
mujoco.mj_comPos(model, data)
current_pos = data.site(site_name).xpos

🔑 Frames : a position is only meaningful relative to something. xpos from MuJoCo is in the world frame. Later (in the RL chapter) this becomes important: a policy trained on positions in the world frame only works in one specific spot, whereas a policy trained in the base frame (robot's own local coordinate system) generalizes anywhere.

Working backward: if I know where I want to be, what angles get me there?

This is Inverse Kinematics (IK) the reverse problem. There are two ways to solve it:

• Analytical — solve the geometry exactly with algebra. Fast, but only works for one specific robot design.
• Numerical — start from a guess, measure the error, nudge the joints, repeat. Slower, but works on (almost) any robot arm.

I implemented the numerical version using Damped Least Squares (DLS), which uses the Jacobian, a matrix that says "if I wiggle each joint a tiny bit, how much does my end-effector move?"

jacp = np.zeros((3, num_joints))   # position Jacobian
jacr = np.zeros((3, num_joints))   # orientation Jacobian
mujoco.mj_jacSite(model, data, jacp, jacr, model.site(site_name).id)
J = np.vstack([jacp, jacr])        # shape (6, nv)

weighted_err = np.concatenate((pos_gain * err_pos, np.zeros(3)))
A = J @ J.T + damping * np.eye(6)
x = np.linalg.solve(A, weighted_err)   # avoid explicit matrix inverse
qdot = J.T @ x
qdot = np.clip(qdot, -2.0, 2.0)        # guard against overshoot

data.qvel[:] = 0.0
data.qpos[:] += qdot * dt

🔑 Singularity: a robot pose where the Jacobian loses rank (some directions become impossible to move in instantaneously). The damping * np.eye(6) term is what keeps the solve numerically stable near these poses without it, qdot could explode.

🔑 Why restore qpos at the end? IK here is used purely as a solver, not an actuator, it answers "what angles would reach this point?" without actually moving the simulated robot. That's why the function saves the original qpos, iterates freely, then restores it before returning target_qpos.