2D Kinematics Consier a robotic arm. We can sen it commans like, move that joint so it bens at an angle θ. Once we ve set each joint, that s all well an goo. More interesting, though, is the question of once we set these angles, where is the en of the robotic han? This is kinematics. We ll also look at the problem of inverse kinematics, where we want the en of the han to be at a certain point, an so nee to figure out what angles to set the joints to. Before we move on to the full arm, with all of its possible egrees of freeom, it is easier to consier a two-imensional arm like the following, where our goal is to fin at which point in Cartesian XY plane the en of the arm is locate. Now, suppose that we know the following: Figure : 2D arm We set the first joint to rise from the X-axis at an angle of π/4 raians (the re angle in Figure 2, an the first link (the name for the part of the arm connecting two joints) is 5 meters long. We set the secon joint to be π/6 raians from straight (the blue angle in Figure 2), an the secon link is 3 meters long. Figure 2: 2D arm with labelle angles Notice that on the secon joint, our information is relative from the location of the first link. So, where is the en of the arm? Your first solution may involve trigonometry, so let s o that. The secon joint is locate at (5 cos(π/4), 5 sin(π/4)). That s easy enough. This means the en of the arm is at (5 cos(π/4) + 3 cos(π/4 π/6), 5 sin(π/4) + 3 sin(π/4 π/6), or about (6.4, 4.3). An that s right, it works! But, perhaps you can see why we won t want to o this for a more complicate arm.
You have to be constantly keeping track of the total angles (like when we ha to o π/4 π/6 above). A in another imension, an the trigonometric solution woul get very hairy inee. So let s try something else. Matrix Aition an Multiplication To get this one, we re going to nee some very basic linear algebra, which we ve seen before. First, note that to a (or subtract) two matrices, they must be the same size an shape. This is because matrix aition (an subtraction) is merely piecewise: a b c + g h i j a + g b + h c + i + j. e f k l e + k f + l So, if they re not the same size, aition or subtraction just oesn t make any sense. Before we talk about matrix multiplication, let s efine how we talk about matrix sizes. If a matrix M is m n, that means it has m rows, an n columns. Now, if A an B are matrices, an we want to perform the multiplication AB, then the number of columns in A an the number of rows of B must match. In other wors, if A is m n, then B must be n p. The result of this multiplication woul be m p, where the ith row an the jth column are the ot prouct of the ith row of A an the jth column of B. Consier, for example, the following multiplication: a b c e f Transformational Matrices g h i ag + bh + ci g + eh + f i Now the thing that s going to make our life easier is we can represent a point (x, y) in some coorinate frame x as a vector,. It turns out that we can move this point aroun in our space by using transformation y matrices. Scaling Suppose we have some point (x, y), an we like to move it so that the x-coorinate is five times bigger than it was before. What matrix can we use to perform this transformation? If you look at how matrices are multiplie, hopefully it s clear this can be accomplishe with 5 0 x 5x + 0y 5x 0 x + y y To expan this a bit, if you like to scale x by some factor a, an y by some factor b, you nee the following transformation: a 0 x ax 0 b y by Rotation Now suppose we have some point (x 0, ), an we woul like to perform a rotation aroun the origin by some angle φ, like in Figure 3. x0 x It s not immeiately clear how to change into. So let s just start writing own things that we know, an see if we can solve for x an y. We ll efine to be the istance from the origin for both points (since it s a rotation, it will be the same). y 2
(x_, y_) ϕ (x_0, y_0) θ Figure 3: Rotating a point aroun the origin φ raians cos(θ) x 0 () sin(θ) (2) cos(φ + θ) x (3) sin(φ + θ) y (4) cos(φ + θ) cos(φ) cos(θ) sin(φ) sin(θ) (5) sin(φ + θ) sin(φ) cos(θ) + cos(φ) sin(θ) (6) OK, so an 2 are true just because that s how we convert the polar coorinates (θ, ) to the Cartesian coorinates (x 0, ). 3 an 4 are similar. 5 an 6 are just trigonometric ientities. So now we can set 3 equal to 5, an set 4 equal to 6, an then plug in an 2 wherever possible. Multiply both sies by... x y x cos(φ) cos(θ) sin(φ) sin(θ) sin(φ) cos(θ) cos(φ) sin(θ) cos(φ) x 0 sin(φ) y sin(φ) x 0 + cos(φ) x cos(φ)x 0 sin(φ) y sin(φ)x 0 + cos(φ) An we ve got it! That, we can o as a matrix. To rotate by φ, cos(φ) sin(φ) x0 sin(φ) cos(φ) x y Translation Translating by some istance (not scaling, but just picking it up an moving it some istance in some irection) requires a little bit ifferent intuition. No 2 2 matrix exists that will perform this translation, since our only tool is multiplication, an we nee to o aition. The key is to use Homogeneous Matrices, an a a imension to our points. To move our point in the x irection by m, an in the y irection by n, we multiply as follows: 0 m 0 n x 0 x 0 + m + n 0 0 3
Translation an Rotation Combine If we want to perform a single transformation, in which we both translate an rotate, we can combine these into a single matrix: cos(φ) sin(φ) m sin(φ) cos(φ) n x0 cos(φ)x 0 sin(φ) + m sin(φ)x + cos(φ) + n 0 0 0 We will rarely o this explicitly, but it will be useful to refer to later. 2D Kinematics using Transformations Let s consier the rotational transformation of Figure 3. One way to look at this is that we move the point within the axes. Another way to look at it is that we ha two ifferent points, an we foun a new set of axes such that the coorinates remaine the same, but the axes were ifferent between the two points. In other wors, x x 0, but on ifferent axes (re axes vs. black axes in the figure 4). (x_, y_) ϕ (x_0, y_0) θ Figure 4: Rotating the axis aroun the origin φ raians This is what we will o for our arm; we will rotate an translate the axes, so that our final point is at the origin. So, we ll take our reference frame, or the original black axes, an rotate it π/4, creating a new frame N. We ll then translate along that frame s x-axis 5 meters, creating a new frame N 2. We ll then rotate π/6 raians, creating N 3. We ll then translate along the x-axis to the en of the arm, creating N 4. These can be seen in Figure 5. N_ N_2 N_3 N_4 N_R Figure 5: Transforme axes 4
The matrix transformations to o this, therefore, are: cos(π/4) sin(π/4) 0 sin(π/4) cos(π/4) 0 0 5 0 0 0 0 0 0 } {{ } Rotate to N ( R T ) Translate to N 2 ( T 2 ) When you perform all this multiplication, you get cos( π/6) sin(π/6) 0 sin(π/6) cos(π/6) 0 0 0 } {{ } Rotate to N 3 ( 2 T 3 ) 6.4 4.3 0 3 0 0 0 0 Translate to N 4 ( 3 T 4 ) 0 0 Starting at the origin, or, the same location in the reference frame that we got from our trigonometry. You ll notice our notation, A T B, which is how we notate a transition matrix from frame A to frame B. R T 4 R T T 2 2 T 3 3 T 4. It turns out all of this is much, much easier than trying to o trig, because everything is relative to where you were before. In our thir transformation matrix above, we in t nee to know that before, we ha alreay rotate π/4 raians; we only neee to know that now, we rotate own π/6 from wherever it is we alreay are. This concept keeps things much simpler with 3D arms. Calculating Orientation The above etails how to fin the position of the enpoint of the arm. But, what if we are also intereste in the orientation? In 3D, this will mean the roll, pitch, an yaw of the en of the arm. In 2D, this means only the angle from the X axis in the reference frame. This is remarkably easy. We note that our matrix R T 4 in the above example, is itself a general transformation matrix from the reference frame R to the frame N 4, of the form iscusse in the paragram Translation an Rotation Combine. So, whatever is in the 0, 0 spot of that matrix, is the cosine of our total rotation ψ. So, ψ cos ( R T 4 0, 0). With a perfect computer, with no roun-off or precision errors, this woul be exactly correct. Unfortunately... those on t exist. The problem comes from the fact that arccos an arcsin are pretty volatile. As x approaches or -, small changes in x (like from roun-off errors) result in huge changes in arccos(x) an arcsin(x). So, just using one of those on their own is out. What s left, of course, is arctan, which is fortunately much more stable. If you remember your SOHC- AHTOA, tan(ψ) y x, so ψ tan ( y x ). But, note that because the single fraction is entere, we on t know which quarant we re in. If x an y are both positive, (meaning ψ shoul be between 0 an π/2) we ll get the same answer as if x an y are both negative (meaning ψ shoul be between π an π/2). OK. Fine. For exactly this reason, we won t use arctan(x). Instea, we ll use a commonly-provie arctan2(y,x). Notice this function takes both arguments, meaning the function knows which quarant we re suppose to be in. OK! So, arctan2 will give us a stable answer in the correct quarant... but what o we enter for its two parameters x an y? It s not the final point s x an y, as that gives us the angle from the x axis, not the orientation of the final arm segment. Remember that tan(ψ) sin(ψ) cos(ψ)... an those two things we have (sin(ψ) R T 4, 0 an cos(ψ) R T 4 0, 0). So, our final, stable answer in the correct quarant is: Huzzah! ψ arctan2( R T 4, 0, R T 4 0, 0). 5