## 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[0] = self.mouse_x
self.target[1] = 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 , , , , ,

## Arm visualization with PyGame

So, in my neverending quest for better arm visualizations so I can make prettier pictures / videos and improve my submission acceptance rates I’ve started looking at PyGame. I started out hoping to find something that was quick and easy in Python for animating using SVG images, and PyGame is as close as I got. All in all, I’m decently happy with the result. It runs slower than the Matplotlib animation, and you can’t zoom in like you can on a Matplotlib graph, but it’s prettier. So, tradeoffs!

There were a few things that I ran into that tripped me up a bit when I was doing this implementation, so I thought that I would share, and that’s what this post is going to be about. Below is an animation showing what the arm visualization looks like, and as always the code for everything can be found on my Github. That links you to the control folder, and you can find all of the code developed / used for this post in this folder up on my GitHub.

Alright, there were a few parts in doing this that were a bit tricky. Let’s go through them one at a time.

Rotations in PyGame

Turns out rotating images is a pain right off the bat, but once you get over the initial hurdles it looks pretty slick.

To start we’re just going to attempt to rotate an image in place, here’s a first pass:

```import pygame
import pygame.locals

white = (255, 255, 255)

pygame.init()

display = pygame.display.set_mode((300, 300))
fpsClock = pygame.time.Clock()

while 1:

display.fill(white)

image = pygame.transform.rotate(image, 1)
rect = image.get_rect()

display.blit(image, rect)

# check for quit
for event in pygame.event.get():
if event.type == pygame.locals.QUIT:
pygame.quit()
sys.exit()

pygame.display.update()
fpsClock.tick(30)
```

First thing you notice upon running this is that the image flies off to the side very quickly, as shown below:

This is because we need to recenter the image after each rotation. To do that we can add in this after line 21:

```    rect.center = (150, 150)
```

and so now our animation looks like:

At which point a very egregious problem becomes clear: the image is destroying itself as it rotates.

Transforming from a base image

Basically what’s happening is that every transformation the image gets distorted a little bit, and continues moving farther and farther away from the original. To prevent this we’re going save the original and also track the total degrees to rotate the image. Then we’ll perform one rotation (with the minimal distortion caused by one transformation) and plot that every time step. To do this we’ll introduce a `degrees` variable to track the total rotations. The changes to the code start on line 12:

```radians = 0

while 1:

display.fill(white)

rotated_image = pygame.transform.rotate(image, radians)
rect = rotated_image.get_rect()
rect.center = (150, 150)
```

And the resulting animation looks like:

Which is significantly better! Pretty good, in fact. Looking close, however, you can notice that it gets a little choppy. And this is because the `pygame.transform.rotate` method doesn’t use anti-aliasing to smooth out the image. `pygame.transformation.rotozoom` does, however! So we’ll use `rotozoom` and set the zoom level to 1, changing line 20:

```    rotated = pygame.transform.rotozoom(image, degrees, 1)
```

And now we have a nice smooth animation!

At this point, we’re going to create a class to take care of this rotation business for us, storing the original image and providing a function that rotates smoothly and recenters the image.

```import numpy as np
import pygame

class ArmPart:
"""
A class for storing relevant arm segment information.
"""
def __init__(self, pic, scale=1.0):
# some handy constants
self.length = self.base.get_rect()[2]
self.scale = self.length * scale
self.offset = self.scale / 2.0

self.rotation = 0.0 # in radians

def rotate(self, rotation):
"""
Rotates and re-centers the arm segment.
"""
self.rotation += rotation
# rotate our image
image = pygame.transform.rotozoom(self.base, np.degrees(self.rotation), 1)
# reset the center
rect = image.get_rect()
rect.center = (0, 0)

return image, rect
```

Everything is just what we’ve seen so far, except the `offset`, which is going to be very useful for the trig we’re about to get into.

Trig

With rotations working and going smoothly one of the major hurdles is now behind us. At this point we can get our arm images and use some trig to make sure that they rotate around the joint ends rather than in the center. Using the above class now we’ll write a script that loads in a picture of an upper arm, and then rotates it around the shoulder.

```import numpy as np
import pygame
import pygame.locals

from armpart import ArmPart

black = (0, 0, 0)
white = (255, 255, 255)

pygame.init()

width = 500
height = 500
display = pygame.display.set_mode((width, height))
fpsClock = pygame.time.Clock()

upperarm = ArmPart('upperarm.png', scale=.7)

base = (width / 2, height / 2)

while 1:

display.fill(white)

ua_image, ua_rect = upperarm.rotate(.01)
ua_rect.center += np.asarray(base)
ua_rect.center -= np.array([np.cos(upperarm.rotation) * upperarm.offset,
-np.sin(upperarm.rotation) * upperarm.offset])

display.blit(ua_image, ua_rect)
pygame.draw.circle(display, black, base, 30)

# check for quit
for event in pygame.event.get():
if event.type == pygame.locals.QUIT:
pygame.quit()
sys.exit()

pygame.display.update()
fpsClock.tick(30)
```

So far the only trig we need is simply calculating the offset from the center point. Here it’s calculated as half of the length of the image multiplied by a scaling term. The scaling term is because we don’t want the rotation point to be the absolute edge of the image but rather we want it to be somewhere inside the arm. We calculate the `x` and `y` offset values using the `cos` and `sin` functions, respectively, with a negative sign in from of the `sin` function because the `y` axis is inverted, using the top of the screen as 0. The black circle is just for ease of viewing while we’re figuring all the trig out, to make it easier to verify it’s rotating around the point we want. The above code results in the following

Going ahead and adding in the other links gives us

```import numpy as np
import pygame
import pygame.locals

from armpart import ArmPart

black = (0, 0, 0)
white = (255, 255, 255)

pygame.init()

width = 750
height = 750
display = pygame.display.set_mode((width, height))
fpsClock = pygame.time.Clock()

upperarm = ArmPart('upperarm.png', scale=.7)
forearm = ArmPart('forearm.png', scale=.8)
hand = ArmPart('hand.png', scale=1.0)

origin = (width / 2, height / 2)

while 1:

display.fill(white)

ua_image, ua_rect = upperarm.rotate(.03)
fa_image, fa_rect = forearm.rotate(-.02)
h_image, h_rect = hand.rotate(.03)

# generate (x,y) positions of each of the joints
joints_x = np.cumsum([0,
upperarm.scale * np.cos(upperarm.rotation),
forearm.scale * np.cos(forearm.rotation),
hand.length * np.cos(hand.rotation)]) + origin[0]
joints_y = np.cumsum([0,
upperarm.scale * np.sin(upperarm.rotation),
forearm.scale * np.sin(forearm.rotation),
hand.length * np.sin(hand.rotation)]) * -1 + origin[1]
joints = [(int(x), int(y)) for x,y in zip(joints_x, joints_y)]

def transform(rect, origin, arm_part):
rect.center += np.asarray(origin)
rect.center += np.array([np.cos(arm_part.rotation) * arm_part.offset,
-np.sin(arm_part.rotation) * arm_part.offset])

transform(ua_rect, joints[0], upperarm)
transform(fa_rect, joints[1], forearm)
transform(h_rect, joints[2], hand)
# transform the hand a bit more because it's weird
h_rect.center += np.array([np.cos(hand.rotation),
-np.sin(hand.rotation)]) * -10

display.blit(ua_image, ua_rect)
display.blit(fa_image, fa_rect)
display.blit(h_image, h_rect)

# check for quit
for event in pygame.event.get():
if event.type == pygame.locals.QUIT:
pygame.quit()
sys.exit()

pygame.display.update()
fpsClock.tick(30)
```

So basically the only thing we’ve done here that was a little more complicated was setting up the centers of the forearm and hand images to be at the end of the upper arm and forearm, respectively. We do that in the `joints_x` and `joints_y` variables, and the trig for that is straight from basic robotics.
The above code results in the following animation (which is a little choppy because I dropped some frames to keep the file size small):

Plotting transparent lines

The other kind of of tricky thing that was done in the visualization code was the plotting of transparent lines of the ‘skeleton’ of the arm. The reason that this was kind of tricky was because you can’t just use the `pygame.draw.lines` method, because it doesn’t allow for transparency. So what you end up having to do instead is generate a set of `Surfaces` of the shape that you want for each of the lines, and then transform and plot them appropriately, which is thankfully pretty straightforward after working things out for the images above.

To generate a transparent surface you use the `pygame.SRCALPHA` variable, so it looks like

```surface = pygame.Surface((width, height), pygame.SRCALPHA, 32)
surface.fill((100, 100, 100, 200))
```

where the variable passed in to `fill` is a 4-tuple, with the first 3 parameters the standard RGB levels, and the fourth parameter being the transparency level.

Then there’s one more snag, in that when you `blit` a surface it goes by the top left position of the rectangle that contains it. So doing the same transformations for the images we then have to transform the surface further because with the images we were specifying the desired center point. This is easy enough; just get a handle to the `rect` for the surface and subtract out half of the width and height (post-rotation).

The code with the transparent lines (and some circles at the joints added in for prettiness) then is

```import numpy as np
import pygame
import pygame.locals

from armpart import ArmPart

black = (0, 0, 0)
white = (255, 255, 255)
arm_color = (50, 50, 50, 200) # fourth value specifies transparency

pygame.init()

width = 750
height = 750
display = pygame.display.set_mode((width, height))
fpsClock = pygame.time.Clock()

upperarm = ArmPart('upperarm.png', scale=.7)
forearm = ArmPart('forearm.png', scale=.8)
hand = ArmPart('hand.png', scale=1.0)

line_width = 15
line_upperarm = pygame.Surface((upperarm.scale, line_width), pygame.SRCALPHA, 32)
line_forearm = pygame.Surface((forearm.scale, line_width), pygame.SRCALPHA, 32)
line_hand = pygame.Surface((hand.scale, line_width), pygame.SRCALPHA, 32)

line_upperarm.fill(arm_color)
line_forearm.fill(arm_color)
line_hand.fill(arm_color)

origin = (width / 2, height / 2)

while 1:

display.fill(white)

# rotate our joints
ua_image, ua_rect = upperarm.rotate(.03)
fa_image, fa_rect = forearm.rotate(-.02)
h_image, h_rect = hand.rotate(.03)

# generate (x,y) positions of each of the joints
joints_x = np.cumsum([0,
upperarm.scale * np.cos(upperarm.rotation),
forearm.scale * np.cos(forearm.rotation),
hand.length * np.cos(hand.rotation)]) + origin[0]
joints_y = np.cumsum([0,
upperarm.scale * np.sin(upperarm.rotation),
forearm.scale * np.sin(forearm.rotation),
hand.length * np.sin(hand.rotation)]) * -1 + origin[1]
joints = [(int(x), int(y)) for x,y in zip(joints_x, joints_y)]

def transform(rect, base, arm_part):
rect.center += np.asarray(base)
rect.center += np.array([np.cos(arm_part.rotation) * arm_part.offset,
-np.sin(arm_part.rotation) * arm_part.offset])

transform(ua_rect, joints[0], upperarm)
transform(fa_rect, joints[1], forearm)
transform(h_rect, joints[2], hand)
# transform the hand a bit more because it's weird
h_rect.center += np.array([np.cos(hand.rotation),
-np.sin(hand.rotation)]) * -10

display.blit(ua_image, ua_rect)
display.blit(fa_image, fa_rect)
display.blit(h_image, h_rect)

# rotate arm lines
line_ua = pygame.transform.rotozoom(line_upperarm,
np.degrees(upperarm.rotation), 1)
line_fa = pygame.transform.rotozoom(line_forearm,
np.degrees(forearm.rotation), 1)
line_h = pygame.transform.rotozoom(line_hand,
np.degrees(hand.rotation), 1)
# translate arm lines
lua_rect = line_ua.get_rect()
transform(lua_rect, joints[0], upperarm)
lua_rect.center += np.array([-lua_rect.width / 2.0, -lua_rect.height / 2.0])

lfa_rect = line_fa.get_rect()
transform(lfa_rect, joints[1], forearm)
lfa_rect.center += np.array([-lfa_rect.width / 2.0, -lfa_rect.height / 2.0])

lh_rect = line_h.get_rect()
transform(lh_rect, joints[2], hand)
lh_rect.center += np.array([-lh_rect.width / 2.0, -lh_rect.height / 2.0])

display.blit(line_ua, lua_rect)
display.blit(line_fa, lfa_rect)
display.blit(line_h, lh_rect)

# draw circles at joints for pretty
pygame.draw.circle(display, black, joints[0], 30)
pygame.draw.circle(display, arm_color, joints[0], 12)
pygame.draw.circle(display, black, joints[1], 20)
pygame.draw.circle(display, arm_color, joints[1], 7)
pygame.draw.circle(display, black, joints[2], 15)
pygame.draw.circle(display, arm_color, joints[2], 5)

# check for quit
for event in pygame.event.get():
if event.type == pygame.locals.QUIT:
pygame.quit()
sys.exit()

pygame.display.update()
fpsClock.tick(30)
```

And the resulting arm visualization now looks like this!

From here you can then `blit` on an image in the background, and the little zoom in I have in the top left is just another surface, it’s all pretty straight forward. And that’s pretty much everything! Hopefully this helps someone trying to get some fancier PyGame things going, and if you have any means of generating fancier PyGame animations yourself please drop any tips below in the comments!

Again, the code from this post is all up on my GitHub.