CRISP: Robust Contact-Implicit Motion Planning with Sequential Convex Programming

CRISP is a C++ library developed to efficiently solve contact-implicit motion planning problems, using a customized trust region Sequential Convex Programming (SCP) algorithm.

CRISP computes entirely new contact sequences from naive and even all-zero initializations.

<div align="center"> <h3>Waiter</h3> <img src="files/waiter.gif" width="600" height="300"/> </div> <div align="center"> <h3>Hopper</h3> <img src="files/hopper.gif" width="600" height="300"/> </div> <div align="center"> <h3>Push Box</h3> <img src="files/pushbox.gif" width="600" height="300"/> </div> <div align="center"> <h3>Push T</h3> <img src="files/pushT.gif" width="600" height="300"/> </div> <div align="center"> <h3>Transport</h3> <img src="files/transport.gif" width="600" height="300"/> </div> <div align="center"> <h3>Cartpole with Soft Walls</h3> <img src="files/pushbot.gif" width="600" height="300"/> </div> <div align="center"> <h3>Real World Push T</h3> <img src="files/pushTREALWORLD.gif" width="600" height="300"/> </div>

News

• CRISP has been accepted by RSS 2025!

Features

• CRISP leverages CppAD and its companion, CppADCodeGen, to facilitate rapid and efficient computation of necessary values and derivatives. This approach automates the generation of autodiff libraries, requiring users only to define the objective and constraint value functions. These libraries are compiled during the initial run and reused in subsequent operations, optimizing computational efficiency.

• CRISP accommodates both parametric and nonparametric value functions. The parametric support allows users to adjust parameters dynamically and resolve problems without the need for regenerating the autodiff libraries—ideal for applications like Model Predictive Control (MPC), where the tracking reference in the objective function might change frequently.

• Python interface pyCRISP is available through pybind11, enabling dynamic parameter adjustments and solver execution within a Python environment.

• All the sparse matrix and vector operations within the library are optimized and accelerated by directly memory copying under Eigen Compressed Row Storage (CRS) scheme. The large-scale sparse convex Quadratic Programming (QP) subproblems that arise are tackled using the Proximal Interior Point Quadratic Programming Solver (PIQP).

Table of Contents

• Installation Instructions
  • Prerequisites
  • Install Third Party Libraries
• Install CRISP
  • Test Examples
• Usage
  • General Workflow
  • C++ Interface
  • Python Interface
• Citing Our Work

1. Installation Instructions

Download the source code from the repository:

    git clone https://github.com/ComputationalRobotics/CRISP-CODE.git

1.1 Prerequisites

• This library is developed and tested on Ubuntu 20.04/22,04.
• Install Common Library through sudo apt install
  • CMake
  • Eigen3
  • Boost
  • Yaml
  • pybind11
  • pkgconfig
  • if you miss any in your system, use sudo apt install to install the corresponding library.

1.2 Install Third Party Libraries

All third party libraries are suggested to be placed in ./src/third_party directory.

CppAD

Create and navigate into the ./src/third_party directory:

    mkdir -p ./src/third_party
    cd ./src/third_party

Install the CppAD repository:

    git clone https://github.com/coin-or/CppAD.git
    cd CppAD
    mkdir build && cd build
    cmake ..
    make install

CppADCodeGen

Install the CppADCodeGen repository:

    git clone https://github.com/joaoleal/CppADCodeGen.git
    cd CppADCodeGen
    mkdir build
    cd build
    cmake ..
    make install

PIQP

Install the PIQP repository:

    git clone https://github.com/PREDICT-EPFL/piqp.git
    git checkout v0.5.0 
    (the latest version has unknown incompatiablity with Eigen)
    cd piqp
    mkdir build
    cd build
    cmake .. -DCMAKE_CXX_FLAGS="-march=native" -DBUILD_TESTS=OFF -DBUILD_BENCHMARKS=OFF
    cmake --build . --config Release
    cmake --install . --config Release

Note: the default CMakeLists in PIQP require cmake version 3.21, while the default cmake version in ubuntu 20.04 is lower.

cmake_minimum_required(VERSION 3.21)

Do not worry about this, just change the above lines in all related CMakeLists.txt file in the PIQP directory to fit your version. For me, it is:

cmake_minimum_required(VERSION 3.16)

2. Install CRISP

cd src
mkdir build
cmake ..
make

Note: CppAD library generate a .pc complier file, which is used to find the path of the library. It can not be found by findpackage, but can be dealt with properly by pkgconfig.

You do not need to care about CppAD codegen, since it is a header-only library and has already been installed to the system default path.

Test Examples

After successfully building the library, you can run the examples to verify the installation.

# in the build directory, run the following examples: 
./examples/pushbot_example
./examples/pushbox_example
./examples/cartTransp_example
./examples/waiter_example
./examples/hopper_example

Feel free to try different hyperparameters, and the weighted mode on these or your own problems. For local solver, the hyperparameters are important for the numerical performance.

3. Usage

3.1 General Workflow

This solve adopts the most general optimization problem format:

$$ \begin{aligned} \min_{x} \quad & J(x) \ \text {s.t.}\quad & c_i(x) = 0, \quad i \in \mathcal{E} \ & c_i(x) \ge 0, \quad i \in \mathcal{I} \ \end{aligned} $$

The objective $J(x)$ is convex, while the $c(x)$ are general nonlinear nonconvex constraints. The workflow for defining your own problem and solving it with the CRISP is simple:

1. Define the objective function and constraints.
2. Create the OptimizationProblem , add the objective and constraints to the problem through addObjective, addEqualityConstraint, and addInequalityConstraint.
3. Create the SolverInterface with the defined optimization problem.
4. Initialize the solver with the initial guess and solve the problem.
5. Retrieve the solution.

3.2 C++ Interface

Let's go through how the solver works with the pushbot example src/examples/pushbot/cpp/SolvePushbot.cpp.

1. Define the objective function and constraints using the ad_function_with_param_t or ad_function_t .

   The ad typed variables are just wrappers to enhance the Eigen base type with CppAD and code generation functionality. So you can operate them in Eigen style to ease your definition of the objective and constraints. x is all the independent variables, y is the function value and p is the parameters.

   Here is the example of the objective function, the objective function is designed as a quadratic form to track the terminal states and minimize the control effort, where the terminal states serves as parameters p.

   Similar for all other constraints and we refer to the source code for more details.

ad_function_with_param_t pushbotObjective = [](const ad_vector_t& x, const ad_vector_t& p, ad_vector_t& y) {
    y.resize(1);
    ad_scalar_t tracking_cost(0.0);
    ad_scalar_t control_cost(0.0);
    ad_matrix_t Q(num_state, num_state);
    Q.setZero();
    Q(0,0) = 100;
    Q(1, 1) = 100;
    Q(2, 2) = 100;
    Q(3, 3) = 100;
    ad_matrix_t R(num_control, num_control);
    R.setZero();
    R(0, 0) = 0.001;

    for (size_t i = 0; i < N; ++i) {
        ad_vector_t state(num_state);
        for (size_t j = 0; j < num_state; ++j)
            state(j) = x(i * (num_state + num_control) + j);

        ad_vector_t control(num_control);
        for (size_t j = 0; j < num_control; ++j)
            control(j) = x(i * (num_state + num_control) + num_state + j);
        
        // terminal cost
        if (i == N - 1) {
            ad_vector_t tracking_error = state - p;
            tracking_cost += tracking_error.transpose() * Q * tracking_error;
        }

        if (i < N - 1) {
            ad_vector_t control_error = control;
            control_cost += control_error.transpose() * R * control_error;
        }
    }

    y(0) = tracking_cost + control_cost;
};

2. After defining the objective function and constraints, we create the OptimizationProblem and add the objective and constraints to the problem.

    std::string problemName = "PushbotSwingUp";
    std::string folderName = "model";
    OptimizationProblem pushbotProblem(variableNum, problemName);

    auto obj = std::make_shared<ObjectiveFunction>(variableNum, num_state, problemName, folderName, "pushbotObjective", pushbotObjective);
    auto dynamics = std::make_shared<ConstraintFunction>(variableNum, problemName, folderName, "pushBotDynamicConstraints", pushBotDynamicConstraints);
    auto contact = std::make_shared<ConstraintFunction>(variableNum, problemName, folderName, "pushBotContactConstraints", pushBotContactConstraints);
    auto initial = std::make_shared<ConstraintFunction>(variableNum, num_state, problemName, folderName, "pushBotInitialConstraints", pushBotInitialConstraints);

    // ---------------------- ! the above four lines are enough for generate the auto-differentiation functions library for this problem and the usage in python ! ---------------------- //

    pushbotProblem.addObjective(obj);
    pushbotProblem.addEqualityConstraint(dynamics);
    pushbotProblem.add