Recapitulation: Forces and Torques for static manipulators

advertisement
Recapitulation: Forces and Torques for static manipulators
For propagation of forces and torques in a non-moving manipulator, the following equations hold:
i
f i = ii+1 R · i+1 f i+1
i
ni = ii+1 R · i+1 ni+1 + i P i+1 × i f i
The force that affects a link is denoted by f (a three-dimensional vector), and the torque on that
link is denoted by n (also three-dimensional). Note that some parts of the forces and torques apply
directly to the corresponding joint, and some parts are absorbed by the mechanics of the robot. The
relation between these quantities is:
 
0
τi = i nTi i Z i = i nTi 0
1
 
0
τi = i f Ti i Z i = i f Ti 0
1
The first equation is used for rotational joints, the second equation for prismatic joints. The quantities
τi thus specify the amount of torque resp. force that is affecting the joint, and thus the amount of
torque resp. force that the robot should counteract in order to remain static. The joint torques/forces
τi are 1-dimensional quantities. The Jacobian plays a very important role here as well: It relates
joint torques/forces τi to endeffector forces and torques f, n:
 
τ1
A  τ2 
f
 
A TA
A T
 ..  = τ = J F = J
A
n
.
τn
Where F is a 6-dimensional Vector of force and torque containing 3D force and 3D torque vectors
stacked above each other. It is important that the force-torque vector has the same frame of reference
as the Jacobian.
Rotational velocities and the Jacobian
In the previous problem sheet, entries of the Jacobian concerning rotations have been computed based
on simplified position descriptions. We have not explained yet how the Jacobian can be computed
efficiently in the general case where arbitrary rotations are possible.
Let p : Rn → R3 be a function that computes the coordinates of the origin of the end effector with
respect to system {0}, then the full Jacobian looks like this:

 ∂p
∂p1
∂p1
1
.
.
.
∂x
∂x
∂x
 ∂p21 ∂p22 . . . ∂pn2 

0
∂x2
∂xn 
1
J =  ∂x
∂p3
∂p3
∂p3 

 ∂x1 ∂x2 . . . ∂x
n
0
0
0
Ẑ1 Ẑ2 . . . Ẑn
We are familiar with the first three rows of this Jacobian, but the final row is something new. First
of all, we need to explain the meaning of j Ẑi . This denotes the z-axis of system i, expressed relative
Figure 1: 3R-Robot
to frame {j}. Thus, it is a three-dimensional unit vector, and we have i Ẑi = (0, 0, 1)T . So we see
that the last row in the above matrix really stands for three rows. An example: For the well-known
planar 3R-manipulator from the previous exercises, the last three rows of the complete Jacobian
would look like this:


0
0
0
J = 0 Ẑ1 0 Ẑ2 0 Ẑ3 = 0 0 0
1 1 1
This is obvious, since all joint axes are parallel and represent rotations around 0 Ẑ0 , and it also
corresponds to the observation that joint rates simply add up. Now what if the third axis were
rotated about 90◦ instead of being parallel to the second axis? The corresponding robot is shown in
Figure 1. According to above rules, the lower part of the Jacobian would look like this:


0
0
s
12
J = 0 Ẑ1 0 Ẑ2 0 Ẑ3 = 0 0 −c12 
1 1
0
This observation often simplifies the computation of the Jacobian: All you have to do is to determine
the direction of the z-axes of the coordinate systems. Note however, that a prismatic joint will never
generate a rotational velocity, thus the entry in the column corresponding to the rotational velocity
of a prismatic joint is set to (0, 0, 0)T instead of 0 Ẑi .
If determining the directions of the joint axes becomes too complicated, you can still apply the
formulas for computing rotational velocities i ωi and derive the Jacobian from that. If you do that,
always keep in mind that you need to be in the right frame of reference, i.e., if the position description
is in frame 0, you need to transform n ωn to frame 0 as well if you want to compute 0 J.
The mathematical justification for above rule is as follows: For the computation of 1 ω1 , 2 ω2 , . . .
there is a recursive formula
i+1
i
i+1
ω i+1 = i+1
Ẑ i+1 .
i R · ω i + Θ̇i+1 ·
If you apply this formula generally to n ωn , a certain pattern becomes visible:
n
ω n = nn−1 R · n−1 ω n−1 + Θ̇n · n Ẑ n
n−2
ω n−2 + Θ̇n−1 · n−1 Ẑ n−1 ) + Θ̇n · n Ẑ n
= nn−1 R · (n−1
n−2 R ·
n−3
n−2
ω n−3 + Θ̇n−2 · n−2 Ẑ n−2 ) + Θ̇n−1 · n−1 Ẑ n−1 ) + Θ̇n · n Ẑ n
= nn−1 R · (n−1
n−2 R · (n−3 R ·
 
 
 
 
0
0
0
0
= n1 R · Θ̇1 · 0 + n2 R · Θ̇2 · 0 + n3 R · Θ̇3 · 0 + . . . + nn R · Θ̇n · 0
1
1
1
1
We are only interested in 0 ωn , thus we further compute:
0
ωn = 0n Rn ωn
 
 
 
 
0
0
0
0
= 0n Rn1 R · Θ̇1 · 0 + 0n Rn2 R · Θ̇2 · 0 + 0n Rn3 R · Θ̇3 · 0 + . . . + 0n Rnn R · Θ̇n · 0
1
1
1
1
 
 
 
 
0
0
0
0
0
0
0
0







= 1 R · Θ̇1 · 0 + 2 R · Θ̇2 · 0 + 3 R · Θ̇3 · 0 + . . . + n R · Θ̇n · 0
1
1
1
1
= Θ̇1 0 Ẑ1 + Θ̇2 0 Ẑ2 + Θ̇3 0 Ẑ3 + . . . + Θ̇n 0 Ẑn
Collecting the factors of Θ̇1 , Θ̇2 , Θ̇3 , . . ., we see that the entries correspond to 0 Ẑ1 , 0 Ẑ2 and so on,
and we see that the scheme for computation of the Jacobian is correct!
Solution 1
a)
The Jacobian relates joint torques to external forces as follows:
T
~τ = 4 J 4 f~
Here, we have 4 joint parameters, and the formula amounts to
 
τ1
τ2 
  = (4 J T )4 f~ = (0 J T )0 f~.
τ3 
τ4
To determine the Jacobian (according to the recipe in the recap.), we need to compute 0 P 4ORG . The
transformation matrices are:




cos Θ1 − sin Θ1 0 0
cos Θ2 − sin Θ2 0 1
 sin Θ1 cos Θ1 0 0 1


0

 , 2 T =  sin Θ2 cos Θ2 0 0
1T = 
 0
0
1 0
0
0
1 0
0
0
0 1
0
0
0 1
√ 



cos Θ3 − sin Θ3
0
0
cos Θ4 − sin Θ4 0
2
cos
sin
√Θ3
√Θ3
√1




−
−1
2
2
2
 , 34 T =  sin Θ4 cos Θ4 0 0 
 Θ2
cos
Θ
1
3 T =  sin
3
3


√
√
√
1
0
0
1 0 
2
2
2
0
0
0 1
0
0
0
1
Based on that we can compute 01 T12 T23 T34 T · (0, 0, 0, 1)T step by step, which results in:
√


− sin (Θ2 + Θ1 ) sin Θ3 +√ 2 cos (Θ2 + Θ1 ) cos Θ3 + sin (Θ2 + Θ1 ) + cos Θ1
 cos (Θ2 + Θ1 ) sin Θ3 + 2 sin (Θ2 + Θ1 ) cos Θ3 − cos (Θ2 + Θ1 ) + sin Θ1 

p(Θ) = 


sin Θ3 + 1
1
The entries of the Jacobian, evaluated for Θ = (0, 90◦ , −90◦ ), are then:
√
∂p1
= − 2s12 c3 − c12 s3 + c12 − s1 = −0 − 0 + 0 − 0 = 0
∂Θ1
√
∂p1
= − 2s12 c3 − c12 s3 + c12 = −0 − 0 + 0 = 0
∂Θ2
√
∂p1
= − 2c12 s3 − s12 c3 = −0 − 0 = 0
∂Θ3
√
∂p2
= 2c12 c3 − s12 s3 + s12 + c1 = 0 − (−1) + 1 + 1 = 3
∂Θ1
√
∂p2
= 2c12 c3 − s12 s3 + s12 = 0 + 1 + 1 = 2
∂Θ2
√
√
√
∂p2
= − 2s12 s3 + c12 c3 = 2 + 0 = 2
∂Θ3
∂p3
=0
∂Θ1
∂p3
=0
∂Θ2
∂p3
= c3 = 0
∂Θ3
Thus we have computed the first three rows of the Jacobian. The lower 3 rows can be determined
according to above considerations as 0 Ẑ 1 , 0 Ẑ 2 , 0 Ẑ 3 , 0 Ẑ 4 : (0, 0, 1)T , (0, 0, 1)T , ( √12 , 0, √12 )T , ( √12 , 0, √12 )T .
All in all, the transposed Jacobian looks like this:


0 3 0 0 0 1
0 2 0 0 0 1 
√

JT = 
0
2 0 √12 0 √12 
0 0 0 √12 0 √12
Since the force-torque-vector is specified relative to system 4, we need to transform it to system
0. This is done by multiplying with the corresponding rotation matrix:
 
0
6
0
0
 
0
R
0
R
0
3×3
3×3
0
4
 
f= 4
f= 4
0
0

03×3 4 R
03×3 4 R 
7
0
8
The rotation matrix 04 R in the current configuration Θ = (0, 90, −90, 0)T is

 √2


0√ 1
0√
0
0 −1 0
2
0 1 2 3
2
2




0√ 1
1 0 0
− √2 0 −√ 2 I =
1 R2 R3 R4 R = I
2
2
0 0 1
−2 0
− 22 0
2
√
2
2

0 .
√
2
2
Since we already have determined the direction of the z-axis of system {4}, we can also derive the
rotation matrix 04 R by figuring out 0 X̂4 , 0 Ŷ4 in addition to 0 Ẑ4 :
 
 √2 
0
2
0
X̂4 =  0√  , 0 Ŷ4 = 1 , 0 Ẑ4 =  √0 
2
0
− 2

√
2
2

2
2
The vectors are the columns of the rotation matrix.
All in all, we obtain:
 
0
6
 
0
0

f =
 √15 
2
 
0
√1
2
Finally, we can apply the formula for computing joint moments:

0


1 6
18.707
 
1   0  12.707


 15  = 
 16.485
√1   √
2
 2
0
√1
8
2

0 3
0 2
√
τ =
0
2
0 0

0
0
0
0
0
0
√1
2
√1
2
0
0
0
0

√1
2
b)
Now we want to compute the forces and torques at the tip of a screwdriver that is attached to the
robot. If we denote the system of the screw driver’s tip with {5}, we have:
4
f4 = 45 R5 f5 ⇒ 5 f5 = 4 f4
       
7
0
0
61
4
4 5
4
4
5
4
4
4







0
0
6
n4 = 5 R n5 + 5 P × f4 ⇒ n5 = n4 − 5 P × f4 =
−
×
= 0
8
9
0
8
The force that the robot causes in direction of the screw driver (along 4 Ẑ4 = 5 Ẑ5 ) is equal to the
third component of 5 f5 , because:
   T  
0
0
0
4 T  



6 = 0
Ẑ4 · 6 = 0
0
1
0
Thus, the robot does not apply any force at all in that direction. Furthermore, along the same axis,
a torque of
 
61
4 T  
Ẑ4 · 0 = 8
8
is caused by the robot.
Figure 2: Choice of coordinate systems
Solution 2
Let’s start by computing the Jacobian based on velocities. The formulas needed for the computation
have been summarized in solution 2. For application of these formulas, we need to determine the
rotation matrices between the systems. To determine these rotation matrices, we first need to
apply the DH convention to determine coordinate systems and DH parameters. Figure 2 shows the
coordinate systems. The robot is shown in configuration Θ1 = Θ2 = Θ3 = 0, and the DH parameters
are now:
i ai−1 αi−1 di Θi
1
0
0◦ 0 Θ1
2
l1
90◦ 0 Θ2
3
l2
0◦ 0 Θ3
(4) l3
0◦ 0 0◦
With these specifications, the rotation matrices are determined as:

c1 −s1

0
s1 c1
1T = 
0
0
0
0

0
0
1
0
c3 −s3

2
s3 c3
3T = 
0
0
0
0

0
0

0
1
0
0
1
0


c2 −s2 0 l1
0
0 −1 0 
1


2T = 
s2 c2
0 0
0
0
0 1

l2
0

0
1

1

3
0
4T = 
0
0
0
1
0
0
0
0
1
0

l3
0

0
1
Note the additional transformation 34 T that is only a translation along 4 x to the endeffector system.
The rotation matrices i+1
i R are equal to the transpose of the 3 × 3 rotation part of the transformation
matrices. The velocities compute as follows:
   
0
0
1
1
~



ω1 = 0 R · 0 + Θ̇1 · 0 = 0 
1
Θ̇1
0
1
1
v1 = 0 R · (~0 + ~0 × P1 ) = 0

   

0
0
s2 Θ̇1
2
ω2 = 21 R ·  0  +  0  = c2 Θ̇1 
Θ̇1
Θ̇2
Θ̇2

   

 

0
0
0
l1
2
v2 = 21 R · ~0 +  0  ×  0  = 21 R · l1 Θ̇1  =  0 
0
Θ̇1
0
−l1 Θ̇1

   

s2 Θ̇1
0
s2 Θ̇1 c3 + c2 Θ̇1 s3
3
ω3 = 32 R · c2 Θ̇1  +  0  = −s2 Θ̇1 s3 + c2 Θ̇1 c3 
Θ̇3
Θ̇2
Θ̇2 + Θ̇3

 

 

  
0
s3 Θ̇2 l2
0
s2 Θ̇1
l2
3

=
Θ̇2 l2
v3 = 32 R ·  0  + c2 Θ̇1  ×  0  = 32 R 
c3 Θ̇2 l2
0
−Θ̇1 l1
−l2 c2 Θ̇1 − Θ̇1 l1
−l2 c2 Θ̇1 − Θ̇1 l1
Θ̇2
4
ω4 = 3 ω3

  


0
l3
4

(Θ̇2 + Θ̇3 )l3
v4 = I 3 v3 + 3 ω3 ×  0  = 3 v3 + 
0
−l3 (c2 Θ̇1 c3 − s2 Θ̇1 s3 )

 

s3 Θ̇2 l2
Θ̇2 (l2 s3 )
=

=
c3 Θ̇2 l2 + Θ̇2 l3 + Θ̇3 l3
Θ̇2 (l2 c3 + l3 ) + l3 Θ̇3
l3 s2 Θ̇1 s3 − l3 c2 Θ̇1 c3 − l2 c2 Θ̇1 − Θ̇1 l1
Θ̇1 (l3 (s2 s3 − c2 c3 ) − l2 c2 − l1 )
The final values 4 v4 and 4 ω4 can be simplified using the trigonometric identities (s2 s3 − c2 c3 = −c23
. . . ), and we can derive the complete Jacobian:


0
l2 s3
0

0
l2 c3 + l3 l3 


−l3 c23 − l2 c2 − l1
0
0
4


J =

s
0
0
23



c23
0
0
0
1
1
Thus, we have determined the Jacobian based solely on velocities. For the following considerations,
the computations will not be as exhaustive, we will more focus on the basic ideas.
For the force/torque-relations we have:
τ = AJ T 4F
We denote the components of 4 F as follows:


F1
 F2 
 
 F3 

F =
N1 
 
N2 
N3
Note that:
τi = i ni (0, 0, 1)T
This is true because our robot has only rotational joints. Applying the formulas for computing forces
and torques yields:
 
 
F1
N1
4
f4 = F2  4 n4 = N2 
F3
N3
 


F1
N1
3
f3 = F2  3 n3 = N2 − F3 l3 
F3
N3 + F2 l3




F1 cos Θ3 − F2 sin Θ3
N1 cos Θ3 − (N2 − F3 l3 ) sin Θ3
2
f2 = F1 sin Θ3 + F2 cos Θ3  2 n2 = N1 sin Θ3 + (N2 − F3 l3 ) cos Θ3 − l2 F3 
F3
l2 (F1 sin Θ3 + F2 cos Θ3 ) + N3 + F2 l3

F1 cos (Θ3 + Θ2 ) − F2 sin (Θ3 + Θ2 )
1

−F3
f1 = 
F1 sin (Θ3 + Θ2 ) + F2 cos (Θ3 + Θ2 )


(F3 l3 − N2 ) sin (Θ3 + Θ2 ) + N1 cos (Θ3 + Θ2 ) + l2 F3 sin Θ2
1
n1 = −F1 l1 sin (Θ3 + Θ2 ) − l1 F2 cos (Θ3 + Θ2 ) − F1 l2 sin Θ3 − F2 l2 cos Θ3 − N3 − F2 l3 
N1 sin (Θ3 + Θ2 ) + (N2 − F3 l3 ) cos (Θ3 + Θ2 ) − l2 F3 cos Θ2 − l1 F3

We are interested in the values of τ1 , τ2 , τ3 which are going to allow us to deduce the entries of the
Jacobian. As stated above, they are the third components of i ni . Note that this means that we need
only compute the third component of 1 n1 , which simplifies the computation. All in all, we have now
computed the following values for τ :
τ1 = N1 sin (Θ3 + Θ2 ) + (N2 − F3 l3 ) cos (Θ3 + Θ2 ) − l2 F3 cos Θ2 − l1 F3
τ2 = l2 (F1 sin Θ3 + F2 cos Θ3 ) + N3 + F2 l3
τ3 = N3 + F2 l3
Now we collect the coefficients of F1 , F2 , F3 , N1 , N2 , N3 , which allows us to determine the shape of
the transposed Jacobian:
 
F1
 F2  

 
N
sin
(Θ
+
Θ
)
+
F
(−l
cos
(Θ
+
Θ
)
−
l
cos
Θ
−
l
)
+
N
cos
(Θ
+
Θ
)
1
3
2
3
3
3
2
2
2
1
2
3
2
 
4 T  F3 

F1 l2 sin Θ3 + F2 (l2 cos Θ3 + l3 ) + N3
J  =
N
1
 
N3 + F2 l3
N2 
N3


0
0
−l3 c23 − l2 c2 − l1 s23 c23 0
0
0
0 1
⇒ 4 J T = l2 s3 l2 c3 + l3
0
l3
0
0
0 1
The geometric method for computing the Jacobian would be the differentiation of the cartesian
coordinates with respect to Θ1 , Θ2 , Θ3 for the upper 3 rows and computing the lower 3 rows as
0
Z1 , 0 Z2 , 0 Z3 , as shown in Solution 1. We can thus determine 0 J:


∂p1
1
 ∂Θ
∂p2

0
1
J =  ∂Θ
∂p3
 ∂Θ
1
0
Z1
∂p1
∂Θ2
∂p2
∂Θ2
∂p3
∂Θ2
0
Z2
∂p1
∂Θ3
∂p2 
∂Θ3 
∂p3 

∂Θ3
0
Z3
The position of the origin of system 4 is computed as:
0 1 2 3
T
1 T2 T3 T4 T(0, 0, 0, 1)
The vectors 0 Z1 , 0 Z2 , 0 Z3 are:
0
Z1 = (0, 0, 1)T
0
Z2 = 01 R(0, 1, 0)T = (−s1 , c1 , 0)
0
Z3 = 0 Z2 = (−s1 , c1 , 0)
Then we can use the following relation:
0
4
03×3 4
03×3 0
0
4
4R
0R
J=
J⇔ J=
J
03×3 04 R
03×3 40 R
This relation has been established by multiplying both sides with the inverse of the compositional
rotation matrices. The computation of the explicit value is pure computational work and will yield
the same result as the other calculations before.
Download