Forward Kinematics and JacobiansKris HauserCS B659: Principles of Intelligent Robot MotionSpring 2013

Articulated Robot• Robot: usually a rigid

articulated structure• Geometric CAD models,

relative to reference frames• A configuration specifies

the placement of those frames (forward kinematics)

q1

q2

Forward Kinematics

• Given:• A kinematic reference frame of the robot• Joint angles q1,…,qn

• Find rigid frames T1,…,Tn relative to T0

• A frame T=(R,t) consists of a rotation R and a translation t so that T·x = R·x + t• Make notation easy: use homogeneous coordinates• Transformation composition goes from right to left:

T1·T2 indicates the transformation T2 first, then T1

Kinematic Model of Articulated Robots: Reference Frame

T0

L0

L1

L2 L3

T1ref

T2ref

T3ref

T4ref

Rotating the first joint

T0

L0

T1ref

q1

T1(q1)

T1(q1) = T1ref·R(q1)

Where is the second joint?

T0

T2ref

q1

T2(q1) ?

Where is the second joint?

T0

T2ref

q1

T2parent(q1) = T1(q1) ·(T1

ref)-1·T2ref

After rotating joint 2

T0

T2R

q1

T2(q1,q2) = T1(q1) ·(T1ref)-1·T2

ref·R(q2)

q2

After rotating joint 2

T0

T2R

q1

Denote T2->1ref = (T1

ref)-1·T2ref (frame relative to parent)

T2(q1,q2) = T1(q1) ·T2->1ref·R(q2)

q2

General FormulaDenote (ref frame relative to parent)

T0

L0

L1

L2L3

T1(q1)

T2(q1,q2)T3(q1,..,q3)

T4(q1,…,q4)

Generalization to tree structures• Topological sort: p[k] = parent of link k• Denote (frame i relative to parent)• Let A(i) be the list of ancestors of i (sorted from root to i)

To 3D…• Much the same, except joint axis must be defined (relative to

parent)• Angle-axis parameterization

Generalizations• Prismatic joints• Ball joints• Prismatic joints• Spirals• Free-floating bases

From LaValle, Planning Algorithms

The Jacobian Matrix

• The Jacobian of a function x = f(q), with and is the m x n matrix of partial derivatives

• Typically written J(q)• (Note the dependence on q)

f1/q1 … f1/qn

… …

fm/q1 … fm/qn

Aside on partial derivatives…

Single Joint Robot Example

q

(x,y)

L [𝑥𝑦 ] (𝑞 )=[𝐿cos𝑞𝐿sin𝑞 ]

Single Joint Robot Example

q

(x,y)

L [𝑥𝑦 ] (𝑞 )=[𝐿cos𝑞𝐿sin𝑞 ]𝐽 (𝑞 )=[𝜕 𝑥 /𝜕𝑞

𝜕 𝑦 /𝜕𝑞 ] (𝑞)=[−𝐿 sin𝑞𝐿 cos𝑞 ]

Single Joint Robot Example

q

(x,y)

L [𝑥𝑦 ] (𝑞 )=[𝐿cos𝑞𝐿sin𝑞 ]𝐽 (𝑞 )=[𝜕 𝑥 /𝜕𝑞

𝜕 𝑦 /𝜕𝑞 ] (𝑞)=[−𝐿 sin𝑞𝐿 cos𝑞 ]

𝐽 (𝑞 )

Significance• If x is an end effector, multiplying J(q) with a joint velocity

vector gives the end effector velocity

q

(x,y)

L𝐽 (𝑞 )

𝐽 (𝑞 )

[ �̇��̇� ]= 𝐽 (𝑞) �̇�

�̇�

[ �̇��̇� ]= 𝐽 (𝑞) �̇�

�̇�

Computing Jacobians in general• How do we compute it?

• Consider derivative w.r.t. qj

Derivative…

Derivative…

T0

L1

L2L3

T1(q1)

T2(q1,q2)T3(q1,..,q3)

T4(q1,…,q4)

xk

𝜕𝜕𝑞 𝑗

𝑥

Consequences…

• Column j of position Jacobian Jx(q) is the speed at which x would change if joint j rotated alone

• Perpendicular and equal in magnitude to vector from x to joint axis

• Larger when x is farther from the joint axis

T0

L1

L2L3

T1(q1)

T2(q1,q2)T3(q1,..,q3)

T4(q1,…,q4)

xk

Orientation Jacobian• Consider end effector orientation θ(q) in plane• All entries of Jθ(q) corresponding to revolute joints are 1!

• In 3D, column j is identical to the axis of rotation of joint j (in world space) at configuration q

T0

L1

L2L3

T1(q1)

T2(q1,q2)T3(q1,..,q3)

T4(q1,…,q4)

xk

Total Jacobian

• Total Jacobian J(q) is the matrix formed by stacking Jx(q), Jθ(q)• 3 rows in 2D, 6 rows in 3D

Next class: Inverse Kinematics• Readings on schedule:• Wang and Chen (1991)• Duindam et al (2008)