TrajOpt

Integration of TrajOpt into ROS

Solvers support

trajopt_ros implements sequential convex optimization to solve the motion planning problem. It implements a penalty method to optimize for joint velocities while satisfying a set of constraints. Internally, it makes use of convex solvers that are able to solve linearly constrained quadratic problems. At the moment, the following solvers are supported:

BPMPD (interior point method, free for non-commercial use only)
Gurobi (simplex and interior point/parallel barrier, license required)
OSQP (ADMM, BSD2 license)
qpOASES (active set, LGPL 2.1 license)

While the BPMPD library is bundled in the distribution, Gurobi, OSQP and qpOASES need to be installed in the system. To compile with Gurobi support, a GUROBI_HOME variable needs to be defined. Once trajopt_ros is compiled with support for a specific solver, you can select it by properly setting the TRAJOPT_CONVEX_SOLVER environment variable. Possible values are GUROBI, BPMPD, OSQP, QPOASES, AUTO_SOLVER. The selection to AUTO_SOLVER is the default and automatically picks the best between the available solvers.

Build Branch Sphinx Documentation

cd gh_pages
sphinx-build . output

Now open gh_pages/output/index.rst and remove output directory before commiting changes.

Installation

Install tesseract

In the same workspace:

git clone https://github.com/ros-industrial/ros_industrial_cmake_boilerplate # this is required for tesseract
git clone -b noetic https://github.com/janneyow/tesseract
git submodule update --init --recursive

If pyconfig.h cannot be found, locate your python include path

find /usr/include -name pyconfig.h 
  # returns /usr/include/python3.8/pyconfig.h
export CPLUS_INCLUDE_PATH="$CPLUS_INCLUDE_PATH:/usr/include/python3.8/" 
  # you can add this line to your bashrc

Clone this package:

In the same workspace:

    git clone -b noetic https://github.com/janneyow/trajopt

To build:

# to speed up trajopt
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build

When running in Ubuntu 20.04 (Note: these have been updated in this repo)

Need to update tesseract/tesseract_rviz/src/render_tools/env/robot_link.cpp

Change "rviz" to Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME

Update compile options to c++14

Running an example

roslaunch trajopt_examples test_trajopt.launch

JSON Config files

Industrial training/Tesseract
Original trajopt
- Note some original functionalities were not transferred to the tesseract implementation

Basic info:

n_steps (int): number of waypoints generated in the trajectory
manip (str): name of the robot manipulator to plan for
start_fixed (bool): whether to force the first trajectory state to be the first state given

Init info: Note start state is given by ferl_mj's config file

Increase reliability with multiple initializations to decrease probability of converging to a local minimum that is not collision-free.
type(str): type of initialization. valid values are
- "stationary": initializes entire trajectory to current joint states of the robot. No data is needed
- "given_traj": the entire initial trajectory must be provided in the data member
- "joint_interpolated": the endpoint member is required. the trajectory is the joint interpolated between the current state and the endpoint
data (trajArray, optional): Array containing the initialization information
endpoint (array, optional): joint states for the end point

Costs and Constraints: Refer to problem_description.hpp for their term_type

type
- joint_pos
- joint_vel
- joint_acc
- joint_jerk
Only added as costs in Tesseract's implementation, original author's implementation only includes the following as constraints
- cart_pose
- dynamic_cart_pose - for when the goal frame is not fixed in space
- cart_vel
- collision
- total_time

Examples

Moving arm to a joint-position target, set constraints and init_info

...
"constraints" :
  [
    {
      "type": "joint_pos",
      "params":{
        "targets": [0.5, -0.6, 0, 0.5, -1.3988, -0.2]
      }
    }
  ],
  "init_info" :
  {
    "type" : "joint_interpolated",
    "endpoint": [0.5, -0.6, 0, 0.5, -1.3988, -0.2]
  }

Moving arm to one or many pose targets, set constraints. Init info can be left as "stationary"

Alternatively, provide a joint space trajectory as a seed in init_info, then include the cartesian pose targets as costs.

...
 "constraints" :
  [
    {
      "name" : "waypoint_cart_1",
      "type" : "cart_pose",
      "params" :
      {
        "timestep" : 0,
        "xyz" : [0.2, 0.0, -0.112],
        "wxyz" : [0.0, 1.0, 0.0, 0.0],
        "link" : "link_eef",
        "pos_coeffs" : [10, 10, 10],
        "rot_coeffs" : [10, 10, 10]
      }
    },
    {
      "name" : "waypoint_cart_2",
      "type" : "cart_pose",
      "params" :
      {
        "timestep" : 1,
        "xyz" : [0.2, 0.2, -0.112],
        "wxyz" : [0.0, 1.0, 0.0, 0.0],
        "link" : "link_eef",
        "pos_coeffs" : [10, 10, 10],
        "rot_coeffs" : [10, 10, 10]
      }
    },
    {
      "name" : "waypoint_cart_3",
      "type" : "cart_pose",
      "params" :
      {
        "timestep" : 2,
        "xyz" : [0.2, 0.2, 0.112],
        "wxyz" : [0.0, 1.0, 0.0, 0.0],
        "link" : "link_eef",
        "pos_coeffs" : [10, 10, 10],
        "rot_coeffs" : [10, 10, 10]
      }
    }
  ],
  "init_info" :
  {
    "type" : "stationary"
  }

Trajoptpy

Uses the Boost Python library to generate python bindings.

Reference for the CMakeLists.txt
trajoptpy.cpp contains the wrapper

janneyow / trajopt