## Inverse kinematics of 3-link arm with constrained minimization in Python

Inverse kinematics is a common topic in robotics control; one that most anyone working with a robotic arm needs to address at some point. The use case I’ll be looking at here is the situation where we want to specify an $(x,y)$ coordinate for the end-effector, and solve for the appropriate joint angle configuration that best achieves this. It’s surprisingly straightforward to handle with the right approach, so it’s a nice little exercise in Python optimization (and we’ll even work in some graphical display!) And per usual, the code is all up on my github.

Most of the time the servos that anyone is working with are position controlled, meaning that you provide them with a set of desired angles for them to move to, rather than force controlled, where you provide them with a torque signal that directly moves them. In this case, inverse kinematics is all you need. You simply work out the set of joint angles you want the servos to move to and send those out directly. In biological systems this clearly isn’t the end of the work, because even if you know where you want to go you still have to provide a force (muscle activation) signal that will move your system there. But! as mentioned in the motor babbling paper review post I wrote a while back, having the target specified in the most basic space (i.e. joint angles instead of $(x,y)$ coordinates for a servo based arm, or muscle lengths instead of joint angles for a muscle based arm) can make calculating, or learning, the appropriate force signal much easier; so something that can do the inverse kinematics calculation is worth having.

Inverse kinematics

Now then, how do we go about finding the joint angle configuration that best achieves a desired $(x,y)$ coordinate? Constrained minimization! Our constraint is that the tip of the arm must be at the specified $(x,y)$ position, and we need to specify some cost function for the system to minimize to choose among all the possible configurations that accomplish this. The cost function that we’re going to use here is a very intuitive one, that I first encountered in the paper ‘Learning operational space control’ by Jan Peters. What we’re going to do is specify a default, or resting, joint state configuration, and minimize our distance from that. Which makes a lot of sense.

I’m going to post the code for calculating the inverse kinematics of a 3-link arm, and then we’ll work through it.

import math
import numpy as np
import scipy.optimize

def __init__(self, q=None, q0=None, L=None):
"""Set up the basic parameters of the arm.
All lists are in order [shoulder, elbow, wrist].

:param list q: the initial joint angles of the arm
:param list q0: the default (resting state) joint configuration
:param list L: the arm segment lengths
"""
# initial joint angles
if q is None: q = [.3, .3, 0]
self.q = q
# some default arm positions
if q0 is None: q0 = np.array([math.pi/4, math.pi/4, math.pi/4])
self.q0 = q0
# arm segment lengths
if L is None: L = np.array([1, 1, 1])
self.L = L

self.max_angles = [math.pi, math.pi, math.pi/4]
self.min_angles = [0, 0, -math.pi/4]

def get_xy(self, q=None):
if q is None: q = self.q

x = self.L*np.cos(q) + \
self.L*np.cos(q+q) + \
self.L*np.cos(np.sum(q))

y = self.L*np.sin(q) + \
self.L*np.sin(q+q) + \
self.L*np.sin(np.sum(q))

return [x, y]

def inv_kin(self, xy):

def distance_to_default(q, *args):
# weights found with trial and error, get some wrist bend, but not much
weight = [1, 1, 1.3]
return np.sqrt(np.sum([(qi - q0i)**2 * wi
for qi,q0i,wi in zip(q, self.q0, weight)]))

def x_constraint(q, xy):
x = ( self.L*np.cos(q) + self.L*np.cos(q+q) +
self.L*np.cos(np.sum(q)) ) - xy
return x

def y_constraint(q, xy):
y = ( self.L*np.sin(q) + self.L*np.sin(q+q) +
self.L*np.sin(np.sum(q)) ) - xy
return y

return scipy.optimize.fmin_slsqp( func=distance_to_default,
x0=self.q, eqcons=[x_constraint, y_constraint],
args=[xy], iprint=0) # iprint=0 suppresses output



I’ve taken out most of the comments for compactness, but there are plenty in the code on github, don’t you worry. Alright, let’s take a look!

First, we go through and set up the parameters of our arm. If nothing was passed in during construction, then we’re going to make an arm that is initially at it’s resting position, with shoulder and elbow joint angles = $\frac{\pi}{4}$, and wrist angle = 0. We also set up the arm segment lengths to all be 1, and the minimum and maximum joint angles.

Next, there’s the get_xy function, which just uses some simple trig to calculate the current $(x,y)$ position of the end-effector. This is going to be a handy thing to have when we want to see how well the joint configurations we calculate achieve our desired hand position.

And then after this, we have the actual inverse kinematics function. inv_kin takes in a desired $(x,y)$ position, and returns to us a list of the joint angles that will best achieve this. We define three methods inside inv_kin that define our constrained optimization problem, distance_to_default, which is the function we are minimizing, and x_constraint and y_constraint, which are the constraints that must be met for a solution to be valid. These are all pretty straight forward functions, the distance_to_default function calculates the Euclidean distance in joint space to the resting state, and the constraint functions calculate the difference between the actual and desired end-effector positions for a given set of joint angles.

There is one point of interest in the distance_to_default method, and that is the use of the weights therein. What is the point of applying a gain to these values, you ask? Why, simply that it lets us weight the relative importance of each of these joints remaining close to their default configuration! If you think about moving your arm on a horizontal plane, the wrist actually bends very little. To mimic this we can set the weight applied to the distance of the wrist from its resting state higher than those of the other joints, so that it becomes a higher priority to keep the wrist near its resting state. I suggest playing around with once the graphical display (below) is built up, it’s fun to see the effects different weightings can give on joint configurations.

With these three functions defined, we can now call up the scipy.optimize.fmin_slsqp function, which performs the sequential least squares method to arrive at a solution.

Really, that’s all there is to it!

To test this now, we have the following method:

def test():
############Test it!##################

# set of desired (x,y) hand positions
x = np.arange(-.75, .75, .05)
y = np.arange(0, .75, .05)

# threshold for printing out information, to find trouble spots
thresh = .025

count = 0
total_error = 0
# test it across the range of specified x and y values
for xi in range(len(x)):
for yi in range(len(y)):
# test the inv_kin function on a range of different targets
xy = [x[xi], y[yi]]
# run the inv_kin function, get the optimal joint angles
q = arm.inv_kin(xy=xy)
# find the (x,y) position of the hand given these angles
actual_xy = arm.get_xy(q)
# calculate the root squared error
error = np.sqrt((np.array(xy) - np.array(actual_xy))**2)
# total the error
total_error += error

if np.sum(error) > thresh:
print '-------------------------'
print 'Initial joint angles', arm.q
print 'Final joint angles: ', q
print 'Desired hand position: ', xy
print 'Actual hand position: ', actual_xy
print 'Error: ', error
print '-------------------------'

count += 1

print '\n---------Results---------'
print 'Total number of trials: ', count
print 'Total error: ', total_error
print '-------------------------'


Which is simply working through a set of target $(x,y)$ points, and calculating the total error across all of them. If any particular point gives an error above the threshold level, print out the information for that point so we can take a closer look if so desired. Fortunately, the threshold error isn’t even approached in this test, and for output we get:

---------Results---------
Total number of trials:  450
Total error:  [  3.33831421e-05   2.89667496e-05]
-------------------------


Visualization

So that’s all great and dandy, but it’s always nice to be able to really visualize these things, so I wrote another method that uses Pyglet to help visualize. This is a really easy to use graphics program that I tested out as a means here, and I was very happy with it. I ended up writing a method that pops up a window with a 3-link arm drawn on it, and the arm uses the above inverse kinematics function in the Arm class written above to calculate the appropriate joint angles for the arm to be at to follow the mouse. Once the joint angles are calculated, then their $(x,y)$ locations are also calculated, and the arm is drawn. Pleasingly, the inv_kin above is fast enough to work in real time, so it’s a kind of fun toy example. Again, I’ll show the code and then we’ll work through it below.

import numpy as np
import pyglet
import time

import Arm

def plot():
"""A function for plotting an arm, and having it calculate the
inverse kinematics such that given the mouse (x, y) position it
finds the appropriate joint angles to reach that point."""

# create an instance of the arm

# make our window for drawin'
window = pyglet.window.Window()

label = pyglet.text.Label('Mouse (x,y)', font_name='Times New Roman',
font_size=36, x=window.width//2, y=window.height//2,
anchor_x='center', anchor_y='center')

def get_joint_positions():
"""This method finds the (x,y) coordinates of each joint"""

x = np.array([ 0,
arm.L*np.cos(arm.q),
arm.L*np.cos(arm.q) + arm.L*np.cos(arm.q+arm.q),
arm.L*np.cos(arm.q) + arm.L*np.cos(arm.q+arm.q) +
arm.L*np.cos(np.sum(arm.q)) ]) + window.width/2

y = np.array([ 0,
arm.L*np.sin(arm.q),
arm.L*np.sin(arm.q) + arm.L*np.sin(arm.q+arm.q),
arm.L*np.sin(arm.q) + arm.L*np.sin(arm.q+arm.q) +
arm.L*np.sin(np.sum(arm.q)) ])

return np.array([x, y]).astype('int')

window.jps = get_joint_positions()

@window.event
def on_draw():
window.clear()
label.draw()
for i in range(3):
pyglet.graphics.draw(2, pyglet.gl.GL_LINES, ('v2i',
(window.jps[i], window.jps[i],
window.jps[i+1], window.jps[i+1])))

@window.event
def on_mouse_motion(x, y, dx, dy):
# call the inverse kinematics function of the arm
# to find the joint angles optimal for pointing at
# this position of the mouse
label.text = '(x,y) = (%.3f, %.3f)'%(x,y)
arm.q = arm.inv_kin([x - window.width/2, y]) # get new arm angles
window.jps = get_joint_positions() # get new joint (x,y) positions

pyglet.app.run()

plot()


There are really only a few things that we’re doing in this method: We create an arm with somewhat normal segment lengths and a window for plotting; we have a function for calculating the $(x,y)$ coordinates of each of the joints for plotting; we have a function that tracks the position of the mouse and updates our arm joint angles by calling the arm.inv_kin function we wrote above; and lastly we plot it! Easy. The functions aren’t quite in that order, and that’s because I chose alphabetical ordering over conceptual. Here’s a picture! I think the only kind of weird thing going on in here is that my variables that I use in both this method and in the overwritten window methods are defined as belonging to window. Aside from that, it’s straightforward trig to calculate the joint angle Cartesian coordinates, and then there’s also a label that displays the mouse $(x,y)$ position for the hell of it. Honestly, the label was in a Pyglet tutorial example and I just left it in.

It’s not going to win any awards for graphics by any means, but it’s simple and it works! And, assuming you have Pyglet installed (which is very easy to do), you should straight up be able to run it and be amazed. Or a little impressed. Or not, whatever. But in any case now you have your very own inverse kinematics example on a 3-link arm! Moving to any other number of joints is exactly the same, you just have to add the corresponding number of default joint configurations and everything should work out as is.

From here, the plan is to look at bringing in an actual force driven arm model to the mix, and then building up a system that can track the mouse, but with a control signal that specifies forces to apply to the arm, rather than positions. Which I think will be pretty neat. And in case you missed it at the top, here’s a link to all the code on my github.

## 36 thoughts on “Inverse kinematics of 3-link arm with constrained minimization in Python”

1. Sanjeev Kumar says:

that is excelent stuff, u have posted here. I have only one clarification here. How the max angle and min angle are used in ur code.

• travisdewolf says:

Thanks! And good question. Actually, they’re not in this implementation (even though I mention them at the beginning), but to include them is just a matter of introducing a set of inequality constraints into the scipy optimization call.

To do that is just a matter of introducing another term into the optimization call, like the eqcons parameter, but this one is called ieqcons. If you just create two functions then, one that checks to make sure that the joint angles are above the lower bound, and one that checks to make sure the joint angles are below the upper bound (and set them up such that when this is true both of these functions return positive values), then you can just set ieqcons = [lower_constraint, upperconstraint], and you should be good to go!

If you wanted to go further, you could also introduce distance to min / max joint angles into the cost function. The standard way is to set it up as an exponential that is pretty much zero for most values but then when you start getting close to the edge the cost starts shooting way way up.

Hope that helps!

• Sanjeev Kumar says:

thanks dear!
What if i use bound in optimization call. I really dont know.

• travisdewolf says:

Bounds would work too! And be simpler to implement.

The ieqcons parameter is more appropriate then when you’re wanting to restrict the values of some function of the state variables. For this straightforward constraint on variable range, I think you’re right, using the bounds parameter is the better way to go about it.

2. […] and plotting the MapleSim arm model In a previous post I used Pyglet to plot out a 3 link arm (a very poor, lines only plot, but sufficient), and […]

3. Lars Bergholm says:

Thanks for a great tutorial. 🙂 When testing I found there was an error in scipy version 0.14.0
The arguments to the optimize call in arm.py had to be entered as

return scipy.optimize.fmin_slsqp( func=distance_to_default,
x0=self.q, eqcons=[x_constraint, y_constraint],
args=(xy,), iprint=0) # iprint=0 suppresses output

( originally in your it was args=[xy] )

Maybe this can be of use for someone testing your code.
Kind Regards
Lars Bergholm

• travisdewolf says:

Thank you very much! I’ve updated the code, thanks for catching this!

4. […] specify the end-effector position and have the robot work out the angles! I’ve implemented an inverse kinematics solver using constrained optimization before for a 3-link planar arm, but we’re not going to go that route. Find out what […]

5. juerg says:

Tried for hours to make the example run. Spent a lot of time with making numpy available and finally found Anaconda so it worked out OK.
In armplot.py it says
Import Arm
but I have no idea where to get that as it tells me “no module named Arm”

• travisdewolf says:

Hi Juerg, sorry that you’ve had trouble with it! The Arm.py file is the other file in my github link, you can download it here: https://github.com/studywolf/blog/blob/master/InvKin/Arm.py
Hopefully it will work smoothly for you now!

• juerg says:

Hi
Just detected that I used lower case letters for the Arm.py. After renaming I am able to run your demo! Thanks for your reply and all the useful info in your blog. I am slowly picking up the what is going on with rotation and transformation

• travisdewolf says:

Ah, glad to hear you got it running. And you’re very welcome! Some of the ideas definitely take a bit to wrap your head around, happy your finding some posts useful! 🙂

6. Chaitanya Awasthi says:

Thank you so much for sharing your code. I was able to run it successfully, but being new to programming, I didn’t understand why, on line 85, you have
q = arm.inv_kin(xy=xy)

What does xy=xy mean and what does it do? (I changed xy =xy to just xy and nothing happened when I reran the code).

Thank again,
Chaitanya

7. Chaitanya Awasthi says:

Thank you so much for sharing your code. I was able to run it successfully, but being new to programming, I didn’t understand why, on line 85, you have
q = arm.inv_kin(xy=xy)

What does xy=xy mean and what does it do? (I changed xy =xy to just xy and nothing happened when I reran the code).

Thank again,
Chaitanya

• travisdewolf says:

Hi Chaitanya,

glad you found it useful! that bit of code is Python specific, where you can specify parameters by name when you pass them into a method. The parameters don’t have to be called the same thing, though, we could have named the xy variable defined on line 83 xy_des, and then the line would be q = arm.inv_kin(xy=xy_des). It’s not necessary to pass parameters in by name, you can also pass them in in order, which is why you can remove the xy= part and it still works. Hope that clarifies!

8. Matt says:

Thanks for the nice example!

I’ve been having a play and spotted that you don’t seem to use self.max_angles and
self.min_angles. Shouldn’t these be passed as a bounds argument to fmin_slsqp ?

• travisdewolf says:

Hi Matt!

You’re totally right! Must have slipped my mind when I was writing it. I’ve added in the joint angle constraints to the code now such that the shoulder can’t move past pi / 2 so you can see the results when you run it. Thanks for the spot!

9. Diane says:

Hi, thank you for this very clear and well written article.
I ran into a problem while trying to execute your code. The following error appears: ModuleNotFoundError: No module named ‘future.utils’ . Do you have any idea how I could solve this?
Thanks you!

• travisdewolf says:

Hm! I haven’t seen this problem before, and I just checked the program runs fine for me in Python 3 on Ubuntu…it might be worth trying a clean Anaconda install (https://www.continuum.io/downloads) with Python 3 and trying again. Sorry I don’t have more thoughts on it than that!

• Diane says:

Thanks for your quick reply! I am working on windows though, that might be the cause ? I’ll try the clean install too.

10. Tyler says:

I ran your files as well as tried many cases but still can not reproduce the total error as small as yours.

Can you give me your set of parameters to obtain that very small total errors.

Do you have any “trial and error” strategy in picking up parameters?

Thanks!

• travisdewolf says:

Hello! Yes wooowww, thanks for the spot. I had added some joint min and max constraints as well and they super mess things up. So i commented them out, if you check the repo now you should find a version that produces the low error I posted in the article! (Note I also changed the printed error to be an actual euclidean distance calculation too)

11. Cristhian De Leon says:

Im having some issues with the scipy.optimize, it gives me the following error,
File “C:\ProgramData\Anaconda2\Lib\site-packages\scipy\optimize\slsqp.py”, line 311, in _minimize_slsqp
meq = sum(map(len, [atleast_1d(c[‘fun’](x, *c[‘args’])) for c in cons[‘eq’]]))

TypeError: x_constrait() takes exactly 2 arguments (3 given)

any idea of how to fix it

• travisdewolf says:

Hmmm, what version Python are you using? This works for me fine using an Anaconda (https://www.anaconda.com/download/) install of Python 3.5. Could you test that out and let me know if you’re still getting errors?

12. Rohan says:

Hi, thanks for this example! I was wondering if you could clarify the physical directions of the x and y planes in this context and why don’t use a z coordinate.

• travisdewolf says:

Hi Rohan,

there’s no strict mapping from the x and y planes to reality, often I think of this plot as horizontal plane with no gravity. There’s no z because I did not want to get a 3-d plot going! 🙂

13. Marcos Vilela says:

Hi, Travis! Great article and great help.
Your code works fine for this case, but I’m wondering what should I do if i want to add a Z coordinate to the movement?

14. Marcos Vilela says:

One other thing, when I change the range of values on the testing to x = np.arange(0.7, 3, .3)
and y = np.arange(0.5, 2, .5), at the final trial all the returns are [NaN], the numpy Not a Number label.

15. Tankie97 says:

Dear studywolf,

Thank you so much for this amazing article!
I have just 1 question, the lengts of the ‘L’ variable, is the distance arm lengts in meters, cm or milimeters? Thanks in advance! 🙂

• travisdewolf says:

Hello! Good question!
I’m not 100% on my recall, but I believe the lengths are set in terms of pixels for PyGame. So there’s no direct mapping to metres, cm, or mm. The relative scaling between them is what’s more important, the specific numbers I chose just so it would show up to be a good-ish length when visualizing. 🙂 Does that answer your question?

16. sreedharmadhanaM. Sreedhar says:

Dear travisdewolf,
it is a great article. when i run the visualization program it is showing the following error
ModuleNotFoundError Traceback (most recent call last)
in
1 import numpy as np
—-> 2 import pyglet
3 import time
4
5 import Arm

• travisdewolf says:

Hello,

it looks like you’re missing the pyglet package. Do you have it installed on your machine? I use conda to manage my Python installation. If you have that installed then you can just pip install pyglet and that should take care of it. I just checked it out myself and the demo runs for me. Hopefully that takes care of it for you!

17. Shahryar Shaheen says:

Hi Travis

Thank you for this nice program. I have read it through but am unable to use it.
I have tried for the past 10 days to find a way to install scipy that is required in Arm.py
I am running Python 3.7.3 on Rasbpian (Linux).
I have tried Many Many Ways recommended on several sites but at the end its the same problem.

It produces some sort of incompatibility with numpy Multiplearray and it cancels the installation.

Many Thanks
Shahryar

• travisdewolf says:

Hi Shahryar,

ahh i’m sorry. My knowledge of setting up scipy on the Rasbpian is little to none. The best I can suggest would be posting on their forums about it, if they have them!
Sorry,

18. Shahryar Shaheen says:

Hi Travis

Greate Demo. Unfortunately I cannot install “scipy” .
Is there a way around it. I have “numpy’ but scipy installation fails everytime.

• travisdewolf says: