Last time, I derived the Forward Kinematics for the boom on M0o+. If you haven’t read the first two posts, then this one won’t make much sense (and I don’t want to repeat myself 😄):
This time, we’ll finish off the series by looking at the Inverse Kinematics and actually controlling the boom.
As a quick recap, here’s the basic boom setup. For Inverse Kinematics purposes there are two joints which can move:
- The lift motor raises and lowers the boom.
- The drawer runner/leadscrew extends and retracts the boom.
And the end goal is to have the ability to say (in code): “move the boom so that the end is 241 mm above the ground and 76 mm in front of the robot,” and then the robot just takes care of all the motor movements to do that. That will form the basis of using the boom to achieve useful work in the challenges.
Inverse Kinematics summary
So last time I derived the forward kinematics, which converts from joint positions (lift angle and leadscrew length) to the resulting coordinates of the end of the boom.
What we really want is the opposite - we want to convert from some desired end effector coordinates to the joint positions needed to reach them, and that’s called inverse kinematics.
So at the end of the last post, the culmination of all my efforts were two
equations which give the “X” and “Y” coordinates (or
b values in the
a = boom_length*cos(lift_angle) + 26.55*sin(lift_angle) + 37.97*cos(lift_angle) b = boom_length*sin(lift_angle) + 37.97*sin(lift_angle) - 26.55*cos(lift_angle)
To do the opposite, it would be necessary to rearrange these two equations to
get two new equations with just
boom_length on the left,
and just expressions in terms of
b on the right.
It might be possible to do such a rearrangement “analytically” (using pen and paper, trigonometric identities and all that fun stuff), and I did spend some time trying to do so, but ultimately I failed. So instead, there’s an alternative technique which requires more computation on the robot, and is somewhat approximate, but avoids the need to do this rearrangement by hand.
This excellent document from the University of Illinois was extremely useful to me for working this out. I’m going to focus on the specific implementation for my boom, but if you want some of the more general maths behind it, I’d definitely recommend that page.
The approach in a nutshell
Looking at the diagram below, we can intuitively see that to move from the current position to the target position, the lift angle and boom length both need to increase, so if we were controlling the boom manually we might increase both of them a little bit, and then re-assess the situation:
The inverse kinematics is going to do exactly that, with code and maths.
We have a position that we want to move the boom to - let’s call that our “target” position - and we know the current position, which can be found using the forward kinematics equations above.
If we can find a small change in joint positions which moves us towards the target position, then we could move that small amount, getting slightly closer to the target, and then repeat.
The “small change” part means that we need to find a relationship between change in joint position and change in boom coordinates, which in maths speak would mean differentiating our position equation(s) (forward kinematics) with respect to the joint positions.
Partial Differentials and Jacobian Matrices
The whole method depends on something called a Jacobian, which is going to need some school-level maths to explain.
If we have an equation, we can differentiate it with respect to one of its variables, to find out how changing the value of one variable affects the other.
Given the equation:
y = 2x
Differentiating with respect to
dy/dx = 2
And this tells us that any change in
x causes a change in
y which is
2 times as big.
If we have an equation with more than two variables, then we can calculate partial derivatives for each of the independent variables in turn, treating the other independent variables as if they are constants.
z = 2x + 3y
Has the partial derivative with respect to
Note that we treat
yas a constant, so
3yhas a derivative of
dz/dx = 2 + 0
And with respect to
dz/dy = 0 + 3
These tell us how much
z changes for each change in
However, note that because we treated the “other” variable as a constant, these
partial derivatives only hold true for a constant value of the “other”
If we took one of the forward kinematics equations, then we can find its
partial derivatives with respect to the two independent variables:
a = boom_length*cos(lift_angle) + 26.55*sin(lift_angle) + 37.97*cos(lift_angle) da/dlift_angle = -boom_length*sin(lift_angle) + 26.55*cos(lift_angle) - 37.07*sin(lift_angle) da/dboom_length = cos(lift_angle)
This isn’t so obvious unless you’ve got a lot of practice calculating
derivatives, but you don’t have to work it through by hand if you don’t want
to: we can get
sympy to do it for us!
>>> a boom_length*cos(lift_angle) + 26.55*sin(lift_angle) + 37.97*cos(lift_angle) >>> diff(a, lift_angle) -boom_length*sin(lift_angle) - 37.97*sin(lift_angle) + 26.55*cos(lift_angle) >>> diff(a, boom_length) cos(lift_angle)
Now, a Jacobian Matrix
is the name for a matrix containing the partial
differentials of a set of functions with respect to their independent variables,
b functions that we got from forward kinematics.
Sticking with our equation for
a, the Jacobian of
a would be a matrix
with one row, where each column is one of the partial differentials from above
sympy syntax, because who wants to do calculus when the computer
can do it for us?):
J_a = [ diff(a, lift_angle), diff(a, boom_length) ]
We actually have two equations, one for
a and one for
b, so we can get a
combined Jacobian, where the first row has the partial differentials for
and the second row, for
J = Matrix([ [ diff(a, lift_angle), diff(a, boom_length) ], [ diff(b, lift_angle), diff(b, boom_length) ], ])
…and that’s all a Jacobian is.
How does this help?
Well, the partial differentials tell us what change we can expect in
b if we change
boom_length. This is exactly the
“relationship between change in joint position and change in boom
coordinates” I mentioned before.
More concretely, if we take our Jacobian and multiply by a vector containing changes in joint positions, we will get back changes in coordinates:
[ change_in_a, change_in_b ]' = J * [change_in_lift_angle, change_in_boom_length]'
Things can get a bit confusing here with all the different variables. To actually use the equation above, you would:
- Calculate the Jacobian matrix, filling in the known values for the current
- Pick some small values for
change_in_boom_lengthand feed them in to the equation
change_in_baccording to the equation.
Step 1 is crucially important here - the Jacobian matrix depends on
boom_length, and so it must be recalculated for each new
The inverse part
OK, so all the derivation above gave us an equation which lets us calculate change in boom coordinates, given a current position and a change in joint positions.
Once again, we still want the opposite of that: an equation that calculates the required changes in joint positions, given a desired change in boom position.
We know the desired change in boom position - that’s
target_position - current_position, what we want to calculate is the required
change in joint positions.
To get that, we “simply” invert the Jacobian, which results in an equation like so:
[ change_in_lift_angle, change_in_boom_length ]' = J.inv() * [ desired_change_in_a, desired_change_in_b ]'
For inverting the Jacobian, we just use the standard 2x2 matrix inverse equation.
Putting that into code
That’s all the derivation done, to actually use it on the robot looks something like this:
while target_position_not_reached(): # Get the current position current_lift, current_length = get_current_joint_positions() # Calculate Jacobian jacobian = calculate_jacobian(current_lift, current_length) inv_jacobian = invert(jacobian) # Find out the desired change in end effector position [d_a, d_b] = target_position - current_position # Find an approximation for the change in joint positions needed [d_lift, d_length] = inv_jacobian * [d_a, d_b] # Move a little in the right direction move_lift_motor_towards(current_lift + d_lift) move_extend_towards(current_length + d_length) wait_a_short_time()
Because the calculated Jacobian only applies for those specific values of
d_length values are
approximations - as soon as the joints move, the Jacobian needs recalculating,
and we need to recalculate new
d_length values. That’s what the
wait_a_short_time() is there for; we wait for the boom to move a little bit,
and then recalculate.
And that’s the basis of a forward kinematics solution for my boom.
The implementation above is always going to try and move directly towards the target position, but what I found was due to the approximate nature of the calculations, and because I don’t have anything like perfect speed control for the joints, the boom would move to the correct position, but it wouldn’t move there in a straight line.
This can be a problem - it can mean that the boom bashes into something (like the apple tree) because the lift motor moved more quickly than the extension motor could keep up, or vice-versa.
To fix this, I added a trajectory planner. The trajectory planner doesn’t only keep track of the current position and the target position, it also keeps track of the starting position.
Then, instead of setting the inverse kinematics to move directly to the target position, it sets a bunch of intermediate targets along the path from the start to the end position.
In summary the trajectory planner does:
- Get the current boom position
- Find the point on the intended path closest to the current position
- Calculate an intermediate target point a small distance further along the intended path from 2.
- Set the inverse kinematics target to 3.
This means that even if the boom moves away from the intended path, the trajectory planner will try and drag it back along the intended path.
Even with this, the boom still doesn’t follow the intended path completely, because the intermediate targets are always on the intended path, the system tends to under-correct; but it works well enough.
So that’s the somewhat complicated process of implementing an inverse kinematics controller for the boom position. With all that in place, I can now tell the boom to move to a target position in the real world (relative to the robot), and the inverse kinematics code will move the two joint motors to get the boom to move to that position.
So next, it’s time to solve some of the actual competition challenges!