manipulatorCHOMP

Covariant Hamiltonian optimizer for rigid body tree motion planning

Since R2023a

Description

The manipulatorCHOMP object optimizes rigid body tree motions using the Covariant Hamiltonian Optimization for Motion Planning (CHOMP) algorithm. CHOMP optimizes trajectories for smoothness and collision avoidance by minimizing a cost function comprised of a smoothness cost and collision cost.

To specify options for smoothness cost, collision cost, and the solver, use the chompSmoothnessOptions, chompCollisionOptions, and chompSolverOptions objects, respectively.

You can specify the environment as a set of collision spheres and as a set of truncated signed distance fields of the environment meshes by using the RigidBodyTreeSpheres and MeshTSDF name-value arguments, respectively. Using the MeshTSDF name-value argument requires Navigation Toolbox™.

Creation

Syntax

manipCHOMP = manipulatorCHOMP(robotRBT)

manipCHOMP = manipulatorCHOMP(robotRBT,Name=Value)

Description

manipCHOMP = manipulatorCHOMP(robotRBT) creates a CHOMP-based optimizer for a rigid body tree robotRBT. The robotRBT argument sets the RigidBodyTree property.

manipCHOMP = manipulatorCHOMP(robotRBT,Name=Value) specifies properties using one or more name-value arguments.

Properties

SmoothnessOptions — Smoothness cost options

chompSmoothnessOptions object

Smoothness cost options, specified as a chompSmoothnessOptions object.

By default, this property contains a chompSmoothnessOptions object with default property values.

CollisionOptions — Collision cost options

chompCollisionOptions object

Collision cost options, specified as a chompCollisionOptions object.

By default, this property contains a chompCollisionOptions object with default property values.

SolverOptions — Motion-planning solver options

chompSolverOptions object

Motion-planning solver options, specified as a chompSolverOptions object.

By default, this property contains a chompSolverOptions object with default property values.

RigidBodyTree — Robot model used for motion planning

rigidBodyTree object

This property is read-only.

Robot model used for motion planning, specified as a rigidBodyTree object at object construction.

You can access the spherical approximation in RigidBodyTreeSpheres as collision geometries on the tree by using the RigidBodyTree property.

RigidBodyTreeSpheres — Spheres of bodies in rigid body tree

M-by-2 table

Spheres of the bodies in the rigid body tree, stored as an M-by-2 table. M is the number of bodies in the rigid body tree. The first column contains the name of a rigid body in RigidBodyTree and the second column contains a corresponding cell array. Each cell array is a single cell that contains a 4-by-N matrix, where N is the number of spheres for the corresponding rigid body. Each column of the matrix of spheres contains the information for a sphere in the form [r; x; y; z], defined with respect to the frame of the rigid body. r is the radius of the sphere, and x, y, and z are the x-, y-, and z-coordinates of the center of the sphere, respectively.

SphericalObstacles — Spherical obstacles in environment

[] (default) | 4-by-N matrix

Spherical obstacles in the environment, specified as a 4-by-N matrix. Each column represents the information about a sphere in the form [r; x; y; z], defined with respect to the base frame of the robot. r is the radius of the sphere, and x, y, and z are the x-, y-, and z-coordinates of the center of the sphere, respectively.

MeshTSDF — Truncated signed distance field of meshes in environment

[] (default) | meshtsdf object

This property is read-only.

Truncated signed distance field of meshes in environment, stored as meshtsdf (Navigation Toolbox) object. The TruncationDistance property of the meshtsdf object must be greater than the radius of the largest sphere in the RigidBodyTreeSpheres property.

You can only specify this property at object construction using the name-value argument syntax. Because MeshTSDF stores a handle of the meshtsdf object, you can still modify mesh TSDFs in the specified meshtsdf object after setting this property.

Object Functions

optimize	Optimize trajectory using CHOMP
show	Visualize CHOMP trajectory of rigid body tree

Examples

Optimize Collision-Free Trajectory with CHOMP

Load a robot model into the workspace, and create a CHOMP solver.

robot = loadrobot("kinovaGen3",DataFormat="row");
chomp = manipulatorCHOMP(robot);

Create spheres to represent obstacles, and add them to the CHOMP solver.

env = [0.20 0.2 -0.1 -0.1;   % sphere, radius 0.20 at (0.2,-0.1,-0.1)
       0.15 0.2  0.0  0.5]'; % sphere, radius 0.15 at (0.2,0.0,0.5)
chomp.SphericalObstacles = env;

To prioritize a collision-free trajectory, set the smoothness cost weight to a lower value than the collision cost weight. Then add the options to the CHOMP solver.

chomp.SmoothnessOptions = chompSmoothnessOptions(SmoothnessCostWeight=1e-3);
chomp.CollisionOptions = chompCollisionOptions(CollisionCostWeight=10);
chomp.SolverOptions = chompSolverOptions(Verbosity="none",LearningRate=7.0);

Initialize a trajectory, optimize it using the CHOMP solver, and show the waypoints in a figure.

startconfig = homeConfiguration(robot);
goalconfig = [0.5 1.75 -2.25 2.0 0.3 -1.65 -0.4];
timepoints = [0 5];
timestep = 0.1;
trajtype = "minjerkpolytraj";
[wptsamples,tsamples] = optimize(chomp, ...
    [startconfig; goalconfig], ...
    timepoints, ...
    timestep, ...
    InitialTrajectoryFitType=trajtype);
show(chomp,wptsamples,NumSamples=10);
zlim([-0.5 1.3])

References

[1] Ratliff, Nathan, Siddhartha Srinivasa, Matt Zucker, and Andrew Bagnell. “CHOMP: Gradient Optimization Techniques for Efficient Motion Planning.” In 2009 IEEE International Conference on Robotics and Automation, 489–94. Kobe, Japan: IEEE, 2009. https://doi.org/10.1109/ROBOT.2009.5152817.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2023a