Forward Kinematics
and Jacobians
Kris Hauser
CS B659: Principles of Intelligent Robot Motion
Spring 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)
q2
q1
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
T2ref
T3ref
L2
T0
T1ref
L0
L1
L3
T4ref
Rotating the first joint
T1(q1) = T1ref·R(q1)
T1(q1)
T0
q1
T1ref
L0
Where is the second joint?
T2(q1) ?
T2ref
T0
q1
Where is the second joint?
T2ref
T0
q1
T2parent(q1) = T1(q1) ·(T1ref)-1·T2ref
After rotating joint 2
q2
T2R
T0
q1
T2(q1,q2) = T1(q1) ·(T1ref)-1·T2ref·R(q2)
After rotating joint 2
q2
T2R
T0
q1
Denote T2->1ref = (T1ref)-1·T2ref (frame relative to parent)
T2(q1,q2) = T1(q1) ·T2->1ref·R(q2)
General Formula
Denote
𝑟𝑒𝑓 −1 𝑟𝑒𝑓
= 𝑇𝑖−1
𝑇𝑖
(ref
𝑘
𝑟𝑒𝑓
=
𝑇𝑖→𝑖−1 𝑅(𝑞𝑖 )
𝑖=1
𝑟𝑒𝑓
𝑇𝑖→𝑖−1
𝑇𝑘 𝑞1 , … , 𝑞𝑘
T2(q1,q2)
L2
T0
T1(q1)
L0
L1
frame relative to parent)
T3(q1,..,q3)
L3
T4(q1,…,q4)
Generalization to tree
structures
• Topological sort: p[k] = parent of link k
• Denote
𝑟𝑒𝑓
𝑇𝑖→𝑝[𝑖]
=
𝑟𝑒𝑓
𝑇𝑝 𝑖
−1
𝑟𝑒𝑓
𝑇𝑖
(frame i relative to parent)
• Let A(i) be the list of ancestors of i (sorted from root to i)
• 𝑇𝑘 𝑞1 , … , 𝑞𝑘 =
𝑟𝑒𝑓
𝑖∈𝐴(𝑘) 𝑇𝑖→𝑝[𝑖] 𝑅(𝑞𝑖 )
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 q ∈
𝑅𝑛 is the m x n matrix of partial derivatives
f1/q1 … f1/qn
…
…
fm/q1 … fm/qn
• Typically written J(q)
• (Note the dependence on q)
Aside on partial derivatives…
Single Joint Robot Example
(x,y)
𝑥
𝐿 cos 𝑞
𝑞
=
𝑦
𝐿 sin 𝑞
L
q
Single Joint Robot Example
(x,y)
𝑥
𝐿 cos 𝑞
𝑞
=
𝑦
𝐿 sin 𝑞
L
q
𝐽 𝑞 =
𝜕𝑥/𝜕𝑞
−𝐿 sin 𝑞
𝑞 =
𝐿 cos 𝑞
𝜕𝑦/𝜕𝑞
Single Joint Robot Example
(x,y)
𝐽 𝑞
𝑥
𝐿 cos 𝑞
𝑞
=
𝑦
𝐿 sin 𝑞
L
q
𝐽 𝑞 =
𝜕𝑥/𝜕𝑞
−𝐿 sin 𝑞
𝑞 =
𝐿 cos 𝑞
𝜕𝑦/𝜕𝑞
Significance
• If x is an end effector, multiplying J(q) with a joint velocity
vector 𝑞 gives the end effector velocity 𝑥 = 𝐽(𝑞)𝑞
𝑥
=𝐽 𝑞 𝑞
𝑦
𝐽 𝑞
(x,y)
𝐽 𝑞
𝑥
=𝐽 𝑞 𝑞
𝑦
𝑞
L
𝑞
q
Computing Jacobians in
general
• How do we compute it?
𝑟𝑒𝑓
𝑇𝑘 𝑞 𝑥𝑘 =
𝑇𝑖→𝑝 𝑖 𝑅 𝑞𝑖
𝑥𝑘
𝑖∈𝐴 𝑘
• Consider derivative w.r.t. qj
𝜕
𝑑
𝑟𝑒𝑓
𝑇𝑘 𝑞 𝑥𝑘 = 𝑇𝑝 𝑗 𝑞 𝑇𝑗→𝑝 𝑗
𝑅 𝑞𝑗 𝑇𝑘→𝑗 𝑞 𝑥𝑘
𝜕𝑞𝑗
𝑑𝑞𝑗
Derivative…
cos 𝜃 − sin 𝜃 0
• 𝑅(𝜃) = sin 𝜃 cos 𝜃 0
0
0
1
0 −1 0
−sin 𝜃 − cos 𝜃 0
𝑑
•
𝑅 𝜃 = cos 𝜃
sin 𝜃
0 = 𝑅(𝜃) 1 0 0
𝑑𝜃
0 0 0
0
0
0
0 −1 0
𝜕
𝑇 𝑞 𝑥𝑘 = 𝑇𝑗 𝑞 1 0 0 𝑇𝑘→𝑗 𝑞 𝑥𝑘
𝜕𝑞𝑗 𝑘
0 0 0
Derivative…
•
𝑑
𝑅
𝑑𝜃
0 −1 0
−sin 𝜃 − cos 𝜃 0
𝜃 = cos 𝜃
sin 𝜃
0 = 𝑅(𝜃) 1 0 0
0 0 0
0
0
0
0 −1 0
𝜕
𝑇 𝑞 𝑥𝑘 = 𝑇𝑗 𝑞 1 0 0 𝑇𝑘→𝑗 𝑞 𝑥𝑘
𝜕𝑞𝑗 𝑘
0 0 0
T2(q1,q2)
L2
T0
T1(q1)
L1
𝜕
𝑥
𝜕𝑞𝑗
T3(q1,..,q3)
L3
xk
T4(q1,…,q4)
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
T2(q1,q2)
L2
T0
T1(q1)
L1
T3(q1,..,q3)
L3
xk
T4(q1,…,q4)
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
T2(q1,q2)
L2
T0
T1(q1)
L1
T3(q1,..,q3)
L3
xk
T4(q1,…,q4)
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)