Main Content


Visualize CHOMP trajectory of rigid body tree

Since R2023a


    The show object function shows one or more configuration trajectories in a Covariant Hamiltonian Optimization for Motion Planning (CHOMP) environment with obstacles. CHOMP is a gradient-descent based planner that plans and optimizes trajectories for smoothness and collision avoidance. For more information about CHOMP, see the manipulatorCHOMP or dlCHOMP object.

    show(chomp) visualizes the rigid body tree in its home configuration in a CHOMP environment.

    show(chomp,traj) visualizes the trajectory of the rigid body tree generated in a CHOMP setting. The spherical obstacles specified by the property SphericalObstacles are also overlaid on the visualization.

    ax = show(chomp,___) returns the axes ax of the visualization.


    [___] = show(___,Name=Value) specifies additional options using one or more name-value arguments.


    collapse all

    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, ...
    zlim([-0.5 1.3])

    Input Arguments

    collapse all

    CHOMP optimizer, specified as a manipulatorCHOMP object.

    Trajectory, specified as an N-by-M matrix. N is the total number of waypoints, and M is the size of a joint configuration for the rigid body tree in the RigidBodyTree property of chomp.

    Name-Value Arguments

    Specify optional pairs of arguments as Name1=Value1,...,NameN=ValueN, where Name is the argument name and Value is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

    Example: show(chomp,chomptraj,NumSamples=30)

    Number of uniformly spaced joint configuration samples along the trajectory, specified as a positive integer scalar.

    Example: show(chomp,chomptraj,NumSamples=10)

    Parent of axes, specified as an Axes object on which to visualize the rigid body tree trajectories. By default, show plots the rigid body tree trajectories on the current axes.

    Collision objects to visualize, specified as a cell array of collision objects.

    Example: show(chomp,trajectory,CollisionObjects={collisionCylinder(1,2)})

    Visualize truncated signed distance field of environment meshes, specified as a true or false. This shows the meshtsdf (Navigation Toolbox) object stored in the MeshTSDF property of chomp.

    Output Arguments

    collapse all

    Axes graphic handle, returned as an Axes object. This object contains the properties of the figure that the rigid body tree is plotted onto.

    Version History

    Introduced in R2023a