optimize

Optimize trajectory using deep-learning-based CHOMP

Since R2024a

collapse all in page

    Syntax

    [optimtraj,timesamples,solninfo] = optimize(dlchomp,start,goal)

    Description

    [optimtraj,timesamples,solninfo] = optimize(dlchomp,start,goal) optimizes the trajectory between the specified robot start and goal configurations using the specified deep-learning-based Covariant Hamiltonian Optimization for Motion Planning (CHOMP) algorithm, and outputs the optimized waypoints, the corresponding sample times, and solution information.

    The optimize function requires Deep Learning Toolbox™.

    example

    Examples

    collapse all

    Open Live Script

    Download a pretrained dlCHOMP object for the KUKA LBR iiwa 7 robot.

    dataZip = matlab.internal.examples.downloadSupportFile("rst/data/dlCHOMP/R2024a/","kukaIiwa7DLCHOMPTrained.zip");
dataFilePaths = unzip(dataZip);

    Load the trainedDLCHOMP MAT file. The file contains the trained DLCHOMP optimizer, obstacles, and start and goal configurations.

    load trainedDLCHOMP.mat
    Warning: Cannot load an object of class 'dlCHOMPDatastore':
Its class cannot be found.

    Add the obstacles to the dlCHOMP object and show the robot in the home configuration with the loaded obstacles.

    trainedDLCHOMP.SphericalObstacles = unseenObstacles;
show(trainedDLCHOMP);
title(["Robot at Home Configuration","in Obstacle Environment"])
axis auto

    Figure contains an axes object. The axes object with title Robot at Home Configuration in Obstacle Environment, xlabel X, ylabel Y contains 46 objects of type patch. These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh, iiwa_link_0_coll_mesh, iiwa_link_1_coll_mesh, iiwa_link_2_coll_mesh, iiwa_link_3_coll_mesh, iiwa_link_4_coll_mesh, iiwa_link_5_coll_mesh, iiwa_link_6_coll_mesh, iiwa_link_7_coll_mesh.

    Optimize trajectory between the start and goal configuration.

    trainedDLCHOMP.RigidBodyTree.DataFormat = "column"
    trainedDLCHOMP = 
  dlCHOMP with properties:

           RigidBodyTree: [1×1 rigidBodyTree]
    RigidBodyTreeSpheres: [11×1 table]
       SmoothnessOptions: [1×1 chompSmoothnessOptions]
           SolverOptions: [1×1 chompSolverOptions]
        CollisionOptions: [1×1 chompCollisionOptions]
      SphericalObstacles: [4×24 double]
              BPSEncoder: [1×1 bpsEncoder]
            NumWaypoints: 40
                 Network: [1×1 dlnetwork]
               NumInputs: [14 10000]
              NumOutputs: 266


    [wpts,tpts,solninfo] = optimize(trainedDLCHOMP,unseenStart,unseenGoal);

    Visualize the trajectory.

    figure
a = show(trainedDLCHOMP,wpts);
title("Optimized Trajectory")
axis equal

    Figure contains an axes object. The axes object with title Optimized Trajectory, xlabel X, ylabel Y contains 464 objects of type patch. These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh, iiwa_link_0_coll_mesh, iiwa_link_1_coll_mesh, iiwa_link_2_coll_mesh, iiwa_link_3_coll_mesh, iiwa_link_4_coll_mesh, iiwa_link_5_coll_mesh, iiwa_link_6_coll_mesh, iiwa_link_7_coll_mesh.

    Input Arguments

    collapse all

    Deep-learning-based CHOMP optimizer, specified as a dlCHOMP object.

    Starting joint configuration of the robot, specified as an M-element vector. M is the size of a joint configuration for the rigid body tree in the RigidBodyTree property of dlchomp.

    Goal joint configuration of the robot, specified as an M-element vector. M is the size of a joint configuration for the rigid body tree in the RigidBodyTree property of dlchomp.

    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, specified by the NumWaypoints property of dlchomp, and M is the size of a joint configuration for the rigid body tree in the RigidBodyTree property of dlchomp.

    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.

    • NeuralGuessTime — Neural network guess prediction time, in seconds.

    Extended Capabilities

    expand all

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

    Version History

    Introduced in R2024a

    expand all

    See Also

    Topics