Introduction to Homogeneous Transformations & Robot Kinematics

Transcription

1 Introduction to Homogeneous Transformations & Robot Kinematics Jennifer Ka Rowan Universit Computer Science Department. Drawing Dimensional Frames in 2 Dimensions We will be working in -D coordinates, and will label the aes,, and. Figure contains a sample -D coordinate frame. Figure. A -D coordinate frame. Figure 2. A cube with one painted side viewed from below. The dotted lines represent edges ou can not see through the painted side. The edge labelled points into the page awa from ou. Figure. A cube with one painted side viewed from above. The dotted lines represent edges ou can not see through the painted side. The edge labelled points out of the page towards ou. Copright (C) 2, Jennifer Ka. Permission is granted to make individual copies of this for educational purposes as long as this copright notice remains intact. Permission to make multiple copies of this for educational purposes will generall be granted when an request is sent to ka@rowan.edu (so I know who is using it). In such cases, I will require that it is distributed for free (or for a minimal coping fee) and this copright notice remains intact. Page of 2

2 Because we are representing -D coordinate frames with 2-D drawings, we have to agree on what these drawings mean. Clearl the ais in Figure points to the right, and the ais points up, but we have to come up with a convention for what direction the ais is pointing. Since the three aes must be perpendicular to each other, we know that the ais either points into the paper, or out of the paper. Most people instantl assume one or the other is the case. Figure 2 and Figure tr to illustrate this. When ou look at the figures, tr and view them in such a wa that the painted side is closer to ou than the clear side parallel to the painted side. For the purposes of this document, we will assume that Figure shows the interpretation we will use. In other words, if ou see aes drawn as the are in Figure, ou should assume that the ais points out of the paper towards ou. If ou actuall wanted the ais to be pointing into the paper, ou should use the illustration shown in Figure. Figure. Another -D coordinate frame. In this frame, points to the right, points up, and points awa from ou into the paper. 2. Right Handed Coordinate Sstems Figure 5. points to the left, and points out of the paper towards ou. Most of the time we are going to use right handed coordinate sstems. In a right handed coordinate sstem, if ou know the directions of two out of the three aes, ou can figure out the direction of the third. Let s suppose that ou know the directions of the and aes. For eample, suppose that points to the left, and points out of the paper, as shown in Figure 5. We want to determine the direction of the ais. To do so, take our right hand, and hold it so that our fingers point in the direction of the ais in such a wa that ou can curl our fingers towards the ais.when ou do this, our thumb will point in the direction of the ais. This process is illustrated in Figure 6. The chart in figure 7 details how to compute the direction of an ais given the directions of the other two. Page 2 of 2

3 Step : hold our right hand in such a wa that our fingers point in the direction of the ais and when ou curl our fingers, the curl towards the ais. Step 2: As ou curl our fingers from the ais towards the ais, stick our thumb in the air. This will be the direction of the ais The final -D coordinate sstem with the ais shown. Figure 6. Using the right hand rule to compute the direction of the ais. If ou know the direction of these aes. Point the fingers of our right hand in the direction of this ais. Curl ou right fingers towards the direction of this ais. Your thumb will point in the direction of this ais. & & & Figure 7. Using the right hand rule to compute the direction of an ais given the directions of the other two.. Direction of Positive Rotation Sometimes we want to talk about rotating around one of the aes of a coordinate frame b some angle. Of course, if ou are looking down an ais and want to spin it, ou need to know whether ou should spin it clockwise or counter-clockwise. We are going to use another right hand rule to determine the direction of positive rotation.. Plotting Points in Dimensions All of us have eperience in plotting points on 2-D aes. When it comes to plotting points on -D aes, things become a bit more difficult. In this section, we will discuss how to plot a number of points on the -D aes presented in Figure. Page of 2

4 (a) The original aes with a right hand determining the direction of positive rotation around the ais. (b) After rotating the original aes (a) 9 o around the ais. (c) After rotating the original aes (a) -9 o around the ais. Figure 8. The right hand rule to determine the direction of positive angles. Point our right thumb along the positive direction of the ais ou wish to rotate around. Curl our fingers. The direction that our fingers curl is the direction of positive rotation. The first step is to draw tick marks on the aes to indicate scale. For the purposes of this document, we will assume that each tick represents one unit. Figure 9 shows several different right handed coordinate sstems with tick marks added. Note that each tick mark is parallel to one of the other aes. This helps the viewer to visualie the -D effect. Figure shows the point (2,,) plotted on the different aes of Figure 9. The technique is quite straightforward if two of our aes form a plane parallel with the ground. First, draw lines to indicate the projection of the point on that plane. Then, draw a line through that point that is parallel to the remaining ais, add tick marks to it, and plot our point. For eample, in Figure (a) the and aes form the groundplane, and so we draw lines to indicate where (2,,) would be. Then, we draw a line through the point (2,,) that is parallel to the ais, add tick marks to it, and finall plot our point. Although Figure (b) looks different, the and aes still form the groundplane and so the procedure is virtuall the same. The onl difference is that the tick marks on the ais have been left out because when the are included the are difficult to distinguish from the tick marks on the vertical line that connects to the point (2,,). In Figure (c), the and aes form the groundplane. Thus, we first plot the point (,, ), then draw a line through the point (,,) parallel to the ais, add tick marks to it, and again plot our point (2,,). 5. Working With Multiple Coordinate Frames Sometimes we ma want the coordinates of a point given with respect to two or more coordinate frames. For eample, consider an airport scenario. The coordinate frame of the airport might have its origin at the base of the control tower, with the ais pointing North, the ais pointing West, and the ais pointing up. While this is a Page of 2

5 : out of page towards ou : right : up : right : out of page towards ou : down : up : right : into page awa from ou Figure 9. Several different right handed -D coordinate frames with tick marks to indicate scale (a) (b) (c) Figure. The point (2,,) plotted on different aes. useful coordinate frame for the air traffic controllers to use, a pilot ma be more interested in where objects are relative to her airplane. Thus, we might have two coordinate frames, tower coordinates and plane coordi- Page 5 of 2

6 nates as illustrated in Figure. We use subscripts to distinguish between t, t, and t (the tower coordinate frame) and p, p, and p (the plane coordinate frame). t p t p 6. Representing Points Up to this point, we have been using the traditional (,,) notation to represent points in -D. However, for the remainder of this document, we are going to use a vector notation to represent points. The point (,,) is repre t p 5 meters 5 meters Figure. The plane is on the ground preparing for takeoff. Tower coordinates are centered at the base of the air traffic control tower. points North, points West, and points up. The airplane coordinate sstem is centered at the nose of the plane. The ais points towards the top of the plane, the ais points out to the right as ou are sitting in the captain s chair, and the ais points straight out the front of the plane. When the plane is stopped on the runwa as depicted in Figure, the nose of the plane might be at location (5, -5, ) in tower coordinates, but it is at the origin (location (,, )) in plane coordinates. Similarl, the base of the tower is at location (,, ) in tower coordinates, but at location (, 5, 5) in plane coordinates. Note that the location of the nose of the plane is fied with respect to the plane, but not with respect to the tower. When the plane begins to take-off as depicted in Figure 2, its nose is still at location (,,) in plane coordinates, but it is at location (, 5, 5) in tower coordinates. 6. Homogeneous Transformations Page 6 of 2

7 p t p p 5 2 t t Figure 2. When the plane takes-off, its nose is still at location (,,) in plane coordinates, but it is at a different location in tower coordinates. To determine where it is in tower coordinates, the tower s and aes have been etended, and the nose of the plane has been plotted in the manner of Figure. Thus we see that the nose of the plane is at location (, 5, ) in tower coordinates sented as the vector. The is a weighting factor. So the vectors 2 and 257 all represent that same point (,, ). Most of the time we will simpl use a weighting factor of. Consider the more concrete eample depicted in Figure 2. The plane is at location (, 5, ) in tower coordinates. We will normall represent that 6 75 location as 5, however it can also be represented as, 5, 7.5, and an infinite number of other vectors. In order to save space in this document, we will often write the point as T (the transpose of ). Page 7 of 2

8 6.2 The Use of Multiple Coordinate Frames in Robotics It is ver common in robotics to use two or more coordinate frames to solve a problem. Suppose the airplane in Figure were automaticall controlled. It would be ver useful to keep track of some things in tower coordinates. For eample, the altitude of the plane is simpl the coordinate of its location in tower coordinates. It also would be useful to keep track of other things in airplane coordinates. For eample, the direction the plane should head to in order to avoid a mountain. Indeed, for man mobile robot applications, it is desirable to know the locations of objects in both world coordinates and robot coordinates. Multiple coordinate frames are also useful in traditional static robots. For eample, consider the simple robot arm depicted in Figure. If we want to have the gripper pick up a widget off of a table, then we need to figure out the widget s location. Perhaps we have a camera that we use to initiall determine the location of the widget (in camera coordinates). We might need to transform that location into world coordinates to evaluate if it is accessible to the robot at all, and to gripper coordinates to determine when we should close the jaws of the gripper. g c c c w camera j j g g gripper w w j joint widget Figure. A ver simple robot arm with one joint and one gripper. The world, camera, joint, and gripper coordinate frames are indicated. 6. Transforming Points Between Coordinate Frames Suppose that ou know the location of a point in one coordinate frame (for eample, airplane coordinates) and ou want to know its location in another frame (for eample, tower coordinates). How do ou do it? We will start with a ver simple case, and then move into more comple eamples. Let s begin b considering the two coordinate sstems in Figure, world coordinates and robot coordinates. Notice that the onl difference between the two coordinate frames is that the robot frame has been translated b units along the ais from the world coordinate frame. Figure 5 is a table of some sample points in world coordinates, and their corresponding values in robot coordinates. For the moment, ignore the third column of Figure 5, and just look at the first two columns. Notice that Page 8 of 2

9 an point abc T in world coordinates is the same as the point a ( b ) c T in robot coordinates. Similarl, an point def T in robot coordinates is the same as the point d ( e + ) f T in world coordinates. units w r w r w r Figure. Two coordinate frames, world (w) and robot (r). The origin of the robot frame is located at the point (,,) in world coordinates. Using our new notation, we can represent this as T. The two aes are parallel, as are the two and the two aes. Note that the origin of the world coordinate aes is at T in robot coordinates. We have seen that one wa to convert world coordinates to robot coordinates for the sstem in Figure is to subtract from the value in world coordinates. Surprisingl, another wa to convert points from the world coordinate frame of Figure to the robot coordinate frame of Figure is to pre-multipl the point b the matri. The third column of Figure 5 does eactl this and results in the same answer! It should be clear that there is nothing magic about the number - in our matri other than the fact that we moved units along the ais between the two frames. Indeed, if we had moved 6 units, then pre-multipling b the matri 6 would convert points from world coordinates to robot coordinates. Page 9 of 2

10 Location of a Point in World Coordinates of Figure Location of the Same Point in Robot Coordinates of Figure Pre-multipling the point in world coordinates b a b c a b c a b c a b c Figure 5. Converting between world and robot coordinates as depicted in Figure. Page of 2

11 Similarl, there s nothing magic about the fact that we did this move along the ais. We could come up with similar matrices for changes in frames that occurred along the or ais. Or indeed an combination of moves along all three matrices. Suppose that to get from the coordinate frame p to the coordinate frame q ou need to move a units along p s ais, b units along p s ais, and c units along p s ais. Then to take a point from q coordinates to p coordinates, ou need to premultipl it b the matri a b c For eample, consider the two coordinate frames of Figure 6. To transform the robot coordinate frame into the world coordinate frame, ou need to translate 5 units along the robot s ais, - units along the robot s ais, and - unit along the robot s ais. Thus to take a point in world coordinates and transform that into a point in robot coordinates, ou need to premultipl that point b 5 the matri. Some eamples of this can be found in Figure 7. c m 5 units c units c m unit m Figure 6. Two coordinate frames that differ b onl a translation. To get from car coordinates (c) to mountain coordinates (m) ou must translate 5 units along the car s ais, - units along the car s ais, and - unit along the car s ais. We call the matri that converts a point from j coordinates to k coordinates the homogeneous transformation from j coordinates to k coordinates, and we abbreviate this as T k. Figure 8 summaries how to compute the j matri that converts between two frames that onl differ b a translation. Page of 2

12 Location of a point in Figure 6 s mountain coordinates Location of the same point in Figure 6 s car coordinates Pre-multipling the point in mountain coordinates b a b c a + 5 b c 5 Figure 7. Converting points from the mountain coordinate frame of Figure 6 to the car coordinate frame of Figure 6. a b c a + 5 b c Page 2 of 2

13 If the coordinate frames j and k onl differ b a translation and to get from k coordinates to j coordinates ou translate (a, b, c) along k s,, and aes, then T k, the matri that takes a j point in j coordinates to a point in k coordinates is a b c We can summarie this with the equation Trans (a,b,c) a b c Figure 8. Converting points between coordinate frames that onl differ b a translation. 6. Coordinate Frames that Differ b a Rotation Around One Ais Consider the two frames depicted in Figure 9. To transform the k coordinate frame into the j coordinate frame, we perform a rotation about k s ais b -9 o. B looking at Figure 9 (a), we can see that k j, k j, and j -* k. Let s look at a few eamples. The origin of the j ais in j coordinates: the point T is the same as the origin of the j ais in k coordinates T. The point abc T in j coordinates is located at b a c T in k coordinates. Again, there is a matri that we can premultipl points in j coordinates b to transform them into points in k coordinates. If to transform the k coordinate frame into the j coordinate frame ou rotate about the ais b θ, then ou can pre-multipl a point in j coordinates b the matri cosθ sinθ sinθ cosθ. Let s check this on the two eamples that we ve done alread. Page of 2

14 j k k j k k k j j j j k (a) the j and k coordinate frames overlaid on each other. (b) the j coordinate frame, shown on its own for clarit. (c) the k coordinate frame, shown on its own for clarit. Figure 9. Two coordinate frames, j and k, that differ onl b a rotation about the ais. To transform the k coordinate frame into the j coordinate frame, we rotate b -9 o about k s ais. In our eample, θ is 9 o. cos -9 o. sin -9 o -. So our matri becomes. When we compute our matri * T, we get T as epected. When we multipl our matri * abc T we get b a c as epected. At this point we know that we can design a matri to convert points between two coordinate frames that onl differ b a translation, or b a rotation about the ais. It should not surprise ou to learn that ou can also design matrices to convert points between two coordinate frames that onl differ b a rotation about the or ais too. 6.5 Putting it All Together So far we have learned how to create the matri that will compute the coordinates of a point in one coordinate frame given the coordinates of that point in another coordinate frame subject to the following condition: the two frames ma onl differ b a translation (along the aes), or b a rotation about one ais. It is indeed possible to convert points between two coordinate frames that differ, perhaps b two translations, or b a rotation then a translation and then another rotation. The ke to understanding how to do this is to understand that there are two was to view an sequence of translations and rotations. The first wa we will call moving coordinate sstems. In moving coordinate sstems, Page of 2

15 If to convert the k coordinate frame into the j coordinate frame ou have to: Then to convert a point in j coordinates into a point in k coordinates, premultipl that point b: The nickname for this transformation is: Translate along k s ais b a, along k s ais b b, along k s ais b c a b c Trans (a, b, c) Rotate about k s ais b θ cosθ sinθ Rot ( θ ) sinθ cosθ cosθ sinθ Rotate about k s ais b θ Rot ( θ ) sinθ cosθ cosθ sinθ Rotate about k s ais b θ sinθ cosθ Rot ( θ ) Figure 2. Summar of transformation matrices each step happens relative to the steps that have come before it. For eample, Figure 2 shows the world and gripper coordinate frames for a particular robotic sstem. Note that not onl is the gripper coordinate frame translated from the world coordinate frame, but there also must be some sort of rotation that caused the gripper s ais to point up instead of out of the page towards ou as the world coordinates do. In the moving aes approach, we sa that to get from world coordinates to gripper coordinates, ou need to do the following sequence of moves: Rotate about w b -9 degrees. Call this new frame intermediate frame, and we ll call its aes,, and. Net rotate about the new b -9 degrees. Call this new frame intermediate Page 5 of 2

16 frame 2, and we ll call its aes 2, 2, and 2. Finall, translate b (,,5) relative to intermediate frame 2. This results in the gripper coordinate frame. The alternative approach is the fied aes approach. In this technique, all of our moves are relative to the original world coordinate frame. In the fied aes approach, the picture is still as depicted in Figure 2, but this time the sequence of steps is: Rot w (-9) then Rot w (-9), then Trans(,5,) relative to world coordinates. Whether ou choose to use the moving aes approach or the fied aes approach, our final matri will be the same. However the wa ou compute it will differ. 6.6 Computing the Transformation Matri Using Moving Aes 5 units w g w g w g Figure 2. World and gripper coordinate frames for some robot. There are several was we can think about transforming between the two coordinate frames. Recall that the moving aes approach is as follows: to get from world coordinates to gripper coordinates, ou need to do the following sequence of moves: Rotate about w b -9 degrees. Call this new frame intermediate frame, and we ll call its aes,, and. Net rotate about the new b -9 degrees. Call this new frame intermediate frame 2, and we ll call its aes 2, 2, and 2. Finall, translate b (,,5) relative to intermediate frame 2. This results in the gripper coordinate frame. When we use moving aes we list the moves that we did from left to right, compute the individual matrices for each part, and then multipl them together. For eample, in this situation, our sequence of equations is: Rot (-9) * Rot (-9) * Trans(,,5) Page 6 of 2

17 The matrices for this product are as follows: 5 5 The resulting matri will transform a point from gripper coordinates to world coordinates. For eample, consider the point (, 2, ) in gripper coordinates. We compute that in world coordinates b premultipling b our new matri as follows: Which ou should be able to verif is correct b looking at Figure Using Fied Aes Recall that the fied aes approach does everthing relative to the original world coordinate frame. The sequence of transformations in this case was as follows: Rot w (-9) then Rot w (-9), then Trans(,5,) relative to world coordinates. When we using fied aes computation, (i.e. each new rotation is relative to the original gripper coordinate frame), we write the equations from right to left. Trans (,5,) * Rot (-9) * Rot (-9) The matrices for this product are as follows: 5 5 Check it out! this is the same equation we got when we did the computation using moving aes! (Phew!) Page 7 of 2

18 7. Forward Kinematics One task that we often wish to do is the following: given the joint angles of a robot arm, compute the transformation between world and gripper coordinates. Of course, at this point given an fied joint angles, we alread have the tools to compute this transformation. However, what we reall would like to do is to come up with a transformation matri that is a function of the joint angles of the robot. 7. A First Eample Consider the robot arm given in Figure 22. This arm has two links (of length L & L2) and one joint which can rotate about its Z ais. There are coordinate frames, world coordinates, joint coordinates, and gripper coordinates. Figure 2 contains a picture of the same arm, but this time, the joint has been rotated b degrees (about its ais - the onl ais about which it can rotate). Now look at the * in both figures. It should be clear that the world coordinates of the * do not change between Figure 22 and Figure 2, but the location of * in link coordinates does change, as does its location in gripper coordinates. L L2 w w j * g j g w j g Figure 22. A simple robot g g w j * j g w w j Figure 2. The arm from Figure 22 with the joint rotated b degrees. Page 8 of 2

19 There is one point in this image whose location does not move in an frame as the joint moves. The point located at the ver center of the joint (i.e. with Joint coordinates (,,)) is alwas at world coordinates location (L,, ), and alwas at (-L2,, ) in gripper coordinates, no matter how ou move the joint. Suppose that we want to be able to convert between gripper coordinates and world coordinates, as a function of the angle of the joint. Let s begin b just looking at Figure 22 again and considering the case where the joint is not rotated at all. In this case, to convert a point from gripper coordinates to world coordinates all we do is add L+L2 to whatever the value is. e.g., suppose the * is located at (,, ) in gripper coordinates. Then it s obviousl located at (+L+L2,, ) in world coordinates. But wait! To convert from gripper to world coordinates isn t as simple as that. Because if the joint is rotated as shown in Figure 2, then it s no longer a simple addition of L+L2. What we want is a wa to easil convert between gripper and world coordinates as a function of joint angle. Let s call the angle that the joint is rotated ψ. We want one matri that has the variable ψ built into it, and if we plug in the value for ψ, that matri will be our matri that we multipl a point in gripper coordinates b to get the point in world coordinates. Now let s do the math. To move our frame from world coordinates to gripper coordinates, we need to translate a distance of L along the ais, and then rotate b whatever angle our joint is twisted to (e.g. degrees in Figure 22 or degrees in Figure 2). We re going to do it as relative motion, so we end up multipling the matrices Trans(L,,) b Rot (ψ) from left to right. So we have the following : L sinψ sinψ sinψ sinψ L Figure 2. Moving a frame from world coordinates to joint coordinates. But of course, this onl moves us from world coordinates to joint coordinates. We wanted to move our frame from world coordinates to gripper coordinates. Once we have a frame in joint coordinates, moving it to gripper coordinates is simpl a translation of [L2,, ]. So we need to multipl the two matrices above b Trans[L2,,]. L sinψ sinψ L2 sinψ L2 + L sinψ L2sinΨ Figure 25. Moving a frame from world coordinates to gripper coordinates. Page 9 of 2

20 Now, our final equation looks prett mess, but it s not too bad. Suppose that ψ is (i.e. we ve got Figure 22). Then we have: sinψ L2 + L sinψ L2sinΨ L2+L Figure 26. Moving a frame from world coordinates to gripper coordinates when the joint angle is ero. Remember that we computed how to move a frame from world coordinates to gripper coordinates. Which turns points in gripper coordinates into points in world coordinates. Wow! Figure 26 is actuall eactl what we predicted it would be. Look at it. It s the matri Trans(L+L2,, ). 7.2 A Second Eample Just for fun, let s compute the transformation when the joint is rotated b 9 degrees, so the gripper is pointing straight up in the air. Well, we plug in 9 degrees for ψ and we get: sinψ sinψ L2 + L L2sinΨ L L2 Figure 27. Moving a frame from world coordinates to gripper coordinates when the joint angle is 9 degrees. Does this make sense? Think about it. Suppose that ou have a point that is located right at the origin of the gripper. So in gripper coordinates, it s location is (,,). Where is that in world coordinates? Let s multipl: L L2 L L2 Figure 28. Transforming a point from gripper coordinates to world coordinates when the joint angle is 9 degrees. He, it works! Page 2 of 2

21 7. Some Tips on Using Mathematica Clearl it s a major pain to do anthing with more than one or two manipulations b hand. Mathematica can reall help. Here are a couple of hints on how I computed the information for this document. I m assuming some familiarit with the ver basics of mathematica. For starters, I wrote mathematica functions to represent each of the rotation and translation matrices. Just in case ou haven t seen a mathematica function before, here s how ou write a function that takes to variables, a and b, and returns the mean (average) of a and b (note: the underscore defines what s a variable. Don t use underscores for anthing else or ou ll have problems with our code: avg[a_, b_] : ((a+b)/2) Figure 29. A simple mathematica function to compute the mean of two variables Matrices are represented as lists of lists. So, here is m function for Rot: Rot[theta_] : ( ) {{,,, }, {, Cos[theta], -*Sin[theta], }, {, Sin[theta], Cos[theta], }, {,,, }} Remember that Mathematica uses radians. So to Rot b 9 degrees I sa Rot[Pi/2] 8. References Much of this tutorial is derived from Essential Kinematics for Autonomous Vehicles b Alono Kell, Carnegie Mellon Universit Robotics Institute technical report number CMU-RI-TR-9-, Ma 99, available on the web at Page 2 of 2