Robotic grasping and manipulation of objects

András Fekete
University of New Hampshire

Abstract— The problem of vision and manipulation is a pieces to move where. This in turn becomes the high level
topic of much research. In this paper, the problem of setting planner for the system and OMPL is used as a low-level
the table is discussed. This relates to many other problems motion planner for each action.
such as vehicle assembly, house cleanup, warehouse In Figure 1 the workspace for the arm can be seen. The
management and restocking. The base problem is that there
are a set of objects that all must reach their destination
arm is able to reach to the edge of the base platform in a
location. The solution to this has three pieces: vision,
manipulation and planning. Vision is necessary for object
recognition. Manipulation is used for getting the objects to
their goal state. Lastly, planning is what determines the order
of operations. In this paper, we concentrate on the
manipulation of the objects. Combining ROS, MoveIt, and
OMPL, a new robotic manipulator is presented which can pick
and place objects without colliding with other objects on the
table. We use Rviz to visualize the workspace and show the
robot in motion while the hardware arm is executing the
planned motion.
I. INTRODUCTION
Setting the table is a complicated planning hierarchy.
First, the location of the objects must be determined. This
involves vision and image recognition. The goal states must
also be recognized based on the workspace. It may be that
there is a pre-configured set of goal states, but those might
become invalid if the workspace doesn't adhere to all the
previous assumptions.
Setting of the table is also just an example in this
problem domain. Similar methodologies can be applied to
manufacturing robotics, courier planning, and home
cleaning robots. The base goals are similar, there are a set of
objects that need to get to their respective places. A vision
system will be necessary to help plan the route.
In this research, Cyton Gamma 300 7-DOF arm was used
with the Robotic Operating System (ROS). ROS is rapidly
becoming the de-facto standard when working with robotic
implementations. Within ROS using the MoveIt library,
Open Motion Planning Library (OMPL) was leveraged as
the library which could do path planning for the arm's Figure 1: Home orientation of the Cyton Gamma 300 arm
motion. OMPL is widely used in the robotics community for half-sphere, but cannot reach the corners. This is the precise
solving path problems with various constraints. In this reason why the 4 corner markers are placed in the corners,
particular research, the constraint was that when the arm because the likelihood of them being occluded is very small.
was moving, it should not allow joints to be below a certain The placement of the arm in the center of the baseboard
height so as to avoid moving or knocking over the pieces on was done to be able to fully utilize the reach of the arm.
the board. In this paper we present a solution where the Since the arm can reach 360º it makes the center the
Cyton Gamma 300 and its derivatives can be easily optimal location.
controlled with the MoveIt library in ROS. The objects in the environment (shown in Figure 2) were
For object recognition, we use object tagging and a simple coasters with the tag in the middle and a small pin
PrimeSense vision system. It is placed above the workspace that could be grasped by the gripper. For the purposes of
and produces a RGBD point cloud. The depth is used to this research, this object was sufficient to test out the arm's
detect foreign objects such as human interaction with the gripping and transporting ability.
tagged objects. The RGBD data is used for the tag The details of getting the hardware and simulation arm
recognition with the ALVAR 3D tracking library. working in ROS is described in detail in the Appendix.
The vision and manipulation components are tied
together by a task planner which instructs the arm which
 expansions from the optimal path.
RRT-Connect expands upon the basic RRT algorithm by
growing the RRT from both the start and the goal states in
addition to using a greedy heuristic that attempts to move
the plan over a longer distance. This algorithm is
significantly faster than its simple RRT equivalent.
Examination of a research problem similar to the one
discussed in this paper can be found in [8] using a mobile
robot with two arms detecting objects in a workspace. Their
approach used reverse kinematics provided by the ActinSE
library to achieve grasping of objects by the arm. One of the
problems mentioned with the ActinSE library is that if a
perfect solution is not found, a semi random pose is
returned by the library. The paper presents an algorithm to
detect when such a pose has been returned.
Others have also experimented with a similar problem in
[9], where the vision system separated the scene into objects
Figure 2: Simple tagged object based on the curvature that existed in the image. Then the
robot would grip the objects in a predefined pose.
II.PREVIOUS WORK
Looking at a completely different way to solve the
Grasping and manipulation problems are not new and problem using a demonstration based approach like the one
has been a problem even as early as 1988 in [1]. Problems shown in [10] would be tedious to implement due to the
involving hierarchical task planning has been studied before amount of training required. This research discusses
like in [2] where there is a switch that must be actuated by a training a robot to perform a variety of tasks and using
manipulator robot, but it is blocked by a box that a larger vision it learns the task. Once it has learned the task, it can
forklift robot must move first. then reproduce the actions it has been taught.
The techniques that are used are varied over time. In [3], On the concept of collision avoidance with a robotic arm,
a system is introduced where the goal completion is split up the research discussed in [11] shows how an adaptive time-
into smaller sub spaces which are either grasping locations stepping with exponential backoff algorithm can create a
or placement locations. They demonstrate a robot arm plan around an object that is directly in the desired path.
planning and executing a plan which removes a rod from
below an obstruction where the arm cannot reach. These sub III. PROPOSED APPROACH
spaces are then connected in a chain that achieves the goal
with the least cost and highest probability. The objective of the arm is to pick up and deliver objects
Two traditional approaches to planning have been A* from one location to another without bumping or knocking
[4] and Rapidly-Exploring Random Trees (RRT) [5]. A* is other pieces over. It must also avoid collision with a human
an expansion on Dijkstra's algorithm for exploring a map to hand at all cost.
find the least cost path from a start state to a goal state In this research, we take a 7-DOF robotic arm that has a
using a set of heuristics to determine which nodes to simple subset of tasks without any need for training. It can
expand. RRTs explore a high dimensional space by transit or transfer between two locations as well as grasp
randomly picking points on the map and planning a path to and un-grasp. These actions are placed in two individual
that point, then picking another random point to plan a path groups: arm and gripper. The reason for this is that transit
to that new point from the closest of the already existing and transfer operations affect the arm's links and positions
points. whereas the graps and un-grasp are actions that only affect
Extensions on both of these algorithms have added to the the gripper. This makes planning much simpler and reduces
speed and efficiency of the base algorithm. One such the problem space complexity.
example is ARA*[6] where not only do the heuristics have In terms of the larger system scope, the arm will listen to
weights attached to them, but the so-called suboptimality locations that are sent by the planning system in the
factor is varying based on the status of the search. This coordinate frame of the arm. The locations would contain
algorithm is also anytime because once a path has been the pickup and dropoff coordinates. The manipulator would
found, each successive iteration will make the path closer to then need to calculate two different paths: one from the
optimal. current pose to the pickup location and one from the pickup
A version of the algorithm where there is a bias applied location to the dropoff location.
toward the optimal path is also explored. This f-biasing The vision system would calibrate the world based on the
[7] uses RRT and RRT* to greatly reduce the time to four tags at the four corners of the base platform. This then
calculate a plan as well as to come up with a lower cost would allow for a transformation to the arm's base
solution. The uniqueness of this algorithm comes from the coordinates. Using the arm's modeling environment, the
fact that it takes nodes from regions that have a lower f arm could then move the end effector based on the
value. This provides a more guided tree that makes fewer transforms of each link back to the base. Thus, messages
that are sent from the camera are all based in the base Universal Robot Definition File (URDF) which the MoveIt
platform's coordinate system since that is what is common library uses to generate a robot model is still necessary to
between all the subsystems. make sure that the hardware precisely matches the model. It
is also possible to connect the point cloud information from
IV. EXPERIMENTS the PrimeSense camera to the MoveIt interface under a
The arm is capable of being modeled in a virtual single launch file. This would create a tighter coupling
environment where pose calculations can be simulated. In between the vision, manipulation and planning pieces.
addition, there is a hardware controller that can perform the Simulation would show what the real hardware
same kind of operations as can be done in simulation. environment is believed to look like which would give more
Using ROS's MoveIt library, we can leverage previous confidence in the model's correctness.
work on path planning implementations. In particular,
support for RRT-Connect exists which is the algorithm VI. CONCLUSION
selected to execute the path planning. RRTk was also Further research needs to be done on what model the
considered, however RRT-Connect took 53% less time in gripper motor is and what its communication protocol is.
execution and 100 times smaller variance in our 10 sample Presumably it is another dynamixel motor that has yet been
experiment. It is shown in Figure 3 that planning time for implemented in the dynamixel_motors driver library.
RRT-Connect on average took 13ms with a maximum of Events such as immediate halt due to a human arm being
17ms, so an update rate of up to 50Hz can reliably be in the path or the vicinity of the arm are handled by adding
achieved. RRTk had an average time of 25ms and a in virtual obstacles and causing the arm to replan its
maximum of 52ms. For each plan, there was a motion. Alternatively, another approach would be to put
simplification task that was also recorded. This task took on down the object in the closest available spot and ask the
average 2ms for both planners, however the time to high level planner what should be the next object that can
complete it also varied based on the result of the specific be moved.
RRT implementation.
VII. APPENDIX
RRT Connect vs RRTk A. Things to know about the Cyton Gamma 300
0.06
First thing to know is that you'll need a 12V battery.
0.05 Supposedly it works off of a 12VDC wall wart power supply,
0.04 but during experimentations it seems to have given
RRT Connect
0.03 RRT Connect Total problems. The suspicion is that a ground loop is created
Time

RRTk through the USB (though it's supposed to be protected

0.02 RRTk Total
against that). According to Robai's documentation, a 2A
0.01 supply is sufficient, but when looking at the Dynamixel
0 motor's datasheet, each motor can use up to 1.5A. In
1 2 3 4 5 6 7 8 9 10
practice, it is easy to overcurrent the 2A supply at which
Sample
point the supply collapses in to current limit operation and
Figure 3: Path planning times resets the motors.
Secondly, the arm's internal hardware is very simple. It is
V. DISCUSSION made up of 7 Dynamixel MX-28 motors and a Dynamixel
In doing this research over again, the first step should MX-64 motor. These motors are daisy chained together on a
have been to throw away the Cyton Gamma programs and communication bus which goes into a little black box in the
work directly with the dynamixel motor drivers. The base of the arm. Inside the box there is nothing more than a
dynamixel drivers are much more robust and allow for couple decoupling capacitors and a plug for the 12V input
faster implementation of algorithms. The main reason is supply. According to the manufacturer, the motors can
due to the deadlocks that exist in the Cyton Gamma handle up to 16.8V. That has not been tested, nor is it
programs. necessary. There is also a minimum operating voltage of 9V
ROS in general is a great tool for bringing together all so care must be taken to not drain the battery below this
the efforts in robotics under a single platform. The trouble voltage. A 4AH battery was used, and needed to be charged
with such a tool is that each entity implements their piece in roughly after 30 minutes of use.
the fashion they desire and the system as a whole becomes a From the little black box in the base of the arm, there is a
connected web of interfaces with little to no documentation 2-wire cable going to the USB2DYNAMIXEL converter (its
nor maintainers. The learning curve is very steep and much the white USB dongle). The two wires are ground and data.
of the development revolves around determining which Lastly, if the arm stops responding, it may very simply be
node causes the whole system to collapse. that you'll need to power-cycle the arm (and possibly the
In further research, the recommendation is to continue USB2DYNAMIXEL). Using the Cyton software, this has
using the dynamixel motor driver as well as the MoveIt been noted on several occasions, whereas with the low level
interface. Fine tuning of the xacro file which generates the dynamixel solution, there hasn't been a need. Both of these
solutions are discussed in the next sections. 1) cyton.ecz
This is a gzip compressed file containing a giant XML
B. Cyton Software
file for the model of the arm. Following this forum
All the smarts for the arm are in the software that (https://groups.google.com/forum/#!msg/cyton-
controls it. Cyton's software is very fragile. There are two robots/GLcqKvljFRg/8-z3CTu9iXMJ) it is possible to get
aspects to the software: the low-level drivers and the the DAE file extracted using the estazione_pnp.py script
interface to ROS using pieces of the low level drivers. The that is mentioned.
best bet is to avoid using this software. If that is not an 2) cytonConfig.xml
option, this section explains how to get it working with If such a file doesn't exist, then one must be created with
ROS. "cytonCreateConfig --gamma300". It is possible to then
1) Low level drivers tweak settings like the default port to look for the arm on.
To get these working, the first step is to get the It'll be at the end of the file under the 'port' tag.
cytonGamma300ViewerSetup-4.0.20130130-ubuntu-12.04- 3) guide_frame.txt
amd64.sh script. It essentially extracts its self and contains This file is an example path file used by the
the proprietary Cyton drivers and ActinSE. It also gives guide_frame_node. The CYTON_EE_FILE environment
some example code for getting communication up with the variable must be set up to point to the absolute path of this
arm. One of these was modified to become a super-simple file.
example that makes the arm go to its 'home' position which
is straight up. This example is called: cytonAndras.cpp. The D.Using low-level dynamixel drivers
simplest way to compile it is to add it into the Robai/Cyton\ It is possible to make the arm work in ROS without the
Gamma\ 300\ Viewer_4.0.20130130/cyton/examples folder use of the Cyton programs. At the time of this writing, the
and modify the Makefile in the parent folder. only component that is non functioning is the gripper
Once the drivers have been compiled, a test should be run motor. The rest of the motors, since they're all dynamixel
with the cytonAndras executable first. On the internet there motors, can be controlled by the library provided by the
are several references to cytonViewer and how it's an following github repository:
amazing tool for testing this arm, but in this experience it is https://github.com/arebgun/dynamixel_motor.git. In order
a buggy program that sometimes loads but mostly gets stuck to use this package, it must be configured. The first step is
in an infinite loop on startup if the arm is connected. When to launch the controller_manager node which will first
using an Ubuntu system, the current user will need to be attempt to communicate with the motors connected to the
added to the 'dialout' group so that when the USB device is USB device. Once the communication is set up, a
plugged in, sufficient permissions to the /dev/ttyUSB0 controller.yaml file needs to be created which describes the
device will be granted. Otherwise a "sudo chmod ugo+rw joint names and constraints of all the motors in the
/dev/ttyUSB0" will be necessary each time the USB device hardware. The motor IDs need to be set properly to match
is plugged into the computer. the joint names that are being published. In the case of the
2) ROS Interface Cyton arm, the IDs were in sequential order from 0 to 7
The interface files are available at the following git starting with the base_joint going until the wrist.
repository: https://code.google.com/p/cyton-ros-pkg/. There Another launch file is created for the hardware controller
is a setup video that is available, but it's poorly done, and it that spawns a controller for the position manager and the
is a lot easier if the install process is encapsulated in a shell action managers required by the dynamixel_controller. At
script located in the cytonInstaller.tar.gz available via the this point, it is possible to issue commands to the arm's
UNH AI Wikipedia page. Simply extract the tar file, put the individual joints. This is because the controller interface
Robai folder in the extracted folder and run the starts publishing topics that are JointActionTrajectories.
'firstCompile.sh'.
E. Creating a MoveIt model of the Cyton arm
If there are problems with functions with too many
parameters (in cyton_move.cpp) it is necessary to remove 1) MoveIt Setup Assistant - URDF
the extra parameters so that the line looks like: As discussed in the Cyton files section, the cyton.ecz file
passed contains a 3D model of the Cyton arm. This can be
&=hardware.setJointCommands(angles,jointRates); extracted and a URDF along with an SRDF file can be
Another problem that can be encountered is with generated. The URDF file will be used to create the MoveIt
template classes. In actinSE_node.cpp it will be necessary to model which describes the geometries of each section along
rename the occurrences of ControlSystem::JointAngles with with the constraints of the link. Using the MoveIt setup
ControlSystem::JointAngle. Same thing for assistant, it is easy to set up a model by adding in each link
ControlSystem::JointVelocities to and configuring the planning groups. For this arm, we had
ControlSystem::JointVelocity. 2 planning groups: an arm planning group, and a gripper
planning group. This was chosen because of the actions that
C.Cyton files describe the problem can be placed in those two groups. The
There are several strange files in Cyton. In this section, grasp/un-grasp actions fall in the gripper planning group,
I'll attempt to explain what my findings are for each. while the transfer/transit actions correspond to the arm
planning group.
 REFERENCES
[1] K. Salisbury, W. Townsend, B. Ebrman, and D. DiPietro,
“Preliminary design of a whole-arm manipulation
system (WAMS),” in Proceedings. 1988 IEEE
International Conference on Robotics and
Automation, pp. 254–260.
[2] S. Cambon, R. Alami, and F. Gravot, “A Hybrid
Approach to Intricate Motion, Manipulation and
Task Planning,” Int. J. Rob. Res., vol. 28, no. 1, pp.
104–126, Jan. 2009.
[3] T. Simeon, “Manipulation Planning with Probabilistic
Roadmaps,” Int. J. Rob. Res., vol. 23, no. 7–8, pp.
Figure 4: Showing the start state with the goal state 729–746, Aug. 2004.
[4] P. E. Hart, N. J. Nilsson, and B. Raphael, “A Formal
2) MoveIt configuration Basis for the Heuristic Determination of Minimum
The setup assistant creates a new project in ROS which Cost Paths,” IEEE Trans. Syst. Sci. Cybern., vol. 4,
contains all the files necessary to model and visualize the no. 2, 1968.
created robot.
[5] S. M. LaValle, “Rapidly-Exploring Random Trees: A
In Figure 4, the visualization of the arm is shown with New Tool for Path Planning,” In, vol. 129, pp. 98–
the start state in gray and an example goal state highlighted 11, 1998.
in orange. This work not only makes it possible to create
simulated environments that may not be possible to easily [6] M. Likhachev, G. Gordon, and S. Thrun, “ARA•:
Anytime A• with provable bounds on sub-
recreate in a physical environment, but now it can be shown optimality,” Adv. Neural Inf. Process. Syst., vol. 16,
what the original path plan was that was issued to the robot p. 12, 2004.
and which can explain the results that were actually
[7] S. Kiesel, E. Burns, and W. Ruml, “Abstraction-guided
achieved. Sampling for Motion Planning.”
F. Connecting hardware to the model [8] G. Kanter, “Robot Manipulator Control Using Stereo
Setting up the connection between simulation and Visual Feedback,” 2012. [Online]. Available:
hardware is done by connecting the MoveIt node's output as lab.cs.ttu.ee/dl28. [Accessed: 14-Nov-2014].
a SimpleControllerManager to the hardware's Joint [9] Y. Hirano, K. Kitahama, and S. Yoshizawa, “Image-
Trajectory Action Controller. This involves creating a based object recognition and dexterous hand/arm
launch file that invokes the motion planning using RRTs for grasping in
MoveItSimpleControllerManager with the controllers.yaml cluttered scene,” 2005 IEEE/RSJ Int. Conf. Intell.
file that was created for the hardware. Robot. Syst., pp. 2041–2046, 2005.
In addition, a joint state publisher needs to be launched [10] R. Zollner, T. Asfour, and R. Dillmann, “Programming
which is a simple python script that takes into account the by demonstration: dual-arm manipulation tasks for
currently running position controllers. humanoid robots,” 2004 IEEE/RSJ Int. Conf. Intell.
The translation for the joint angles is automatically taken Robot. Syst. (IEEE Cat. No.04CH37566), vol. 1,
2004.
care of by the MoveIt configuration files. Proper
configuration of these files will ensure that the model [11] K. Hauser, “On responsiveness, safety, and
matches with what the hardware reports back as the current completeness in real-time motion planning,” Auton.
state. Robots, vol. 32, no. 1, pp. 35–48, Sep. 2011.

G.Putting it all together

In order to correctly initialize the system, a set of launch
files needs to be started in a specific order. In the
cyton_controller package, the controller_manager is started
to initialize the hardware and detect the presence of each
motor. The start_controller launch file is then called to
connect the motors to their respective position controllers.
The joint state publisher for the Dynamixel motors needs to
be called.
Once all these low level systems have come up, it is
possible to invoke the MoveIt library using the
moveit_planning_execution launch file. This will initialize
Rviz as well as connect to the OMPL library which will also
start communicating with the hardware.