Inverse Kinematics

February 4, 2016

Once we have a mathematical model of where the robot’s hand is given the position of the motors (via the angles of the joints) we can begin to ask the real question of what are the joint angles, and thus the motor positons. 1. We can think of the problem as, given a position, how do we get here, if we’re constrained to getting there by following along the links of the robot.

2. We’ve already seen one application of this, when we computed the yaw, pitch, and roll of the hand.

3. This has the nice property of telling us what the yaw pitch and roll are. That’s nice because it turns the ugly upper lefthand 3 × 3 matrix into something we can understand.

4. It has the unfortunate property of potential innaccuracies:

(a) arcsin is innaccurate when β is π/2, ... etc. Small variations in the input result is large changes in the angle. (b) Meanwhile, when β is near π/2, 3π/2, ... etc, the cosine gets close to 0, making those fractions problematic. (c) And ﬁnally, you can’t tell which quadrant the answer is supposed to be in (i.e. arcsin etc all have two correct answers.

5. In general, we like to use arctangent when we can calculate both the sine and cosine parts:

(a) We calculate arctan with two arguments, the sin part and the cos part. These correspond to y and x on the unit circle, right? (b) We assume the range for arctan is −π to π (c) We use the following cases:

(a) x = 0, y = 1. Return π/2 (b) x = 0, y = −1. Return −π/2 (c) y = 0, x = 1. Return 0 (d) y = 0, x = −1. Return π y y (e) x > 0, |y| arctan x

1 y y (f) x < 0, |y| (π − arctan x ) We will call this arctan2 or atan2, though if you see an example of arctan that takes 2 arguments, you know it is this function

6. Now let’s apply it to ﬁguring out the joint angles in two dimensions.

Figure 1: Planar, 2-link arm

Let’s consider this problem by looking at an example. Consider our 2D arm of Figure 1, which we will now interpret as a 3D arm, consisting of these two elbow joints. Hopefully, it is clear that this consists of a rotation around the Z axis by the red angle φ1, a translation along the X axis of l1, a rotation around the Z axis by the blue angle φ2, and a translation along the Y axis of l2. Our transformation matrices look as follows (we use c1 = cos(φ1)):

      c1 −s1 0 l1c1 c2 −s2 0 l2c2 c1c2 − s1s2 −c1s2 − s1c2 0 l2c1c2 − l2s1s2 + l1c1  s1 c1 0 l1s1   s2 c2 0 l2s2   s1c2 + c1s2 −s1s2 + c1c2 0 l2s1c2 + l2c1s2 + l1s1    ·   =    0 0 1 0   0 0 1 0   0 0 1 0  0 0 0 1 0 0 0 1 0 0 0 1

Now, remember, this is an instance of our general transformation matrix T . If we make our desired end of arm location (px, py, pz), and our roll, pitch, and yaw φ, ψ, and θ, respectively, then

    c1c2 − s1s2 −c1s2 − s1c2 0 l2c1c2 − l2s1s2 + l1c1 cθcφ −sθcψ + cθsφsψ sθsψ + cθsφcψ px  s1c2 + c1s2 −s1s2 + c1c2 0 l2s1c2 + l2c1s2 + l1s1   sθcφ cθcψ + sθsφsψ −cθsψ + sθsφcψ py    =    0 0 1 0   −sφ cφsψ cφcψ pz  0 0 0 1 0 0 0 1 | {z } T Given our desired position and roll/pitch/yaw, we can write T (the right side of this equality) exactly, as numbers, giving us a system of equations to solve. Unfortunately, these are nonlin- ear, transcendental equations, sometimes making them diﬃcult to solve. But not impossible! Let’s look back at our example, and see if we can solve for s1, c1, s2, and c2 (because we still prefer using arctan2 to arcsin or arccos).

If you look carefully, you can see that T (1, 4) = l2T (1, 1) + l1c1. Given this, we can easily solve for c1. Similarly, T (2, 4) = l2T (2, 1) + l1s1, allowing us to solve for s1.

2 Another way to see this is by using trig identities. For example, sin(α) cos(β)+cos(α) sin(β) = sin(α + β), and cos(α) cos(β) − sin(α) sin(β) = cos(α + β). If we write cos(θ1 + θ2) as c12 (and similarly with sin), we can write our above matrix as:   c12 −s12 0 l2c12 + l1c1  s12 c12 0 l2s12 + l1s1     0 0 1 0  0 0 0 1

Using the entry in the upper right, which must equal T (1, 4), and using the fact that T (1, 1) = T (1,4)−l2T (1,1) c12, we can now see that c1 = , giving us the same answer as above. Oftentimes, l1 trig identities can make the patterns for a particular arm clearer.

Great! Whichever way you prefer, we are now able to calculate φ1. One down, one to go. Fortunately, there are now a number of ways to set up a system of equations to solve for s2 and c2, to allow us to do θ2 = arctan2(s2, c2). 7. OK, so how do we solve the inverse kinematics problem (i.e. ﬁnd the θs) for a real 3D arm? The same technique as above, but the second matrix has many θs as unknowns. Thus it is really ugly.

(a) Let’s pretend we have a robot with just the ﬁrst 2 links of your arms. In that case, we have:     cos(Θ1) − sin(Θ1) 0 l1 cos(Θ1) cos(Θ2) 0 sin(Θ2) l2 cos(Θ2)  sin(Θ1) cos(Θ1) 0 l1 sin(Θ1)   sin(Θ2) 0 − cos(Θ2) l2 sin(Θ2)    ·   =  0 0 1 0   0 1 0 0  0 0 0 1 0 0 0 1

(the 2nd matrix looks odd because it is the application of both the rotation around z and the rotation around x present in the second link)     C1C2 − S1S2 0 S2C1 + C2S1 l1C1 + l2C1C2 − l2S1S2 xx yx zx px  S1C2 + C1S2 0 S1S2 − C1C2 l1S1 + l2S1C2 + l2C1S2   xy yy zy py    =    0 1 0 0   xz yz zz pz  0 0 0 1 0 0 0 1

(b) Maybe not immediately illucidating, but again, maybe some trig identities will help us:

sin α cos β + cos α sin β = sin(α + β)

cos α cos β − sin α sin β = cos(α + β)

We’ll represent cos(Θ1 + Θ2) as C12, yielding:     C12 0 S12 l1C1 + l2C12 xx yx zx px  S12 0 −C12 l1S1 + l2S12   xy yy zy py    =    0 1 0 0   xz yz zz pz  0 0 0 1 0 0 0 1

3 (c) How does this help? Well, lets start picking out individual equations

C12 = xx

l1C1 + l2C12 = px

l1C1 + l2xx = px

px − l2xx C1 = l1 px − l2xx Θ1 = arccos l1

(d) But recall that we want to use arctan 2 as a function of both cosine and sine...

S12 = xy

l1S1 + l2S12 = py

l1S1 + l2xy = py

py − l2xy S1 = l1 px − l2xx py − l2xy Θ1 = arctan 2 , l1 l1

Θ1 + Θ2 = arctan 2 (C12,S12)

= arctan 2 (xy, xx)

Θ2 = arctan 2 (xy, xx) − Θ1

8. Is that all there is? Yes, and no. That is the basic technique, but there are lots of tricks that you can use from your tool kit on solving equations with trigonometric functions. For instance: (a) What if we can ﬁnd the cosine, but not the sine? We know that cos2 θ + sin2 θ = 1, so p 2 if we know C3, we can calculate θ3 = arctan 2(C3, ± 1 − C3 ). Note the plus or minus, this situation often comes up when there are multiple solutions. The danger with this is that for very small values of the cosine, the square might drip below the range of the ﬂoating point representation.

(b) What if we have no single function in a cell? That is, what if nothing like C5 happens alone, but only things like C5S34? We can divide one equation by another:

−S34S5 = yz

C5S34 = xz −S S y 34 5 = z C5S34 xz yz −T5 = xz yz θ5 = arctan − xz

4 See that in this case we don’t get to use arctan2, so there are two potential solutions here too, but only one is valid. (c) In general, any technique for solving systems of equations, especially trigonometric, that you can bring to bear can help you solve these. It is an art, not a formulaic process I can describe to you. Fortunately, you just have to do this once for a robot.

9. Now what about all these multiple solutions? What does that mean? In many cases it means exactly that, there are multiple positions of the robot that can result in the same hand position. Do we have to ﬁnd them all or can we just pick one?

(a) Why not try to find them all? Because if there are two answers for θt and two answers for θu, and θu is calculated using θt, there are four solutions for θu. With n joints and m solutions at each joint, we could end up with mn solutions. yuck. But wait! There’s more! Since we not only solve for individual angles, but often first need to solve for angle pairs (or triples! quadruples! etc.), in worst case, we might need to solve for very many subsets of all angle sums. This means there are O(2n) variables, and therefor the number of solutions is O(m(2n)). This grows pretty quickly. If m = 2, and there are 5 joints, we could conceivably see over 4 billion solutions. Maybe finding one is good enough... (b) But, in fact, many of those solution might not be valid after all. That can be because the motor doesn’t turn that far, the environment blocks it, or even some other link blocks it. For example, on your robot, motor 2 cannot turn all the way to π. And if we used regular arctan, instead of arctan2, one of the potential solution is actually wrong. Thus, we might have to check them all, all 4 billion of them. (c) The good news is that we never really see worst case. The angle sums come from joints turning in the same plane. We’re practically speaking not going to see all the joints in the same plane, or if we do, there will be a small number of them, like 2. Our final matrices are also not in practice going to contain all subsets of angle sums, so the calculated worst case just doesn’t happen. In the real world, we’ll end up with 8 or 16 possible solutions to check. That’s not so bad, so we usually just check them all.