Ch. 3: Forward and Inverse Kinematics
ES159/259
Updates
•
•
Document clarifying the Denavit-Hartenberg convention is posted
Labs and section times announced
– If you haven’t already, please forward your availability to Shelten & Ben
•
Matlab review session Tuesday 2/13, 6:00 MD 221
ES159/259
Recap: The Denavit-Hartenberg (DH) Convention
•
Representing each individual homogeneous transformation as the
product of four basic transformations:
Ai  Rot z, i Trans z,d i Trans x ,ai Rot x , i
c i
s
  i
0

0
c i
s
  i
0

0
 s i
0 0 1
c i
0 0 0
0
1 0 0

0
0 1 0
 s i c i
s i s i
c i c i
s i
0
0  1
1 0 0  0
0 1 d i  0

0 0 1  0
ai c i 
 c i s i ai s i 
c i
di 

0
1 
0 0
ES159/259
0 0 ai   1 0
1 0 0  0 c i
0 1 0  0 s i

0 0 1  0 0
0
 s i
c i
0
0
0
0

1
Recap: the physical basis for DH parameters
•
•
•
•
ai: link length, distance between the o0 and o1 (projected along x1)
i: link twist, angle between z0 and z1 (measured around x1)
di: link offset, distance between o0 and o1 (projected along z0)
i: joint angle, angle between x0 and x1 (measured around z0)
ES159/259
General procedure for determining forward kinematics
1. Label joint axes as z0, …, zn-1 (axis zi is joint axis for joint i+1)
2. Choose base frame: set o0 on z0 and choose x0 and y0 using righthanded convention
3. For i=1:n-1,
i.
Place oi where the normal to zi and zi-1 intersects zi. If zi intersects zi-1, put
oi at intersection. If zi and zi-1 are parallel, place oi along zi such that di=0
ii. xi is the common normal through oi, or normal to the plane formed by zi-1
and zi if the two intersect
iii. Determine yi using right-handed convention
4.
5.
6.
7.
Place the tool frame: set zn parallel to zn-1
For i=1:n, fill in the table of DH parameters
Form homogeneous transformation matrices, Ai
Create Tn0 that gives the position and orientation of the end-effector in
the inertial frame
ES159/259
Example 2: three-link cylindrical robot
•
3DOF: need to assign four coordinate frames
1.
2.
3.
4.
Choose z0
Choose z1
Choose z2
Choose z3
•
•
axis (axis of rotation for joint 1, base frame)
axis (axis of translation for joint 2)
axis (axis of translation for joint 3)
axis (tool frame)
This is again arbitrary for this case since we have described no wrist/gripper
Instead, define z3 as parallel to z2
ES159/259
Example 2: three-link cylindrical robot
•
Now define DH parameters
– First, define the constant parameters ai, i
– Second, define the variable parameters i, di
c1  s1
s
c1
A1   1
0
0

0
0
0
1 0
0 0
0 0 
, A2  
0  1
1 d1 


0 1
0 0
0
0
1
0
1 0 
, A3  
0
0 d2 


0 1
0
0
0
1 0 0 
0 1 d3 

0 0 1
0 0
c1 0  s1  s1d 3 
s
0
c1
c1d 3 
1
0

T3  A1A2 A3 
 0  1 0 d1  d 2 


0
1 
0 0
ES159/259
link ai
i
di
1
0
0
d1 1
2
0
-90 d2 0
3
0
0
i
d3 0
Example 3: spherical wrist
•
3DOF: need to assign four coordinate frames
–
1.
2.
3.
4.
yaw, pitch, roll (4, 5, 6) all intersecting at one point o (wrist center)
Choose z3 axis (axis of rotation for joint 4)
Choose z4 axis (axis of rotation for joint 5)
Choose z5 axis (axis of rotation for joint 6)
Choose tool frame:
•
•
•
z6 (a) is collinear with z5
y6 (s) is in the direction the gripper closes
x6 (n) is chosen with a right-handed convention
ES159/259
Example 3: spherical wrist
•
Now define DH parameters
– First, define the constant parameters ai, i
– Second, define the variable parameters i, di
c 4
s
A4   4
0

0
0
 s4
0
c4
1
0
0
0
0
c5
s
0
, A5   5
0
0


1
0
c 4c5c6  s4s6
s c c  c s
4 6
3
T6  A4 A5 A6   4 5 6
  s5 c 6

0

0
 s5
0
c5
1
0
0
0
0
c6
s
0
, A6   6
0
0


1
0
 c 4c 5 s6  s 4 c 6
c 4 s5
 s 4c 5 s6  c 4 c 6
s 4 s5
s5 c 6
c5
0
0
 s6
c6
0
0
c 4 s5 d 6 
s4s5d 6 
c5d 6 

1 
ES159/259
0
0 0 
1 d6 

0 1
0
link ai
i
di
i
4
0
-90 0
4
5
0
90
0
5
6
0
0
d6 6
Example 4: cylindrical robot with spherical wrist
•
6DOF: need to assign seven coordinate frames
– But we already did this for the previous two examples, so we can fill in the
table of DH parameters:
o3, o4, o5 are all at
the same point oc
ES159/259
link ai
i
di
1
0
0
d1 1
2
0
-90 d2 0
3
0
0
4
0
-90 0
4
5
0
90
0
5
6
0
0
d6 6
i
d3 0
Example 4: cylindrical robot with spherical wrist
•
Note that z3 (axis for joint 4) is collinear with z2 (axis for joint 3), thus we
can make the following combination:
r11 r12
r
r
0
0 3
T6  T3 T6   21 22
r31 r32

0 0
r13
r23
r33
0
dx 
d y 
dz 

1
r11  c1c 4c5c 6  c1s4s6  s1s5c 6
r21  s1c 4c5c 6  s1s4s6  c1s5c 6
r31  s4c5c 6  c 4s6
r12  c1c 4c5s6  c1s4c 6  s1s5c 6
r22  s1c 4c5s6  s1s4s6  c1s5c 6
r32  s4c5c 6  c 4c 6
r13  c1c 4s5  s1c5
r23  s1c 4s5  c1c5
r33  s4s5
d x  c1c 4s5d 6  s1c5d 6  s1d 3
d y  s1c 4s5d 6  c1c5d 6  c1d 3
d z  s4s5d 6  d1  d 2
ES159/259
Example 5: the Stanford manipulator
•
6DOF: need to assign seven coordinate frames:
1.
2.
3.
4.
5.
Choose z0 axis (axis of rotation for joint 1, base frame)
Choose z1-z5 axes (axes of rotation/translation for joints 2-6)
Choose xi axes
Choose tool frame
Fill in table of DH parameters:
link ai
i
di
i
1
0
-90 0
1
2
0
90
d2 2
3
0
0
d3 0
4
0
-90 0
4
5
0
90
0
5
6
0
0
d6 6
ES159/259
Example 5: the Stanford manipulator
•
Now determine the individual homogeneous transformations:
c1 0  s1
s
0
c1
A1   1
0 1 0

0
0 0
c 4 0  s 4
s
0
c4
A4   4
 0 1 0

0
0 0
0
c 2
s
0
, A2   2
0
0


1
0
0
c5
s
0
, A5   5
0
0


1
0
0
1 0
0 1
0  c 2 0 
, A3  
0 0
1 0 d2 


0 0
1
0 0
0
s5 0 
c 6
s
0  c5 0
, A6   6
0
 1 0 0


0
0
1
0
0
s2
ES159/259
0
0
1
0
 s6
c6
0
0
0
0 
d3 

1
0 0
0 0 
1 d6 

0 1
Example 5: the Stanford manipulator
•
Finally, combine to give the complete description of the forward
kinematics:
r11  c1c 2 c 4c5c 6  s4s6   s2s5c 6   d 2 s4c5c 6  c 4s6 
r21  s1c 2 c 4c5c 6  s4s6   s2s5c 6   c1 s4c5c 6  c 4s6 
r31  s2 c 4c5c 6  s4s6   c 2s5c 6
r12  c1 c 2 c 4c5s6  s4c 6   s2s5s6   s1  s4c5s6  c 4c 6 
r11 r12
r
r
0
T6  A1    A6   21 22
r31 r32

0 0
r13
r23
r33
0
dx 
d y 
dz 

1
r22  s1 c 2 c 4c5s6  s4c 6   s2s5s6   c1  s4c5s6  c 4s6 
r32  s2 c 4c5s6  s4c 6   c 2s5s6
r13  c1 c 2c 4s5  s2c5   s1s4s5
r23  s1 c 2c 4s5  s2c5   c1s4s5
r33  s2c 4s5  c 2c5
d x  c1s2d 3  s1d 2  d 6 c1c 2c 4s5  c1c5s2  s1s4s5 
d y  s1s2d 3  c1d 2  d 6 c1s4s5  c 2c 4s1s5  c5s1s2 
d z  c 2d 3  d 6 c 2c5  c 4s2s5 
ES159/259
Example 6: the SCARA manipulator
•
4DOF: need to assign five coordinate frames:
1.
2.
3.
4.
5.
Choose z0 axis (axis of rotation for joint 1, base frame)
Choose z1-z3 axes (axes of rotation/translation for joints 2-4)
Choose xi axes
Choose tool frame
Fill in table of DH parameters:
i
di
i
1
a1 0
0
1
2
a2 180 0
2
3
0
0
d3 0
4
0
0
d4 4
link ai
ES159/259
Example 6: the SCARA manipulator
•
Now determine the individual homogeneous transformations:
c1  s1
s
c1
A1   1
0
0

0
0
0 a1c1 
c2
s
0 a1s1 
, A2   2
0
1 0 


0 1 
0
s2
 c2
0
0
a2c2 
1
0
0 a2s2 
, A3  

0
1 0


0
1 
0
0
c12c 4  s12s4
s c  c s
12 4
0
T4  A1    A4   12 4

0

0

 c12s4  s12c 4
 s12s4  c12c 4
0
0
ES159/259
0
c 4
s
1 0 0 
, A4   4
0
0 1 d3 


0 0 1
0
0 0
a1c1  a2c12 
0 a1s1  a2s12 
 1  d3  d 4 

0
1

0
 s4
c4
0
0
0
0 0 
1 d4 

0 1
0
Forward kinematics of parallel manipulators
•
•
•
Parallel manipulator: two or more series chains connect the endeffector to the base (closed-chain)
# of DOF for a parallel manipulator determined by taking the total DOFs
for all links and subtracting the number of constraints imposed by the
closed-chain configuration
Gruebler’s formula (3D):
nj
# DOF  6nL  n j    fi
i 1
number of links*
#DOF for joint i
number of joints
ES159/259
*excluding ground
Forward kinematics of parallel manipulators
•
Gruebler’s formula (2D):
•
Example (2D):
nj
# DOF  3nL  n j    fi
i 1
– Planar four-bar, nL = 3, nj = 4, fi = 1(for all joints)
• 3(3-4)+4 = 1DOF
– Forward kinematics:
  2  2  2L 2
2
  cos 

2
2
 2 L1     L2
1

  tan 1 L2
L 

 1

ES159/259
 
 
 2
Inverse Kinematics
•
Find the values of joint parameters that will put the tool frame at a
desired position and orientation (within the workspace)
– Given H:
R o 
H
  SE 3 
0
1


– Find all solutions to:
Tn0 q1,..., qn   H
– Noting that:
Tn0 q1,..., qn   A1q1     An qn 
– This gives 12 (nontrivial) equations with n unknowns
ES159/259
Example: the Stanford manipulator
•
For a given H:
0
0
H
1

0
•
1 0  0.154
0 1 0.763 
0 0
0 

0 0
1 
c1c 2 c 4c5c 6  s4s6   s2s5c 6   d 2 s4c5c 6  c 4s6   0
s1c 2 c 4c5c 6  s4s6   s2s5c 6   c1 s4c5c 6  c 4s6   0
 s2 c 4c5c 6  s4s6   c 2s5c 6  1
c1 c 2 c 4c5s6  s4c 6   s2s5s6   s1  s4c5s6  c 4c 6   1
 s1 c 2 c 4c5s6  s4c 6   s2s5s6   c1  s4c5s6  c 4s6   0
s2 c 4c5s6  s4c 6   c 2s5s6  0
Find 1, 2, d3, 4, 5, 6:
c1 c 2c 4s5  s2c5   s1s4s5  0
s1 c 2c 4s5  s2c5   c1s4s5  1
 s 2 c 4 s5  c 2 c 5  0
c1s2d 3  s1d 2  d 6 c1c 2c 4s5  c1c5s2  s1s4s5   0.154
s1s2d 3  c1d 2  d 6 c1s4s5  c 2c 4s1s5  c5s1s2   0.763
c 2d 3  d 6 c 2c5  c 4s2s5   0
•
One solution: 1 = /2, 2 = /2, d3 = 0.5, 4 = /2, 5 = 0, 6 = /2
ES159/259
Inverse Kinematics
•
•
•
The previous example shows how difficult it would be to obtain a
closed-form solution to the 12 equations
Instead, we develop systematic methods based upon the manipulator
configuration
For the forward kinematics there is always a unique solution
– Potentially complex nonlinear functions
•
The inverse kinematics may or may not have a solution
– Solutions may or may not be unique
– Solutions may violate joint limits
•
Closed-form solutions are ideal!
ES159/259
Overview: kinematic decoupling
•
Appropriate for systems that have an arm a wrist
– Such that the wrist joint axes are aligned at a point
•
For such systems, we can split the inverse kinematics problem into two
parts:
1. Inverse position kinematics: position of the wrist center
2. Inverse orientation kinematics: orientation of the wrist
•
First, assume 6DOF, the last three intersecting at oc
R60 q1,..., q6   R
o60 q1,..., q6   o
•
Use the position of the wrist center to determine the first three joint
angles…
ES159/259
Overview: kinematic decoupling
•
Now, origin of tool frame, o6, is a distance d6 translated along z5 (since
z5 and z6 are collinear)
– Thus, the third column of R is the direction of z6 (w/ respect to the base
frame) and we can write:
0
o  o60  oco  d 6R 0
1
– Rearranging:
0
oco  o  d 6R 0
1
– Calling o = [ox oy oz]T, oc0 = [xc yc zc]T
 xc  o x  d 6 r13 
 y   o  d r 
6 23 
 c  y
 zc  oz  d 6 r33 
ES159/259
Overview: kinematic decoupling
•
Since [xc yc zc]T are determined from the first three joint angles, our
forward kinematics expression now allows us to solve for the first three
joint angles decoupled from the final three.
– Thus we now have R30
– Note that:
R  R30R63
– To solve for the final three joint angles:
 
R63  R30
1
 
T
R  R30 R
– Since the last three joints for a
spherical wrist, we can use a set of
Euler angles to solve for them
ES159/259
Inverse position
•
Now that we have [xc yc zc]T we need to find q1, q2, q3
– Solve for qi by projecting onto the xi-1, yi-1 plane, solve trig problem
– Two examples: elbow (RRR) and spherical (RRP) manipulators
– For example, for an elbow manipulator, to solve for 1, project the arm onto
the x0, y0 plane
ES159/259
Background: two argument atan
•
We use atan2(·) instead of atan(·) to account for the full range of
angular solutions
– Called ‘four-quadrant’ arctan
 atan2 y , x 

 y
  atan  
 x


atan2y , x    atan y 
x




2
 undefined

y 0
y  0, x  0
y  0, x  0
y  0, x  0
y  0, x  0
ES159/259
Example: RRR manipulator
1. To solve for 1, project the arm onto the x0, y0 plane
1  atan2xc , y c 
– Can also have: 1    atan2xc , y c 
•
This will of course change the solutions for 2 and 3
ES159/259
Caveats: singular configurations, offsets
•
If xc=yc=0, 1 is undefined
–
•
i.e. any value of 1 will work
If there is an offset, then we will
have two solutions for 1: left arm
and right arm
–
ES159/259
However, wrist centers cannot
intersect z0
Left arm and right arm solutions
•
Left arm:
•
Right arm:
1    
  atan2xc , y c 
1   - 
  atan2xc , y c 
    atan2 xc  y c  d 2 , d 
2
2
2
  atan2 xc  y c  d 2 , d 



2

2
2
 atan2  xc  y c  d 2 ,d 


ES159/259
Left arm and right arm solutions
•
•
Therefore there are in general two solutions for 1
Finding 2 and 3 is identical to the planar two-link manipulator we have
seen previously:
r 2  s 2  a2  a3
cos 3 
2a2a3
2
2
r 2  xc  y c  d 2
2
2
s  zc  d1
xc  y c  d 2  zc  d1   a2  a3
 cos 3 
D
2a2a3
2
•
2
2
2
2
Therefore we can find two solutions for 3:

 3  atan2 D, 1  D 2

ES159/259
Left arm and right arm solutions
•
•
The two solutions for 3 correspond to the elbow-down and elbow-up
positions respectively
Now solve for 2:
 2  atan2r , s   atan2a2  a3c3 , a3s3 
2
2
 atan2 xc  y c  d 2 , zc  d1   atan2a2  a3c3 , a3s3 


•
Thus there are two solutions for the pair (2, 3)
ES159/259
RRR: Four total solutions
•
In general, there will be a maximum of four solutions to the inverse
position kinematics of an elbow manipulator
– Ex: PUMA
ES159/259
Example: RRP manipulator
•
Spherical configuration
– Solve for 1 using same method as with RRR
1  atan2xc , y c 
– Again, if there is an offset, there
will be left-arm and right-arm solutions
– Solve for 2:
 2  atan2s, r 
r 2  xc  y c
2
2
s  zc  d1
– Solve for d3:
d3  r 2  s 2
 xc  y c  zc  d1 
2
2
2
ES159/259
Next class…
•
Complete the discussion of inverse kinematics
– Inverse orientation
– Introduction to other methods
•
Introduction to velocity kinematics and the Jacobian
ES159/259