Sniff is a "Scratch-like" programming language that's designed to help Scratchers move gently from Scratch to more conventional languages. They can start writing programs, without having to learn a new language because Sniff is based on Scratch. They learn a little more about variables, compiling, syntax errors (!), and they can have fun controlling real hardware while they're doing it.

Friday, 9 October 2015

Inverse Kinematics (IK) for your Robot Arm

A basic robot arm has a number of segments of (usually) fixed length. Between each of these is a joint which can rotate. We move the arm by changing the angles at each of the joints. If we know the length of each segment, and the angles then we can start at the base of the robot, and calculate the position of the end of the arm:

when getEndOfSegementPosition
.make count number
.set displayX to 0
.set displayY to 0
.set angle to 0
.repeat segementNumber using count
..change angle by item count of segementAngles
..change displayX by item count of segementLengths * cos of angle
..change displayY by item count of segementLengths * sin of angle

This is called Forward Kinematics or FK, and its also used in computer animation to position and draw a characters limbs. It's provides complete, precise control. However its also a bit slow and tedious. If we want a character (robot arm) to press a button, then we need to mess around with all of the angles until we get the hand in the right place.

Wouldn't it be easier if we could just tell the computer where we want the end of the arm to be, and let it figure out all the necessary angles for us? This is Inverse Kinematics, as we're calculating the required input from the desired output (whereas in FK we apply the inputs to see the output - forwards!).

If your arm only has one or two joints then you can perhaps work this out analytically (using maths!), but if you're arm is lots of segments, then finding the best set of angles for all of them becomes tricky. Especially as there are likely to be many possible sets of angles that would all get the same result. 

One commonly used way of tackling the project is to use Cyclic Coordinate Descent (CCD):
[WELMAN C.: Inverse Kinematics and Geometric Constraints for Articulated Figure Manipulation. Master’s thesis, Simon Fraser University, 1989] . This works well, and is simple enough to implement that we can do it in Sniff.

We look at a joint somewhere along the arm and consider the position of the end effector, compared to the target. We then rotate that joint to minimise the difference between the current position and the desired position of the end effector. Because we can only move one joint, the optimal angle for the joint is always to orient the arm so that the vector from the joint to the end effector is pointing towards the target. When the desired and actual locations line up then we've done the best we can with that single joint.
Lets try that again. Consider joint B. We'd like to move C to T, but the best we can do is C'.

make jointNumber number
when updateJoint
.make sX number
.make sY number
.make dX number
.make dY number
.make desiredAngle number
.make actualAngle number
.make deltaAngle number
.set segementNumber to jointNumber - 1
.broadcast getEndOfSegementPosition and wait
.set sX to displayX
.set sY to displayY
.set dX to mouseX-sX
.set dY to mouseY-sY
.set desiredAngle to atan of (dY/dX)
.set segementNumber to length of segementLengths
.broadcast getEndOfSegementPosition and wait
.set dX to displayX-sX
.set dY to displayY-sY
.set actualAngle to atan of (dY/dX)
.set deltaAngle to desiredAngle-actualAngle
.set angle to item jointNumber of segementAngles
.change angle by deltaAngle
.if angle >90
..set angle to 90
.if angle <-90
..set angle to -90
.replace item jointNumber of segementAngles with angle

To do that in Sniff, we just get the end of the previous section (the position of the joint), find the vector to the target, and calculate the desiredAngle. Then we do the same, but using the end position of the arm to find the actualAngle. Calculate the difference and add it to the angle of this joint.

You'll notice that there's some code to stop angle being greater or less than 90 degrees - this is a constraint. One of the neat bits about this approach is that its really easy to tweak it, in this case to prevent the arm bending more than 90degrees at any single joint but we could have different constraints for each joint, or contain deltaAngle so that it can't move to quickly.

Of course its quite likely that we can't get to correct position by moving just one joint, so we apply this adjustment to each joint in turn:

...set jointNumber to length of segementLengths
...repeat length of segementLengths
....broadcast updateJoint and wait
....change jointNumber by -1

However note that we start at the end of the arm, and move back to the root. Intuitively you can think about reaching for something: you move your wrist first, and only move your whole arm from your shoulder when finer motions can't to the job.

Typically you'd apply this whole process a few and the arm will end up pretty much where its supposed to be!

Try it!!! The full code will be in the next release of Sniff.

The code works pretty well for a basic implementation, but it has a few glitches. Tweaking the constraints can dramatically help the quality of the final position. However the main issue is that we're using the atan function to calculate an angle from the ration of dy/dx. While its certainly true that tan(theta)=dy/dx its less clear cut that theta=atan(dy/dx). This can break, not only because dx might be zero, but also because it assumes that we're working positive numbers. We can't tell the difference between 4/5 and -4/-5 because they're the same, but the required angles are 180 degrees out!! In C we'd use atan2(y,x) which does the divide for itself, so it works reliably for all angles. The full code includes an implementation of atan2, which makes everything nice and stable.

For a real arm we might also want to do the whole thing in 3D, which is much harder, but if you've got one of the simple USB robot arms, then they only support pivoting at the base and at the grabber. The main body of the arm remains in a 2D plane. You can therefore calculate the rotation of the base required to place the target in the plane of the arm, then treat the rest as a 2D problem. I've not got one of these but it should simple to adapt this code to control one.

No comments:

Post a Comment