Main Content

optimize

Optimize trajectory using CHOMP

Since R2023a

collapse all in page

    Syntax

    [optimtraj,timesamples] = optimize(manipCHOMP,wpts,tpts,timestep)
    [optimtraj,timesamples] = optimize(manipCHOMP,wpts,tpts,timestep,InitialTrajectoryFit=fittype)
    [___,solninfo] = optimize(___)

    Description

    The optimize object function optimizes a trajectory for both smoothness and collision avoidance using Covariant Hamiltonian Optimization for Motion Planning (CHOMP), a gradient-descent based planner. To change the weights of the smoothness costs and the collision costs, set the SmoothnessOptions, CollisionOptions, and SolverOptions properties of the manipulatorCHOMP object. The function also assumes that both the environment and the collision geometry of the rigid body tree robot model are approximated as collision spheres.

    [optimtraj,timesamples] = optimize(manipCHOMP,wpts,tpts,timestep) optimizes the trajectory specified by the waypoints wpts at times tpts, and using the Covariant Hamiltonian Optimization for Motion Planning (CHOMP) algorithm, outputs the optimized waypoints optimtraj at the sample times timesamples. The initial trajectory fit between waypoints at their respective times is linear interpolation.

    example

    [optimtraj,timesamples] = optimize(manipCHOMP,wpts,tpts,timestep,InitialTrajectoryFit=fittype) specifies a trajectory type fittype for the initial trajectory fitting.

    [___,solninfo] = optimize(___) returns solution information from the optimization.

    Examples

    collapse all

    Open Live Script

    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])

    Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 263 objects of type patch. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh, Shoulder_Link_coll_mesh, HalfArm1_Link_coll_mesh, HalfArm2_Link_coll_mesh, ForeArm_Link_coll_mesh, Wrist1_Link_coll_mesh, Wrist2_Link_coll_mesh, Bracelet_Link_coll_mesh, base_link_coll_mesh.

    Input Arguments

    collapse all

    CHOMP solver, specified as a manipulatorCHOMP object.

    Trajectory waypoints to optimize, 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 manipCHOMP.

    Trajectory waypoint times, specified as an N-element row vector. N is the number of specified waypoints.

    Time step size at which to discretize trajectory, specified as a positive numeric scalar.

    Initial trajectory fit type, specified as one of these options:

    • "minjerkpolytraj" — Fit a minimum jerk polynomial trajectory. See the minjerkpolytraj function for more details about this trajectory type.

    • "interp1" — Fit a linearly-interpolated trajectory. See the interp1 function for more details about this trajectory type.

    • "quinticpolytraj" — Fit a quintic polynomial trajectory. See the quinticpolytraj function for more details about this trajectory type.

    The trajectory fit type is the trajectory that the optimize function uses to fit between waypoints at their specified time points.

    Data Types: char | string

    Output Arguments

    collapse all

    Optimized waypoint samples of the optimized trajectory, returned 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 manipCHOMP.

    Time samples for optimized waypoint samples, returned as an N-element row vector. N is the number of optimized waypoint samples.

    Solution information, returned as a structure containing these fields:

    • Iterations — Number of iterations taken to optimize trajectory.

    • SmoothnessCost — Smoothness cost at each iteration.

    • CollisionCost — Collision cost at each iteration.

    • ObjectiveFunction — Objective function value at each iteration.

    • InitialSmoothnessCost — Initial smoothness cost of the trajectory.

    • InitialCollisionCost — Initial collision cost of the trajectory.

    • InitialObjectiveFunction — Initial objective function value of the trajectory.

    • OptimizationTime — Time taken to optimize trajectory.

    • IsCollisionFree — Indication of whether trajectory is collision free or not.

    Extended Capabilities

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

    Version History

    Introduced in R2023a

    See Also

    | | | |