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 6 — Velocity Kinematics: The Jacobian

Chapter 5 answered where the gripper is: given the joint variables θ\boldsymbol\theta, forward kinematics gives the pose Te0(θ)\mathbf{T}_e^0(\boldsymbol\theta). This chapter answers how fast it moves:

Velocity kinematics: given the joint variables θ\boldsymbol\theta and the joint velocities θ˙\dot{\boldsymbol\theta}, find the linear velocity v\mathbf{v} and the angular velocity ω\boldsymbol\omega of the gripper.

The answer is a single matrix, the Jacobian J(θ)\mathbf{J}(\boldsymbol\theta). You met its 2D version for the planar 2R arm in Chapter 3. Here we build it for any serial arm, column by column, from the very frames that forward kinematics already computes — no new differentiation needed. Then we put it to work: finding singularities, commanding straight-line motion (inverse velocity), and relating gripper forces to joint torques (statics).

SectionTopic
6.1From the 2D Jacobian to the 3D Jacobian
6.2The column formula
6.3Why the column formula works
6.4Jacobian examples: planar 2R, spherical wrist, SCARA, UR5e
6.5Singularities
6.6Inverse velocity, and a first look at numerical inverse kinematics
6.7Statics: τ=J⊤F\boldsymbol\tau = \mathbf{J}^\top\mathbf{F}

Notation (same as Chapter 5). Ti0\mathbf{T}_i^0 is the pose of frame {i}\{i\} relative to the ground frame {0}\{0\}. Frame {i}\{i\} is attached to link ii with its zz-axis along joint ii. θ=(θ1,…,θn)\boldsymbol\theta = (\theta_1,\dots,\theta_n) collects the joint variables; for a prismatic joint θi\theta_i is a length, which we also write did_i.

Tools and robots from Chapter 5

The next cell collects the forward-kinematics tools of Chapter 5 — the basic HTMs, the Robot class that implements the three-step procedure, and a frame-drawing helper — and rebuilds the four robots we use in this chapter with the same frames and dimensions. Run it and move on; nothing in it is new.

Robots ready: Planar 2R, Spherical wrist, SCARA, UR5e

6.1 From the 2D Jacobian to the 3D Jacobian

For the planar 2R arm in Chapter 3 we differentiated the forward kinematics x(θ)x(\boldsymbol\theta), y(θ)y(\boldsymbol\theta) with the chain rule and found

[x˙y˙]=J(θ)[θ˙1θ˙2],J(θ)=[−L1s1−L2s12−L2s12L1c1+L2c12L2c12].\begin{bmatrix}\dot x\\ \dot y\end{bmatrix} = \mathbf{J}(\boldsymbol\theta)\begin{bmatrix}\dot\theta_1\\ \dot\theta_2\end{bmatrix}, \qquad \mathbf{J}(\boldsymbol\theta) = \begin{bmatrix}-L_1s_1 - L_2s_{12} & -L_2s_{12}\\ L_1c_1 + L_2c_{12} & L_2c_{12}\end{bmatrix}.

The Jacobian is a matrix that depends on the configuration θ\boldsymbol\theta, and that maps joint velocities linearly to gripper velocity. Each column is the gripper velocity produced when only that joint moves at unit speed.

In 3D the gripper has a full pose, so it has two kinds of velocity:

  • the linear velocity v=o˙e\mathbf{v} = \dot{\mathbf{o}}_e of the gripper point oe\mathbf{o}_e (the origin of frame {e}\{e\}, i.e. the last column of Te0\mathbf{T}_e^0), and

  • the angular velocity ω\boldsymbol\omega of the gripper frame — a vector along the instantaneous axis of rotation (right-hand rule) whose length is the rotation rate in rad/s.

Figure 6.1 — Joint velocities produce gripper velocity. (a) For the planar 2R arm, θ̇1 and θ̇2 give the tip a velocity v = (ẋ, ẏ) in the plane. (b) In 3D the gripper has a linear velocity v of its point oe and an angular velocity ω along its instantaneous axis of rotation.

Both are 3-vectors expressed in the ground frame {0}\{0\}. Stacking them, the 3D Jacobian is the 6×n6\times n matrix that maps the nn joint velocities to these six numbers:

[vω]=J(θ) θ˙=[JvJω]θ˙,J∈R6×n\boxed{\begin{bmatrix}\mathbf{v}\\ \boldsymbol\omega\end{bmatrix} = \mathbf{J}(\boldsymbol\theta)\,\dot{\boldsymbol\theta} = \begin{bmatrix}\mathbf{J}_v\\ \mathbf{J}_\omega\end{bmatrix}\dot{\boldsymbol\theta}, \qquad \mathbf{J}\in\mathbb{R}^{6\times n}}

The top three rows Jv\mathbf{J}_v give the linear velocity; the bottom three rows Jω\mathbf{J}_\omega give the angular velocity. As in 2D, column ii of J\mathbf{J} is the gripper’s (v,ω)(\mathbf{v},\boldsymbol\omega) when joint ii moves at unit speed and all other joints are still.

Figure 6.2 — Structure of the 6×n Jacobian: one column Ji per joint. The top three rows Jv give v; the bottom three rows Jω give ω.

The linear part could be found exactly as in Chapter 3, by differentiating the last column of Te0\mathbf{T}_e^0. But for a 6-joint arm that expression is long (see Section 5.8), and the angular part has no such simple formula. The next section gives a far easier way.

Key Takeaways — Section 6.1

  • The Jacobian maps joint velocities to gripper velocity: [v;ω]=J(θ)θ˙[\mathbf{v};\boldsymbol\omega] = \mathbf{J}(\boldsymbol\theta)\dot{\boldsymbol\theta}, with J\mathbf{J} of size 6×n6\times n.

  • Jv\mathbf{J}_v (top 3 rows) gives the linear velocity of oe\mathbf{o}_e; Jω\mathbf{J}_\omega (bottom 3 rows) gives the angular velocity of the gripper.

  • Column ii = gripper velocity when only joint ii moves at unit speed.

6.2 The Column Formula

From the forward kinematics of Chapter 5 we already have, for every joint ii, the frame Ti0\mathbf{T}_i^0. Two of its columns are all we need:

Ti0=[xiyizioi0001]⟹{zi:direction of joint axis i (column 3)oi:a point on joint axis i (column 4)\mathbf{T}_i^0 = \begin{bmatrix} \mathbf{x}_i & \mathbf{y}_i & \mathbf{z}_i & \mathbf{o}_i\\ 0&0&0&1\end{bmatrix} \quad\Longrightarrow\quad \begin{cases} \mathbf{z}_i: & \text{direction of joint axis } i \text{ (column 3)}\\ \mathbf{o}_i: & \text{a point on joint axis } i \text{ (column 4)} \end{cases}

This works because in Step 1 of the forward kinematics procedure we put ziz_i along joint ii and the origin oio_i on it. With oe\mathbf{o}_e the gripper position (column 4 of Te0\mathbf{T}_e^0), column ii of the Jacobian is:

revolute joint:Ji=[zi×(oe−oi)zi]prismatic joint:Ji=[zi0]\boxed{ \text{revolute joint:}\quad \mathbf{J}_i = \begin{bmatrix}\mathbf{z}_i\times(\mathbf{o}_e - \mathbf{o}_i)\\ \mathbf{z}_i\end{bmatrix} \qquad\qquad \text{prismatic joint:}\quad \mathbf{J}_i = \begin{bmatrix}\mathbf{z}_i\\ \mathbf{0}\end{bmatrix} }

and J=[J1J2⋯Jn]\mathbf{J} = \begin{bmatrix}\mathbf{J}_1 & \mathbf{J}_2 & \cdots & \mathbf{J}_n\end{bmatrix}.

Procedure — Jacobian from forward kinematics:

  1. Run forward kinematics and keep the partial products T10,T20,…,Tn0\mathbf{T}_1^0, \mathbf{T}_2^0, \dots, \mathbf{T}_n^0 and Te0\mathbf{T}_e^0.

  2. Read off zi\mathbf{z}_i and oi\mathbf{o}_i (columns 3 and 4 of Ti0\mathbf{T}_i^0) and oe\mathbf{o}_e (column 4 of Te0\mathbf{T}_e^0).

  3. Build one column per joint with the formula above and stack the columns.

Figure 6.3 — (a) zi and oi are columns 3 and 4 of Ti0, and oe is column 4 of Te0. (b) The three-step procedure for building the Jacobian.

Note for readers of other textbooks. Books that use the Denavit–Hartenberg convention place joint ii along zi−1z_{i-1} and write the columns with zi−1\mathbf{z}_{i-1} and oi−1\mathbf{o}_{i-1}. In our convention joint ii lies along ziz_i of frame {i}\{i\} itself, so there is no index shift. Also note that Ti0\mathbf{T}_i^0 includes the joint’s own Rotz,θi\mathrm{Rot}_{z,\theta_i} (or Transz,di\mathrm{Trans}_{z,d_i}); a rotation about ziz_i does not change zi\mathbf{z}_i or oi\mathbf{o}_i, so it does not matter.

The function below is the whole procedure in code. It works for every robot built with the Robot class.

J (SCARA) =
[[-0.9796 -0.5796  0.      0.    ]
 [ 0.8481  0.1553  0.      0.    ]
 [ 0.      0.     -1.      0.    ]
 [ 0.      0.      0.      0.    ]
 [ 0.      0.      0.      0.    ]
 [ 1.      1.      0.     -1.    ]]

Matches finite differences of the FK? True

Key Takeaways — Section 6.2

  • Everything the Jacobian needs is already inside the forward-kinematics frames: zi\mathbf{z}_i and oi\mathbf{o}_i from Ti0\mathbf{T}_i^0, oe\mathbf{o}_e from Te0\mathbf{T}_e^0.

  • Revolute: Ji=[zi×(oe−oi); zi]\mathbf{J}_i = [\mathbf{z}_i\times(\mathbf{o}_e-\mathbf{o}_i);\ \mathbf{z}_i]. Prismatic: Ji=[zi; 0]\mathbf{J}_i = [\mathbf{z}_i;\ \mathbf{0}].

  • No differentiation is needed — just cross products of vectors read off the frames.

6.3 Why the Column Formula Works

We only need three facts from introductory physics.

1. Only joint ii moves (revolute). Freeze every joint except joint ii. Everything outboard of joint ii — including the gripper — now turns as one rigid body about the fixed axis through oi\mathbf{o}_i along zi\mathbf{z}_i, at rate θ˙i\dot\theta_i. Its angular velocity is therefore ω=θ˙i zi\boldsymbol\omega = \dot\theta_i\,\mathbf{z}_i, and the gripper point moves on a circle around that axis. For a point on a body rotating about a fixed axis, physics gives v=ω×r\mathbf{v} = \boldsymbol\omega\times\mathbf{r}, where r\mathbf{r} points from any point on the axis to the moving point:

v=θ˙i zi×(oe−oi),ω=θ˙i zi.\mathbf{v} = \dot\theta_i\,\mathbf{z}_i\times(\mathbf{o}_e-\mathbf{o}_i), \qquad \boldsymbol\omega = \dot\theta_i\,\mathbf{z}_i .

Dividing by θ˙i\dot\theta_i gives exactly the revolute column Ji\mathbf{J}_i. (Any point on the axis can play the role of oi\mathbf{o}_i: moving it along zi\mathbf{z}_i changes r\mathbf{r} by a multiple of zi\mathbf{z}_i, and zi×zi=0\mathbf{z}_i\times\mathbf{z}_i = \mathbf{0}.)

2. Only joint ii moves (prismatic). Everything outboard slides along zi\mathbf{z}_i at speed d˙i\dot d_i without rotating: v=d˙i zi\mathbf{v} = \dot d_i\,\mathbf{z}_i, ω=0\boldsymbol\omega = \mathbf{0}. That is the prismatic column.

Figure 6.4 — Only joint i moves. (a) Revolute: everything outboard rotates about the joint axis, so oe moves on a circle with v = ω × r. (b) Prismatic: everything outboard slides along zi without rotating.

3. All joints move: contributions add. The gripper position is a function oe(θ1,…,θn)\mathbf{o}_e(\theta_1,\dots,\theta_n), so by the chain rule

v=o˙e=∂oe∂θ1θ˙1+⋯+∂oe∂θnθ˙n,\mathbf{v} = \dot{\mathbf{o}}_e = \frac{\partial\mathbf{o}_e}{\partial\theta_1}\dot\theta_1 + \cdots + \frac{\partial\mathbf{o}_e}{\partial\theta_n}\dot\theta_n ,

a sum of the one-joint-at-a-time velocities. Angular velocities about the joint axes add the same way (a spinning turntable carrying a spinning top gives the top the sum of the two spins), so

v=∑i=1nJv,i θ˙i,ω=∑revolute iθ˙i zi⟺[vω]=J θ˙.\mathbf{v} = \sum_{i=1}^n \mathbf{J}_{v,i}\,\dot\theta_i,\qquad \boldsymbol\omega = \sum_{\text{revolute } i} \dot\theta_i\,\mathbf{z}_i \qquad\Longleftrightarrow\qquad \begin{bmatrix}\mathbf{v}\\ \boldsymbol\omega\end{bmatrix} = \mathbf{J}\,\dot{\boldsymbol\theta}.

The figure below shows fact 1 and fact 2 for the SCARA. In each panel only one joint moves: the dotted curve is the path of the gripper point (a circle about the dashed joint axis, or a straight line for the prismatic quill), and the black arrow is the linear part of that joint’s column, Jv,i\mathbf{J}_{v,i} — always tangent to the path.

<Figure size 1600x480 with 4 Axes>

In the last panel the gripper point lies on joint 4’s axis, so turning joint 4 only spins the tool: Jv,4=z4×(oe−o4)=0\mathbf{J}_{v,4} = \mathbf{z}_4\times(\mathbf{o}_e-\mathbf{o}_4) = \mathbf{0} because oe−o4\mathbf{o}_e-\mathbf{o}_4 is parallel to z4\mathbf{z}_4. Its column still has the angular part z4\mathbf{z}_4.

Key Takeaways — Section 6.3

  • One revolute joint moving = rigid rotation about a fixed axis: v=ω×r\mathbf{v} = \boldsymbol\omega\times\mathbf{r} with ω=θ˙izi\boldsymbol\omega = \dot\theta_i\mathbf{z}_i, r=oe−oi\mathbf{r} = \mathbf{o}_e-\mathbf{o}_i.

  • One prismatic joint moving = pure sliding along zi\mathbf{z}_i, no rotation.

  • With all joints moving, linear velocities add (chain rule) and angular velocities add, so the columns simply stack into J\mathbf{J}.

6.4 Jacobian Examples

We now apply the three-step procedure of Section 6.2 to four robots from Chapter 5: the planar 2R arm, the spherical wrist and the SCARA by hand (checked with SymPy), and the 6-joint UR5e numerically.

6.4.1 Planar 2R Arm

Step 1 — Forward kinematics. From Section 5.4, with T10=Rotz,θ1\mathbf{T}_1^0 = \mathrm{Rot}_{z,\theta_1}, T20=Rotz,θ1Transx,L1Rotz,θ2\mathbf{T}_2^0 = \mathrm{Rot}_{z,\theta_1}\mathrm{Trans}_{x,L_1}\mathrm{Rot}_{z,\theta_2} and Te0=T20 Transx,L2\mathbf{T}_e^0 = \mathbf{T}_2^0\,\mathrm{Trans}_{x,L_2}.

Step 2 — Read off the vectors.

zi\mathbf{z}_ioi\mathbf{o}_i
joint 1(0,0,1)(0,0,1)(0,0,0)(0,0,0)
joint 2(0,0,1)(0,0,1)(L1c1, L1s1, 0)(L_1c_1,\ L_1s_1,\ 0)
gripperoe=(L1c1+L2c12, L1s1+L2s12, 0)\mathbf{o}_e = (L_1c_1+L_2c_{12},\ L_1s_1+L_2s_{12},\ 0)

Step 3 — Columns. Both joints are revolute. For a vector r=(rx,ry,0)\mathbf{r} = (r_x, r_y, 0),  (0,0,1)×r=(−ry, rx, 0)\ (0,0,1)\times\mathbf{r} = (-r_y,\ r_x,\ 0), so

J=[−L1s1−L2s12−L2s12L1c1+L2c12L2c1200000011].\mathbf{J} = \begin{bmatrix} -L_1s_1-L_2s_{12} & -L_2s_{12}\\ L_1c_1+L_2c_{12} & L_2c_{12}\\ 0 & 0\\ 0 & 0\\ 0 & 0\\ 1 & 1 \end{bmatrix}.

The SymPy cell below carries out the three steps symbolically, and then checks the result the Chapter 3 way: by differentiating oe(θ)\mathbf{o}_e(\boldsymbol\theta) directly.

Loading...
Top three rows equal d o_e / d θ ? True

The top two rows are exactly the Chapter 3 Jacobian, and the formula also delivers the angular velocity for free: ωz=θ˙1+θ˙2\omega_z = \dot\theta_1 + \dot\theta_2 — the rate of change of the gripper’s heading θ1+θ2\theta_1+\theta_2 that we found in Section 5.4.

Use the sliders below to set the configuration and the joint velocities. The black arrow is v\mathbf{v} and the purple arrow is ω\boldsymbol\omega (both scaled by 0.3 for display); ω\boldsymbol\omega always points along z0z_0 for this planar arm.

Loading...

Key Takeaways — Section 6.4.1

  • For the 2R arm the column formula reproduces the Chapter 3 Jacobian in rows 1–2, and adds ωz=θ˙1+θ˙2\omega_z = \dot\theta_1+\dot\theta_2 in row 6.

  • Rows 3–5 are zero: a planar arm can never move out of its plane or tilt.

6.4.2 3R Spherical Wrist

Step 1 — Forward kinematics (Section 5.5): T10=Rotz,θ1\mathbf{T}_1^0 = \mathrm{Rot}_{z,\theta_1}, T20=T10 Rotx,−90∘Rotz,θ2\mathbf{T}_2^0 = \mathbf{T}_1^0\,\mathrm{Rot}_{x,-90^\circ}\mathrm{Rot}_{z,\theta_2}, T30=T20 Rotx,90∘Rotz,θ3\mathbf{T}_3^0 = \mathbf{T}_2^0\,\mathrm{Rot}_{x,90^\circ}\mathrm{Rot}_{z,\theta_3}, Te0=T30 Transz,L\mathbf{T}_e^0 = \mathbf{T}_3^0\,\mathrm{Trans}_{z,L}.

Step 2 — Read off the vectors. All three axes pass through the wrist center, so o1=o2=o3=0\mathbf{o}_1 = \mathbf{o}_2 = \mathbf{o}_3 = \mathbf{0}, and

z1=[001],z2=[−s1c10],z3=[c1s2s1s2c2],oe=L z3.\mathbf{z}_1 = \begin{bmatrix}0\\0\\1\end{bmatrix},\quad \mathbf{z}_2 = \begin{bmatrix}-s_1\\ c_1\\ 0\end{bmatrix},\quad \mathbf{z}_3 = \begin{bmatrix}c_1s_2\\ s_1s_2\\ c_2\end{bmatrix},\quad \mathbf{o}_e = L\,\mathbf{z}_3 .

Step 3 — Columns. Because oe−o3=L z3\mathbf{o}_e - \mathbf{o}_3 = L\,\mathbf{z}_3 is parallel to z3\mathbf{z}_3, the third column has no linear part: rolling the last joint only spins the gripper about its own axis.

Loading...
Top three rows equal d o_e / d θ ? True

The bottom three rows, Jω=[z1 z2 z3]\mathbf{J}_\omega = [\mathbf{z}_1\ \mathbf{z}_2\ \mathbf{z}_3], are the part that matters for a wrist: its job is to orient the gripper. Any angular velocity ω\boldsymbol\omega can be produced as long as the three axes z1,z2,z3\mathbf{z}_1,\mathbf{z}_2,\mathbf{z}_3 are not in one plane — we return to this in Section 6.5. Notice that the angular part could not have been found by differentiating oe\mathbf{o}_e; it comes straight from the joint axes.

Loading...

Key Takeaways — Section 6.4.2

  • When joint axes intersect, their oi\mathbf{o}_i coincide and the linear parts of the columns are short cross products.

  • A joint whose axis passes through oe\mathbf{o}_e contributes no linear velocity, only angular velocity.

  • For a wrist, Jω=[z1 z2 z3]\mathbf{J}_\omega = [\mathbf{z}_1\ \mathbf{z}_2\ \mathbf{z}_3] — the joint axes themselves.

6.4.3 SCARA Robot

Step 1 — Forward kinematics (Section 5.7): T10=Transz,hRotz,θ1\mathbf{T}_1^0 = \mathrm{Trans}_{z,h}\mathrm{Rot}_{z,\theta_1}, T20=T10 Transx,L1Rotz,θ2\mathbf{T}_2^0 = \mathbf{T}_1^0\,\mathrm{Trans}_{x,L_1}\mathrm{Rot}_{z,\theta_2}, T30=T20 Transx,L2Rotx,180∘Transz,d3\mathbf{T}_3^0 = \mathbf{T}_2^0\,\mathrm{Trans}_{x,L_2}\mathrm{Rot}_{x,180^\circ}\mathrm{Trans}_{z,d_3}, T40=T30 Rotz,θ4\mathbf{T}_4^0 = \mathbf{T}_3^0\,\mathrm{Rot}_{z,\theta_4}, Te0=T40 Transz,de\mathbf{T}_e^0 = \mathbf{T}_4^0\,\mathrm{Trans}_{z,d_e}.

Step 2 — Read off the vectors.

typezi\mathbf{z}_ioi\mathbf{o}_i
joint 1R(0,0,1)(0,0,1)(0, 0, h)(0,\ 0,\ h)
joint 2R(0,0,1)(0,0,1)(L1c1, L1s1, h)(L_1c_1,\ L_1s_1,\ h)
joint 3P(0,0,−1)(0,0,-1)(not needed)
joint 4R(0,0,−1)(0,0,-1)(L1c1+L2c12, L1s1+L2s12, h−d3)(L_1c_1+L_2c_{12},\ L_1s_1+L_2s_{12},\ h-d_3)
gripperoe=(L1c1+L2c12, L1s1+L2s12, h−d3−de)\mathbf{o}_e = (L_1c_1+L_2c_{12},\ L_1s_1+L_2s_{12},\ h-d_3-d_e)

Step 3 — Columns. Joint 3 is prismatic, so its column is just [z3;0][\mathbf{z}_3;\mathbf{0}].

Loading...
Top three rows equal d o_e / d θ ? True

Reading the SCARA Jacobian column by column:

  • Columns 1–2 (vertical revolute joints): the x,yx,y rows are the planar 2R Jacobian, and each adds +1 to ωz\omega_z.

  • Column 3 (prismatic quill): v=(0,0,−d˙3)\mathbf{v} = (0,0,-\dot d_3) — straight down, no rotation.

  • Column 4 (tool roll): no linear velocity, and ωz=−θ˙4\omega_z = -\dot\theta_4 because z4\mathbf{z}_4 points down. This matches the tool yaw φ=θ1+θ2−θ4\varphi = \theta_1+\theta_2-\theta_4 from Section 5.7: φ˙=θ˙1+θ˙2−θ˙4\dot\varphi = \dot\theta_1+\dot\theta_2-\dot\theta_4.

  • Rows 4–5 (ωx\omega_x, ωy\omega_y) are always zero: the SCARA tool can never tilt. Only four of the six rows carry information — (vx,vy,vz,ωz)(v_x, v_y, v_z, \omega_z) — one for each joint.

Loading...

Key Takeaways — Section 6.4.3

  • A prismatic column is just the sliding direction: [zi;0][\mathbf{z}_i;\mathbf{0}].

  • Rows of J\mathbf{J} that are identically zero show directions the robot can never move in (SCARA: tilting).

  • The SCARA’s useful Jacobian is the 4×44\times4 block of rows (vx,vy,vz,ωz)(v_x, v_y, v_z, \omega_z).

6.4.4 UR5e

For the 6-joint UR5e of Section 5.8 we do not write the Jacobian out by hand; the procedure is the same three steps, carried out by the jacobian function. The result is a square 6×66\times6 matrix: six joints controlling the six components of (v,ω)(\mathbf{v},\boldsymbol\omega).

Step 2 — vectors read off the frames:
  joint 1:  z_1 = [0. 0. 1.]   o_1 = [0.    0.    0.163]
  joint 2:  z_2 = [ 0.5   -0.866  0.   ]   o_2 = [0.    0.    0.163]
  joint 3:  z_3 = [ 0.5   -0.866  0.   ]   o_3 = [-0.184 -0.106  0.531]
  joint 4:  z_4 = [ 0.5   -0.866  0.   ]   o_4 = [-0.478 -0.276  0.335]
  joint 5:  z_5 = [-0.866 -0.5    0.   ]   o_5 = [-0.411 -0.392  0.335]
  joint 6:  z_6 = [ 0.  0. -1.]   o_6 = [-0.498 -0.442  0.335]
  gripper:  o_e = [-0.498 -0.442  0.235]

J (UR5e) =
[[ 0.442  -0.0624  0.2563  0.0866  0.05    0.    ]
 [-0.4976 -0.036   0.148   0.05   -0.0866  0.    ]
 [ 0.     -0.652  -0.4395 -0.1     0.      0.    ]
 [ 0.      0.5     0.5     0.5    -0.866   0.    ]
 [ 0.     -0.866  -0.866  -0.866  -0.5     0.    ]
 [ 1.      0.      0.      0.      0.     -1.    ]]

Matches finite differences of the FK? True
Loading...

Key Takeaways — Section 6.4.4

  • The column formula scales to a 6-DOF arm with no extra effort: six cross products.

  • With six joints the Jacobian is square, so (away from singularities) any gripper velocity (v,ω)(\mathbf{v},\boldsymbol\omega) can be produced.

6.5 Singularities

A singular configuration (singularity) is a configuration θ\boldsymbol\theta where the Jacobian loses rank: its columns become linearly dependent, so there is at least one direction of gripper motion that no combination of joint velocities can produce. For a square Jacobian, this is simply

det⁡J(θ)=0.\det\mathbf{J}(\boldsymbol\theta) = 0 .

Only the rows a robot can actually control count. The planar 2R uses rows (vx,vy)(v_x, v_y), the SCARA rows (vx,vy,vz,ωz)(v_x, v_y, v_z, \omega_z), the spherical wrist rows (ωx,ωy,ωz)(\omega_x,\omega_y,\omega_z), and the UR5e all six. We call this square block the task Jacobian.

Two things go wrong at (and near) a singularity:

  1. Lost direction. Some gripper velocity is impossible, however fast the joints move.

  2. Huge joint speeds nearby. Producing a velocity close to the lost direction needs joint velocities proportional to 1/det⁡J1/\det\mathbf{J} — they blow up as det⁡J→0\det\mathbf{J}\to0 (Section 6.6 shows this).

Loading...
Loading...
Loading...

6.5.1 Planar 2R and SCARA: arm fully stretched or fully folded

det⁡J=L1L2sin⁡θ2\det\mathbf{J} = L_1L_2\sin\theta_2 for both robots: singular when θ2=0\theta_2 = 0 (arm fully stretched) or θ2=180∘\theta_2 = 180^\circ (fully folded). The two links are then collinear, and both joints can only move the tip perpendicular to the arm — motion along the arm (radially) is lost. The SCARA inherits this from its 2R horizontal arm; the quill and tool roll do not change it.

Figure 6.5 — Singularities of the planar 2R arm. With the links collinear, both columns of Jv are perpendicular to the arm, so the tip cannot move radially.

A good way to see how close a configuration is to singular is to look at all the tip velocities that unit-size joint velocities ∥θ˙∥=1\|\dot{\boldsymbol\theta}\| = 1 can produce. They fill an ellipse, the manipulability ellipse. As θ2→0\theta_2\to0 the ellipse flattens into a line segment: the radial direction disappears.

<Figure size 1600x420 with 4 Axes>

6.5.2 Spherical wrist: two axes line up

det⁡Jω=−sin⁡θ2\det\mathbf{J}_\omega = -\sin\theta_2: the wrist is singular when θ2=0\theta_2 = 0 or 180∘180^\circ. Then z3=±z1\mathbf{z}_3 = \pm\mathbf{z}_1 — the first and last joints spin about the same axis, so together they provide only one rotation direction, and one direction of angular velocity is lost. Note that the wrist’s home configuration is singular. This is the same gimbal lock you met for Euler angles in Chapter 4: the wrist’s orientation is exactly ZYZ Euler angles (Section 5.5).

Figure 6.6 — Spherical-wrist singularity. (a) At θ2 = 0 the axes z1 and z3 are collinear, and rotation about the in-page axis perpendicular to z1 and z2 is lost. (b) At θ2 = 90° the three axes are independent.

θ2 =   0°:  z1 = [0. 0. 1.],  z3 = [0. 0. 1.],  rank J_ω = 2,  det J_ω = +0.000
θ2 =  10°:  z1 = [0. 0. 1.],  z3 = [0.174 0.    0.985],  rank J_ω = 3,  det J_ω = -0.174
θ2 =  90°:  z1 = [0. 0. 1.],  z3 = [1. 0. 0.],  rank J_ω = 3,  det J_ω = -1.000

6.5.3 UR5e: shoulder, elbow and wrist singularities

Carrying the same computation through for the UR5e (by computer algebra — it takes a while, so we only quote the result) gives a determinant that factors neatly:

det⁡J=− a2 a3  sin⁡θ3  sin⁡θ5  (a2c2+a3c23−d5s234).\det\mathbf{J} = -\,a_2\,a_3\;\sin\theta_3\;\sin\theta_5\;\big(a_2c_2 + a_3c_{23} - d_5s_{234}\big).

Each factor is one family of singularities:

Factor =0=0NameWhat happens
sin⁡θ3=0\sin\theta_3 = 0elbowupper arm and forearm are collinear (stretched or folded) — the 2R singularity again
sin⁡θ5=0\sin\theta_5 = 0wristaxes 4 and 6 become parallel — the spherical-wrist singularity again
a2c2+a3c23−d5s234=0a_2c_2 + a_3c_{23} - d_5s_{234} = 0shoulderthe wrist point o6\mathbf{o}_6 lies in the plane of axes 1 and 2, so turning the base cannot move it sideways

The determinant does not depend on θ1\theta_1 (turning the whole arm about the base) or on θ6\theta_6 (spinning the tool). The cell below checks the formula numerically and shows one example of each singularity.

det J vs. formula, 200 random poses: max difference = 1.1e-16

To find a shoulder singularity, pick θ2\theta_2 and θ3\theta_3 and solve the shoulder condition for θ4\theta_4:  sin⁡(θ2+θ3+θ4)=(a2c2+a3c23)/d5\ \sin(\theta_2+\theta_3+\theta_4) = (a_2c_2 + a_3c_{23})/d_5.

<Figure size 1500x520 with 3 Axes>

Key Takeaways — Section 6.5

  • A singularity is a configuration where the (task) Jacobian loses rank: det⁡J=0\det\mathbf{J} = 0 for a square J\mathbf{J}.

  • At a singularity some gripper velocity is impossible; near it, the joint speeds needed grow like 1/det⁡J1/\det\mathbf{J}.

  • 2R and SCARA: det⁡J=L1L2sin⁡θ2\det\mathbf{J} = L_1L_2\sin\theta_2 (arm stretched or folded). Spherical wrist: det⁡Jω=−sin⁡θ2\det\mathbf{J}_\omega = -\sin\theta_2 (axes 1 and 3 aligned, gimbal lock).

  • UR5e: det⁡J=−a2a3 s3 s5 (a2c2+a3c23−d5s234)\det\mathbf{J} = -a_2a_3\,s_3\,s_5\,(a_2c_2+a_3c_{23}-d_5s_{234}) — elbow, wrist and shoulder singularities.

6.6 Inverse Velocity, and a First Look at Numerical Inverse Kinematics

6.6.1 Inverse velocity

The Jacobian answers “given θ˙\dot{\boldsymbol\theta}, how does the gripper move?”. Very often we need the reverse: given a desired gripper velocity x˙\dot{\mathbf{x}}, what joint velocities produce it? If the task Jacobian is square and not singular, just invert it:

θ˙=J−1(θ) x˙\boxed{\dot{\boldsymbol\theta} = \mathbf{J}^{-1}(\boldsymbol\theta)\,\dot{\mathbf{x}}}

Unlike inverse position kinematics, this is easy: it is a linear system, with exactly one answer. Repeating it at every time step gives resolved-rate motion, the standard way to move a gripper along a straight line:

  1. Measure the current θ\boldsymbol\theta and compute the task Jacobian J(θ)\mathbf{J}(\boldsymbol\theta).

  2. Solve J θ˙=x˙desired\mathbf{J}\,\dot{\boldsymbol\theta} = \dot{\mathbf{x}}_{\text{desired}} for θ˙\dot{\boldsymbol\theta}.

  3. Command θ˙\dot{\boldsymbol\theta} for a short time Δt\Delta t: θ←θ+θ˙ Δt\boldsymbol\theta \leftarrow \boldsymbol\theta + \dot{\boldsymbol\theta}\,\Delta t. Repeat.

(In code we use np.linalg.solve(J, xdot), which solves the linear system without forming J−1\mathbf{J}^{-1} explicitly.)

<Figure size 1600x460 with 5 Axes>
Path B: det J falls from 0.560 to 0.079; |θ̇| grows from 0.64 to 9.71 rad/s

Path A is easy: smooth, moderate joint velocities. Path B asks the tip to move radially toward full extension, which is exactly the direction lost at the θ2=0\theta_2 = 0 singularity. As det⁡J→0\det\mathbf{J}\to0 the joint velocities climb without bound; a real motor would saturate, and the tip would fall behind or leave the line.

The same three lines of code drive the SCARA along a sloped straight line in 3D while holding the tool’s yaw fixed — the task velocity is x˙=(vx,vy,vz,ωz)\dot{\mathbf{x}} = (v_x, v_y, v_z, \omega_z) with ωz=0\omega_z = 0:

<Figure size 1400x500 with 2 Axes>
Tool yaw at start / end: 90.00° / 90.00°

To keep the yaw φ=θ1+θ2−θ4\varphi = \theta_1+\theta_2-\theta_4 constant, the solver automatically turns the tool joint at θ˙4=θ˙1+θ˙2\dot\theta_4 = \dot\theta_1 + \dot\theta_2 (the dotted line lies on top of θ˙4\dot\theta_4), cancelling the rotation that the arm joints would otherwise give the tool.

6.6.2 A first look at numerical inverse kinematics

Inverse position kinematics asks for the joints that put the gripper at a desired position xd\mathbf{x}_d. In Chapter 3 we solved it with geometry for the 2R arm, but for most robots no such closed-form solution is convenient. The Jacobian gives a general alternative. If the gripper is at x(θ)\mathbf{x}(\boldsymbol\theta) and we want it at xd\mathbf{x}_d, treat the error xd−x(θ)\mathbf{x}_d - \mathbf{x}(\boldsymbol\theta) as a small displacement and use inverse velocity to find the joint change that would produce it:

θk+1=θk+J−1(θk) (xd−x(θk))\boxed{\boldsymbol\theta_{k+1} = \boldsymbol\theta_k + \mathbf{J}^{-1}(\boldsymbol\theta_k)\,\big(\mathbf{x}_d - \mathbf{x}(\boldsymbol\theta_k)\big)}

Because the Jacobian is only a linear approximation, one step does not land exactly on the target — but repeating the step converges very quickly (this is Newton’s method).

Initial guess (20°, 60°):
 k    θ1 (°)   θ2 (°)   position error
 0    20.000   60.000   4.53e-01
 1    -6.513   88.134   1.07e-01
 2    -4.455   77.772   7.44e-03
 3    -4.019   77.210   2.94e-05
 4    -4.019   77.207   6.54e-10
 5    -4.019   77.207   1.11e-16
 6    -4.019   77.207   0.00e+00
 7    -4.019   77.207   0.00e+00

Initial guess (70°, -60°):
 k    θ1 (°)   θ2 (°)   position error
 0    70.000  -60.000   4.91e-01
 1    64.313  -89.827   1.35e-01
 2    57.351  -78.057   7.95e-03
 3    57.152  -77.211   4.63e-05
 4    57.149  -77.207   1.16e-09
 5    57.149  -77.207   1.11e-16
 6    57.149  -77.207   1.11e-16
 7    57.149  -77.207   1.11e-16

Closed-form IK (Chapter 3):
  elbow +1: θ = [-4.019 77.207]°
  elbow -1: θ = [ 57.149 -77.207]°

In a handful of iterations the error drops to machine precision, and the two initial guesses converge to the two elbow solutions of Chapter 3 — numerical IK finds one solution, the one “closest” to where it starts. The same update works for the SCARA or the UR5e with their task Jacobians. Making it robust — handling orientation targets, non-square Jacobians, and the singularities of Section 6.5 (where J−1\mathbf{J}^{-1} does not exist) — is the subject of the inverse kinematics chapter.

Key Takeaways — Section 6.6

  • Inverse velocity is a linear solve: θ˙=J−1x˙\dot{\boldsymbol\theta} = \mathbf{J}^{-1}\dot{\mathbf{x}} (square, non-singular task Jacobian).

  • Resolved-rate motion repeats that solve every time step to drive the gripper along a straight line or hold an orientation.

  • Near a singularity the required joint velocities blow up.

  • Iterating θ←θ+J−1(xd−x(θ))\boldsymbol\theta \leftarrow \boldsymbol\theta + \mathbf{J}^{-1}(\mathbf{x}_d - \mathbf{x}(\boldsymbol\theta)) solves inverse position kinematics numerically; the answer depends on the initial guess.

6.7 Statics: τ=J⊤F\boldsymbol\tau = \mathbf{J}^\top\mathbf{F}

The Jacobian also tells us how forces pass through the arm. Suppose the gripper pushes on its surroundings with a force f\mathbf{f} and a moment n\mathbf{n}, collected in the wrench F=[f; n]\mathbf{F} = [\mathbf{f};\ \mathbf{n}] (both in frame {0}\{0\}). What joint torques τ\boldsymbol\tau (forces, for prismatic joints) must the motors supply to hold it, with the arm at rest?

Use power. Imagine a tiny motion θ˙\dot{\boldsymbol\theta}. The motors deliver power τ⊤θ˙\boldsymbol\tau^\top\dot{\boldsymbol\theta}; the gripper delivers power f⊤v+n⊤ω=F⊤Jθ˙\mathbf{f}^\top\mathbf{v} + \mathbf{n}^\top\boldsymbol\omega = \mathbf{F}^\top\mathbf{J}\dot{\boldsymbol\theta} to the surroundings. Without friction (and ignoring the links’ own weight) the two must be equal for every θ˙\dot{\boldsymbol\theta}, so

τ=J⊤(θ) F\boxed{\boldsymbol\tau = \mathbf{J}^\top(\boldsymbol\theta)\,\mathbf{F}}

The same matrix that maps joint velocities out to gripper velocity maps gripper wrench back to joint torques — transposed. Note there is no inverse here, so statics is well defined even at a singularity.

Example: the 2R arm holding a payload

Turn the 2R arm so that it moves in a vertical plane, with gravity along −y0-y_0. To hold a payload of mass mm, the gripper must push up on it: f=(0, mg, 0)\mathbf{f} = (0,\ mg,\ 0), n=0\mathbf{n} = \mathbf{0}. Then

τ=J⊤F=[mg (L1c1+L2c12)mg L2c12],\boldsymbol\tau = \mathbf{J}^\top\mathbf{F} = \begin{bmatrix} mg\,(L_1c_1+L_2c_{12})\\ mg\,L_2c_{12}\end{bmatrix} ,

each torque being the weight times the horizontal distance from that joint to the payload — the familiar “force × lever arm”.

<Figure size 800x420 with 1 Axes>
Worst case (arm horizontal, θ = (0, 0)): τ = [33.35 13.73] N·m
Arm pointing straight up, θ = (90°, 0):  τ = [0. 0.] N·m

The worst case — arm stretched out horizontally — sets the torque the motors must be sized for (Chapter 2). With the arm pointing straight up the joints need no torque: the arm is at the θ2=0\theta_2 = 0 singularity, and the load acts along the lost (radial) direction, where the links themselves carry it. Singularities are bad for motion, but good for carrying load.

Example: SCARA pressing and screwing

A SCARA driving a screw pushes down with 20 N while applying 0.5 N·m of tightening moment (clockwise seen from above, so nz=−0.5n_z = -0.5):

  θ₁: -0.50 N·m
  θ₂: -0.50 N·m
  d₃: +20.00 N
  θ₄: +0.50 N·m

The 20 N push is carried entirely by the quill (f3=20f_3 = 20 N) — joints 1 and 2 feel none of it, because their axes are vertical. The moment about the vertical is reacted by every revolute joint with a vertical axis: τ1=τ2=nz\tau_1 = \tau_2 = n_z and τ4=−nz\tau_4 = -n_z (sign flipped because z4\mathbf{z}_4 points down). This is the SCARA’s “selective compliance”: stiff vertically, compliant in the horizontal plane.

Key Takeaways — Section 6.7

  • Joint torques that hold a gripper wrench F=[f;n]\mathbf{F} = [\mathbf{f};\mathbf{n}]: τ=J⊤F\boldsymbol\tau = \mathbf{J}^\top\mathbf{F} (from equal power at the joints and at the gripper).

  • Each torque is a “force × lever arm” about that joint’s axis.

  • At a singularity, loads along the lost direction need no joint torque.

Chapter 6 Summary

  • The Jacobian maps joint velocities to gripper velocity: [v;ω]=J(θ) θ˙[\mathbf{v};\boldsymbol\omega] = \mathbf{J}(\boldsymbol\theta)\,\dot{\boldsymbol\theta}, J∈R6×n\mathbf{J}\in\mathbb{R}^{6\times n}.

  • Column formula, with zi\mathbf{z}_i, oi\mathbf{o}_i from Ti0\mathbf{T}_i^0 and oe\mathbf{o}_e from Te0\mathbf{T}_e^0: revolute Ji=[zi×(oe−oi); zi]\mathbf{J}_i = [\mathbf{z}_i\times(\mathbf{o}_e-\mathbf{o}_i);\ \mathbf{z}_i], prismatic Ji=[zi; 0]\mathbf{J}_i = [\mathbf{z}_i;\ \mathbf{0}].

  • Why: one joint moving is a rotation about (or a slide along) its axis, v=ω×r\mathbf{v} = \boldsymbol\omega\times\mathbf{r}; contributions of all joints add.

  • Singularities: the task Jacobian loses rank (det⁡J=0\det\mathbf{J}=0); a gripper direction is lost and nearby joint speeds blow up.

  • Inverse velocity: θ˙=J−1x˙\dot{\boldsymbol\theta} = \mathbf{J}^{-1}\dot{\mathbf{x}}; iterated, it becomes numerical inverse kinematics.

  • Statics: τ=J⊤F\boldsymbol\tau = \mathbf{J}^\top\mathbf{F}.

RobotTask rowsdet⁡Jtask\det\mathbf{J}_{task}Singular when
Planar 2Rvx,vyv_x, v_yL1L2sin⁡θ2L_1L_2\sin\theta_2arm stretched or folded
Spherical wristωx,ωy,ωz\omega_x,\omega_y,\omega_z−sin⁡θ2-\sin\theta_2axes 1 and 3 aligned
SCARAvx,vy,vz,ωzv_x, v_y, v_z, \omega_zL1L2sin⁡θ2L_1L_2\sin\theta_2arm stretched or folded
UR5eall six−a2a3 s3 s5 (a2c2+a3c23−d5s234)-a_2a_3\,s_3\,s_5\,(a_2c_2+a_3c_{23}-d_5s_{234})elbow, wrist, shoulder