Arm Navigation using MoveIt! and OpenRAVE

Mobile Pi Table While it is relatively easy to add an arm to a robot, it is much harder to make it do anything useful.  When using our own arms and hands, it seems straightforward to reach for an object in space, even when constrained by nearby obstacles.  (Imagine for example reaching for the butter at the dinner table without spilling the wine glass near your elbow.)  However, it turns out that controlling such movements is a complex mathematical problem when we try to program a robot to perform similar actions.

Fortunately, Willow Garage recently announced a new motion planning framework called MoveIt! which is aimed at simplying the control of robot tasks involving a combination of complex kinematics (e.g. multi-jointed arms), collision checking, grasping and perception.  The software is really quite beautiful to behold and Pi Robot and I have been playing with it for the past few weeks.

MoveIt is sill in alpha release (version 0.4.3-alpha as I write) and I have been doing my small part in helping to debug the various functions.  One of the nice features of MoveIt is that it does not care if it is asked to control a virtual robot or a real one.  So we can save some time by testing things out in simulation before messing about with reality.

The image above shows virtual Pi part way through the task of reaching for a mug on a table while not allowing any part of his arm to collide with either the table or the block to his right.  The video below shows how well MoveIt is able to carry out this action.




In the video above, the grasping of the mug is "scripted" by my code meaning that the MoveIt Python API does not yet compute the actual grasping pose.  Hopefully that part of the package will be completed before long.  However, notice how nicely the MoveIt motion planner accounts for the presence of the table and block and deftly positions Pi's gripper without touching any other surface.


Pi and Balloon For the next test, I wanted to see how quickly MoveIt could recalculate the inverse kinematics of the arm as the hand attempts to track a moving target.  The image to the right shows the one-armed "table top version" of Pi Robot.  This version is a little easier to carry around and set up than Pi's mobile version so it makes a nice alternative test platform.

The following video demonstrates the hand tracking task.  First I programmed a ROS node to move a simulated balloon on a sinusoidal path in front of virtual Pi.  Then I used a second ROS node to move Pi's head to track the balloon just as he has done countless times in the past.

The real story is the motion of Pi's arm which attempts to track the balloon by placing the palm of his hand more-or-less behind the center of the balloon.  (In the virtual world, I have allowed the hand to pass inside the balloon.)  The reaching trajectory is recomputed twice every second using a custom IKFast solver made possible by OpenRAVE.   More about OpenRAVE after the video.






As you can see from the video, MoveIt and OpenRAVE do a fairly good job of moving the hand to track the balloon.  It is not as smooth as the head tracking and there are clearly pauses while the trajectory is updated.  However, I have also made no attempt to optimize the tracking process or model the motion of the balloon to anticipate its next position so the result is not bad for a first attempt.

Now back to OpenRAVE.  OpenRAVE (Open Robotics Automation Virtual Environment) is an open source project created and maintained by Rosen Diankov for computing analytic solutions for robot motion planning problems.  By "analytic" we mean that a solution to a given problem--for example, the inverse kinematics of a multi-jointed arm--can be computed directly rather than numerically.  Numerical methods typically rely on random sampling of the solution space until a solution is found that satisfies the current constraints.  This can be quite time consuming and is too slow to be used in the hand tracking task demonstrated above.  MoveIt uses a numerical solver by default, so before bringing up the MoveIt framework, an analytic solution to Pi's arm kinematics was computed using OpenRAVE thanks to a number of helper scripts by Dave Coleman and colleagues. This analytic solver can be plugged into MoveIt for computing arm trajectories.  The result is that we can do relatively nice hand tracking in real time as we saw above.

Our next challenge will be to implement MoveIt and OpenRAVE on the real Pi Robot rather than using a simulation.  So stay tuned for future updates.