Train Deep-Learning-Based CHOMP Optimizer for Motion Planning

Since R2024a

This example shows you how to train a dlCHOMP optimizer for motion planning in a complex spherical obstacle environment.

This example shows you how to set data generation parameters to generate a spherical obstacle environment, generate a training dataset using these parameters, train the dlCHOMP optimizer and then use it for motion planning for the KUKA LBR iiwa 7 robot in a new obstacle environment.

To see pretrained networks for other robots, see Pretrained Optimizers.

Overview

Deep Learning guess-powered Covariant Hamiltonian Optimization for Motion Planning (DLCHOMP) is a trajectory-optimization-based motion planner that optimizes neural-network-predicted trajectories to make the trajectories smoother and avoid obstacles in the environment. It does this by minimizing a cost function, comprised of a smoothness cost and a collision cost. It is similar to the manipulatorCHOMP object but dlCHOMP enables you to achieve faster optimization times for robots operating in relatively-static environments by training the optimizer.

This example shows you how to load a manipulator robot, generate a complex collision geometries as spherical obstacles around the robot for DLCHOMP, and visualize the resulting optimized trajectories. This example assumes that you have some familiarity with the manipulatorCHOMP object.

Create dlCHOMP Optimizer

To create a dlCHOMP object that meets our requirements, first determine the robot to work with. For instance, let's consider the kukaIiwa7 robot.

rbt = loadrobot("kukaIiwa7",DataFormat="row");

Then, determine the basis point set encoding parameters to be used to encode the obstacle environment of interest and create a bpsEncoder object accordingly. In this instance, use the uniform-ball-3d arrangement of 10,000 points for the encoder. Associate a seed value with a random number generator to ensure that each run gives the same random arrangement of points for the encoder.

seed = 100;
rng(seed,"twister");
encoder = bpsEncoder("uniform-ball-3d",10000);

Finally, set the number of waypoints desired in the optimized trajectory output of dlCHOMP.

numWaypoints = 40;

Now construct a dlCHOMP object using the loaded robot rbt, the environment encoder encoder and the desired number of waypoints numWaypoints.

dlchomp = dlCHOMP(rbt,encoder,numWaypoints);

Assume that a manipulatorCHOMP optimizer object is available to work in our apple-picking target environment of interest.

So, set the CollisionOptions, SmoothnessOptions and SolverOptions properties of the dlCHOMP optimizer to be the same as that of the equivalent manipulatorCHOMP optimizer that works. For more information on choosing the right optimizer options, refer to the Tune Optimizer section of the Pick-And-Place Workflow Using CHOMP for Manipulators example.

dlchomp.CollisionOptions = chompCollisionOptions(CollisionCostWeight=120, ...
                                                 IgnoreSelfCollision=false);
dlchomp.SolverOptions = chompSolverOptions(LearningRate=2, ...
                                           MaxIterations=200, ...
                                           FunctionTolerance=1e-4);
dlchomp.SmoothnessOptions = chompSmoothnessOptions(SmoothnessCostWeight=0.001);

Determine Dataset Generation Parameters

For generating data to train the DLCHOMP optimizer, you must use a dlCHOMPDataOptions object to specify dataset generation parameters. Consider the problem of fruit-picking using a manipulator arm. In this application, you must determine parameters based on the size of tree branches and fruits.

In this image, assume the fruits to be spheres having a radius range between 5 cm to 10 cm.

Create a dlCHOMPDataOptions object.

dataOptions = dlCHOMPDataOptions;

Next, you must set obstacles that determine environment complexity.

Set a range of possible radii for the spherical obstacles to be between 5cm and 10cm.

dataOptions.RadiusRangeOfObstacles = [0.05 0.10];

Set the number of obstacles to be between 20 and 25 spheres. This is assuming that there are about 20 to 25 fruits within reach of the manipulator.

dataOptions.CountRangeOfObstacles = [20 25];

Set the minimum distance that spheres can be from the base to 21 cm.

dataOptions.MinDistanceFromBase = 0.21;

Set the number of samples to generate to 5,000 samples.

dataOptions.NumSamples = 5000;

Set the validation split such that you keep 20% of the generated data set for validating the trained optimizer and using the rest for training the optimizer.

dataOptions.ValidationSplit = 0.2;

For more information about these parameters, see dlCHOMP.

Generate Training and Validation Data Samples

Next, you need training and validation data sets. Note that generating training and validation data takes a long time. In this example, generating 5,000 samples takes approximately 21 hours on a Windows 10 system with 48 GB of RAM. In this section you have the option of either downloading a dataset, generated specifically for this example, or generating the data yourself.

Set generateData to false to download a generated dataset.

generateData = false;
dataSamplesFolder = "allData";
if generateData == false
    dlCHOMPComponent = "rst/data/dlCHOMP/R2024a";
    dataSamplesFilename = "kukaIiwa7DataForTrainingDLCHOMP.zip";
    disp("Downloading previously generated kukaIiwa7 DLCHOMP data (1 GB)...");
    dataSamplesFolder = exampleHelperDownloadData(dlCHOMPComponent,dataSamplesFilename);
    validIndices = 1:floor(dataOptions.ValidationSplit*5e3);
    trainIndices = (numel(validIndices)+1):5e3;
    validDS = dlCHOMPDatastore(dataSamplesFolder,validIndices);
    trainDS = dlCHOMPDatastore(dataSamplesFolder,trainIndices);
else
    rng(seed,"twister");
    [trainDS,validDS] = generateSamples(dlchomp,dataOptions,dataSamplesFolder);
end

Downloading previously generated kukaIiwa7 DLCHOMP data (1 GB)...

Train dlCHOMP Optimizer

Next step is to train the dlCHOMP optimizer object using the data that has been obtained.

Create training options using the validation datastore object validDS obtained earlier. Choose appropriate training options to train the regression network present inside the dlCHOMP optimizer object. For more information on choosing the right training options, see trainingOptions (Deep Learning Toolbox). Also identify the appropriate loss type for training the dlCHOMP object. For more information about the supported loss types, see trainDLCHOMP.

These values work best for the current dataset.

trainOptions = trainingOptions("adam", ...
      "MaxEpochs",200, ...
      "MiniBatchSize",256, ...
      "InitialLearnRate",0.001, ...
      "LearnRateDropFactor",0.9, ...
      "LearnRateDropPeriod",10, ...
      "LearnRateSchedule","piecewise", ...
      "ValidationData",validDS, ...
      "ExecutionEnvironment","parallel", ...
      "Plots","training-progress", ...
      'Shuffle','every-epoch');
lossType = "mean-squared-error";

Finally, train the dlCHOMP optimizer object using the training datastore trainDS, loss type lossType and the training options trainOptions.

Set doTraining to false to download the trained dlCHOMP object. To train the dlCHOMP object yourself, set doTraining to true. Note that training takes around 40 mins on a Windows 10 system with 48 GB RAM using 8 parallel pool workers. So download this pretrained dlCHOMP object by default as the doTraining is set to false below.

doTraining = false;
if doTraining == false
    pretrainedDLCHOMPFilename = "kukaIiwa7DLCHOMPTrained.zip";
    disp("Downloading previously trained kukaIiwa7 dlCHOMP optimizer (46 MB)...");
    pretrainedDLCHOMPFolder = exampleHelperDownloadData(dlCHOMPComponent,pretrainedDLCHOMPFilename);
    pretrainedDLCHOMPMATPath = fullfile(pretrainedDLCHOMPFolder,"trainedDLCHOMP.mat");  
    loadedData = load(pretrainedDLCHOMPMATPath,"trainedDLCHOMP","trainInfo");
    dlchomp = loadedData.trainedDLCHOMP;
    trainInfo = loadedData.trainInfo;
else
    trainInfo = trainDLCHOMP(dlchomp,trainDS,lossType,trainOptions);
end

Downloading previously trained kukaIiwa7 dlCHOMP optimizer (46 MB)...

Infer Using Trained dlCHOMP Optimizer

Now use the trained optimizer to infer on a target environment. For the purpose of this example, use generate another data sample to get a target environment that is similar to the training data.

Generate Target Environment

Generate a target environment of spherical obstacles by using the generateSamples function with the same dlCHOMPDataOptions that you used for the training.

First, set the number of samples to generate to 1. Then set the number of samples to save for validation to 0.

dataOptions.NumSamples = 1;
dataOptions.ValidationSplit = 0;

Next, to ensure that you are generating a data sample that you did not previously generate during the sample generation step, set the random number generator seed a different value than used for generating the training and validation data previously. Because the default Seed value was 0 previously, set the Seed property to 500 now.

dataOptions.Seed = 500;

Set a new location for saving the generated unique sample and generate a unique target environment.

folderForTargetEnvData = "targetEnvData";
rng(dataOptions.Seed,"twister");
testDS = dlchomp.generateSamples(dataOptions,folderForTargetEnvData);

------------------------
Starting data generation
[1/1] Data Samples Written	|	┃┃┃┃┃┃┃┃┃┃┃┃┃┃┃┃┃┃┃┃ 100.00% complete	|	
Total Run Time: 00:00:11 secs
Estimated Time Remaining: 00:00:00 secs
------------------------

filePathToUnseenSample = resolve(testDS.FileSet).FileName(1);
[unseenStart,unseenGoal,environmentObstacles,chompTrajectory] = robotics.datagen.internal.DLCHOMPUtils.extractDataFromSample(filePathToUnseenSample);

Infer On Target Environment

Now make the dlCHOMP optimizer aware of the unseen obstacles.

dlchomp.SphericalObstacles = environmentObstacles;

Finally, use the trained dlCHOMP optimizer for inference on this unseen target environment.

[optimWptsDLCHOMP,optimTptsDLCHOMP,solninfoDLCHOMP] = dlchomp.optimize(unseenStart,unseenGoal);

Visualize the trajectory as an animation. Rotate the figure during the animation to make the robot motion easier to see.

% Compute camera angles for camera rotation
numPredWaypoints = size(optimWptsDLCHOMP,1);
azRange = [90 125];
elRange = [0 15];
azimuths = linspace(azRange(1),azRange(2),numPredWaypoints);
elevations = linspace(elRange(1),elRange(2),numPredWaypoints);

% Show animated robot trajectory
figure
ax = show(dlchomp,unseenStart);
title("dlCHOMP Optimized End-Effector Trajectory:","Unseen Target Environment");
axis equal
hold on
exampleHelperShowAnimatedRobotTrajectory(ax,rbt,optimWptsDLCHOMP);
hold off