Homework 5 Solutions

Solutions for Problems 3.9, 3.10, 3.11, and 3.12
3.9. Here, we are given a three-link Cartesian manipulator with a spherical wrist. We had considered
a three-link Cartesian manipulator in Problem 3.7 in Homework 4. Using the D-H coordinate
frames defined in the solution of Problem 3.7 in Homework 4 and adding the standard coordinate
frames for the spherical wrist, we get the D-H table shown below:
ai
αi
di
θi
Link 1 0 -90o d∗1
0
Link 2 0 -90o d∗2 90o
Link 3 0
0
d∗3
0
o
Link 4 0 -90
0
θ4∗
Link 5 0 90o
0
θ5∗
Link 6 0
0
d6 θ6∗
Hence, the homogeneous transformation matrices A1 , . . . , A6 and the homogeneous transformation matrix H60 are given as:






1 0 0 0
0 0 −1 0
1 0 0 0
 0 0 1 0 



0
0 
 ; A2 =  1 0
 ; A3 =  0 1 0 0 
A1 = 
(1)
 0 −1 0 d1 
 0 −1 0 d2 
 0 0 1 d3 
0 0 0 1
0 0
0
1
0 0 0 1






c4 0 −s4 0
c5 0 s5 0
c6 −s6 0 0
 s4 0




c4 0 
 ; A5 =  s5 0 −c5 0  ; A6 =  s6 c6 0 0  (2)
A4 = 
 0 −1
 0 1 0 0 
 0
0
0 
0
1 d6 
0
0
0
1
0 0 0 1
0
0
0 1


c6 s5
−s5 s6
−c5
−d3 − d6 c5


−c
s
−
c
c
s
c
s
s
−
c
c
−s
s
d
4 6
5 6 4
5 4 6
4 6
4 5
2 − d6 s4 s5 
(3)
H60 = A1 A2 A3 A4 A5 A6 = 
 s4 s6 − c4 c5 c6 c6 s4 + c4 c5 s6 −c4 s5 d1 − d6 c4 s5 
0
0
0
1
3.10 The choice of the D-H coordinate frames is shown in the figure below.
The corresponding D-H table is:
1
Link
Link
Link
Link
Link
Link
1
2
3
4
5
6
ai
0
a2
0
0
0
0
αi
90o
0
90o
-90o
90o
0
di
d1
d2
0
d4
0
d6
θi
θ1∗
θ2∗
θ3∗
θ4∗
θ5∗
θ6∗
Here, a2 = d4 = 8 inches and d1 = 13 inches. In the figure, there is an offset between the 1-frame
and the 2-frame; this offset would correspond to d2 . d6 is the distance from the wrist center to
the end-effector tip.
Hence, the homogeneous transformation matrices A1 , . . . , A6 and the homogeneous transformation matrix H60 are given as:






c1 0 s1
0
c2 −s2 0 a2 c2
c3 0 s3 0
 s1 0 −c1 0 
 s2 c2 0 a2 s2 



 ; A3 =  s3 0 −c3 0 

A1 = 
 0 1 0 d1  ; A2 =  0


0
1 d2
0 1 0 0 
0 0 0
1
0
0
0
1
0 0 0 1
(4)






c6 −s6 0 0
c5 0 s5 0
c4 0 −s4 0




 s4 0

s
s
0
−c
0
c
0
5
4
 ; A6 =  6 c6 0 0 
 ; A5 =  5
A4 = 




 0 −1
0
0
1 d6 
0 1 0 0
0
d4
0
0
0 1
0 0 0 1
0
0
0
1
(5)


r11 r12 r13 r14
 r21 r22 r23 r24 
0

H6 = A1 A2 A3 A4 A5 A6 = 
(6)
 r31 r32 r33 r34 
0
0
0
1
where
r11 = s6 (c4 s1 + s4 (c1 s2 s3 − c1 c2 c3 )) + c6 (c5 (s1 s4 − c4 (c1 s2 s3 − c1 c2 c3 )) − s5 (c1 c2 s3 + c1 c3 s2 ))
r12 = c6 (c4 s1 + s4 (c1 s2 s3 − c1 c2 c3 )) − s6 (c5 (s1 s4 − c4 (c1 s2 s3 − c1 c2 c3 )) − s5 (c1 c2 s3 + c1 c3 s2 ))
r13 = s5 (s1 s4 − c4 (c1 s2 s3 − c1 c2 c3 )) + c5 (c1 c2 s3 + c1 c3 s2 )
r14 = d6 (s5 (s1 s4 − c4 (c1 s2 s3 − c1 c2 c3 )) + c5 (c1 c2 s3 + c1 c3 s2 )) + d4 (c1 c2 s3 + c1 c3 s2 ) + d2 s1 + a2 c1 c2
r21 = −s6 (c1 c4 − s4 (s1 s2 s3 − c2 c3 s1 )) − c6 (c5 (c1 s4 + c4 (s1 s2 s3 − c2 c3 s1 )) + s5 (c2 s1 s3 + c3 s1 s2 ))
r22 = s6 (c5 (c1 s4 + c4 (s1 s2 s3 − c2 c3 s1 )) + s5 (c2 s1 s3 + c3 s1 s2 )) − c6 (c1 c4 − s4 (s1 s2 s3 − c2 c3 s1 ))
r23 = c5 (c2 s1 s3 + c3 s1 s2 ) − s5 (c1 s4 + c4 (s1 s2 s3 − c2 c3 s1 ))
r24 = d4 (c2 s1 s3 + c3 s1 s2 ) − d6 (s5 (c1 s4 + c4 (s1 s2 s3 − c2 c3 s1 )) − c5 (c2 s1 s3 + c3 s1 s2 )) − d2 c1 + a2 c2 s1
r31 = c6 (s5 (c2 c3 − s2 s3 ) + c4 c5 (c2 s3 + c3 s2 )) − s4 s6 (c2 s3 + c3 s2 )
r32 = −s6 (s5 (c2 c3 − s2 s3 ) + c4 c5 (c2 s3 + c3 s2 )) − c6 s4 (c2 s3 + c3 s2 )
r33 = c4 s5 (c2 s3 + c3 s2 ) − c5 (c2 c3 − s2 s3 )
r34 = d1 − d4 (c2 c3 − s2 s3 ) + a2 s2 − d6 (c5 (c2 c3 − s2 s3 ) − c4 s5 (c2 s3 + c3 s2 ))
3.11 We are given a three-link planar arm. The choice of the D-H coordinate frames is shown in the
figure below.
2
The corresponding D-H table is:
ai αi di θi
Link 1 a1 0 0 θ1∗
Link 2 a2 0 0 θ2∗
Link 3 a3 0 0 θ3∗
Hence, the homogeneous transformation matrices A1 , A2 , A3 and the homogeneous transformation matrix H30 are given as:






c1 −s1 0 a1 c1
c2 −s2 0 a2 c2
c3 −s3 0 a3 c3
 s1 c1 0 a1 s1 




 ; A2 =  s2 c2 0 a2 s2  ; A3 =  s3 c3 0 a3 s3 
A1 = 
 0




0
1
0
0
0
1
0
0
0
1
0 
0
0
0
1
0
0
0
1
0
0
0
1
(7)
H30 = A1 A2 A3
(8)
0
The translational part (the first three elements of the last column) of H3 can be found to be


a1 c1 + a2 c12 + a3 c123
o06 =  a1 s1 + a2 s12 + a3 s123 
(9)
0
where c12 = cos(θ1 + θ2 ), etc. Hence, if only the desired position is given as d = [dx , dy , 0]T , we
have three unknown variables (θ1 , θ2 , θ3 ) to be found from the two equations:
dx = a1 c1 + a2 c12 + a3 c123
(10)
dy = a1 s1 + a2 s12 + a3 s123 .
(11)
Hence, we will, in general, have infinitely many solutions to the inverse position kinematics
problem. More specifically, the number of solutions will be
• infinitely many solutions if d is inside the workspace (the set of points that the end-effector
can possibly reach given all possible joint configurations of the robotic manipulator)
• 1 solution if d is on the workspace boundary
• 0 solutions if d is outside the workspace
Since we have three unknown variables and two equations to solve for, we get infinitely many
solutions in general. One way to find all solutions is to take one of the variables as specified
(e.g., θ3 or, more generally, some combination of θ1 , θ2 , and θ3 ) and then find the other two
variables in terms of this specified variable. Then, we will get all the infinitely many solutions
as parameterized in terms of this single variable, which can then take any value in the feasible
range for that joint variable. This is equivalent to specifying one additional quantity, e.g., the
desired angular orientation. Hence, the set of all possible inverse position kinematics solutions
can be thought of as the union of the solutions for the inverse kinematics problem (with specified
desired position and orientation) for all possible desired orientations, i.e., for all possible θd in
the inverse kinematics computed below.
3
If the desired orientation θd of the end-effector is also specified, then, for this planar manipulator,
we should have
θ1 + θ 2 + θ3 = θd .
(12)
Since, we know θ1 + θ2 + θ3 , we can simplify the equations (10) and (11) as
dx − a3 cos(θd ) = a1 c1 + a2 c12
(13)
dy − a3 sin(θd ) = a1 s1 + a2 s12 .
(14)
The equations (13) and (14) can also be thought of as corresponding to a wrist center wherein
we consider the last joint to be a one degree-of-freedom wrist and then find the coordinates of
the point before this last joint as [dx − a3 cos(θd ), dy − a3 sin(θd ), 0]T . This is analogous to the
method of finding the wrist center for a spherical wrist.
Denote wx = dx − a3 cos(θd ) and wy = dy − a3 sin(θd ). Taking the squares of (13) and (14) and
adding, we get:
wx2 + wy2 = a21 + a22 + 2a1 a2 c2 .
(15)
Hence,
w 2 + w 2 − a2 − a2 x
y
1
2
θ2 = cos−1
.
(16)
2a1 a2
This gives us two possible values for θ2 . Now, with either of these possible values of θ2 , we can
solve for θ1 utilizing, for example, equation (13), which when expanded gives us an equation of
form
Ac1 + Bs1 = wx
(17)
where A = a1 + a2 c2 and B = −a2 s2 . From (14), we similarly get the equation
As1 − Bc1 = wy
(18)
Define α to be the angle Atan2(A, B) where Atan2 denotes the two-argument arc-tangent function wherein Atan2(x, y) is essentially tan−1 (y/x), but with the signs of x and y also taken into
account.
Hence, sin(α) = √A2B+B 2 and cos(α) = √A2A+B 2 . Therefore, from (17) and (18), we get
wx
cα c1 + sα s1 = √
(19)
2
A + B2
wy
cα s1 − sα c1 = √
(20)
A2 + B 2
wy
x
Hence, cos(θ1 − α) = √Aw2 +B
and sin(θ1 − α) = √A2 +B
. Therefore,
2
2
θ1 = α + Atan2(wx , wy ) = Atan2(wx , wy ) − Atan2(a1 + a2 c2 , a2 s2 )
(21)
Hence, given a solution for θ2 , we can find θ1 using (21). Once solutions for θ1 and θ2 have been
found, θ3 is found simply as θ3 = θd − θ1 − θ2 . Since we find, in general, two possible values of
θ2 from (16), we see that when the desired position and orientation are both given, the number
of solutions will be
• 2 solutions if the wrist center (wx , wy ) is inside the workspace of the two-link manipulator
formed by only considering the first two links of the given manipulator
• 1 solution if the point (wx , wy ) is on the boundary of the workspace of the two-link manipulator
• 0 solutions if the point (wx , wy ) is outside the workspace of the two-link manipulator
• infinitely many solutions if the point (wx , wy ) is the origin and this point is inside the
workspace of the two-link manipulator; in this case, we will have a1 = a2 and the solution
for θ2 to place (wx , wy ) at the origin has to be θ2 = π. θ1 can then be any angle and
therefore, we get infinitely many solutions.
3.12 We are given a three-link planar manipulator with first joint revolute, second joint prismatic, and
third joint revolute. The direct kinematics for this manipulator was written in Homework 3.5 in
4
Homework 4. For the inverse position kinematics, if only the desired position of the end-effector
is given as d = [dx , dy , 0]T , we have three unknown variables (θ1 , d2 , θ3 ) to be found from the two
equations:
dx = d2 s1 + a3 c13
(22)
dy = −d2 c1 + a3 s13 .
(23)
Hence, we will, in general, have infinitely many solutions to the inverse position kinematics
problem. More specifically, the number of solutions will be
• infinitely many solutions if d is inside the workspace (the set of points that the end-effector
can possibly reach given all possible joint configurations of the robotic manipulator)
• 1 solution if d is on the workspace boundary
• 0 solutions if d is outside the workspace
Since we have three unknown variables and two equations to solve for, we get infinitely many
solutions in general. One way to find all solutions is to take one of the variables as specified
(e.g., θ3 or, more generally, some combination of θ1 , d2 , and θ3 ) and then find the other two
variables in terms of this specified variable. Then, we will get all the infinitely many solutions
as parameterized in terms of this single variable, which can then take any value in the feasible
range for that joint variable. This is equivalent to specifying one additional quantity, e.g., the
desired angular orientation. Hence, the set of all possible inverse position kinematics solutions
can be thought of as the union of the solutions for the inverse kinematics problem (with specified
desired position and orientation) for all possible desired orientations, i.e., for all possible θd in
the inverse kinematics computed below.
If the desired orientation θd of the end-effector is also specified, then, for this planar manipulator,
we should have
θ1 + θ3 = θd .
(24)
Since, we know θ1 + θ3 , we can simplify the equations (22) and (23) as
dx − a3 cos(θd ) = d2 s1
(25)
dy − a3 sin(θd ) = −d2 c1
(26)
The equations (25) and (26) can also be thought of as corresponding to a wrist center wherein
we consider the last joint to be a one degree-of-freedom wrist and then find the coordinates of
the point before this last joint as [dx − a3 cos(θd ), dy − a3 sin(θd ), 0]T . This is analogous to the
method of finding the wrist center for a spherical wrist.
Denote wx = dx − a3 cos(θd ) and wy = dy − a3 sin(θd ). From (25) and (26), we get:
θ1 = Atan2(−wy , wx )
(27)
q
d2 = wx2 + wy2
(28)
where Atan2 denotes the two-argument arc-tangent function wherein Atan2(x, y) is essentially
tan−1 (y/x), but with the signs of x and y also taken into account. Once the solution for θ1 has
been found, θ3 is found simply as θ3 = θd − θ1 . Hence, we see that when the desired position
and orientation are both given, the number of solutions will be
• 1 solution if the wrist center (wx , wy ) is on or inside the workspace of the two-link manipulator formed by only considering the first two links of the given manipulator
• 0 solutions if the point (wx , wy ) is outside the workspace of the two-link manipulator
• infinitely many solutions if the point (wx , wy ) is the origin and this point is inside the
workspace of the two-link manipulator; in this case, we will have d2 = 0 and θ1 can be any
angle; therefore, we get infinitely many solutions.
5