Introduction to Robotics (CS223A) (Winter 2006/2007) Homework #5 solutions

Transcription

1 Introduction to Robotics (CS3A) Handout (Winter 6/7) Hoework #5 solutions. (a) Derive a forula that transfors an inertia tensor given in soe frae {C} into a new frae {A}. The frae {A} can differ fro frae {C} by both translation and rotation. You ay assue that frae {C} is located at the center of ass. Solving this proble involves using the Parallel Axis Theore to translate the inertia tensor to a frae at a different location, and a siilarity transforation to rotate it into the new frae. These operations can be done in either order, as long as we re careful that the vectors we use are expressed in the correct frae. However, it is definitely easier to do the rotation first. Assue that we have A CT, the transforation fro frae {C} coordinates to frae {A} coordinates, which contains the rotation atrix A C R and the translation vector A p C which locates the origin of frae {C} with respect to {A}. Let s first solve the proble by a rotation followed by a translation. Consider an interediate frae {C } which has the sae origin as {C}, but whose axes are parallel to frae {A}. Using a siilarity transforation (see p of Lecture Notes), we know that C I C C R C I C C R T However, since frae {C } has the sae orientation as frae {A}, we know that C C R A C R, so C I A C R C I A CR T We now have the inertia tensor expressed in the interediate frae {C }. Since {C } is parallel to {A}, we can use the Parallel Axis Theore to transfor C I to A I. To use this theore, we just need the vector A p C that locates the center of frae {C } with respect to {A}, expressed in frae {A}, which yields the forula A I C I + ( A p T C A p C )I 3 A p A C p T C where is the total ass of the object and I 3 is the 3 3 identity atrix. Since {C } and {C} have the sae origin, the vector A p C is just A p C. Substituting this value and our previous expression for C I yields: A I A C R C I A CR T + ( A p T C A p C )I 3 A p A C p T C Equivalently, we could do this proble with a translation first, and then a rotation. To do that, we can define an interediate frae {A }, which has the sae origin as {A}, but whose axes are parallel to {C}. We can get the intertia tensor in the interediate frae by using the Parallel Axis Theore. To use it, however, we need the vector A p C which locates the origin of frae {C} with respect to frae {A }, expressed in frae {A }. Using this forula with the vector expressed in frae {A} is incorrect. We can R, and then siplify: A I C I + ( A p T C A p C )I 3 A A p C p T C C I + ( C AR A p C ) T ( C AR A p C )I 3 ( C AR A p C )( C AR A p C ) T get A p C by rotating A p C with A A R C A

2 C I + A p T C( C AR T AR) C A p C I 3 C A R( A p A Cp T C) C AR T C I + A p T C A p C I 3 C A R( A p A Cp T C) C AR T Then, to get the inertia tensor in frae {A}, we can use a siilarity transforation to rotate A I: A I A A RA I A A RT A C R A I A CR T ( ) A C CR I + A p T C A p C I 3 C A R( A p A Cp T C) C AR T A C RT A CR C I A CR T + A A CR p T C A p C I 3 C A R( A p A Cp T C) C AR T A C RT A CR C I A CR T + ( A p T C A p C ) A A CRI 3C R T A C RAR( C A p A Cp T C) C AR T A CR T A I A CR C I A CR T + ( A p T C A p C )I 3 A p A Cp T C This is the sae expression that we got fro the other approach. (b) Consider, for exaple, the unifor density box shown below. It has ass kg, and diensions 6 4 : Y C {C} Y A 6 X C {A} Z C Z A X A 4 Frae {C} lies at the center of ass of the box, and the coordinate axes are ligned up with the principal axes of the box. In other words, Y C is aligned with the long axis of the box, and X C and Z C are aligned with the short axes of the box. Copute the inertia tensor of the box in frae {C}. Here, we just put nuerical values into the forula given in the hoework, to get: 4 C I 5 (c) Given the transforation atrix fro {C} to {A}: A CT

3 use your forula fro part (a) and your inertia tensor fro part (b) to copute the inertia tensor of the box in frae {A}. We apply our forula fro part (a). In this case, fro A CT, we know: A CR, p C The first part of the transforation (into the interediate frae {A }) is A I A C R C I A CR T 4 5 To copute the parallel axis transforation, we need to find the atrix p T C p C 6, p C p T C 4, (p T C p C )I 3 p C p T C We now copute the entire transforation: A I A CR C I A CR T + (p C p T C)I 3 p C p T C A I (p T C p C)I 3 p C p T C 5 5. In the rest of this proble set, we will walk through the process of finding the equations of otion for a siple anipulator fro the Lagrange forulation. Consider the RP spatial anipulator shown below. The links of this anipulator are odeled as bars of unifor density, having square cross-sections of thickness h, lengths of L and L, and total asses of and, with centers of ass shown. Assue that the joints theselves are assless. :

4 Fro the derivation on pp.3-33 of the notes, we know that the equations of otion have the for: M(q) q + C(q) + B(q) + G(q) τ where M is the ass atrix, C is the atrix of coefficients for centrifugal forces, B is the atrix of coefficients for Coriolis forces, and G is the vector of gravity forces. (a) For each link i, we have attached a frae {C i } to the center of ass (in this case, frae {} is the sae as {C }). Copute kineatics for these fraes: that is, calculate the atrices T and C T. The transforation T is just a constant offset of L / along the x axis; the other transforations are found in the regular anner: T c s L c s c L s c s L c, C T s c L s d For a two-link anipulator, the ass atrix has the for M J T v J v + J T v J v + J T ω I J ω + J T ω C I J ω where J vi is the linear Jacobian of the center of ass of link i, J ωi is the angular velocity of link i, and C i I i is the inertia tensor of link i expressed in frae {C i }. (b) Calculate J v and J v. These atrices are found directly by differentiating the last coluns of C i T : J v p C θ L s L c, J v p C θ p C d L s L c (c) Calculate J ω and C J ω. J ω ɛ z, C J ω ɛ C z ε C z (d) Calculate I and C I in ters of the asses and diensions of the links. You can use the sae forula that was given for a box of unifor density in Proble (b). Be careful which easureents you use along the axes. Using the forula fro proble, we see that the inertia tensor written at the center of ass of a unifor density rectangular solid is (s y + s z) C I (s x + s z) (s x + s y)

5 (e) where s x, s y and s z are the diensions of the solid along the x C, y C and z C axes, respectively. Plugging in the values for our links yields 6 h I (L + h ), (L + h ) I (L + (L + h ) h ) Calculate the ass atrix, M(q). To ake your algebra easier, leave the inertia tensors in sybolic for until the end, i.e. I xx I I yy I zz This just requires a bit of atrix algebra: L Jv T J v 4, Jv T J v Jω T Izz I J ω, J T C ω I J ω L Izz 6 h M Jv T J v + Jv T J v + Jω T I J ω + Jω T C I J ω 4 L + (L ) + I zz + I zz M 3 L + h + L + 6 h Now we need to calculate the centrifugal and Coriolis forces. We will derive the for directly. (f) Beginning with the equation fro p. 36 in the lecture notes, v(q, ) Ṁ M T q T M, q anipulate this equation sybolically into the for v(q, ) C(q) + B(q) where C and B are atrices in ters of the partial derivatives ijk of the ass atrix. Don t actually substitute in your answer fro part (e) into this equation yet: just leave the eleents of these atrices in ijk sybolic for. v(q, ) Ṁ ṁ ṁ ṁ ṁ T M q T M q

6 (g) v(q, ) So we have C, B Using your answer to part (e), copute the atrices C(q) and B(q) in ters of the asses, diensions, and configuration q of the anipulator. This wasn t eant to be tricky - the ass atrix is independent of the joints, so C, B The last thing that reains is to derive the gravity vector G(q). This you should be able to figure out for yourself. (h) (i) Calculate, G(q), the gravity vector in frae {}, in ters of the asses, the configuration q, and the gravity constant g (g is positive). Assue that gravity pulls things along the z direction. Be careful with your signs. In ters of a unit gravity vector g, we have G Jv T g + Jv T g In frae {}, the gravity vector is g g T, which yields G L s L c L s L c g G g g As a final step, use your answers to parts (e), (g) and (h) to write out the equations of otion as two great big equations M θ d τ τ f ( q,, q) τ f ( q,, q) + C θ θ + B θ d τ + G τ ( 3 L + h + L + ) 6 h θ τ d + g