ABR Control repo public release!

https://github.com/abr/abr_control

Last August I started working on a new version of my old control repo with a resolve to make something less hacky, as part of the work for Applied Brain Research, Inc, a startup that I co-founded with others from my lab after most of my cohort graduated. Together with Pawel Jaworski, who comprises other half of ABR’s motor team, we’ve been building up a library of useful functions for modeling, simulating, interfacing, and controlling robot arms.

Today we’re making the repository public, under the same free for non-commercial use that we’ve released our Nengo neural modelling software on. You can find it here: ABR_Control

It’s all Python 3, and here’s an overview of some of the features:

• Automates generation of functions for computing the Jacobians, joint space and task space inertia matrices, centrifugal and Coriolis effects, and Jacobian derivative, provided each link’s inertia matrix and the transformation matrices
• Option to compile these functions to speedy Cython implementations
• Operational space, joint space, floating, and sliding controllers provided with PyGame and VREP example scripts
• Obstacle avoidance and dynamics adaptation add-ons with PyGame example scripts
• Interfaces with VREP
• Configuration files for one, two, and three link arms, as well as the UR5 and Jaco2 arms in VREP
• Provides Python simulations of two and three link arms, with PyGame visualization
• Path planning using first and second order filtering of the target and example scripts.

Structure

The ABR Control library is divided into three sections:

1. Arm descriptions (and simulations)
2. Robotic system interfaces
3. Controllers

The big goal was to make all of these interchangeable, so that to run any permutation of them you just have to change which arm / interface / controller you’re importing.

To support a new arm, the user only needs to create a configuration file specifying the transforms and inertia matrices. Code for calculating the necessary functions of the arm will be symbolically derived using SymPy, and compiled to C using Cython for efficient run-time execution.

Interfaces provide send_forces and send_target_angles functions, to apply torques and put the arm in a target state, as well as a get_feedback function, which returns a dictionary of information about the current state of the arm (joint angles and velocities at a minimum).

Controllers provide a generate function, which take in current system state information and a target, and return a set of joint torques to apply to the robot arm.

VREP example

The easiest way to show it is with some code examples. So, once you’ve cloned and installed the repo, you can open up VREP and the jaco2.ttt model in the abr_control/arms/jaco2 folder, and to control it using an operational space controller you would run the following:

import numpy as np
from abr_control.arms import jaco2 as arm
from abr_control.controllers import OSC
from abr_control.interfaces import VREP

# initialize our robot config for the ur5
robot_config = arm.Config(use_cython=True, hand_attached=True)

# instantiate controller
ctrlr = OSC(robot_config, kp=200, vmax=0.5)

# create our VREP interface
interface = VREP(robot_config, dt=.001)
interface.connect()

target_xyz = np.array([0.2, 0.2, 0.2])
# set the target object's position in VREP
interface.set_xyz(name='target', xyz=target_xyz)

count = 0.0
while count < 1500:  # run for 1.5 simulated seconds
# get joint angle and velocity feedback
feedback = interface.get_feedback()
# calculate the control signal
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target_pos=target_xyz)
# send forces into VREP, step the sim forward
interface.send_forces(u)

count += 1
interface.disconnect()

This is a minimal example of the examples/VREP/reaching.py code. To run it with a different arm, you can just change the from abr_control.arms import as line. The repo comes with the configuration files for the UR5 and a onelink VREP arm model as well.

PyGame example

I’ve also found the PyGame simulations of the 2 and 3 link arms very helpful for quickly testing new controllers and code, as an easy low overhead proof of concept sandbox. To run the threelink arm (which runs in Linux and Windows fine but I’ve heard has issues in Mac OS), with the operational space controller, you can run this script:

import numpy as np
from abr_control.arms import threelink as arm
from abr_control.interfaces import PyGame
from abr_control.controllers import OSC

# initialize our robot config
robot_config = arm.Config(use_cython=True)
# create our arm simulation
arm_sim = arm.ArmSim(robot_config)

# create an operational space controller
ctrlr = OSC(robot_config, kp=300, vmax=100,
use_dJ=False, use_C=True)

def on_click(self, mouse_x, mouse_y):
self.target = self.mouse_x
self.target = self.mouse_y

# create our interface
interface = PyGame(robot_config, arm_sim, dt=.001,
on_click=on_click)
interface.connect()

# create a target
feedback = interface.get_feedback()
target_xyz = robot_config.Tx('EE', feedback['q'])
interface.set_target(target_xyz)

try:
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])

# generate an operational space control signal
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target_pos=target_xyz)

new_target = interface.get_mousexy()
if new_target is not None:
target_xyz[0:2] = new_target
interface.set_target(target_xyz)

# apply the control signal, step the sim forward
interface.send_forces(u)

finally:
# stop and reset the simulation
interface.disconnect()

The extra bits of code just set up a hook so that when you click on the PyGame display somewhere the target moves to that point.

So! Hopefully some people find this useful for their research! It should be as easy to set up as cloning the repo, installing the requirements and running the setup file, and then trying out the examples.

If you find a bug please file an issue! If you find a way to improve it please do so and make a PR! And if you’d like to use anything in the repo commercially, please contact me.

Tagged , , , , ,

Quick calculations with SymPy and Cython

Alrighty! Last time I posted about SymPy we weren’t worrying too much about computation speed. It was worth the time saved by automating the Jacobian, inertia matrix, etc calculations and it didn’t affect simulation speed really all that much. But! When working with a real arm it suddenly becomes critical to have highly efficient calculations.

Turns out that I still don’t want to code those equations by hand. There are probably very nice options for generating optimized code using Mathematica or MapleSim or whatever pay software, but SymPy is free and already Python compatible so my goal is to stick with that.

Options

I did some internet scouring, and looked at installing various packages to help speed things up, including

1. trying out the Sympy simplify function,
2. trying out SymEngine,
3. trying out the Sympy compile to Theano function,
4. trying out the Sympy autowrap function, and
5. various combinations of the above.

The SymEngine backend and Theano functions really didn’t give any improvements for the kind of low-dimensional vector calculations performed for control here. Disclaimer, it could be the case that I gave up on these implementations too quickly, but from some basic testing it didn’t seem like they were the way to go for this kind of problem.

So down to the simplify function and compiling to the Cython backend options. First thing you’ll notice when using the simplify is that the generation time for the function can take orders of magnitude longer (up to 12ish hours for inertia matrices for the Kinova Jaco 2 arm with simplify vs about 1-2 minutes without simplify, for example). And then, as a nice kick in the teeth after that, there’s almost no difference between straight Cython of the non-simplified functions and the simplified functions. Here are some graphs:

So you can see how the addition of the simplify drastically increases the compile time in the top half of the graphs there. In the lower half, you can see that the simplify does save some execution time relative to the baseline straight lambdify function, but once you start compiling to Cython with the autowrap the difference is on the order of hundredths to thousandths of milliseconds.

Results

So! My recommendation is

• Don’t use simplify,
• do use autowrap with the Cython backend.

To compile using the Cython backend:

from sympy.utilities.autowrap import autowrap
function = autowrap(sympy_expression, backend="cython",
args=parameters)

In the last Sympy post I looked at a hard-coded calculation against the Sympy generated function, using the timeit function, which runs a given chunk of code 1,000,000 times and tells you how long it took. To save tracking down the results from that post, here are the timeit results of a Sympy function generated using just the lambdify function vs the hard-coding the function in straight Numpy: And now using the autowrap function to compile to Cython code, compared to hard-coding the function in Numpy: This is a pretty crazy improvement, and means that we can have the best of both worlds. We can declare our transforms in SymPy and automate the calculation of our Jacobians and inertia matrices, and still have the code execute fast enough that we can use it to control real robots.

That said, this is all from my basic experimenting with some different optimisations, which is a far from thorough exploration of the space. If you know of any other ways to speed up execution, please comment below!

You can find the code I used to generate the above plots up on my GitHub.

Operational space control of 6DOF robot arm with spiking cameras part 1: Getting access to the arm through Python

From June 9th to the 19th we ran a summer school (brain camp) in our lab with people from all over to come and learn how to use our neural modeling software, Nengo, and then work on a project with others in the school and people from the lab. We had a bunch of fun hardware available, from neuromorphic hardware from the SpiNNaker group over at Cambridge to lego robots on omni-wheels to spiking cameras (i’ll discuss what a spiking camera is exactly in part 3 of this series) and little robot arms. There were a bunch of awesome projects, people built neural models capable of performing a simplified version of the Wisconsin card sorting task that they then got running on the SpiNNaker boards, a neural controller built to move a robot leech, a spiking neurons reinforcement-learning system implemented on SpiNNaker with spiking cameras to control the lego robot that learned to move towards green LEDs and avoid red LEDs, and a bunch of others. If you’re interested in participating in these kinds of projects and learning more about this I highly suggest you apply to the summer school for next year!

I took the summer school as a chance to break from other projects and hack together a project. The idea was to take the robot arm, implement an operational space controller (i.e. find the Jacobian), and then used spiking cameras to track an LED and have the robot arm learn how to move to the target, no matter where the cameras were placed, by learning the relationship between where the target is in camera-centric coordinates and arm-centric coordinates. This broke down into several steps: 1) Get access to the arm through Python, 2) derive the Jacobian to implement operational space control, 3) sample state space to get an approximation of the camera-centric to arm-centric function, 4) implement the control system to track the target LED.

Here’s a picture of the set-up during step 3, training. So in the center is the 6DOF robot arm with a little LED attached to the head, and highlighted in orange circles are the two spiking cameras, expertly taped to the wall with office-grade scotch tape. You can also see the SpiNNaker board in the top left as a bonus, but I didn’t have enough time to involve it in this project.

I was originally going to write this all up as one post, because the first two parts are re-implementing previous posts, but even skimming through those steps it was getting long and I’m sure no one minds having a few different examples to look through for generating Cython code or deriving a Jacobian. So I’m going to break this into a few parts. Here in this post we’ll work through the first step (Cython) of our journey.

Get access to the arm through Python

The arm we had was the VE026A Denso training robot, on loan from Dr. Bryan Tripp of the neuromorphic robotics lab at UW. Previously an interface had been built up by one of Dr. Tripp’s summer students, written in C. C is great and all but Python is much easier to work with, and the rest of the software I wanted to work you know what I’m done justifying it Python is just great so Python is what I wanted to use. The end.

So to get access to the arm in Python I used the great ol’ C++ wrapper hack described in a previous post. Looking at Murphy-the-summer-student’s C code there were basically three functions I needed access to:

// initialize threads, connect to robot
void start_robot(int *semid, int32_t *ctrlid, int32_t *robotid, float **angles)
// send a set of joint angles to the robot servos
void Robot_Execute_slvMove(int32_t robotid, float j1, float j2, float j3, float j4, float j5, float j6, float j7, float j8)
// kill threads, disconnect from robot
void stop_robot(int semid, int32_t ctrlid, int32_t robotid)

So I changed the extension of the file to ‘.cpp’ (I know, I know, I said this was a hack!), fixed some compiler errors that popped up, and then appended the following to the end of the file:

/* 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 (June, 2015)
*/

class Robot {
/* Very simple class, just stores the variables
* needed for talking to the robot, and a gives access
* to the functions for moving it around */

int semid;
int32_t ctrlid;
int32_t robotid;
float* angles;

public:
Robot();
~Robot();
void connect();
void move(float* angles_d);
void disconnect();
};

Robot::Robot() { }

Robot::~Robot() { free(angles); }

/* Connect to the robot, get the ids and current joint angles */
/* char* usb_port: the name of the port the robot is connected to */
void Robot::connect()
{
start_robot(&amp;semid, &amp;ctrlid, &amp;robotid, &amp;angles);
printf("%i %i %i", robotid, ctrlid, semid);
}

/* Move the robot to the specified angles */
/* float* angles: the target joint angles */
void Robot::move(float* angles)
{
// convert from radians to degrees
Robot_Execute_slvMove(robotid,
angles * 180.0 / 3.14,
angles * 180.0 / 3.14,
angles * 180.0 / 3.14,
angles * 180.0 / 3.14,
angles * 180.0 / 3.14,
angles * 180.0 / 3.14,
angles * 180.0 / 3.14,
angles * 180.0 / 3.14);
}

/* Disconnect from the robot */
void Robot::disconnect()
{
stop_robot(semid, ctrlid, robotid);
}

int main()
{
Robot robot = Robot();
// connect to robot
robot.connect();

// move robot to some random target angles
float target_angles = {0, np.pi / 2.0, 0.0, 0, 0, 0, 0};
robot.move(target_angles);

sleep(1);

// disconnect
robot.disconnect();

return 0;
}

So very simple class. Basically just wanted to create a set of functions to hide some of the unnecessary parameters from Python, do the conversion from radians to degrees (who works in degrees? honestly), and have a short little main function to test the creation of the class, and connection, movement, and disconnection of the robot. Like I said, there were a few compiler errors when switching from C to C++, but really that was the only thing that took any time on this part. A few casts and everything was gravy.

The next part was writing the Cython pyRobot.pyx file (I describe the steps involved in this in more detail in this post):

import numpy as np
cimport numpy as np

cdef extern from "bcap.cpp":
cdef cppclass Robot:
Robot()
void connect()
void move(float* angles)
void disconnect()

cdef class pyRobot:
cdef Robot* thisptr

def __cinit__(self):
self.thisptr = new Robot()

def __dealloc__(self):
del self.thisptr

def connect(self):
"""
Set up the connection to the robot, pass in a vector,
get back the current joint angles of the arm.
param np.ndarray angles: a vector to store current joint angles
"""
self.thisptr.connect()

def move(self, np.ndarray[float, mode="c"] angles):
"""
Step the simulation forward one timestep. Pass in target angles,
get back the current arm joint angles.
param np.ndarray angles: 7D target angle vector
"""
self.thisptr.move(&amp;angles)

def disconnect(self):
"""
Disconnect from the robot.
"""
self.thisptr.disconnect()

the setup.py file:

from distutils.core import setup
from distutils.extension import Extension
from Cython.Distutils import build_ext

setup(
name = 'Demos',
ext_modules=[
Extension("test",
sources=["pyRobot.pyx"],
language="c++"),
],
cmdclass = {'build_ext': build_ext},

)

and then compiling!

run setup.py build_ext -i

With all of this working, a nice test.so file was created, and it was now possible to connect to the robot in Python with

import test
rob = test.pyRobot()
rob.connect()
target_angles = np.array([0, np.pi/2, 0, np.pi/4, 0, 0, 0, 0], dtype='float32')
rob.move(target_angles)
import time
time.sleep(1)
rob.disconnect()

In the above code we’re instantiating the pyRobot class, connecting to the robot, defining a set of target angles and telling the robot to move there, waiting for 1 second to give the robot time to actually move, and then disconnecting from the robot. Upon connection we have to pass in a set of joint angles for the servos, and so you see the robot arm jerk into position, and then move to the target set of joint angles, it looks something exactly like this: Neat, phase 1 complete.

At the end of phase 1 we are able to connect to the robot arm through Python and send commands in terms of joint angles. But we don’t want to send commands in terms of joint angles, we want to just 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 we’ll do by joining us next time! or by remembering what I said we’d do at the beginning of this post.

Tagged , , ,

Wrapping MapleSim C code for Python

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:

/*  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 = 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 = S.w;
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);
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’.

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, &params)
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, &ic)
else: self.thisptr.reset(&out, 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, &u)

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. If we were passing along 2D arrays we would use &var.

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},
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

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

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
# 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*np.cos(out),
L*np.cos(out) + L*np.cos(out+out)]) + window.width/2
y = np.array([ 0,
L*np.sin(out),
L*np.sin(out) + L*np.sin(out+out)]) + 100

# update line endpoint positions for drawing
window.jps = np.array([x, y]).astype('int')
label_time.text = 'time: %.3f'%out
label_values.text = '(01, 02) = (%.2f, %.2f)'%(out, out)

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[i], window.jps[i],
window.jps[i+1], window.jps[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.

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},
language="c++",
include_dirs=[numpy.get_include()]),],
)

Done!

Cython Journey Part 3: Wrapping C code and passing arrays

Basic C interfacing example

OK, so this code is taken straight from an example here, I’m just going to explicitly work through all the steps so that when I try to do this again later I don’t have to look everything up again.

Let’s say we have some c code that we want to work with from Python, in a file called ‘square.c’:

void square(double* array, int n) {
int i;
for (i=0; i<n; ++i) {
array[i] = array[i] * array[i];
}
}

The first thing we’re going to need to do is compile this to a shared object file, so we punch the following into our terminal:

gcc -c square.c # compile the code to an object file
gcc square.o -shared -o square.so # compile to shared object file

And so now we have our square.so file. Super.

Here is the code that we can then use to interact with this shared object file from Python:

import numpy as np
import ctypes

n = 5
a = np.arange(n, dtype=np.double)

aptr = a.ctypes.data_as(ctypes.POINTER(ctypes.c_double))
square.square(aptr, n)

print a

OK, so there are a couple of lines with a lot of jargon but aside from that this looks pretty straightforward. The first noticeable thing is that we’re importing the ctypes library. So, make sure that you have that installed before you try to run this code. Right after importing ctypes then we create a handle into our shared object file, using the ctypes.cdll.LoadLibrary() command.

Then it’s some standard python code to create an array, and then there’s this funky command that lets us get a handle to the array that we just created as a c pointer that we can pass in to our square() function. You can see a list of all the different kinds of data types here.

Once we have that handle to our array, we simply pass it through to the square.square() function, and let it go and do whatever it’s going to do with it. Then we print out our array a, and to our great joy, all of the values have been squared.

So that’s that. Instead of returning anything like double* from the C library I would recommend just passing in the array as a parameter and modifying it instead. This is all nice and straightforward enough, and it seems like the returning business can lead to a lot of memory allocation and ownership headaches.

The code for this example can be found at my github, here: Square.

Tagged , , , ,

Cython Journey Part 2: Including Eigen

As I mentioned in my last Cython Journey post, the reason I’m using Cython is to take my speedy C++ code that does a bunch of matrix operations using the Eigen library, and to make them accessible from Python. Now, I have no desire to make an interface for the Eigen library, all I need is to be able to access the MatrixXf objects from the Eigen library, and convert them into numpy.ndarray objects for manipulation in Python. The logical first step towards this end would then seem to be to successfully compile C++ code that links to the Eigen library.

HOWEVER. After messing around with a bunch of linking problems and the like, I came across code by charanpald that provides an interface for Eigen sparse matrices and sparse matrix operations to Python (https://github.com/charanpald/SpPy). In this code, he links to the Eigen library through a buffer class that extends the Eigen::SparseMatrix class and adds some more functionality. There is very possibly a better way to do this, but I also needed to define some extra functions operating on the Eigen matrices, so defining an extension to the MatrixXf class worked out well. For this example, though, I’ll just use a barebones class extension to keep the size manageable. Here is my class:
cpp_matrixxfpy.h

#ifndef MATRIXXFPY_H
#define MATRIXXFPY_H
#include <Eigen/Dense>
using namespace Eigen;

class MatrixXfPy : public MatrixXf {
public:
MatrixXfPy() : MatrixXf() { }
MatrixXfPy(int rows,int cols) : MatrixXf(rows,cols) { }
MatrixXfPy(const MatrixXf other) : MatrixXf(other) { }
};
#endif

Like, I said, barebones.
With this now we can go to our .pyx file and create a Cython handle into the MatrixXf object.
matrixxfpy.pyx

cdef extern from "cpp_matrixxfpy.h":
cdef cppclass MatrixXfPy:
MatrixXfPy()
MatrixXfPy(int d1, int d2)
MatrixXfPy(MatrixXfPy other)
int rows()
int cols()
float coeff(int, int)

Same as for the cpp_test code that we wanted to interface with Python; first thing we do in the .pyx file is redefine the class and variables / functions that we want access to in Python, along with the constructors. In this simple example we’re going to only access three functions from MatrixXf, rows(), cols(), and coeff(int, int). These functions return the number of rows, the number of columns, and the matrix coefficient value at a given index.

So now that we have a Cython handle on the Eigen code, let’s make this functionality available to Python. HOWEVER. Let’s keep in mind the goal, we’re not here to provide a Python wrapper for MatrixXf objects. What we really want is just to have Eigen and our C++ code over there, somewhere else, doing all of the calculations etc and then for us to be over in Python and just be able to get a numpy.ndarray for Python to play with and plot. Let us set about that now.

Let’s go back to the Test class we defined in Cython Journey Part 1. First thing is first, we need to edit the C++ code to create and return a MatrixXf object. So let’s just throw in a simple function:
cpp_test.h

#ifndef TEST_H
#define TEST_H

#include "cpp_matrixxfpy.h"
using namespace Eigen;

class Test {
public:
int test1;
Test();
Test(int test1);
~Test();
int returnFour();
int returnFive();
Test operator+(const Test& other);
Test operator-(const Test& other);
MatrixXfPy getMatrixXf(int d1, int d2);
};
#endif

and define it in cpp_test.cpp as

#include "cpp_test.h"

Test::Test() {
test1 = 0;
}

Test::Test(int test1) {
this->test1 = test1;
}

Test::~Test() { }

int Test::returnFour() { return 4; }

int Test::returnFive() { return returnFour() + 1; }

Test Test::operator+(const Test& other) {
return Test(test1 += other.test1);
}

Test Test::operator-(const Test& other) {
return Test(test1 -= other.test1);
}

MatrixXfPy Test::getMatrixXf(int d1, int d2) {
MatrixXfPy matrix = (MatrixXf)MatrixXfPy::Ones(d1,d2);
matrix(0,0) = -10.0101003; // some manipulation, to show it carries over
return matrix;
}

I put up the whole class again so it’s all in one place.

Alright, that’s done. Now we can look at our .pyx file for Test. We’re going to need to make the MatrixXfPy class defined just above available to our Test class. To do this is easy, all we need to throw in is:
test.pyx

include "cpp_matrixxfpy.h"

at the top! And while we’re at it, we’re going to be adding some more functionality to test that requires numpy, so also throw in:

import numpy

This additional functionality is going to access the getMatrix(int,int), create an numpy.ndarray object, and fill it out with the values from the matrix. Let’s get about doing that. We need to expose the new C++ function to Cython, so add this line to the cdef cppclass Test declarations:

MatrixXfPy getMatrixXf(int d1, int d2)

And now we can add the code that will be accessible from Python. I’ll post the code and then describe it below (although it’s pretty straightforward):

def getNDArray(self, int d1, int d2):
cdef MatrixXfPy me = self.thisptr.returnMatrixXf(d1,d2) # get MatrixXfPy object

result = numpy.zeros((me.rows(),me.cols())) # create nd array
# Fill out the nd array with MatrixXf elements
for row in range(me.rows()):
for col in range(me.cols()):
result[row, col] = me.coeff(row, col)
return result

I just flat out stripped the form for this from the SpPy project and simplified it, so it’s not going to be optimized for speed in any way, but it gets the job done and it’s very easy to read. First we just set up a numpy.zeros matrix to index into, and then we go through each element in the array and fill it in with the appropriate value by calling to the coeff(int row, int col) function we defined above. Easy!

Now we can use the same setup file from the last Cython post to build our shared library:

from distutils.core import setup
from distutils.extension import Extension
from Cython.Distutils import build_ext

setup(
name = 'Test',
ext_modules=[
Extension("test",
sources=["test.pyx", "cpp_test.cpp"], # Note, you can link against a c++ library instead of including the source
language="c++"),
],
cmdclass = {'build_ext': build_ext},

)

Again reproduced here for hassle-freeness. Now we can compile and test it! At the IPython terminal:

run setup.py build_ext -i
import test
t = test.pyTest(10)
m = t.getNDArray(4,4)

And that’s it! We can see that that last command returns to us a (4x4) matrix with the first element set to -10.0101003, which is exactly what we did over in our C++ files. Now we’re all set to go ahead and generate matrices in C++, and then ship ’em out over to Python for plotting. Excellent.

Tagged , ,

Cython Journey Part 1: The Basics

I’ve recently begun writing up some algorithms is C++ for the speed benefits, as they require an insane amount of trials and overall simulation time, and there is a significant speedup in moving code from Matlab or Python over to C++.

BUT

It’s a large project, and I’d like to

1. Be able to easily plot the results in Python, using matplotlib
2. Be able to “plug and play” C++ parts with Python parts for speed comparison and error checking

So I’ve been looking into Cython, which, after some digging, seems a very viable candidate for building up this interface. I’ve been following the examples and whatnot they have on the Cython page (http://docs.cython.org/src/userguide/wrapping_CPlusPlus.html#a-simple-tutorial), but it tends not to be abundantly clear what the finished version of the files are supposed to look like. So I’ve been writing up my own as I go along based on what their examples and trial and error. You can find the whole set of files up on my GitHub.

I’ve called the project test. I’ve created two files to hold the C++ code, cpp_test.h and cpp_test.cpp, which look like this:
cpp_test.h

#ifndef TEST_H
#define TEST_H

class Test {
public:
int test1;
Test();
Test(int test1);
~Test();
int returnFour();
int returnFive();
Test operator+(const Test& other);
Test operator-(const Test& other);
};
#endif

and cpp_test.cpp

#include "cpp_test.h"

Test::Test() {
test1 = 0;
}

Test::Test(int test1) {
this->test1 = test1;
}

Test::~Test() { }

int Test::returnFour() { return 4; }

int Test::returnFive() { return returnFour() + 1; }

Test Test::operator+(const Test& other) {
return Test(test1 + other.test1);
}

Test Test::operator-(const Test& other) {
return Test(test1 - other.test1);
}

To start, we need a wrapper .pyx file that will specify how we actually interface between Python and the above C++ code. So, we make a new file, and call it “test.pyx”.

• Important! Do not name the .pyx file the same as your .cpp file! When the cythonize function is called later, a new .cpp file with the same name as your .pyx file is generated. If you name both your original .cpp and your .pyx file the same, your .cpp file will be overwritten! Which is bad.

The first thing we need to do in our test.pyx file (which is written in Cython) is redefine all of the variables and functions that we want to use from our C++ code, including the constructors. In this example we’re going to expose the operator functions and returnFive(), which calls returnFour(), but not returnFour() itself. All of our C++ interaction is defined in this .pyx file.
Here is the first part of test.pyx:

cdef extern from "cpp_test.h":
cdef cppclass Test:
Test()
Test(int test1)
int test1
int returnFive()
Test add "operator+"(Test other)
Test sub "operator-"(Test other)

Here we have a redeclaration of everything that we want to use from our C++ code, which is everything except returnFour(). All of the class variables and functions are defined inside cdef cppclass Test. One of the first things that jumps out from looking at this code is that we can rename C++ commands with Python names here, by declaring a name, and then the mapped C++ code inside quotation marks. The above code gives Cython a handle into the C++ code, but we are not done! We need to make this functionality available to regular ol’ Python.

So! Still inside test.pyx, we need to write some more code. I’ll show the code and then walk through it.

cdef class pyTest:
cdef Test* thisptr # hold a C++ instance
def __cinit__(self, int test1):
self.thisptr = new Test(test1)
def __dealloc__(self):
del self.thisptr

def __add__(pyTest left, pyTest other):
cdef Test t = left.thisptr.add(other.thisptr)
cdef pyTest tt = pyTest(t.test1)
return tt
def __sub__(pyTest left, pyTest other):
cdef Test t = left.thisptr.sub(other.thisptr)
cdef pyTest tt = pyTest(t.test1)
return tt

def __repr__(self):
return "pyTest[%s]" % (self.thisptr.test1)

def returnFive(self):
return self.thisptr.returnFive()

def printMe(self):
return "hello world"

Let’s look at what’s going on. We define a new class, and it’s called Test. The first thing we do is then define a pointer to the class defined above in test.pyc, c_Test. This will be how we access the variables and functions of our C++ code. The first method we define is __cinit__, which is the very first thing called when we make an instance of Test, called even before __init__. It’s important to do this here, because of some issues that come up with __init__ itself not being called soon enough for instantiating our pointer, thisptr. To create pointers to the class we simply call new Test(int test1). The next method we define is __dealloc__, which just lets us delete our pointer, which calls the deconstructor of our C++ class and frees memory appropriately, through del self.thisptr.

After these, we create a method to call returnFive(), which we appropriately also title returnFive(). All this does is call upon thisptr to access it’s own returnFive() function, which links to the C++ returnFive() function. Note that the C++ code calls upon returnFour(), which we did not define in c_Test. This just shows that not everything has to be redefined in our .pyx file, only what we want exposed for access from Python.

The next two methods are the Cython equivalent of our operator+ and operator- C++ functions. These functions are a little trickier, because the c_Test functions return pointers to new c_Test instances, because the operator functions in our C++ code in turn returns new instances of the C++ Test class. So what we have to do is create a handle to the c_Test instance returned by the self.thisptr.add and self.thisptr.sub calls, and then create a new instance of the Test class from our .pyx code, and return that.

The __repr__ function is simply an aesthetic feature, such that when the user calls, from Python, for the Test class to print we get a nice printout.

And finally, just to show that this is a Python class we’re defining, we make another method that is strictly superfluous, and unrelated to the C++ code, just to emphasize that additional functionality specific to our Python implementation can be added here.

Now we have 3 files, cpp_test.h, cpp_test.cpp, and test.pyx. We need one more, setup.py. This is file that will compile the C++ and .pyx files together and generate a usable python module. The setup file can be written several different ways, here is template I’ve been using:
setup.py

from distutils.core import setup
from distutils.extension import Extension
from Cython.Distutils import build_ext

setup(
name = 'Demos',
ext_modules=[
Extension("test",
sources=["test.pyx", "cpp_test.cpp"], # Note, you can link against a c++ library instead of including the source
language="c++"),
],
cmdclass = {'build_ext': build_ext},

)

The important thing here is to set the sources parameter to include the relevant .pyx and .cpp files that we just wrote.
And that’s it! Now open up IPython and punch in:

run setup.py build_ext -i

and now we can call up our test module! i.e.

from test Import pyTest
k = pyTest(10)
k.returnFive()
k + k
k - k
k.printMe()

And that’s it! We’ve successfully wrapped a basic C++ class and made it accessible from Python! This is a good first step, we’ll continue the Cython journey in later posts.

One last thing though, if you use IPython, it’s should be noted that if you change any files and recompile with the

run setup.py build_ext -i

you need to exit and then re-enter IPython before the updated code will be recognized. If you don’t know this you can get very frustrated trying to figure out why nothing is changing!

Tagged ,