Previously, I worked through a simple inverse kinematics example with at three link arm. The arm, however, was uninteresting, and in fact had no dynamics of its own. I’ve also previously (but not in a blog post) built up a number of arm models using MapleSim that were interesting, and ported them into C. From there I used them in Matlab / Simulink, but I’m on a Python kick now and tired of Matlab; so I’ve been looking at how to wrap this C code generated by MapleSim using Cython such that I can interact with it from Python.
So the code that simulates my models is exported to C, which is great in that it’s super fast, but it’s bad because it’s a pain in the ass to work with. I don’t want to have to write my controllers and learning algorithms all in C, it would be much nicer to do it all in Python. So I was looking at interfacing Python with the C library, and did a little (as seen in my last post), but then I already know how to make Cython play nice with C++ so I figured let’s shoot for that instead. What follows now is a hacky guide to getting your MapleSim model simulations to work with Python. First we’ll get the C code to just run, then we’ll port it to Python, then we’ll get it going graphically.
Get the MapleSim code to run in C++ with classes and input
Step 1: Rename your file from ‘whatever.c’ to ‘whatever.cpp’, in the code here I’m renaming ‘c2LinkArm.c’ to ‘2LinkArm.cpp’. I know, I know, it’s painful but hopefully that’s the worst of it.
Step 2: Add #include "mplshlib.h" to the beginning of the file. The code is meant to be run in a MapleSim environment (or something), so it requires one of the Maple library files. We’ll just add this in here and everything should be happy.
Step 3: For whatever reason, there is a function in here that sets all of the system input to 0. Why, you ask? Good question. The function in question is static void inpfn(double T, double *U), and you can see that all it does is set every element of U = 0. Now, you can either comment out the body of this function, or, several lines down, the first line of the SolverUpdate function (after a long i variable declaration) is the call to this function, so you can comment that out and then everything’s fine. Hurray! We can give input to the system now!
And that’s all the inline code editing we have to do. We’re not done with this file though, we still need to append some things so that we can use it easily.
Step 4: The reason that I want to use C++ is because with C++ we can make a class, which we’ll call the Sim class, that can store all of our simulation details, and hold some wrapper functions to hide the full blown interface to the simulator functions. Let’s make that class now. Go down to the bottom of the file and paste in the following code:
c2LinkArm.cpp
/* A class to contain all the information that needs to
be passed around between these functions, and can
encapsulate it and hide it from the Python interface.
Written by Travis DeWolf (May, 2013)
*/
class Sim {
/* Very simple class, just stores the variables we
need for simulation, and has 2 functions. Reset
resets the state of the simulation, and step steps it
forward. Tautology ftw!*/
double* params;
double dt, t0;
double u0[NINP], other_out[NOUT+1], y[NOUT];
double w[1 + 2 * NEQ + NPAR + NDFA + NEVT];
SolverStruct S;
public:
Sim(double dt_val, double* params_pointer);
void reset(double* out, double* ic);
void step(double* out, double* u);
};
Sim::Sim(double dt_val, double* params_pointer)
{
t0 = 0.0; // set up start time
dt = dt_val; // set time step
for (int i = 0; i < NINP; i++)
u0[i] = 0.0; // initial control signal
params = params_pointer; // set up parameters reference
/* Setup */
S.w = w;
S.err = 0;
}
void Sim::reset(double* out, double* ic)
{
SolverSetup(t0, ic, u0, params, y, dt, &S);
/* Output */
out[0] = t0;
for(int j = 0; j < NOUT; j++)
out[j + 1] = y[j];
}
void Sim::step(double* out, double* u)
/* u: control signal */
{
for (int k = 0; k < NOUT; k++)
out[k] = *dsn_undef; // clear values to nan
/* Integration loop */
/* Take a step with states */
EulerStep(u, &S);
if (S.err <= 0)
{
/* Output */
SolverOutputs(y, &S);
out[0] = S.w[0];
for(long j = 0; j < NOUT; j++)
out[j + 1] = y[j];
}
}
int main (void)
{
FILE *fd;
double *ic, *p, *out;
char* errbuf;
long i, j, outd;
long internal = 0;
double dt = 0.00001;
int time_steps = 1000000;
double u[NINP];
for (int k = 0; k < NINP; k++) u[k] = .1;
fd = fopen("output.dat", "w");
Sim sim = Sim(dt, NULL);
sim.reset(out, NULL); // ic = NULL, use default start state
for(i=0;i<time_steps;i++)
{
sim.step(out, u);
fprintf(fd,"%lf ",out[0]);
for(j=0;j<NOUT;j++)
{
fprintf(fd,"%lf ",out[j+1]);
}
fprintf(fd, "\n");
}
fclose(fd);
return 0;
}
So, this is based off of the ParamDriverC() function from the MapleSim generated code. I just separated out the initialization and the simulation into two functions (reset() and step(), respectively), so that it’s possible to explicitly control the simulation stepping through time, and change the input control signal at every time step.
NOTE: The dt here must be <= 1e-5, or the constraint solver in the code throws an error. Don’t fear though, this simulator is still crazy fast.
OK. This code then can be compiled (once you have the library files in the same folder) and run with:
g++ c2LinkArm.cpp -out sim
./sim
You may have noticed a bunch of warning messages scrolling by, I’m not about to touch those and they don’t affect the simulation, so as long as one saying ‘error’ doesn’t pop up let’s just leave well enough alone.
Once you’ve run the executable you should now have a file called ‘output.dat’ filled with simulation data. The first column is time, and the rest are the output variables from your MapleSim model. Alright, we’ve got our code up and running! Let’s go an and make a Cython wrapper for it.
Making a Cython wrapper for the MapleSim code
The first thing that we need to do is take the main function we just added in out of the simulation code above. We’re done with running the C code on its own so we don’t need it anymore. Get rid of it already.
Now, this is going to be very similar to the code from the previous Cython posts, but in this one there’s no getting around it: We need to pass back and forth arrays. It turns out this isn’t very complicated, but there’s a template you have to follow and it can be difficult if you don’t know what that is.
I’ll post the code and then we can go through the tricky parts. As always, we create a ‘pyx’ file for our Cython wrapper code, I called this one ‘py2LinkArm.pyx’.
py2LinkArm.pyx
import numpy as np
cimport numpy as np
cdef extern from "c2LinkArm.cpp":
cdef cppclass Sim:
Sim(double dt, double* params)
void reset(double* out, double* ic)
void step(double* out, double* u)
cdef class pySim:
cdef Sim* thisptr
def __cinit__(self, double dt = .00001,
np.ndarray[double, mode="c"] params=None):
"""
param float dt: simulation timestep, must be < 1e-5
param array params: MapleSim model internal parameters
"""
if params: self.thisptr = new Sim(dt, ¶ms[0])
else: self.thisptr = new Sim(dt, NULL)
def __dealloc__(self):
del self.thisptr
def reset(self, np.ndarray[double, mode="c"] out,
np.ndarray[double, mode="c"] ic=None):
"""
Reset the state of the simulation.
param np.ndarray out: where to store the system output
NOTE: output is of form [time, output]
param np.ndarray ic: the initial conditions of the system
"""
if ic: self.thisptr.reset(&out[0], &ic[0])
else: self.thisptr.reset(&out[0], NULL)
def step(self, np.ndarray[double, mode="c"] out,
np.ndarray[double, mode="c"] u):
"""
Step the simulation forward one timestep.
param np.ndarray out: where to store the system output
NOTE: output is of form [time, output]
param np.ndarray u: the control signal
"""
self.thisptr.step(&out[0], &u[0])
This is pretty familiar: We make a reference to a class defined in C++, and then we wrap it in a python class and define functions that we can actually call from Python.
The main difference here is that we have to handle passing in numpy arrays, and having them be in the appropriate double* form that our C++ code is expecting to see. There are three parts involved in this. First, note at the top that we have an import numpy as np followed by a cimport numpy as np, this gives us access to the functions that we’ll need. Second, for each of the arrays accepted as parameter we have a special type declaration: np.ndarray[double, mode="c"] var. And finally, we pass the arrays on with a dereferencing &var[0]. If we were passing along 2D arrays we would use &var[0][0].
In the __init__() and reset() methods, I’m allowing the model parameters, params, and initial conditions, ic, arrays to be undefined, in which case NULL is passed to our C code and the default values we defined inside MapleSim are used. The reason for this is that these require some pretty intimate knowledge of the MapleSim model, and if a casual user wants to just use this business they shouldn’t have to know anything about the internal states.
With our wrapper code done now the only thing left is our setup file! This one is just straight from the previous posts, with the exception that only the ‘pyx’ file is included in the sources declaration because our ‘py2LinkArm.pyx’ file references the ‘c2LinkArm.cpp’ file directly, instead of referencing a header file.
setup.py
from distutils.core import setup
from distutils.extension import Extension
from Cython.Distutils import build_ext
setup(
cmdclass = {'build_ext': build_ext},
ext_modules=[Extension("py2LinkArm",
sources=["py2LinkArm.pyx"],
language="c++"),],
)
With our setup file we should now be good to go. Let’s compile! To do this I pull open IPython and punch in
run setup.py build_ext -i
And it will complain and throw some warnings, but it should compile just fine. And now we have access to the pySim class!
Note: that should read ‘build_ext’ with an underscore in there, I’m not sure why it’s not displaying but that underscore needs to be there or it won’t run.
To see it do some things, type in
import numpy as np
import py2LinkArm
sim = py2LinkArm.pySim()
u = np.ones(2) # input array
out = np.zeros(3) # output array
sim.reset(out)
print out # initial values
for i in range(10):
sim.step(out, u)
print out
And we can see it moving forward! If you set u = np.ones(2) * .1, which is what it was when we generated the ‘output.dat’ file using the C code only and run it forward you’ll see that the we get the same results for both. Excellent. OK now let’s display it!
Running 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 I’m going to use it again here (for a very poor, lines only plot). The file is called ‘run.py’, and is less than 80 lines. I’ll post it and then walk through it below:
run.py
import numpy as np
import pyglet
import time
import py2LinkArm
def run():
"""Runs and plots a 2 link arm simulation generated by MapleSim."""
# set up input control signal to simulation
u = np.ones(2) * .1
# set up array to receive output from simulation
out = np.zeros(3)
# create MapleSim sim class
sim = py2LinkArm.pySim()
# set up initial conditions of the system
sim.reset(out)
###### Plotting things
# segment lengths = (.31, .27)m (defined in MapleSim)
L = np.array([31, 27]) * 3
# make our window for drawin'
window = pyglet.window.Window()
# set up some labels to display time
label_time = pyglet.text.Label('time', font_name='Times New Roman',
font_size=36, x=window.width//2, y=window.height - 50,
anchor_x='center', anchor_y='center')
# and joint angles
label_values = pyglet.text.Label('values', font_name='Times New Roman',
font_size=36, x=window.width//2, y=window.height - 100,
anchor_x='center', anchor_y='center')
def update_arm(dt):
""" Simulate the arm ahead a chunk of time, then find the
(x,y) coordinates of each joint, and update labels."""
# simulate arm forward 15ms
for i in range(1500):
sim.step(out, u)
# find new joint (x,y) coordinates, offset to center of screen-ish
x = np.array([ 0,
L[0]*np.cos(out[1]),
L[0]*np.cos(out[1]) + L[1]*np.cos(out[1]+out[2])]) + window.width/2
y = np.array([ 0,
L[0]*np.sin(out[1]),
L[0]*np.sin(out[1]) + L[1]*np.sin(out[1]+out[2])]) + 100
# update line endpoint positions for drawing
window.jps = np.array([x, y]).astype('int')
label_time.text = 'time: %.3f'%out[0]
label_values.text = '(01, 02) = (%.2f, %.2f)'%(out[1], out[2])
update_arm(0) # call once to set up
@window.event
def on_draw():
window.clear()
label_time.draw()
label_values.draw()
for i in range(2):
pyglet.graphics.draw(2, pyglet.gl.GL_LINES, ('v2i',
(window.jps[0][i], window.jps[1][i],
window.jps[0][i+1], window.jps[1][i+1])))
# set up update_arm function to be called super often
pyglet.clock.schedule_interval(update_arm, 0.00001)
pyglet.app.run()
run()
Alright. In the first part, we’re just doing the same as above when we imported our brand new module and set it up for simulation.
What’s different is that we’ve now pushed the actual simulation into a method called update_arm. The idea is that the drawing is significantly slower than the simulation, so we’ll simulate a bunch, then update the plots once, then repeat, and hopefully get something that simulates at a decent speed. Once we have the joint angles of the arm stored safely in our out variable, we calculate the (x,y) locations of the elbow and wrist and update the system. Then the last thing this method does is update the text of the labels in the window.
In the on_draw method we’ve overwritten, we clear the window, then draw our labels and the lines corresponding to upper and lower arm segments.
Then the last thing is to schedule our update_arm method to be called periodically, in this case in extremely often.
When you run this file, you should see something that looks like:
You’ll have to trust me it looks better when it’s moving, and when it’s moving it will look better when the input isn’t constant.
But there you go! Now you are able to take your very own MapleSim model’s optimized C code, append some C++ code to make a Sim class, wrap it in Cython, and access it as a Python module! Why would anyone ever want to go through this trouble? Well, finding good simulations with appropriate dynamics can be a huge pain in the ass, and sometimes you just want something stand-alone. Also, now that this is done, this exact same code should be good for any MapleSim model, so hopefully the process is relatively straightforward. At least that’s my dream.
The code that I’ve used in this post can be found at my github: 2LinkArm.
Addendum
This was all written for compiling on Ubuntu, my labmate and pal Xuan Choo also went through getting this all to work on Windows too, here are his notes!
Step 1: Get and install cython
Step 2: Follow the instructions here. Essentially:
- 2a: Download and install the windows SDK for your python version
- 2b: Start the windows SDK command shell (start menu->programs->windows sdk->CMD shell)
- 2c: Type these things into the cmd shell:
set DISTUTILS_USE_SDK=1
setenv /x64 /release
- 2d: in the same cmd shell, navigate to the folder with the pyx file you want to compile, and run:
python setup.py build_ext -i
- 2d*: If it complains about undefined numpy references, add “include_dirs=[numpy.get_include()]” to the Extension class call (also import numpy in setup.py).
from distutils.core import setup
from distutils.extension import Extension
from Cython.Distutils import build_ext
import numpy
setup(
cmdclass = {'build_ext': build_ext},
ext_modules=[Extension("py3LinkArm",
sources=["py3LinkArm.pyx"],
language="c++",
include_dirs=[numpy.get_include()]),],
)
Done!
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 
, and wrist angle = 0. We also set up the arm segment lengths to all be 1, and the minimum and maximum joint angles. 
studywolf 