# optimize

Optimize trajectory using CHOMP

Since R2023a

## 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.

example

````[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.```
````[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

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

## 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.

## Version History

Introduced in R2023a