# TrajOpt Planner

Note

The TrajOpt planner is still an alpha feature of MoveIt from a summer 2019 intern project, and needs further hardening. Please contact hello@picknik.ai if you are interested in further development.

TrajOpt is a sequential convex optimization algorithm for motion planning problems where the non-convex, non-affine equality, and non-equality constraints are relaxed, approximately linearized and convexified to create an objective function. This work has been done based on [schulman2013] and the original implementation. Sequential convex programming, simply refers to using a convex model and repeatedly minimizing it. Consider the following non-convex problem:

where *f(x)* is the minimum-length path given by

The basic idea is to iterate by maintaining an estimate of the solution and a convex trust region over which we trust our solution. The two key points are:

convex approximation of

*f(x)*and*g(x)*over the trust region. Then the approximated ones are converted to penalty functions.affine approximation of

*h(x)*over the trust region. Then the approximated*h(x)*is convetered to penalty function by considering its absolute value.

The following figure shows how TrajOpt algorithm works

The bold italic parameters are the ones loaded from yaml file. All the constraints including obstacle avoidance, joint limits and target poses in joint space or Cartesian space are converted to penalty functions so they make the final objective function. The most inner loop is where sequential quadratic programming is used as a trust region method. It calculates the second-order Taylor approximation in a box trust region.

One important part of TrajOpt is how the obstacle avoidance constraint is formulated. In discrete case, the constraint is basically the difference between the signed distance (between robot link with itself or with obstacles) and a safe value and in the continuous case, the signed distance is between convex hull of two waypoints and obstacles.

**Note:** The current implementation of TrajOpt supports constraints in joint space only.

## Getting Started

If you haven’t already done so, make sure you’ve completed the steps in Getting Started.

You should also have gone through the steps in Visualization with MoveIt RViz Plugin

## Prerequisites

To use TrajOpt with your robot you must already have a MoveIt configuration package for your robot. For example, if you have a Panda robot, it’s called `panda_moveit_config`

.
This is typically configured using the MoveIt Setup Assistant.

**Note:** in this tutorial, we use `panda_moveit_config`

from the ros-planning/panda_moveit_config repository which contains the necessary config files for trajopt planner.

## Running the Demo

To run the example, you need to run `panda_moveit_config`

from the ros-planning/panda_moveit_config repository at first by passing `trajopt`

as the planner:

```
roslaunch panda_moveit_config demo.launch pipeline:=trajopt
```

Then you can run the trajopt example from ros-planning/moveit_tutorials:

```
roslaunch moveit_tutorials trajopt_example_launch.launch
```

## How TrajOpt works

Motion planning problem in TrajOpt is defined by a set of cost (COST) and constraints (CNT) functions that are added to `TrajOptProblem`

through `ConstructProblem`

function. This function gets the information regarding to the problem (`ProblemInfo`

) which carries different types of information explained below:

**BasicInfo**: This type holds general information of the optimization algorithm. (These parameters are further investigated in the following section): *`n_steps`

: The number of steps from start to goal *`convex_solver`

: Which convex solver is to be used *`use_time`

: Set to`false`

value to use a unitless timestep. x1-x0 is the velocity *`start_fixed`

: Set to`true`

to add a constraint for the current joint value**InitInfo**: It defines how to initialize the optimization problem by setting a guessed trajectory in a matrix whose number of rows is the same as number of timesteps and whose number of columns is equal to the degrees of freedom. There are three different types for initialization:*STATIONARY*: the initialization matrix has joint values of the current state for all timesteps.*JOINT_INTERPOLATED*: the initialization matrix is a trajectory interpolated between the current state and the joint state that the user provides for`data`

member.*GIVEN_TRAJ*: the user provides the entire trajectory for`data`

member.

**TermInfo**: This is the base struct for all types of COST and CNT functions that are carried by`cost_infos`

and`cnt_infos`

members. COST functions are the objectives that are supposed to be minimized and CNT are the ones that must be satisfied. The current implementation contains`JointPoseTermInfo`

,`JointVelTermInfo`

(hard-coded) and`CartPoseTermInfo`

(is partially implemented). Member*term_type*dictates the type of the term we are adding; it could be`TT_COST`

or`TT_CNT`

which means a cost term or constraint term respectively. Also`TT_USE_TIME`

can be selected for this member which allows time parameterization. In this case*use_time*of`BasicInfo`

should be set to`true`

as well. The other parameters of these terms which need to be set are loaded from`trajopt_planning.yaml`

file. The following list describes these parametrs:*coeffs*: weight coefficients for joints*targets*: the values of the joints at the constraint*upper_tols*: the upper limits for joint values at the constraint*lower_tols*: the lower limits for joint values at the constraint*first_step*: the first step that is the term is applied to*last_step*: the last step that is the term is applied to

Moreover, the algorithm needs parameters specific to `BasicTrustRegionSQP`

which are defined in a yaml file under *trajopt_param*.

The following flowchart illustrates how the problem gets constructed:

## How to use TrajOpt

In this section, we describe how to convert the request in MoveIt to TrajOpt constraints. The first important point is that in TrajOpt, we can have the start state to be different than current state. Basically, any joint state that we want the robot pass through is just a constraint that should be added to the objective function. This joint state can be received from start state in MoveIt `MotionPlanRequest`

or the current state from Planning Scene. The designed architecture for TrajOpt in MoveIt creates *JointPoseTermInfo* for all the constraints in *goal_constraints* and *start_state* of `MotionPlanRequest`

. On the other hand, current state of the robot also can be a constraint to add as a *JointPoseTermInfo* if the user wants the robot to start from the current state. Look at the following figure for a an example with two goal constraints:

*start_fixed* is the parameter that determines the relation between current state and start state. If it is set to true, then the algorithm adds a constraint to restrict the trajectory to start from the current state. If it is false, then the trajectory will start from request start state. The following two gifs show the above example in action; notice the difference between starting from the current state and starting from the request start state, left and right respectively:

The use case example of this is when we are trying to execute a process like sanding, the critical part is the actual process path not how we get to the start of the process path. So we plan the process path first leaving the start free to hopefully get the most optimal and then we plan from the current location start fixed to the start of the process path.

Two other important parameters are *first_timestep* and *last_timestep*. These are the steps that we want a specific `TermInfo`

gets applied to. For our example in this section, we have one constraint from request start state and two goal constraints as well as the current state constraint. If the number of steps (*num_stpes*) is set to 20, then the index that our last goal constraint applied to is 19. The user does not set the parameters of current state constraint that is created by *start_fixed=true*; its timestep is set to 0 by default. So basically the figure above changes to the following if we add the steps the constraints are applied to:

*term_type* of the above constraint should be set to *TT_CNT* as we want the robot pass through all those states exactly.

Note that if we want to use TrajOpt through MotionPlanning Display in RViz, we should set *start_fixed* to false as the current state of the robot is sent through request start state, meaning current and start are the same so we do not need two constraints (that are the same) for the same state. In this case, do not forget to set the *first_timestep* and *last_timestep* of the start state constraint to zero. So our example above changes to the following:

For any new joint constraint, the corresponding information should be added to the yaml file. The current implementation can only handle the joint space constraints as `JointPoseTermInfo`

from `TermInfo`

. The some of the remaining constraints to add are `JointAccTermInfo`

, `JointJerkTermInfo`

, `CartPoseTermInfo`

, `TotalTimeTermInfo`

and `CollisionTermInfo`

.

## References

- schulman2013
Schulman, John & Ho, Jonathan & Lee, Alex & Awwal, Ibrahim & Bradlow, Henry & Abbeel, Pieter. (2013). Finding Locally Optimal, Collision-Free Trajectories with Sequential Convex Optimization. 10.15607/RSS.2013.IX.031.