CS 4610/5335
Differential and Inverse Kinematics
Robert Platt Northeastern University
Material adapted from:
1. Lawson Wong, CS 4610/5335
2. Peter Corke, Robotics, Vision and Control
3. Marc Toussaint, U. Stuttgart Robotics Course 4. Oussama Khatib, Stanford CS 223A
Inverse Kinematics (IK) Problem
Given a desired position and orientation (pose) of the end effector, what arm joint angles achieve that pose?
IK Problem:
Given a desired position and orientation of the end effector
Calculate the corresponding joint angles
q3
Two approaches to IK
Suppose we want to move the end effector here
q2
q1
Approach #1: Analytic IK solve for joint angles directly using algebra
Approach #2: Numerical IK solve for joint angles using an iterative numerical technique like gradient descent
this approach can also be used as an online controller
Analytic Inverse Kinematics
Motivating Example Suppose you want to move the end
Goal: move the end effector onto this line
effector above a specified point, Analytic IK:
xg
l
q
x xg
Inverse kinematics
There is no general analytical inverse kinematics solution
All analytical inverse kinematics solutions are specific to a robot or class of robots.
based on geometric intuition about the robot Ill give one example there are many variations.
Inverse kinematics
q3
q5
Inverse kinematics
q2
q4
q6
Spherical wrist: the axes of the last three joints intersect in a point.
q1
Consider this 6-joint robot
Puma 560 Robot
Inverse kinematics
q3
q5
Problem:
Given: desired transform,
q2 q1
q4
q6
Find: Note:
qq q q qq 1234 n
The desired transform (pose) encodes six degrees of freedom (this info can be represented by six numbers)
Since we only have six joints at our disposal, there is no manifold of redundant solutions.
For this manipulator, the problem can be decomposed into a position component (the first three joints) and an orientation component (the last three joints)
The first three joints tell you what the position of the spherical wrist
Example: Inverse kinematics q3 q2
q4
q6
Solution:
First, back out the position of the spherical wrist:
q1
Since its a spherical wrist, the last three joints can be thought of as rotating
about a point.
A constant transform exists that goes from the last wrist joint out to the end
swT eff
effector (sometimes this is called the tool transform):
Back out the position of the wrist:
1 sw eff eff
b b sw TT T
q5
Example: Inverse kinematics q3
Next, solve for the first three joints
q4
q6
q5 q2
First, solve for q1. (look down from above)
q atan2x ,y 1gg
or
q atan2x ,y 1gg
q1
Goal position in horizontal plane
q1
Example: Inverse kinematics
Finally, the last three joints completely specify the orientation of the end effector.
Note that the last three joints look just like ZYZ Euler angles
Determination of the joint angles is easy just calculate the ZYZ Euler angles corresponding to the desired orientation.
q3
q5
q2 q1
q4
q6
Example: Inverse kinematics
Next, solve for q2 and q3 .
(look at the manipulator orthogonal to the plane of the
q3
q5
q2 first two links) q1
q4
q6
Given and , calculate joint angles that cause eff to reach
zyz
( )( )( ) 0 01sin0cos0 01
ZYZ Euler Angles
cos sin 0 cos 0 sin cos sin 0 R (,,)=sin cos 0 0 1 0 sin cos 0
cccss ccssc cs Rzyz(,,)= s c c +c s s c s +c c s s
( ) s c s s c
=a tan2 1r ,r ( 332 33)
atan2r ,r k 23 13
atan2r ,r 32 31
Inverse kinematics for a humanoid arm
You can do similar types of things for a humanoid (7-DOF) arm.
Since this is a redundant arm, there are a manifold of solutions
Spherical wrist
Spherical shoulder
General strategy:
Solve for elbow angle
elbow
Solve for a set of shoulder angles that places the wrist in the right position (note that you have to choose an elbow orbit angle)
Solve for the wrist angles
Differential Kinematics
Up to this point, we have only considered the relationship of the joint angles to the Cartesian location of the end effector:
f (q) x
But what about the first derivative?
This would tell us the velocity of the end effector as a function of joint angle velocities.
f (q) q
Consider a one-link arm
As the arm rotates, the end effector sweeps out an arc
Lets assume that we are only interested in the x coordinate
Forward kinematics: Differentialkinematics:
l
q
Motivating Example
x l cos(q)
dx lsin(q)
x
dq
x l sin(q)q
q 1 x l sin(q)
Suppose you want to move the end effector
Goal: move the end effector onto this line
above a specified point, Analytic IK:
xg
Motivating Example
i0,q0 arbitrary 2. xi l cos(qi )
3. x xg xi
4.q 1 x l sin(qi )
Numerical IK: 1.
l
xg
q
x
5.
6. igoto2.
qi1 qi q
Motivating Example
l
xg
q
q qd
q
q
x
x
x
g
x
1
l sin(q)
joint ctlr
l cos()
joint position sensor
This controller moves the link asymptotically toward the goal position.
Jacobian
Jacobian
x=[l1cos(q1)+l2 cos(q1+q2)] l1sin(q1)+l2sin(q1+q2)
Forward kinematics of the two- link manipulator
Velocity Jacobian
l2 q2
l1
y
q1
x
Jacobian
Chain rule: x Jq
If the Jacobian is square and full rank, then we can
l2 q2
l1
invert it:
1 qJ x
y q1
x
J1
joint ctlr
joint position sensor
Numerical IK Solution
Input: x*
Output: q*
1. repeat until dx is small:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J-1 dx q = q + dq
return q* = q
Jacobian
The Jacobian relates joint velocities with end effector twist:
Jq
End effector twist Jacobian
Joint angle velocities
First derivative of joint angles:
It turns out that you can easily compute the Jacobian for arbitrary manipulator structures
This makes differential kinematics a much easier sub-problem than kinematics in general.
Jacobian
The Jacobian relates joint velocities with end effector twist: What is twist?
Jq
End effector twist Jacobian
Joint angle velocities
First derivative of joint angles:
It turns out that you can easily compute the Jacobian for arbitrary manipulator structures
This makes differential kinematics a much easier sub-problem than kinematics in general.
End effector twist:
Twist is a concatenation of linear velocity and angular velocity:
Linear velocity Angular velocity
What is Twist?
End effector twist:
Twist is a concatenation of linear velocity and angular velocity:
Linear velocity Angular velocity
What is Twist?
What is angular velocity?
Angular velocity is a vector that:
points in the direction of the axis of rotation
has magnitude equal to the velocity of rotation
What is Angular Velocity?
Angular velocity is a vector that:
points in the direction of the axis of rotation
has magnitude equal to the velocity of rotation
Symbol for angular velocity:
Relation between angular velocity and linear velocity:
Breakdown of the Jacobian:
v J q v
J q
=Jv q [J]
Jv x but q
Jacobian
Relation to the derivative:
r J
q
Thats not an angular velocity
Jacobian in different reference frames
In the preceeding, the Jacobian has been expressed in the base frame
It can be expressed in other reference frames using rotation matrices
Velocity is transformed from one reference frame to another using:
3x 3y
q3
l3
l2
2x
1y
1x q2
2y
q1
0y
kkb 0 pR p
0x z
l1
b kkb
b pR p
Therefore, the velocity Jacobian can be
transformed using:
kkb
JR J vbv
Inverse Kinematics: An iterative approach
Jacobian
x Jq
If the Jacobian is square and full rank, then we
l2 q2
l1
can invert it:
1 qJ x
y q1
x
J1
joint ctlr
joint position sensor
Numerical IK Solution (method 1)
Input: x*
Output: q*
1. repeat until dx is small:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J-1 dx q = q + dq
return q* = q
Numerical IK Solution (method 1)
Input: x*
Output: q*
1. repeat until dx is small:
Idea:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J-1 dx q = q + dq
return q* = q
Numerical IK Solution (method 2)
Input: x*
Output: q*
1. repeat until dx is small:
This also works
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * JT dx q = q + dq
return q* = q
This results in different behavior (see homework)
Non-square Jacobian matrix
Input: x*
Output: q*
1. repeat until dx is small:
2. 3. 4. 5. 6. 7. 8.
init q to random joint configuration repeat K times:
x = FK(q)
dx = x*-x
dq = stepsize * J-1 dx q = q + dq
return q* = q
Can only take inverse of Jacobian if it is square
Non-square Jacobian matrix
Example of a non-square Jacobian matrix:
3x 3y
l3 2x
l2
J (q)=[l1 s1l2 s12l3 s123 l1 c1+l2 c12+l3 c123
l1 s1l2 s12 l1 c1+l2 c12
l1 s1 ] l1 c1
1y 2y
q3
1x q2
q 1 q
0y
q1
x =J q []()
0 x z
l
Two equations of three y [2] variableseach
1
0
q 3
This is an under-constrained system of equations.
multiple solutions
there are multiple joint angle velocities that realize the same EFF velocity.
Non-square Jacobian matrix
If the Jacobian is not a square matrix, then you cant invert it.
3x 3y
what next?
l3 2x
l2
q3
1x q2
1y 2y
1
0x
0y
We have:
We are looking for a matrix
qJ#x
x Jq
q
l
1
such that:
xJq
0z
Two cases:
Underconstrained manipulator (redundant) Overconstrained
Generalized inverse:
for the underconstrained manipulator: given that minimizes s.t.
, find a vector
Generalized inverse
for the overconstrained manipulator: given , find a vector s.t. is minimized
#TT1 J J JJ
#T1T J J J J
J# J1
Underconstrained case (if there are more columns than rows (m
Reviews
There are no reviews yet.