28 Mar 2013

REPL for a robot

As a new employee of ZenRobotics, I wanted to share some first-hand experiences of controlling an industrial robot with a REPL.

One nice thing about programming in Lisp is the REPL, i.e. the interactive loop in which you can enter code and get immediate feedback about what the code does. One nice thing about programming with robots is that you get very physical feedback about what your code does. Naturally, in a Lisp shop that
programs robotic systems you want to combine these.

Every now and then one gets the opportunity to connect to our recycling robot with a REPL. The other day I got to do this with a ZenRobotics Recycler Heavy Picker. This was the first time I actually interacted with the robot, and it was such an interesting experience to sit in the recycler control room with blinkenlichten around, that I wanted to share some examples. Below I have three examples showing the Clojure REPL and for each, a video of the robot. There are many reasons why ZenRobotics is a cool place to work, and the robot REPL is certainly one of these!

Some simple movements

First, some simple movements. Here's a video, and the REPL input and output is below.

;; First, a couple of small movements to the right and left. The first three items in the parameter 
;; vector are x y z in meters. The x axis points towards the camera, the y axis to the right, and z is up.
repl=> (.moveToAxisPtpAsync *io* (.tryIk *io* [0 0.3 0 1 0 0 0]))
repl=> (.moveToAxisPtpAsync *io* (.tryIk *io* [0 0 0 1 0 0 0]))

;; Then a few moves in a sequence. This is much less verbose with a helper function.
repl=> (defn move-xyz [x y z] (.moveToAxisPtpAsync *io* (.tryIk *io* [x y z 1 0 0 0])))
repl=> (doseq [y [-0.5 0.5 0 0.2 -0.2]] (move-xyz 0 y 0))
;; We use quaternions to represent rotations:
;; http://en.wikipedia.org/wiki/Quaternion
;; Euler angles are probably more widely used for this
;; purpose but quaternions are much nicer because they
;; avoid the singularities and they are easier to compose.
;; But quaternions may not be as intuitive for simple
;; movements. I used a handy (deprecated) function to convert 
;; Euler angles to quaternions. The first parameter to this 
;; function is xyz, and  the the first item in the second 
;; parameter is roughly the robot wrist rotation angle in 
;; radians.

repl=> (transform-from-xyz-abc-deprecated [0 0 0] [0 0 0])
[0.0 0.0 0.0 1.0 0.0 0.0 0.0]
repl=> (transform-from-xyz-abc-deprecated [0 0 0] [0.4 0 0])
[0.0 0.0 0.0 0.9800665778412416 0.0 0.0 0.19866933079506122]
repl=> (.moveToAxisPtpAsync *io* (.tryIk *io* (transform-from-xyz-abc-deprecated [0 0 0] [0.4 0 0])))

;; Try out the gripper jaw with the close-jaw and open-jaw functions.
repl=> (use 'com.zenrobotics.control.jaw)
repl=> (close-jaw *io*)
repl=> (open-jaw *io*)

Picking and throwing some waste with the REPL

Then to get some action I actually picked up a piece of waste wood and threw it in the wood bin.

;; For these movements I had to set the position and angle, so I defined a new helper
;; function which also has a rotation angle parameter.
repl=> (defn move-xyza [x y z a] (.moveToAxisPtpAsync *io* (.tryIk *io* (transform-from-xyz-abc-deprecated [x y z] [a 0 0]))))

;; ...
;; Then I did quite a bit of boring moving around to find the coordinates
;; of the long piece of wood. After finding suitable coordinates I ran the
;; commands below in a sequence to show the picking and throwing action.

;; Move to a position above the target piece.

repl=> (move-xyza 0.5 0.4 0 0.75)
;; Go down.
repl=> (move-xyza 0.5 0.4 -0.36 0.75)
;; Close the gripper jaw.
repl=> (close-jaw *io*)
;; Move up a bit.
repl=> (move-xyza 0.5 0.4 -0.25 0.75)
;; Throw it in the wood bin. The throwing would be hard to get right with the 
;; simple movement commands, so I threw it with a test function.
repl=> (test-throw-trajectory-from-acc *io* :wood-bin 0.4 0.7)

Another throw

The little pile of waste looks interesting, so for another example, I tried to pick up some wood out of
the pile in a more random fashion.

repl=> (move-xyza 0.1 0.35 -0.25 0)
repl=> (close-jaw *io*)
repl=> (move-xyza 0.1 0.35 0 0)
repl=> (test-throw-trajectory-from-acc *io* :wood-bin 0.4 0.7)

1 comment:

  1. Really, really cool. Rubbing salt in my ZenRobotics-rejection-wounds, but still, really cool ;)