or_cdchomp is a ROS package providing
orcdchomp, an implementation of the
CHOMP trajectory optimizer for the OpenRAVE simulation environment.
Internally, it uses the OpenRAVE-agnostic implementation of CHOMP in libcd
(whose code is included in this package in the
Documentation for the package is provided primarily by way of the following
usage examples. An example as a runnable script can be located at
Loading the Module
Once the package has been built, an instance of the module can be created as
follows. As a generic module, all communication occurs over the SendCommand
interface; however, the package also includes python bindings, which we
attach to the module instance below. This allows us to run commands as
methods of the module object (e.g.
m_chomp = openravepy.RaveCreateModule(e, 'orcdchomp') import orcdchomp.orcdchomp orcdchomp.orcdchomp.bind(m_chomp)
The module uses a sphere model of the robot, which in general differs from
the normal geometry used for collision checking. This sphere model can be
defined in two ways as specified below. In either case, the spheres defined
for a robot as understood by the
orcdchomp module can be viewed using the
This command will instantiate a new sphere kinbody in the environment for
for each robot sphere (in the location given by the robot's current
configuration). To remove the spheres, simply remove bodies from the
environment whose names start with the string "
Robot spheres in an orcdchomp XML tag
See the file
scripts/barrettwam_withspheres.robot.xml for an example.
Here is a snippet:
<kinbody> .. <orcdchomp> <spheres> <!-- shoulder spheres --> <sphere link="wam0" pos=" 0.22 0.14 0.346" radius="0.15" /> <!-- upper arm spheres --> <sphere link="wam2" pos=" 0.0 0.0 0.2 " radius="0.06" /> <sphere link="wam2" pos=" 0.0 0.0 0.3 " radius="0.06" /> ... </orcdchomp> ... </kinbody>
Here, every sphere is specified relative to a robot link via a space-separated x y z location and radius.
Robot spheres in a special geometry group
Newer versions of OpenRAVE support geometry groups which are not used by default by collision checkers, and CHOMP spheres can also be specified in this way.
Environment signed distance fields
The CHOMP algorithm uses signed distance fields to represent environment
orcdchomp module computes and manages these fields
internally, and will use all fields which have been defined to it.
To define (and add) a new field, use the following command:
This method takes the following named arguments:
kinbody(required): the kinbody whose base link to attach the distance field to. If the kinbody is later moved, the field will move along with it.
cube_extent: the extent of the cubes which constitute the discrete field.
aabb_padding: the padding (the same in x,y,z) used around the bounding box of the kinbody used to determine the size of the field.
cache_filename: the field, once calculated, will be saved to this file (binary format); it will then be read from this file subsequently.
Running the optimizer
The module instance can work on any number of optimization runs at a time. To manage these runs, one can use the following methods:
run_handle = m_chomp.create(...) m_chomp.iterate(run=run_handle, ...) m_chomp.gettraj(run=run_handle, ...) m_chomp.destroy(run=run_handle)
create() method takes the bulk of the options, and is used to set
parameters to the optimizer (described below). Once an optimization run has
been created, it is referenced by a string run handle, which must be passed
to the remaining methods.
iterate() method performs iterations of the CHOMP algorithm.
gettraj() method returns the current trajectory from the optimizer,
and also optionally performs collision checks on the trajectory.
destroy() method simply cleans up internal data structures for the run.
While this API was designed so that
gettraj() can be called
multiple times, it is common that the four methods will simply be called
once in turn. To simplify that use case, the module also provides a single
runchomp() method, which encapsulates handling of the run object for you,
and returns the trajectory directly.
create() method (as well as the
supports a large number of parameters to control the behavior of the
optimizer. Below is a partial list.
robot(required): the robot object whose active dofs to use for planning
adofgoal: the goal dof values for the robot's active dofs (the current configuration is used for the start dof values)
basegoal: the goal pose of the base of the robot (for floating base optimization)
floating_base: whether to do floating base optimization
lambda_: the update rate of the optimizer
starttraj: the starting trajectory to use (defaults to a straight line in configuration space)
n_points: the number of moving points in the waypoint trajectory representation
con_tsrs: constraints to be respected (experimantal)
start_tsr: constraint to be respected at the start (deprecated)
start_cost: additional cost function callback for start configurations
everyn_tsr: a constraint to be respected at every
use_momentum: whether to do second-order optimization (used with HMC)
use_hmc: whether to do Hamiltonian Monte-Carlo randomization
hmc_resample_lambda: the HMC resampling parameter
seed: the HMC random number generator seed
epsilon: the obstacle padding distance
epsilon_self: the self-collision padding distance
obs_factor: the coefficient for environment obstacle cost
obs_factor_self: the coefficient for self-collision obstacle cost
no_report_cost: whether to disable reporting costs at each iteration
dat_filename: a file to dump optimization data to
or_cdchomp was primarily developed by Chris Dellin (email@example.com)
at the Robotics Institute at Carnegie Mellon University,
both as a member of the Personal Robotics Lab with
Sidd Srinivasa and as a graduate student of
Chris Atkeson. Other significant contributions were made by Jen
King, Michael Koval, and Anca Dragan.