Skip to article frontmatterSkip to article content
Site not loading correctly?

This may be due to an incorrect BASE_URL configuration. See the MyST Documentation for reference.

Chapter 7 — Inverse Position Kinematics

Chapter 5 answered where is the gripper, given the joints? This chapter answers the reverse question, the one a robot faces every time it is told to pick something up:

Inverse position kinematics (IK): given a desired gripper pose, or part of it (for example a position and a pointing direction), find joint variables θ\boldsymbol\theta that put the gripper there.

Forward kinematics is a recipe: multiply the matrices and you get exactly one answer. Inverse kinematics means solving nonlinear trigonometric equations, and the answer may not exist, may be one of several, or may not be unique at all. There are two ways to solve them:

  • Analytical (closed-form) IK uses geometry (triangles, atan2, the law of cosines) to write each joint variable as a formula. It is fast and exact and gives every solution, but it has to be derived by hand for each robot.

  • Numerical IK starts from a guess and repeatedly corrects it with the Jacobian of Chapter 6 until the forward kinematics hits the target. It works for any robot whose forward kinematics we can compute, but it returns one solution (the one near the guess) and needs care near singularities and joint limits.

SectionTopic
7.1The IK problem: existence, multiple solutions, workspace
7.2Analytical toolkit: atan2, law of cosines, acos⁡θ+bsin⁡θ=ca\cos\theta + b\sin\theta = c, decoupling
7.3Analytical examples: planar 3R, SCARA, spherical wrist, 6-joint arms
7.4SO-ARM101 analytical IK (position + pitch)
7.5Numerical IK: Newton, pseudo-inverse, Jacobian transpose, damped least squares, optimization-based IK
7.6Numerical examples: planar 2R, SO-ARM101, UR5e
7.7Analytical vs. numerical, and chapter summary

Notation (same as Chapters 5 and 6). Te0(θ)\mathbf{T}_e^0(\boldsymbol\theta) is the gripper pose, p=oe\mathbf{p} = \mathbf{o}_e its position (last column) and Re0\mathbf{R}_e^0 its orientation. A subscript dd marks a desired value: pd\mathbf{p}_d, Rd\mathbf{R}_d, φd\varphi_d. The code works in radians and prints degrees.

Tools and robots from Chapters 5 and 6

The next cell collects the forward-kinematics tools of Chapter 5 (basic HTMs, the Robot class, drawing helpers), the jacobian function of Chapter 6, and the robots we reuse. It adds two robots: a planar 3R arm, and the SO-ARM101, built from the pos and quat of each body in its MuJoCo model (so101_new_calib.xml). This chain reproduces MuJoCo’s own gripperframe site to about 10-8 m (we check this in Section 7.6.2). Run the cell and move on.

Robots ready: Planar 2R, Planar 3R, Spherical wrist, SCARA, UR5e, SO-ARM101

7.1 The IK Problem

Forward vs. inverse. Forward kinematics maps joints to pose, θ↦Te0(θ)\boldsymbol\theta \mapsto \mathbf{T}_e^0(\boldsymbol\theta): one product of matrices, always exactly one answer. Inverse kinematics asks for the joints that solve

Te0(θ)=Td.\mathbf{T}_e^0(\boldsymbol\theta) = \mathbf{T}_d .

This 4×44\times4 equation hides 12 nonlinear equations in cos⁡θi\cos\theta_i and sin⁡θi\sin\theta_i (9 rotation entries and 3 position entries), but only 6 are independent: 3 for position and 3 for orientation, because only 3 of the 9 numbers in a rotation matrix are free (Chapter 4).

Count equations and unknowns. Let mm be the number of task coordinates we ask for and nn the number of joints:

RobotWhat we ask formmnnTypical number of solutions
Planar 2R(x,y)(x, y)220, 1 or 2
Planar 3R(x,y,φ)(x, y, \varphi)330, 1 or 2
SCARA(x,y,z,yaw)(x, y, z, \text{yaw})440, 1 or 2
SO-ARM101(x,y,z,pitch)(x, y, z, \text{pitch}); roll θ5\theta_5 set directly44 (+1)up to 4, but only 1 within the joint limits
UR5efull pose66up to 8
7-joint arm (e.g. Franka)full pose67infinitely many

So there are three possible outcomes:

  1. No solution. The target is outside the workspace, asks for an orientation the robot cannot produce (a 5-joint arm cannot reach an arbitrary 6-number pose), or every solution violates a joint limit.

  2. A finite number of solutions (m=nm = n). They differ by discrete branches: elbow up or down, shoulder left or right, wrist flipped. A general 6R arm can have up to 16.

  3. Infinitely many solutions. When n>mn > m the robot is redundant: it can move its joints without moving the gripper (self-motion). This also happens at some singular configurations, such as the aligned spherical wrist of Section 6.5.2.

Workspace. The reachable workspace is the set of gripper positions the robot can reach with at least one orientation; the dexterous workspace is the smaller set it can reach with every orientation. For the planar 2R arm the reachable workspace is the annulus ∣L1−L2∣≤∥p∥≤L1+L2|L_1 - L_2| \le \|\mathbf{p}\| \le L_1 + L_2 (Section 3.2.1), and joint limits shrink it further.

Figure 7.1 — The planar 2R arm (L1 = 1, L2 = 0.7): two solutions inside the workspace, one on its boundary, none outside.

The 2R solution, again. Section 3.2.2 solved the 2R arm by geometry, and that solution is the template for everything in this chapter:

cos⁡θ2=xd2+yd2−L12−L222L1L2,θ2=atan2 ⁣(±1−cos⁡2θ2, cos⁡θ2),θ1=atan2(yd,xd)−atan2(L2sin⁡θ2, L1+L2cos⁡θ2).\cos\theta_2 = \frac{x_d^2 + y_d^2 - L_1^2 - L_2^2}{2L_1L_2},\qquad \theta_2 = \mathrm{atan2}\!\left(\pm\sqrt{1-\cos^2\theta_2},\ \cos\theta_2\right),\qquad \theta_1 = \mathrm{atan2}(y_d, x_d) - \mathrm{atan2}(L_2\sin\theta_2,\ L_1 + L_2\cos\theta_2).

The ± is the branch choice. With L1=1L_1 = 1, L2=0.7L_2 = 0.7 (the arm of Chapters 5 and 6), θ2>0\theta_2 > 0 is the elbow-down solution in Figure 7.1(a).

target (1.050, 0.750): 2 solution(s)
    θ = [ 2.97 82.82]°    FK check: p = [1.05 0.75]
    θ = [ 68.1  -82.82]°    FK check: p = [1.05 0.75]
target (1.492, 0.815): 1 solution(s)
    θ = [28.65  0.  ]°    FK check: p = [1.4919 0.815 ]
target (1.750, 0.750): 0 solution(s)

Every analytical solution in this chapter should end the same way: plug the answer back into the forward kinematics and confirm that it reproduces the target.

Key Takeaways — Section 7.1

  • IK solves Te0(θ)=Td\mathbf{T}_e^0(\boldsymbol\theta) = \mathbf{T}_d: nonlinear equations with 0, finitely many, or infinitely many solutions.

  • Compare mm (task coordinates asked for) with nn (joints): m=nm = n gives discrete branches, n>mn > m gives redundancy, m>nm > n asks for too much.

  • Reachable vs. dexterous workspace; joint limits remove solutions.

  • Always check an IK answer with the forward kinematics.

7.2 The Analytical Toolkit

Almost every closed-form IK solution is built from four tools.

7.2.1 Use atan2, not acos or asin alone

acos returns angles in [0,π][0, \pi] and loses the sign; asin returns angles in [−π/2,π/2][-\pi/2, \pi/2] and loses the back half of the circle. Both are also inaccurate near ±1\pm1, where a tiny error in the input becomes a large error in the angle. The rule:

Find both sin⁡θ\sin\theta and cos⁡θ\cos\theta (or two numbers proportional to them with the same positive factor), then take θ=atan2(sin⁡θ,cos⁡θ)\theta = \mathrm{atan2}(\sin\theta, \cos\theta).

The 2R solution does exactly this: cos⁡θ2\cos\theta_2 comes from the law of cosines, and sin⁡θ2=±1−cos⁡2θ2\sin\theta_2 = \pm\sqrt{1-\cos^2\theta_2}, where the sign is the branch we choose.

true angle      =  -120.00°
acos(c)         =   120.00°   wrong sign
asin(s)         =   -60.00°   wrong half of the circle
atan2(s, c)     =  -120.00°   correct
atan2(5s, 5c)   =  -120.00°   a common positive factor does not matter

7.2.2 The law of cosines

Two links of lengths L1L_1 and L2L_2 joined by a revolute joint, and a distance DD between their free ends, form a triangle. The joint angle θ2\theta_2 is the bend between the links (the exterior angle), so the interior angle is π−θ2\pi - \theta_2 and

D2=L12+L22−2L1L2cos⁡(π−θ2)=L12+L22+2L1L2cos⁡θ2⟹cos⁡θ2=D2−L12−L222L1L2D^2 = L_1^2 + L_2^2 - 2L_1L_2\cos(\pi - \theta_2) = L_1^2 + L_2^2 + 2L_1L_2\cos\theta_2 \quad\Longrightarrow\quad \boxed{\cos\theta_2 = \frac{D^2 - L_1^2 - L_2^2}{2L_1L_2}}
  • ∣cos⁡θ2∣>1|\cos\theta_2| > 1: no triangle exists, so the target is out of reach.

  • Otherwise there are two mirror-image triangles, sin⁡θ2=±1−cos⁡2θ2\sin\theta_2 = \pm\sqrt{1-\cos^2\theta_2}: the two branches.

  • The direction of the first link is then the direction of the target minus the angle that link 2 adds: θ1=atan2(y,x)−atan2(L2sin⁡θ2, L1+L2cos⁡θ2)\theta_1 = \mathrm{atan2}(y,x) - \mathrm{atan2}(L_2\sin\theta_2,\ L_1+L_2\cos\theta_2).

7.2.3 The equation acos⁡θ+bsin⁡θ=ca\cos\theta + b\sin\theta = c

This equation appears whenever one joint has to rotate a point onto a line or a plane, for example when the plane of an arm is offset sideways from the base axis (we meet it on the SO-ARM101 in Section 7.4.5). Write (a,b)(a, b) in polar form, a=ρcos⁡γa = \rho\cos\gamma, b=ρsin⁡γb = \rho\sin\gamma with ρ=a2+b2\rho = \sqrt{a^2+b^2} and γ=atan2(b,a)\gamma = \mathrm{atan2}(b, a). Then acos⁡θ+bsin⁡θ=ρcos⁡(θ−γ)=ca\cos\theta + b\sin\theta = \rho\cos(\theta - \gamma) = c, so

θ=atan2(b,a)±atan2 ⁣(a2+b2−c2, c)\boxed{\theta = \mathrm{atan2}(b, a) \pm \mathrm{atan2}\!\left(\sqrt{a^2 + b^2 - c^2},\ c\right)}
  • It has a solution only if c2≤a2+b2c^2 \le a^2 + b^2.

  • In general there are two solutions; there is one when c2=a2+b2c^2 = a^2 + b^2.

  • The special case c=0c = 0 gives θ=atan2(b,a)±90∘\theta = \mathrm{atan2}(b, a) \pm 90^\circ.

1000 random (a, b, c): largest residual |a cos θ + b sin θ - c| = 2.0e-15
Example 3 cos θ + 4 sin θ = 2:  θ = [119.552 -13.292] °

7.2.4 Decoupling: split the problem

Two structural features make most practical arms solvable by hand.

  1. A point that only some of the joints move. If the last three joint axes intersect in one point, the wrist center (a spherical wrist, Section 5.5), then the wrist joints only turn the gripper about that point. Its position depends on the first three joints alone. Back off from the target along the approach axis to find it, pw=pd−de Rdz^\mathbf{p}_w = \mathbf{p}_d - d_e\,\mathbf{R}_d\hat{\mathbf{z}}, solve the first three joints for pw\mathbf{p}_w (position IK), then the wrist joints for the remaining rotation (orientation IK). This is kinematic decoupling. Pieper (1968) showed that a 6R arm with three consecutive axes meeting in a point always has a closed-form solution. The planar version of the same trick is the wrist-point method of Section 7.3.1.

  2. A plane. If the base joint turns the arm about a vertical axis, and the next joints have parallel axes perpendicular to it, those joints move the gripper inside one vertical plane. The base angle aims the plane at the target; the rest is a planar problem. The SCARA, the SO-ARM101 and the UR5e (axes 2, 3, 4 parallel) all have this structure.

Key Takeaways — Section 7.2

  • Get sin⁡\sin and cos⁡\cos of the unknown angle, then use atan2.

  • Law of cosines: cos⁡θ2=(D2−L12−L22)/(2L1L2)\cos\theta_2 = (D^2 - L_1^2 - L_2^2)/(2L_1L_2); ∣cos⁡θ2∣>1|\cos\theta_2|>1 means out of reach; ±sin⁡θ2\pm\sin\theta_2 gives two branches.

  • acos⁡θ+bsin⁡θ=ca\cos\theta + b\sin\theta = c: θ=atan2(b,a)±atan2(a2+b2−c2, c)\theta = \mathrm{atan2}(b,a) \pm \mathrm{atan2}(\sqrt{a^2+b^2-c^2},\, c).

  • Decouple whenever possible: wrist point (position, then orientation) and arm plane (base angle, then a planar arm).

7.3 Analytical Examples

7.3.1 Planar 3R arm with orientation: the wrist-point method

A planar 3R arm with link lengths L1,L2,L3L_1, L_2, L_3 has three joints, so we can ask for the position (xd,yd)(x_d, y_d) and the direction φd\varphi_d of the last link, where φ=θ1+θ2+θ3\varphi = \theta_1 + \theta_2 + \theta_3. The last link must point along φd\varphi_d, so the third joint, the wrist point, must sit at

w=pd−L3[cos⁡φdsin⁡φd],\mathbf{w} = \mathbf{p}_d - L_3\begin{bmatrix}\cos\varphi_d\\ \sin\varphi_d\end{bmatrix},

which depends only on the target. The problem splits into three steps:

  1. Compute the wrist point w\mathbf{w}.

  2. Solve the 2R problem (links L1L_1, L2L_2) to reach w\mathbf{w}: this gives θ1\theta_1 and θ2\theta_2, with two branches.

  3. The last joint makes up the rest of the orientation: θ3=φd−θ1−θ2\theta_3 = \varphi_d - \theta_1 - \theta_2.

Figure 7.2 — Wrist-point method for the planar 3R arm: back off L3 from the target along φ, then solve a 2R problem.

θ = [  17.67   75.18 -112.85]°  ->  tip [1.2 0.9], heading -20.00°
θ = [ 77.37 -75.18 -22.19]°  ->  tip [1.2 0.9], heading -20.00°
<Figure size 650x500 with 1 Axes>

7.3.2 SCARA

From Section 5.7 the SCARA gripper position and tool yaw are

oe=[L1c1+L2c12L1s1+L2s12h−d3−de],φ=θ1+θ2−θ4.\mathbf{o}_e = \begin{bmatrix}L_1c_1 + L_2c_{12}\\ L_1s_1 + L_2s_{12}\\ h - d_3 - d_e\end{bmatrix},\qquad \varphi = \theta_1 + \theta_2 - \theta_4 .

The problem is already decoupled: the height depends only on the quill, the horizontal position only on the 2R arm, and the yaw is a linear combination of joint angles. So:

  1. (θ1,θ2)(\theta_1, \theta_2): 2R IK for (xd,yd)(x_d, y_d), two branches;

  2. d3=h−de−zdd_3 = h - d_e - z_d;

  3. θ4=θ1+θ2−φd\theta_4 = \theta_1 + \theta_2 - \varphi_d.

θ1 =   -6.51°, θ2 =   86.42°, d3 = 0.300 m, θ4 =   49.91°   ->  o_e = [0.9  0.5  0.35], yaw = 30.00°
θ1 =   64.62°, θ2 =  -86.42°, d3 = 0.300 m, θ4 =  -51.80°   ->  o_e = [0.9  0.5  0.35], yaw = 30.00°

7.3.3 Spherical wrist: orientation IK

In Section 5.5 the factors Rotx,−90∘ Rotz,θ2 Rotx,90∘\mathrm{Rot}_{x,-90^\circ}\,\mathrm{Rot}_{z,\theta_2}\,\mathrm{Rot}_{x,90^\circ} make a rotation about yy, so the wrist orientation is the ZYZ Euler angle product Re0=Rz(θ1) Ry(θ2) Rz(θ3)\mathbf{R}_e^0 = \mathbf{R}_z(\theta_1)\,\mathbf{R}_y(\theta_2)\,\mathbf{R}_z(\theta_3):

Re0=[c1c2c3−s1s3−c1c2s3−s1c3c1s2s1c2c3+c1s3−s1c2s3+c1c3s1s2−s2c3s2s3c2].\mathbf{R}_e^0 = \begin{bmatrix} c_1c_2c_3 - s_1s_3 & -c_1c_2s_3 - s_1c_3 & c_1s_2\\ s_1c_2c_3 + c_1s_3 & -s_1c_2s_3 + c_1c_3 & s_1s_2\\ -s_2c_3 & s_2s_3 & c_2 \end{bmatrix}.

Given a desired Rd=[rij]\mathbf{R}_d = [r_{ij}], read the angles off the last row and column:

  • r33=c2r_{33} = c_2 and r132+r232=s22r_{13}^2 + r_{23}^2 = s_2^2, so θ2=atan2 ⁣(±r132+r232, r33)\theta_2 = \mathrm{atan2}\!\left(\pm\sqrt{r_{13}^2 + r_{23}^2},\ r_{33}\right);

  • if s2≠0s_2 \ne 0: θ1=atan2(r23/s2, r13/s2)\theta_1 = \mathrm{atan2}(r_{23}/s_2,\ r_{13}/s_2) and θ3=atan2(r32/s2, −r31/s2)\theta_3 = \mathrm{atan2}(r_{32}/s_2,\ -r_{31}/s_2).

Dividing by s2s_2 keeps the signs right. The two choices of sign give the two solutions (θ1,θ2,θ3)(\theta_1, \theta_2, \theta_3) and (θ1+π,−θ2,θ3+π)(\theta_1 + \pi, -\theta_2, \theta_3 + \pi), which are the “wrist flip”.

At s2=0s_2 = 0 (θ2=0\theta_2 = 0 or π\pi) axes 1 and 3 line up: the gimbal-lock singularity of Section 6.5.2. Then only θ1+θ3\theta_1 + \theta_3 (or θ1−θ3\theta_1 - \theta_3) is determined, and there are infinitely many solutions. A controller keeps θ1\theta_1 at its current value and solves for θ3\theta_3.

θ = [  40.   65. -110.]°   reproduces R_d: True
θ = [-140.  -65.   70.]°   reproduces R_d: True
singular, keep θ1 =  0°:  θ = [ 0.  0. 60.]°   reproduces R_d: True
singular, keep θ1 = 25°:  θ = [25.  0. 35.]°   reproduces R_d: True
singular, keep θ1 = 50°:  θ = [50.  0. 10.]°   reproduces R_d: True

7.3.4 Six-joint arms (overview)

For a 6-joint arm with a spherical wrist (the PUMA and many ABB, KUKA and Fanuc arms), decoupling gives the complete solution:

  1. wrist center pw=pd−d6 Rdz^\mathbf{p}_w = \mathbf{p}_d - d_6\,\mathbf{R}_d\hat{\mathbf{z}};

  2. θ1,θ2,θ3\theta_1,\theta_2,\theta_3 from pw\mathbf{p}_w: shoulder left/right ×\times elbow up/down, up to 4 solutions;

  3. R63=(R30)⊤Rd\mathbf{R}_6^3 = (\mathbf{R}_3^0)^\top\mathbf{R}_d, then the wrist angles as in Section 7.3.3: wrist flip, ×2\times2.

That gives up to 8 solutions. The UR5e has no spherical wrist, but its three parallel axes (2, 3, 4) also allow a closed form with up to 8 solutions. We won’t derive it here; in Section 7.6.3 we find its solutions numerically instead. Tools such as IKFast (OpenRAVE) generate these closed-form solvers automatically from a robot model.

Key Takeaways — Section 7.3

  • Planar 3R: wrist point w=pd−L3(cos⁡φd,sin⁡φd)\mathbf{w} = \mathbf{p}_d - L_3(\cos\varphi_d, \sin\varphi_d), a 2R problem, then θ3=φd−θ1−θ2\theta_3 = \varphi_d - \theta_1 - \theta_2.

  • SCARA: 2R for (x,y)(x,y), d3d_3 from zz, θ4\theta_4 from yaw.

  • Spherical wrist: ZYZ angles from the last row and column of Rd\mathbf{R}_d; two solutions, infinitely many at s2=0s_2 = 0.

  • 6-joint arms with a spherical wrist: decouple position and orientation, up to 8 solutions.

7.4 SO-ARM101 Analytical IK (Position + Pitch)

7.4.1 What can we ask the SO-ARM101 to do?

The SO-ARM101 has five arm joints, but a gripper pose has six numbers, so it cannot reach an arbitrary pose. Its structure (Figure 7.3, the body frames of its MuJoCo model) tells us what it can do:

  • Joint 1 (shoulder_pan) has a vertical axis and turns the whole arm.

  • Joints 2, 3, 4 (shoulder_lift, elbow_flex, wrist_flex) have parallel, horizontal axes, perpendicular to the arm. They move the gripper within one vertical plane, the arm plane.

  • Joint 5 (wrist_roll) turns about the gripper’s approach axis xe\mathbf{x}_e and spins the jaws.

So the approach axis xe\mathbf{x}_e always lies in the arm plane. Its heading is set by θ1\theta_1, and its tilt is the pitch φ\varphi: the angle of xe\mathbf{x}_e above the horizontal. φ=0\varphi = 0 points straight out, and φ=−90∘\varphi = -90^\circ points straight down (the top-down grasp). The IK task is therefore

Given the gripper position pd=(xd,yd,zd)\mathbf{p}_d = (x_d, y_d, z_d) and pitch φd\varphi_d, find θ1,…,θ4\theta_1,\dots,\theta_4. The roll θ5\theta_5 is chosen directly, e.g. to line the jaws up with a block.

That is 4 equations for 4 unknowns.

Figure 7.3 — SO-ARM101 frames and link offsets from the MuJoCo model. Joint i turns about the blue z-axis of frame {i}.

7.4.2 The arm-plane geometry

We measure everything from the forward kinematics at home (θ=0\boldsymbol\theta = \mathbf{0}):

  • The pan axis is vertical at (x,y)=(xp,0)(x, y) = (x_p, 0) with xp=0.0388x_p = 0.0388 m, and z1=−z0\mathbf{z}_1 = -\mathbf{z}_0 points down. A positive θ1\theta_1 therefore turns the arm clockwise seen from above, and the heading ψ\psi of the arm plane (measured counterclockwise from x0x_0) is ψ=−θ1\psi = -\theta_1.

  • In the arm plane we use coordinates (r,z)(r, z): rr is the horizontal distance from the pan axis and zz the height above the table (frame {0}\{0\}).

  • S,E,WS, E, W are the points where the axes of joints 2, 3, 4 pierce the arm plane, and PP is the gripper point oe\mathbf{o}_e (the gripperframe site between the fingertips).

Approach axis x_e at home: [ 1. -0.  0.] (horizontal, along x0)

x_p    = 0.03884 m      pan axis position
S      = (0.03040, 0.11660) m   shoulder-lift axis in the arm plane (r, z)
L1     = 0.11600 m      |SE|, upper arm
L2     = 0.13500 m      |EW|, forearm
L3     = 0.15942 m      |WP|, wrist to gripper point
α1     = 76.032°     heading of SE at home
γ0     = 106.175°    elbow angle S-E-W at home
δ      = -2.841°     tilt of WP from the approach axis x_e

Figure 7.4 — SO-ARM101 IK geometry. (a) Top view: θ1 aims the arm plane at the target. (b) The arm plane: θ2, θ3, θ4 are measured from the home pose (faded), each from where its link points with that joint at zero (dashed); α1 and γ0 are read off the home pose. (c) Solving: back off from Pd to the wrist point W, then solve the triangle S, E, W.

Measure the servo angles from home. The servo angles are rotations of the links about joint axes 2, 3, 4, so we draw them directly in the arm plane (Figure 7.4(b)):

  • Direction. At home the axes of joints 2, 3, 4 point along +y0+y_0. In Figure 7.4(b) (r to the right, z up) that is into the page, and a right-handed rotation about an into-the-page axis is clockwise in the picture. So a positive θ\theta turns its link down.

  • Zero. Each angle is measured from where its link points when that joint is at zero (the dashed lines): θ2\theta_2 from the home upper arm, θ3\theta_3 from the forearm carried along rigidly by the upper arm, and θ4\theta_4 from the last link carried along by the forearm.

Two constants of the home pose, printed above, pin down the picture: α1\alpha_1, the angle of the upper arm SESE above the horizontal, and γ0\gamma_0, the interior angle SS–EE–WW (how bent the elbow is when θ3=0\theta_3 = 0).

Pitch. PP sits slightly below the approach axis through WW, so the last link WPWP is tilted by a fixed angle δ\delta from xe\mathbf{x}_e (at home xe\mathbf{x}_e is horizontal, so δ\delta is just the home heading of WPWP). All three joints turn xe\mathbf{x}_e clockwise, so

φ=−(θ2+θ3+θ4),\varphi = -(\theta_2 + \theta_3 + \theta_4),

and the last link WPWP points at φ+δ\varphi + \delta above the horizontal.

(This is the planar 3R arm of Section 7.3.1 with its base at SS; measuring the angles from home lets us solve for the servo angles directly.) The cell below checks this picture against the full 3D forward kinematics: it rebuilds PP and φ\varphi from θ2,θ3,θ4\theta_2, \theta_3, \theta_4 in the arm plane and compares them with so101.fk.

Sideways offsets. SS, EE, WW sit 18.3 mm to the side of the pan axis, but that offset is along the joint axes and does not change the planar geometry. The wrist bracket brings the gripper back: with θ5=0\theta_5 = 0, PP is only 0.011 mm from the plane through the pan axis. We ignore those 0.011 mm until Section 7.4.5. (A chain that rounds the wrist_roll rotation to clean 90° turns drops a 2.79° twist in the model’s quaternion; it puts PP 0.4 mm off the plane, and the IK below then misses by about 0.4 mm instead of 0.01 mm.)

Planar FK vs. 3D FK, 500 random poses: max |ΔP| = 1.7e-16 m, max |Δφ| = 2.0e-13 rad

7.4.3 The solution, step by step

Given pd=(xd,yd,zd)\mathbf{p}_d = (x_d, y_d, z_d), the pitch φd\varphi_d and the roll θ5\theta_5 (Figure 7.4(c)):

Step 1, aim the arm plane (θ1\theta_1). ψ=atan2(yd, xd−xp)\psi = \mathrm{atan2}(y_d,\ x_d - x_p),  θ1=−ψ\ \theta_1 = -\psi, and the target in the arm plane is Pd=(r,zd)P_d = (r, z_d) with r=(xd−xp)2+yd2r = \sqrt{(x_d - x_p)^2 + y_d^2}.

Step 2, back off to the wrist point. The last link must point at φd+δ\varphi_d + \delta above the horizontal, so

W=Pd−L3(cos⁡(φd+δ), sin⁡(φd+δ)),D=∣W−S∣.W = P_d - L_3\big(\cos(\varphi_d + \delta),\ \sin(\varphi_d + \delta)\big),\qquad D = |W - S| .

What is left is the triangle S,E,WS, E, W with sides L1L_1, L2L_2 and DD.

Step 3, elbow (θ3\theta_3). The law of cosines (Section 7.2.2) gives the interior angle at EE:

γ=acosL12+L22−D22L1L2.\gamma = \mathrm{acos}\frac{L_1^2 + L_2^2 - D^2}{2L_1L_2}.

If the fraction is outside [−1,1][-1, 1], WW is out of reach. A positive θ3\theta_3 bends the elbow further, closing the triangle from its home value γ0\gamma_0, so  θ3=γ0−γ\ \theta_3 = \gamma_0 - \gamma.

Step 4, shoulder (θ2\theta_2). The upper arm makes the angle σ=acos((L12+D2−L22)/(2L1D))\sigma = \mathrm{acos}\big((L_1^2 + D^2 - L_2^2)/(2L_1D)\big) with the line SWSW, whose angle above the horizontal is ψSW=atan2(W−S)\psi_{SW} = \mathrm{atan2}(W - S). With the elbow up, SESE lies above SWSW, so the upper arm points at ψSW+σ\psi_{SW} + \sigma. At home it pointed at α1\alpha_1, and θ2\theta_2 is how far it has turned clockwise since:  θ2=α1−(ψSW+σ)\ \theta_2 = \alpha_1 - (\psi_{SW} + \sigma).

Step 5, wrist (θ4\theta_4). From the pitch:  θ4=−φd−θ2−θ3\ \theta_4 = -\varphi_d - \theta_2 - \theta_3.

Step 6, roll and limits. θ5\theta_5 is the roll you want. Check all five joints against their limits.

Elbow down is the mirror-image triangle, with EE below SWSW: θ2=α1−(ψSW−σ)\theta_2 = \alpha_1 - (\psi_{SW} - \sigma) and θ3=γ0+γ−360∘\theta_3 = \gamma_0 + \gamma - 360^\circ.

Note that θ3=γ0−γ\theta_3 = \gamma_0 - \gamma depends only on the distance D=∣SW∣D = |SW|: when the arm is straight (γ=180∘\gamma = 180^\circ) the elbow servo reads γ0−180∘=−73.8∘\gamma_0 - 180^\circ = -73.8^\circ.

Target p_d = [0.25 0.05 0.02] m, pitch = -90°:
    shoulder_pan     -13.32°
    shoulder_lift     19.46°
    elbow_flex        -2.59°
    wrist_flex        73.13°
    wrist_roll         0.00°
    FK: p = [0.25 0.05 0.02] m, pitch = -90.00°   (position error 0.0112 mm)

The error of about 0.01 mm is the sideways offset we ignored; Section 7.4.5 removes it. Let’s test the solver on many random targets. We generate each target from a random joint vector (so we know it is reachable), solve, and compare poses rather than joints, because the random joint vector may be on a different branch.

2470 IK solves: max position error 0.0112 mm, max pitch error 7.2e-12°

7.4.4 Branches, joint limits, and where top-down grasps are possible

Mathematically there are up to four solutions: elbow up or down, and the arm facing the target or reaching backward over its own base (θ1±180∘\theta_1 \pm 180^\circ). The joint limits remove nearly all of them:

  • Elbow down needs θ3=γ0+γ−360∘≥−96.8∘\theta_3 = \gamma_0 + \gamma - 360^\circ \ge -96.8^\circ (the elbow limit), so γ>157∘\gamma > 157^\circ: only an almost straight arm.

  • Reaching backward bends the shoulder far past its ±100∘\pm100^\circ limit. In thousands of random targets on the table, no backward solution was within the limits.

So in practice the SO-ARM101 has one usable solution: elbow up, facing the target. That is why so101_ik defaults to elbow='up'.

elbow up:  within limits = True
    shoulder_pan      18.32°
    shoulder_lift      5.39°
    elbow_flex        12.64°
    wrist_flex        71.97°
    wrist_roll         0.00°
    FK: p = [ 0.22 -0.06  0.03] m, pitch = -90.00°   (position error 0.0112 mm)
elbow down:  within limits = False
    shoulder_pan      18.32°
    shoulder_lift    100.00°
    elbow_flex      -160.29°   <-- outside [-96.8, 96.8]°
    wrist_flex       150.29°   <-- outside [-95.0, 95.0]°
    wrist_roll         0.00°
    FK: p = [ 0.22 -0.06  0.03] m, pitch = -90.00°   (position error 0.0112 mm)

Where can the gripper point straight down? Sweep θ2\theta_2 and θ3\theta_3 over their ranges, set θ4=90∘−θ2−θ3\theta_4 = 90^\circ - \theta_2 - \theta_3 so that φ=−90∘\varphi = -90^\circ, keep only θ4\theta_4 within its limit, and plot the gripper point with the planar FK. The light background shows every point the arm can reach with some pitch.

<Figure size 900x550 with 1 Axes>
Top-down grasp at z = 0.015 m possible for r from 0.048 to 0.265 m (world x from 0.086 to 0.304 m when y = 0)

Below the table the region is mathematically reachable but physically blocked; IK knows nothing about the table, so your code has to. When a target is too far for a top-down grasp, a common fix is to tilt the approach: try φ=−90∘,−85∘,…\varphi = -90^\circ, -85^\circ, \dots and take the first pitch whose solution is within the limits.

target x (m)  pitch used   joint angles (°)
        0.15        -90°   [  0.  -31.8  57.1  64.8]
        0.25        -90°   [ 0.  17.3  2.8 69.9]
        0.30        -90°   [  0.   49.5 -49.8  90.3]
        0.33        -80°   [  0.   52.4 -54.5  82.1]
        0.36        -70°   [  0.   56.4 -59.1  72.8]
        0.40        -55°   [  0.   59.  -56.5  52.5]

Explore it. The sliders set the target and pitch, and the arm is drawn from so101_ik. Joints outside their limits are flagged. Try pushing the target outward with a top-down pitch and watch θ4\theta_4 run into its limit, then tilt the pitch.

Loading...

7.4.5 Exact solution for any wrist roll (optional)

The gripper point lies ∣c∣=7.9|c| = 7.9 mm off the roll axis, so rolling θ5\theta_5 swings it on a small circle: up to 7.9 mm sideways, out of the arm plane, and it also changes the in-plane offset. The problem stays solvable in closed form, and it uses the equation of Section 7.2.3.

Tool offsets for a given roll. Joints 2 to 4 only rotate frame {4}\{4\} about the sideways axis, so the gripper point seen from frame {4}\{4\} depends on θ5\theta_5 alone: pe4(θ5)=\mathbf{p}_e^4(\theta_5) = origin of T54(θ5) Te5\mathbf{T}_5^4(\theta_5)\,\mathbf{T}_e^5. At home, −y4-\mathbf{y}_4 is the approach axis, −x4-\mathbf{x}_4 is “up” across it, and z4\mathbf{z}_4 is sideways. So

a(θ5)=−py,c(θ5)=−px,d(θ5)=d4+pz,a(\theta_5) = -p_y,\qquad c(\theta_5) = -p_x,\qquad d(\theta_5) = d_4 + p_z,

where d4=−18.3d_4 = -18.3 mm is the sideways offset of o4\mathbf{o}_4 from the plane through the pan axis. In the arm plane, the last link becomes L3(θ5)=a2+c2L_3(\theta_5) = \sqrt{a^2 + c^2} with tilt δ(θ5)=atan2(c,a)\delta(\theta_5) = \mathrm{atan2}(c, a) from xe\mathbf{x}_e.

Step 1 with a sideways offset. Write the target relative to the pan axis as X=xd−xpX = x_d - x_p, Y=ydY = y_d. If the arm plane has heading ψ\psi, the gripper point is rr along the plane and dd sideways: (X,Y)=r (cos⁡ψ,sin⁡ψ)+d (−sin⁡ψ,cos⁡ψ)(X, Y) = r\,(\cos\psi, \sin\psi) + d\,(-\sin\psi, \cos\psi). Taking the sideways component gives

−Xsin⁡ψ+Ycos⁡ψ=d,-X\sin\psi + Y\cos\psi = d ,

which is exactly acos⁡ψ+bsin⁡ψ=ca\cos\psi + b\sin\psi = c with (a,b,c)=(Y,−X,d)(a, b, c) = (Y, -X, d). Of its two roots, one faces the target (r>0r > 0) and the other faces away. The forward-facing one is

r=X2+Y2−d2,ψ=atan2(Y,X)−atan2(d,r),θ1=−ψ.r = \sqrt{X^2 + Y^2 - d^2},\qquad \psi = \mathrm{atan2}(Y, X) - \mathrm{atan2}(d, r),\qquad \theta_1 = -\psi .

Steps 2 to 6 are unchanged, using L3(θ5)L_3(\theta_5) and δ(θ5)\delta(\theta_5) in Step 2.

θ5 =   0°:  a = 159.227 mm,  c = -7.901 mm,  d = -0.011 mm
θ5 =  45°:  a = 159.227 mm,  c = -5.705 mm,  d = -5.647 mm
θ5 =  90°:  a = 159.227 mm,  c = -0.167 mm,  d = -8.079 mm

Exact IK, random targets and random roll: max position error = 3.5e-14 m
Roots of -X sinψ + Y cosψ = d: [  21.475 -162.895] °;  forward-facing root used above: 21.475 °

Key Takeaways — Section 7.4

  • The SO-ARM101 has 5 joints, so we ask for position + pitch (4 numbers) and set the roll θ5\theta_5 directly.

  • Structure: the pan joint aims the arm plane; joints 2 to 4 swing the links SESE, EWEW, WPWP in it. Measure θ2,θ3,θ4\theta_2, \theta_3, \theta_4 from the home pose (positive = clockwise in Figure 7.4), using the home constants α1\alpha_1, γ0\gamma_0, δ\delta; the pitch is φ=−(θ2+θ3+θ4)\varphi = -(\theta_2+\theta_3+\theta_4).

  • Recipe: θ1=−atan2(yd,xd−xp)\theta_1 = -\mathrm{atan2}(y_d, x_d - x_p); wrist point W=Pd−L3u(φd+δ)W = P_d - L_3\mathbf{u}(\varphi_d + \delta); law of cosines on S,E,WS, E, W: θ3=γ0−γ\theta_3 = \gamma_0 - \gamma, θ2=α1−(ψSW+σ)\theta_2 = \alpha_1 - (\psi_{SW} + \sigma); then θ4=−φd−θ2−θ3\theta_4 = -\varphi_d - \theta_2 - \theta_3.

  • Joint limits leave one usable branch (elbow up, facing the target). A top-down grasp at table height works only in a band of rr; beyond it, tilt the pitch.

  • With θ5≠0\theta_5 \neq 0 the gripper point moves sideways, and θ1\theta_1 comes from acos⁡ψ+bsin⁡ψ=ca\cos\psi + b\sin\psi = c.

7.5 Numerical IK

Analytical IK needs a derivation for each robot and a robot with convenient structure. For anything else (offset wrists, 7-joint arms, humanoids, or tasks that mix several goals) we solve numerically. All we need is the forward kinematics and the Jacobian, and we already have both.

7.5.1 IK as root finding: Newton–Raphson

Write the forward kinematics as a function f\mathbf{f} of the joint angles:

Yd=f(θd),Yd=[pdod],pd=[xyz] (position),od=[ϕϑψ] (orientation)\mathbf{Y}_d = \mathbf{f}(\boldsymbol\theta_d),\qquad \mathbf{Y}_d = \begin{bmatrix} \mathbf{p}_d \\ \mathbf{o}_d \end{bmatrix},\quad \mathbf{p}_d = \begin{bmatrix} x \\ y \\ z \end{bmatrix}\ \text{(position)},\quad \mathbf{o}_d = \begin{bmatrix} \phi \\ \vartheta \\ \psi \end{bmatrix}\ \text{(orientation)}

where θd\boldsymbol\theta_d are the joint angles. Yd\mathbf{Y}_d can also hold only part of the pose (position only; position + pitch), with mm entries in general.

Inverse kinematics: given Yd\mathbf{Y}_d, solve for the corresponding θd\boldsymbol\theta_d. Move everything to one side and define

g(θ)=Yd−f(θ)=0,\mathbf{g}(\boldsymbol\theta) = \mathbf{Y}_d - \mathbf{f}(\boldsymbol\theta) = \mathbf{0},

a set of nonlinear equations in θ\boldsymbol\theta. The vector g\mathbf{g} is the error between where we want the gripper and where joints θ\boldsymbol\theta put it, and the IK solution is its root: g(θd)=0\mathbf{g}(\boldsymbol\theta_d) = \mathbf{0}.

Let θ0\boldsymbol\theta^0 be an initial guess close to θd\boldsymbol\theta_d. Expanding f\mathbf{f} in a Taylor series about θ0\boldsymbol\theta^0,

Yd=f(θd)=f(θ0)+∂f∂θ∣θ0⏟J(θ0)(θd−θ0)+higher-order terms.\mathbf{Y}_d = \mathbf{f}(\boldsymbol\theta_d) = \mathbf{f}(\boldsymbol\theta^0) + \underbrace{\left.\frac{\partial\mathbf{f}}{\partial\boldsymbol\theta}\right|_{\boldsymbol\theta^0}}_{\mathbf{J}(\boldsymbol\theta^0)}(\boldsymbol\theta_d - \boldsymbol\theta^0) + \text{higher-order terms}.

The derivative is the Jacobian J(θ0)\mathbf{J}(\boldsymbol\theta^0) of Chapter 6. Dropping the higher-order terms and solving for θd\boldsymbol\theta_d,

θd≈θ0+J−1(θ0)[Yd−f(θ0)].\boldsymbol\theta_d \approx \boldsymbol\theta^0 + \mathbf{J}^{-1}(\boldsymbol\theta^0)\left[\mathbf{Y}_d - \mathbf{f}(\boldsymbol\theta^0)\right].

Because the higher-order terms were dropped, this is only a better guess, not the answer. Repeating it gives Newton’s method (Section 6.6.2), the iterative rule at step kk:

θk=θk−1+J−1(θk−1)[Yd−f(θk−1)]\boxed{\boldsymbol\theta^k = \boldsymbol\theta^{k-1} + \mathbf{J}^{-1}(\boldsymbol\theta^{k-1})\left[\mathbf{Y}_d - \mathbf{f}(\boldsymbol\theta^{k-1})\right]}

In the rest of this section we write the step as Δθ=θk−θk−1\Delta\boldsymbol\theta = \boldsymbol\theta^k - \boldsymbol\theta^{k-1} and the current error as e=g(θk−1)=Yd−f(θk−1)\mathbf{e} = \mathbf{g}(\boldsymbol\theta^{k-1}) = \mathbf{Y}_d - \mathbf{f}(\boldsymbol\theta^{k-1}), so each Newton step solves J Δθ=e\mathbf{J}\,\Delta\boldsymbol\theta = \mathbf{e}.

Close to a solution it converges quadratically: the number of correct digits roughly doubles every iteration. But it

  • needs a square, invertible J\mathbf{J} (m=nm = n, away from singularities);

  • can jump far away or diverge from a poor initial guess;

  • has nothing sensible to converge to when the target is unreachable.

The one-joint version. Take a unit link turning about a fixed joint, and let the task be the height of its tip. Each piece of the general method becomes a scalar:

GeneralOne joint
forward kinematics f(θ)\mathbf{f}(\boldsymbol\theta)f(θ)=sin⁡θf(\theta) = \sin\theta
target Yd\mathbf{Y}_dYd=hdY_d = h_d
g(θ)=Yd−f(θ)\mathbf{g}(\boldsymbol\theta) = \mathbf{Y}_d - \mathbf{f}(\boldsymbol\theta)g(θ)=hd−sin⁡θg(\theta) = h_d - \sin\theta
Jacobian J=∂f/∂θ\mathbf{J} = \partial\mathbf{f}/\partial\boldsymbol\thetaJ(θ)=dfdθ=cos⁡θJ(\theta) = \dfrac{df}{d\theta} = \cos\theta
Taylor expansion Yd≈f(θk−1)+J(θk−1)(θk−θk−1)\mathbf{Y}_d \approx \mathbf{f}(\boldsymbol\theta^{k-1}) + \mathbf{J}(\boldsymbol\theta^{k-1})(\boldsymbol\theta^k - \boldsymbol\theta^{k-1})hd≈sin⁡θk−1+cos⁡θk−1 (θk−θk−1)h_d \approx \sin\theta^{k-1} + \cos\theta^{k-1}\,(\theta^k - \theta^{k-1})
Newton rule θk=θk−1+J−1[Yd−f(θk−1)]\boldsymbol\theta^k = \boldsymbol\theta^{k-1} + \mathbf{J}^{-1}\left[\mathbf{Y}_d - \mathbf{f}(\boldsymbol\theta^{k-1})\right]θk=θk−1+hd−sin⁡θk−1cos⁡θk−1\theta^k = \theta^{k-1} + \dfrac{h_d - \sin\theta^{k-1}}{\cos\theta^{k-1}}

so the iteration is

θk=θk−1+hd−sin⁡θk−1cos⁡θk−1\boxed{\theta^k = \theta^{k-1} + \frac{h_d - \sin\theta^{k-1}}{\cos\theta^{k-1}}}

Geometrically, sin⁡θk−1+cos⁡θk−1 (θ−θk−1)\sin\theta^{k-1} + \cos\theta^{k-1}\,(\theta - \theta^{k-1}) is the tangent line to f(θ)f(\theta) at θk−1\theta^{k-1}, and θk\theta^k is where that line reaches the height hdh_d.

Reachable target, hd=0.8h_d = 0.8, starting from θ0=0.2\theta^0 = 0.2 rad:

kkθk\theta^k (rad)g(θk)=0.8−sin⁡θkg(\theta^k) = 0.8 - \sin\theta^kJ(θk)=cos⁡θkJ(\theta^k) = \cos\theta^kθk+1−θk=g/J\theta^{k+1} - \theta^k = g / J
00.20000.60130.98010.6136
10.81360.07330.68690.1067
20.92020.00430.60560.0070
30.9273≈2×10−5\approx 2\times10^{-5}0.6000≈3×10−5\approx 3\times10^{-5}

The error shrinks 0.60→0.073→0.0043→0.000020.60 \to 0.073 \to 0.0043 \to 0.00002: each error is roughly the square of the one before, which is quadratic convergence. The limit is the exact solution θ∗=arcsin⁡0.8=0.9273\theta^* = \arcsin 0.8 = 0.9273 rad.

Unreachable target, hd=1.2>1h_d = 1.2 > 1: g(θ)=1.2−sin⁡θ≥0.2g(\theta) = 1.2 - \sin\theta \ge 0.2 for every θ\theta, so there is no root to find. The tangent steps overshoot θ=π/2\theta = \pi/2, where J=cos⁡θJ = \cos\theta changes sign, and the iteration bounces from side to side (0.20→1.22→1.98→1.27→2.11→⋯0.20 \to 1.22 \to 1.98 \to 1.27 \to 2.11 \to \cdots). Near θ=π/2\theta = \pi/2 the Jacobian cos⁡θ→0\cos\theta \to 0 and a single step g/cos⁡θg/\cos\theta can be enormous: the one-joint picture of a singularity.

The figure shows both cases: each step follows the tangent line to the target height.

h_d = 0.8 (reachable):   θ_k = [0.2    0.8136 0.9202 0.9273 0.9273 0.9273]
h_d = 1.2 (unreachable): θ_k = [0.2   1.222 1.983 1.274 2.108 1.442 3.065 1.938]
<Figure size 1400x460 with 2 Axes>

7.5.2 Non-square Jacobians: the pseudo-inverse

When n>mn > m (a redundant robot) the equation JΔθ=e\mathbf{J}\Delta\boldsymbol\theta = \mathbf{e} has infinitely many solutions. The natural choice is the smallest joint step that does the job, the minimum-norm solution

Δθ=J+e,J+=J⊤(JJ⊤)−1.\Delta\boldsymbol\theta = \mathbf{J}^{+}\mathbf{e},\qquad \mathbf{J}^{+} = \mathbf{J}^\top(\mathbf{J}\mathbf{J}^\top)^{-1}.

When there is no exact solution (m>nm > n, an unreachable target, or a singular J\mathbf{J}), the best we can do is the least-squares step that makes ∥JΔθ−e∥\|\mathbf{J}\Delta\boldsymbol\theta - \mathbf{e}\| as small as possible. The pseudo-inverse np.linalg.pinv(J) (computed with the SVD) gives the right answer in every case, and equals J−1\mathbf{J}^{-1} when J\mathbf{J} is square and invertible.

7.5.3 Jacobian transpose: gradient descent

Instead of solving equations, minimize the squared error E(θ)=12∥e(θ)∥2E(\boldsymbol\theta) = \tfrac12\|\mathbf{e}(\boldsymbol\theta)\|^2. By the chain rule its gradient is ∇E=−J⊤e\nabla E = -\mathbf{J}^\top\mathbf{e}, so a step downhill is

Δθ=α J⊤e(α>0 a step size).\Delta\boldsymbol\theta = \alpha\,\mathbf{J}^\top\mathbf{e}\qquad(\alpha > 0 \text{ a step size}).

It needs no inverse, so it is always defined, even at singularities, and it is cheap. But it converges slowly (linearly), and α\alpha must be tuned. There is a nice physical picture from Section 6.7: attach a spring from the gripper to the target, pulling with force F=e\mathbf{F} = \mathbf{e}. The joint torques it creates are τ=J⊤F\boldsymbol\tau = \mathbf{J}^\top\mathbf{F}, and the method simply lets each joint move in the direction of its torque.

7.5.4 Damped least squares (Levenberg–Marquardt)

The robust compromise looks for a step that reduces the linearized error without being too large:

Δθ=arg⁡min⁡Δθ ∥JΔθ−e∥2+λ2∥Δθ∥2⟹Δθ=J⊤(JJ⊤+λ2I)−1e\Delta\boldsymbol\theta = \arg\min_{\Delta\boldsymbol\theta}\ \|\mathbf{J}\Delta\boldsymbol\theta - \mathbf{e}\|^2 + \lambda^2\|\Delta\boldsymbol\theta\|^2 \quad\Longrightarrow\quad \boxed{\Delta\boldsymbol\theta = \mathbf{J}^\top\left(\mathbf{J}\mathbf{J}^\top + \lambda^2\mathbf{I}\right)^{-1}\mathbf{e}}
  • λ→0\lambda \to 0: the pseudo-inverse (Newton) step, which is fast but huge near singularities.

  • Large λ\lambda: ≈λ−2J⊤e\approx \lambda^{-2}\mathbf{J}^\top\mathbf{e}, a small, safe gradient step.

  • Near a singularity, λ2I\lambda^2\mathbf{I} keeps the matrix invertible and the step bounded: ∥Δθ∥≤∥e∥/(2λ)\|\Delta\boldsymbol\theta\| \le \|\mathbf{e}\|/(2\lambda). For an unreachable target the iteration settles near the closest reachable point instead of oscillating, provided that bound is small (a large enough λ\lambda, or a step limit).

  • Typical λ\lambda is a few percent of the arm’s size in metres. Levenberg–Marquardt variants adapt λ\lambda: increase it when a step makes the error worse, decrease it when the error drops.

Update Δθ\Delta\boldsymbol\thetaNameNeedsSpeedNear singularity / unreachable
J−1e\mathbf{J}^{-1}\mathbf{e}Newtonsquare, invertible J\mathbf{J}quadraticfails / oscillates
J+e\mathbf{J}^{+}\mathbf{e}pseudo-inverseany J\mathbf{J}quadratichuge steps
αJ⊤e\alpha\mathbf{J}^\top\mathbf{e}Jacobian transposestep size α\alphaslowsafe
J⊤(JJ⊤+λ2I)−1e\mathbf{J}^\top(\mathbf{J}\mathbf{J}^\top + \lambda^2\mathbf{I})^{-1}\mathbf{e}damped least squaresdamping λ\lambdafast (slower near the solution if λ\lambda is large)safe, closest point

7.5.5 A practical numerical IK solver

input: error function e(θ), task Jacobian J(θ), initial guess θ0, limits, tolerance, k_max
for k = 0, 1, ..., k_max:
    e = e(θ)
    if ||e|| < tolerance:  return θ (success)
    Δθ = Jᵀ (J Jᵀ + λ² I)⁻¹ e                      (or another rule from the table)
    if ||Δθ|| > Δ_max:  scale Δθ down to length Δ_max   (avoid wild jumps)
    θ = clamp(θ + Δθ, θ_min, θ_max)                (respect joint limits)
return θ (failure: report the remaining error)

Practical points:

  • Initial guess. Use the robot’s current joints. The solver then returns the solution nearest to where the robot is, which keeps the branch consistent and the motion smooth. To find other solutions, restart from random guesses.

  • Units and weights. Position errors are in metres and angles in radians. A weight matrix W\mathbf{W} sets what matters most: Δθ=(J⊤WJ+λ2I)−1J⊤We\Delta\boldsymbol\theta = (\mathbf{J}^\top\mathbf{W}\mathbf{J} + \lambda^2\mathbf{I})^{-1}\mathbf{J}^\top\mathbf{W}\mathbf{e}.

  • Joint limits. After each step, clamp the joints into their limits. A joint pinned at a limit wastes every step that pushes it further out, so our solver also freezes such a joint: it zeroes that column of J\mathbf{J} and recomputes the step, letting the other joints do the work. Optimization-based IK (Section 7.5.6) builds the limits into the problem itself.

  • Always check the result. The solver may stop at an unreachable target’s closest point, or at a local minimum. Check ∥e∥\|\mathbf{e}\| before sending the joints to the robot.

Orientation error. For full-pose tasks we cannot subtract rotation matrices. With the current orientation R=[x y z]\mathbf{R} = [\mathbf{x}\ \mathbf{y}\ \mathbf{z}] and the desired one Rd=[xd yd zd]\mathbf{R}_d = [\mathbf{x}_d\ \mathbf{y}_d\ \mathbf{z}_d], use

eo=12(x×xd+y×yd+z×zd).\mathbf{e}_o = \tfrac12\left(\mathbf{x}\times\mathbf{x}_d + \mathbf{y}\times\mathbf{y}_d + \mathbf{z}\times\mathbf{z}_d\right).

Each cross product turns one current axis toward its desired direction, and eo=0\mathbf{e}_o = \mathbf{0} exactly when R=Rd\mathbf{R} = \mathbf{R}_d. For a rotation by angle ϑ\vartheta about axis k^\hat{\mathbf{k}} (Chapter 4) taking R\mathbf{R} to Rd\mathbf{R}_d, eo=sin⁡ϑ k^\mathbf{e}_o = \sin\vartheta\,\hat{\mathbf{k}}, so for small errors eo\mathbf{e}_o is the rotation vector, in the same units as ω\boldsymbol\omega. It pairs with the angular rows Jω\mathbf{J}_\omega: the full error e=[pd−p; eo]\mathbf{e} = [\mathbf{p}_d - \mathbf{p};\ \mathbf{e}_o] goes with the 6×n6\times n Jacobian.

e_o            = [0.1667 0.3333 0.3333]
sin(30°) · k̂   = [0.1667 0.3333 0.3333]

7.5.6 IK as optimization, and modern solvers

Damped least squares is already the solution of a small optimization problem. Modern IK libraries make this explicit. At every step they solve a quadratic program (QP) for the joint step:

min⁡Δθ ∑iwi ∥JiΔθ−αiei∥2+λ∥Δθ∥2subject to{θmin⁡≤θ+Δθ≤θmax⁡joint limits∣Δθ∣≤θ˙max⁡ Δtvelocity limitslinearized distance between bodies≥dmin⁡collision avoidance\min_{\Delta\boldsymbol\theta}\ \sum_i w_i\,\big\|\mathbf{J}_i\Delta\boldsymbol\theta - \alpha_i\mathbf{e}_i\big\|^2 + \lambda\|\Delta\boldsymbol\theta\|^2 \quad\text{subject to}\quad \begin{cases} \boldsymbol\theta_{\min} \le \boldsymbol\theta + \Delta\boldsymbol\theta \le \boldsymbol\theta_{\max} & \text{joint limits}\\ |\Delta\boldsymbol\theta| \le \dot{\boldsymbol\theta}_{\max}\,\Delta t & \text{velocity limits}\\ \text{linearized distance between bodies} \ge d_{\min} & \text{collision avoidance} \end{cases}
  • Each task ii is a wish with a weight wiw_i and a gain αi\alpha_i: a gripper pose (often position with a high weight and orientation with a lower one), a posture task that pulls the joints toward a comfortable configuration (this resolves redundancy), the center of mass for a humanoid, and so on. Conflicting tasks are traded off by their weights instead of making the solver fail.

  • Limits are hard inequality constraints. They are never violated, unlike clamping.

  • With one task and no limits, the problem reduces to damped least squares.

  • Run once per control cycle with a gain αi<1\alpha_i < 1, it becomes differential IK: the robot follows a moving target smoothly. This is the resolved-rate idea of Section 6.6.1, with constraints added.

Recent open-source tools built on these ideas:

LibraryBuilt onMethodNotes
Mink (K. Zakka)MuJoCoQP differential IK: tasks + limitsPython port of Pink; frame, posture and center-of-mass tasks; joint, velocity and collision limits
Pink (S. Caron)Pinocchiothe same QP formulationPython
placo (Rhoban)PinocchioQP-based whole-body kinematicsused by LeRobot for SO-100/SO-101 end-effector control (RobotKinematics)
TRAC-IK (Beeson & Ames, 2015)KDL / ROSpseudo-inverse Newton with random restarts and an SQP solver, run in parallelcommon MoveIt plugin
PyRoki (Kim et al., 2025)JAXLevenberg–Marquardt nonlinear least squaresCPU/GPU, batched; also motion retargeting and trajectory optimization
cuRobo (NVIDIA, 2023)PyTorch / CUDAmany seeds optimized in parallel on the GPUcollision-free IK and motion generation
IKFast (OpenRAVE)—generates analytical solversall solutions in microseconds

We use Mink on the SO-ARM101 in Section 7.6.2.

Key Takeaways — Section 7.5

  • Numerical IK solves g(θ)=Yd−f(θ)=0\mathbf{g}(\boldsymbol\theta) = \mathbf{Y}_d - \mathbf{f}(\boldsymbol\theta) = \mathbf{0} by repeated linearization with the Jacobian.

  • Newton / pseudo-inverse: fast, but fragile near singularities and for unreachable targets. Jacobian transpose: safe but slow. Damped least squares: the practical default.

  • Use the current joints as the initial guess; limit the step; respect joint limits; check the final error.

  • Orientation error: eo=12∑ixi×xd,i\mathbf{e}_o = \tfrac12\sum_i \mathbf{x}_i\times\mathbf{x}_{d,i}.

  • Modern IK is a QP: weighted tasks plus hard limits (Mink, Pink, placo); GPU and batched optimizers (PyRoki, cuRobo) find many solutions in parallel.

7.6 Numerical Examples

7.6.1 Planar 2R: comparing the update rules

The task is the tip position (x,y)(x, y), and the task Jacobian is the top two rows of jacobian(robot_2R, q). First a reachable target, then one beyond the reach of L1+L2=1.7L_1 + L_2 = 1.7.

DLS with step limit ends at θ = [26.92  1.58]°, ‖e‖ = 0.5373 m; direction to target = 26.57°
<Figure size 1700x480 with 3 Axes>

For the reachable target, Newton and lightly damped DLS converge in a handful of iterations, with the error dropping faster and faster (quadratic convergence). Heavier damping and the Jacobian transpose are slower.

For the unreachable target, Newton wanders around, because every step aims at a point it can never reach. Lightly damped DLS also wanders: with a leftover error of 0.5 m its step can be as large as ∥e∥/(2λ)=5\|\mathbf{e}\|/(2\lambda) = 5 rad. Limiting the step (or using a larger λ\lambda) fixes this: DLS stretches the arm toward the target and settles next to the closest reachable point, with an error equal to the distance from the reach limit. That is the behavior a controller wants. The last centimetre is slow for λ=0.3\lambda = 0.3 and for the transpose method, because near the stretched (singular) arm the error hardly changes with θ2\theta_2.

Which solution do we get? Run DLS from a grid of initial guesses and color each starting point by the branch it converges to:

<Figure size 650x550 with 1 Axes>
Converged from 98% of the initial guesses (white: not converged in 60 iterations)

Mostly, the solver lands on the branch whose θ2\theta_2 has the same sign as the initial guess, but the boundary between the basins is not a straight line. This is why we start from the robot’s current configuration: the answer is then the one “next to” the robot.

7.6.2 SO-ARM101: position + pitch, numerically

The task is x(θ)=(x,y,z,φ)\mathbf{x}(\boldsymbol\theta) = (x, y, z, \varphi) with the roll θ5\theta_5 held at its chosen value, so we solve for θ1,…,θ4\theta_1,\dots,\theta_4 with a 4×44\times4 task Jacobian:

  • rows 1 to 3: the linear part Jv\mathbf{J}_v of the Chapter 6 Jacobian, columns 1 to 4;

  • row 4: the pitch is a linear function of the joints, φ=−(θ2+θ3+θ4)\varphi = -(\theta_2+\theta_3+\theta_4) (Section 7.4.2), so its row is simply [0, −1, −1, −1][0,\ -1,\ -1,\ -1].

Converged in 10 iterations, final error 8.8e-12
numerical  θ = [-13.3242  19.461   -2.5863  73.1253   0.    ] °
analytical θ = [-13.3242  19.461   -2.5863  73.1253   0.    ] °
error history: [1.6e+00 1.2e+00 7.9e-01 2.3e-01 7.9e-03 4.4e-04 1.3e-05 3.8e-07 1.1e-08
 3.1e-10 8.8e-12]

Starting from home, DLS converges to the analytical (elbow-up) solution in a few iterations. Now a pick-and-place sequence: each waypoint is solved starting from the previous solution, just as a controller would. The block is a 1-inch cube on the table, so we grasp at z=0.015z = 0.015 m, top-down, and approach from 6 cm above.

waypoint     iters        θ1      θ2      θ3      θ4  (°)   |θ_num - θ_analytical|
above pick      11    -28.90   11.60  -14.91   93.31         1.0e-08°
pick             7    -28.90   15.27    5.77   68.95         5.4e-09°
lift             7    -28.90   11.60  -14.91   93.31         8.0e-08°
above place      9     31.82    2.21   -2.64   90.43         4.9e-08°
place            7     31.82    7.25   16.93   65.82         1.6e-09°
retreat          7     31.82    2.21   -2.64   90.43         1.1e-08°
<Figure size 800x650 with 1 Axes>

When the analytical answer breaks a limit. Hovering 10 cm above the table at pd=(0.20,0,0.10)\mathbf{p}_d = (0.20, 0, 0.10) m, a top-down pose needs θ4≈99∘\theta_4 \approx 99^\circ, past the 95∘95^\circ wrist limit. The analytical formula happily returns that infeasible answer. The numerical solver keeps θ4\theta_4 at 95∘95^\circ and finds the best compromise between position and pitch, and the weights W=diag(1,1,1,wφ)\mathbf{W} = \mathrm{diag}(1, 1, 1, w_\varphi) decide what “best” means. A smaller pitch weight says position matters more: the gripper tilts a little more and lands closer to the target. (Units: wφw_\varphi trades metres² against radians².)

Analytical:
    shoulder_pan      -0.00°
    shoulder_lift    -10.83°
    elbow_flex         1.58°
    wrist_flex        99.24°   <-- outside [-95.0, 95.0]°
    wrist_roll         0.00°
    FK: p = [ 0.2 -0.   0.1] m, pitch = -90.00°   (position error 0.0000 mm)

pitch weight   position error      pitch   θ1..θ4 (°)
           1         9.613 mm    -89.93°   [ -0.   -11.89   6.82  95.  ]
        0.01         3.610 mm    -87.30°   [ -0.   -14.84   7.14  95.  ]
      0.0001         0.057 mm    -85.74°   [ -0.   -16.68   7.42  95.  ]

The same problem with Mink. Mink formulates it as the QP of Section 7.5.6, directly on the SO-ARM101 MuJoCo model. We give it two tasks and one limit:

  • a FrameTask on the gripperframe site: position cost 1. The orientation cost is [0,0.1,0.1][0, 0.1, 0.1] on the site’s local axes: zero about xe\mathbf{x}_e (the roll is free), and nonzero about the other two axes, so only the pointing direction of xe\mathbf{x}_e is constrained. The target orientation points xe\mathbf{x}_e straight down;

  • a weak PostureTask that keeps the joints near home (with a larger cost on the roll joint, which nothing else pins down);

  • a ConfigurationLimit: the joint limits as hard constraints.

Each call of solve_ik returns a joint velocity; integrate_inplace takes the step. The cell runs only if mink is installed (pip install mink daqp) and MODEL_PATH points to the SO-ARM101 MuJoCo model (scene.xml in the Simulation/SO101 folder of TheRobotStudio’s SO-ARM100 repository).

Skipped: install mink and daqp (pip install mink daqp) and set MODEL_PATH to the SO-ARM101 scene.xml.

When you run the cell, Mink finds the analytical configuration for the reachable target. For the high target it keeps the wrist at its 95∘95^\circ limit, a hard constraint, and should land close to the compromise of our DLS with pitch weight 0.01. That is no coincidence: a Mink cost cc enters the objective squared, so orientation_cost = 0.1 is our weight wφ=0.01w_\varphi = 0.01, and both solvers minimize the same weighted error subject to the same limit. Change the costs and the trade-off changes. That freedom is the main point of optimization-based IK.

7.6.3 UR5e: full pose, and several solutions

For a 6-joint arm we can ask for the full pose. The error stacks the position error and the orientation error of Section 7.5.5, and the task Jacobian is the full 6×66\times6 Jacobian. We take a target pose from a known configuration and run DLS from 60 random initial guesses.

60 of 60 runs converged; 8 distinct solutions (joint angles in °):
  1: [-126.8 -120.   -90.   -60.    90.    68.2]   ‖T(θ) - T_d‖ = 9e-11
  2: [-126.8 -148.7  -38.5   97.2  -90.  -111.8]   ‖T(θ) - T_d‖ = 8e-11
  3: [-126.8  174.4   38.5   57.1  -90.  -111.8]   ‖T(θ) - T_d‖ = 5e-11
  4: [-126.8  154.6   90.  -154.6   90.    68.2]   ‖T(θ) - T_d‖ = 2e-11
  5: [ 30.   25.4 -90.  -25.4 -90.   45. ]   ‖T(θ) - T_d‖ = 7e-12
  6: [  30.     5.6  -38.5  122.9   90.  -135. ]   ‖T(θ) - T_d‖ = 6e-11
  7: [  30.   -31.3   38.5   82.8   90.  -135. ]   ‖T(θ) - T_d‖ = 7e-11
  8: [  30.  -60.   90. -120.  -90.   45.]   ‖T(θ) - T_d‖ = 2e-11
<Figure size 1600x750 with 8 Axes>

All of these configurations put the gripper at exactly the same pose: the UR5e’s eight branches (shoulder left/right, elbow up/down, wrist flipped/not). Each run finds one of them, decided by its initial guess. Random restarts are how numerical solvers such as TRAC-IK and cuRobo find several solutions and keep the best one.

Key Takeaways — Section 7.6

  • Newton and lightly damped DLS converge quadratically for reachable targets; for unreachable targets only damped methods with a bounded step settle (near the closest point).

  • The solution found depends on the initial guess, so use the robot’s current joints.

  • SO-ARM101: task (x,y,z,φ)(x, y, z, \varphi), Jacobian = Jv\mathbf{J}_v columns 1 to 4 plus the row [0,−1,−1,−1][0, -1, -1, -1]. Numerical and analytical answers agree to 10-7 degrees.

  • Weights trade off conflicting goals (position vs. pitch); a QP solver such as Mink adds hard joint limits.

  • Full-pose IK with e=[pd−p; eo]\mathbf{e} = [\mathbf{p}_d - \mathbf{p};\ \mathbf{e}_o] finds the UR5e’s solutions one restart at a time.

7.7 Analytical vs. Numerical, and Chapter Summary

AnalyticalNumerical
Needsa derivation for each robot (special structure)forward kinematics + Jacobian only
Solutionsall of them, at onceone, near the initial guess
Timemicroseconds, always the sameiterations; varies, may fail
Accuracyexactto the tolerance
Unreachable targetdetected cleanly (∣cos⁡θ2∣>1\lvert\cos\theta_2\rvert > 1)DLS returns the closest point: check the error
Singularitiesformulas break: handle as special casesdamping and QP handle them gracefully
Joint limits, collisions, extra goalschecked afterwardsbuilt in (QP-based IK)
Redundant robotsneed an extra parameternatural (pseudo-inverse, posture task)

In practice, industrial 6-axis controllers ship analytical solvers. Research robots, humanoids, teleoperation, and learned policies that output gripper targets (including LeRobot on the SO-ARM101) use optimization-based differential IK. A good habit is to use the analytical solution when you have one, to see every branch, and a numerical solver to track targets, handle limits, and combine goals.

Chapter 7 Summary

  • IK solves Te0(θ)=Td\mathbf{T}_e^0(\boldsymbol\theta) = \mathbf{T}_d, or a part of it. There may be no solution, a few (branches), or infinitely many (redundancy, singularities). Always verify with the forward kinematics.

  • Analytical toolkit: atan2 of sine and cosine; the law of cosines cos⁡θ2=(D2−L12−L22)/(2L1L2)\cos\theta_2 = (D^2 - L_1^2 - L_2^2)/(2L_1L_2); acos⁡θ+bsin⁡θ=ca\cos\theta + b\sin\theta = c; decoupling through a wrist point or an arm plane.

  • SO-ARM101 (position + pitch): θ1=−atan2(yd,xd−xp)\theta_1 = -\mathrm{atan2}(y_d, x_d - x_p); wrist point W=Pd−L3u(φd+δ)W = P_d - L_3\mathbf{u}(\varphi_d + \delta); law of cosines for θ2\theta_2, θ3\theta_3 (elbow up); θ4=−φd−θ2−θ3\theta_4 = -\varphi_d - \theta_2 - \theta_3; θ5\theta_5 = roll. Check the limits.

  • Numerical IK iterates θ←θ+Δθ\boldsymbol\theta \leftarrow \boldsymbol\theta + \Delta\boldsymbol\theta with Δθ=J−1e\Delta\boldsymbol\theta = \mathbf{J}^{-1}\mathbf{e} (Newton), J+e\mathbf{J}^+\mathbf{e}, αJ⊤e\alpha\mathbf{J}^\top\mathbf{e}, or J⊤(JJ⊤+λ2I)−1e\mathbf{J}^\top(\mathbf{J}\mathbf{J}^\top + \lambda^2\mathbf{I})^{-1}\mathbf{e} (damped least squares, the practical default).

  • Optimization-based IK (Mink, Pink, placo, PyRoki, cuRobo): weighted tasks plus hard limits, solved as a QP or nonlinear least squares.

RobotMethodKey step
Planar 2Ranalyticallaw of cosines, two branches
Planar 3Ranalyticalwrist point w=pd−L3u(φd)\mathbf{w} = \mathbf{p}_d - L_3\mathbf{u}(\varphi_d)
SCARAanalytical2R + d3=h−de−zdd_3 = h - d_e - z_d + θ4=θ1+θ2−φd\theta_4 = \theta_1+\theta_2-\varphi_d
Spherical wristanalyticalZYZ angles from Rd\mathbf{R}_d; singular at s2=0s_2 = 0
SO-ARM101analytical and numericalarm plane + wrist point; 4×44\times4 task Jacobian
UR5enumericalfull-pose DLS with eo\mathbf{e}_o; random restarts find the branches

Practice problems

  1. Planar 2R with L1=1L_1 = 1, L2=0.7L_2 = 0.7: find both IK solutions for the target (0.5,1.2)(0.5, 1.2) by hand, and check them with robot_2R.fk.

  2. Planar 3R with L=(1,0.7,0.3)L = (1, 0.7, 0.3): for (xd,yd,φd)=(0.8,1.0,90∘)(x_d, y_d, \varphi_d) = (0.8, 1.0, 90^\circ), compute the wrist point and both solutions.

  3. Spherical wrist: find all solutions for Rd=Rz(30∘)Ry(−45∘)Rz(60∘)\mathbf{R}_d = \mathbf{R}_z(30^\circ)\mathbf{R}_y(-45^\circ)\mathbf{R}_z(60^\circ). What happens for Rd=Rz(90∘)\mathbf{R}_d = \mathbf{R}_z(90^\circ)?

  4. SO-ARM101: using the constants printed in Section 7.4.2, compute by hand the top-down (φd=−90∘\varphi_d = -90^\circ) solution for pd=(0.22,0.05,0.02)\mathbf{p}_d = (0.22, 0.05, 0.02) m, and check it with so101_ik. Which joint limit stops a top-down grasp as the target moves farther out?

  5. Run DLS on the 2R arm for the unreachable target (2.0,1.0)(2.0, 1.0) with λ=0.001\lambda = 0.001, 0.05 and 0.5. Explain the differences using the table in Section 7.5.4.