Dynobench 🦖

Dynobench 🦖 is a universal benchmark for kinodynamic motion planning. Develop, combine and compare different methods, from trajectory optimization and sample based motion planning to supervised and reinforcement learning.

You will find multiple planners in Dynoplan 🦖

Try out the Python Package!

Install with

pip3 install dynobench

One line check

python3 -c 'import dynobench; import numpy as np; print(dynobench.robot_factory(dynobench.PKGDIR + "models/unicycle1_v0.yaml", [], []).stepOut(np.zeros(3), np.ones(2),.1))'

Run a simple test (it loads an environment, checks collisions...)

python3 -m  dynobench.test.test_1

Dev

Create a Python Package for your local computer:

pip3 wheel . --verbose

and now install,

pip3 install NAME_OF_WHEEL

How to create the python package to upload to PIP

Docker Container

docker run -it --network common  -v  (pwd):/io   quay.io/pypa/manylinux2014_x86_64

Install Dependencies

bash /io/install_all_docker.sh

Create Wheels

bash /io/build_wheels.sh

this will create wheels in wheelhouse_audit

In one line:

docker run -it --network common -v $(pwd):/io quay.io/pypa/manylinux2014_x86_64 /bin/bash -c "bash /io/install_all_docker.sh && bash /io/build_wheels.sh"

Upload to PIP

python3 -m twine upload v002/*

Change v002 for folder where the wheels are!

Robots and Problem Description

TODO

Motion Primitives

Acrobot

Planar Rotor

Planar Rotor Pole

Quadrotor

Unicycle1

Using Dynobench

Submodule

You can use Dynobench as a submodule.

Using cmake, import the library with:

add_subdirectory(dynobench EXCLUDE_FROM_ALL) # use EXCLUDE_FROM_ALL to avoid
                                             # building the tests
...
target_link_libraries(
  my_target
  PRIVATE dynobench::dynobench )

As an example, you can check the CMakeLists.txt and the project structure in Dynoplan

As external Project

First, build Dynobench from source and install with:

git clone https://github.com/quimortiz/dynobench
cd dynobench && mkdir build && cd build
cmake .. -DCMAKE_INSTALL_PREFIX=MY_PATH && make install

Then, add the following lines in CMakeLists.txt of your repository:

find_package(dynobench REQUIRED)
...
target_link_libraries(my_target PRIVATE dynobench::dynobench )

And add the path of the local installation

cmake .. -DCMAKE_PREFIX_PATH=MY_PATH

Hello World with Dynobench

C++ library

main.cpp

#include <iostream>
#include "dynobench/robot_models.hpp"

int main() {
  Model_car_with_trailers car;
  std::cout << "Hello World!" << std::endl;
}

CMakeLists.txt (using Dynobench as an external project)

cmake_minimum_required(VERSION 3.5)
project(
  use_dynobench
  VERSION 0.1.0
  LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED On)

find_package(Boost REQUIRED COMPONENTS program_options unit_test_framework
                                       serialization)
find_package(fcl REQUIRED)
find_package(dynobench REQUIRED)
find_package(yaml-cpp REQUIRED)

add_executable(main main.cpp)

# target_include_directories(main PRIVATE ${DYNOBENCH_INCLUDE_DIRS} )

target_link_libraries(main PRIVATE dynobench::dynobench yaml-cpp)

Python Viewer

Check the viewers with:

 python3 ../utils/viewer/viewer_test.py

and

VISUALIZE=1 python3 ../utils/viewer/viewer_test.py

Python Bindings

We provide python bindings for the dynamical systems

Check the example with,

python ../example/test_robot.py

from the build directory.

Adding a new dynamical system

In this short tutorial, we summarize the steps we followed to add the model Integrator2_2d.

Integrator2_2d is a double integrator in 2d:

The state is $\mathbf{x} = [x,y, \dot{x}, \dot{y}]$, control is $\mathbf{u} = [\ddot{x} , \ddot{y}]$. Second order dynamics are $\frac{d}{d t}[ \dot{x}, \dot{y} ] = \mathbf{u}$. Thus, the step function is $\mathbf{x}_{k+1} = A \mathbf{x} + B \mathbf{u} $ with:

$$A = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\\ 0 & 1 & 0 & \Delta t \\\ 0 & 0 & 1 & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix} , B = \begin{bmatrix} 0 & 0 \\\ 0 & 0 \\\ \Delta t & 0 \\\ 0 & \Delta t \end{bmatrix}$$

Control Bounds are $|u_x| \leq 1$, $|u_y| \leq 1$, and state bounds on velocity $|\dot{x}| \leq 1 $, $|\dot{y}| \leq 1 $.

First, we have implemented a new class in src/integrator2_2d.cpp and include/dynobench/integrator2_2d.hpp. We store all parameters in a separate class, Integrator2_2d_params. A robot model implements 4 main functionalities: distance and cost bounds between states, a dynamics function, bounds on state and control, and collision against obstacles. Check the code!

// dynobench/double_integrator_2d.hpp and src/double_integrator_2d.hpp
struct Integrator2_2d_params { ... } ;
struct Integrator2_2d : public Model_robot { ... };

The base class Model_robot already provides default implementation of some methods.

For example, we only have to implement the dynamics in continuous time $\dot{x} = f(x,u)$ and the derivatives, while the Euler step is computed in the base class.

Once the model is ready, we add it to the factory:

// src/robot_models.cpp
#include "dynobnech/double_integrator_2d.hpp"
...
std::unique_ptr<Model_robot> robot_factory(
...

 else if (dynamics == "double_intergrator_2d") {
    return std::make_unique<Double_integrator_2d>(file, p_lb, p_ub);
  }

It is recommend to check the Jacobians using finite differences. We add the test t_integrator2_2d in test in test/test_models.cpp.

// test/test_models.cpp
  model->calcDiffV(Jx, Ju, x0, u0);

  finite_diff_jac(
      [&](const Eigen::VectorXd &x, Eigen::Ref<Eigen::VectorXd> y) {
        model->calcV(y, x, u0);
      },
      x0, 4, Jx_diff);

  finite_diff_jac(
      [&](const Eigen::VectorXd &u, Eigen::Ref<Eigen::VectorXd> y) {
        model->calcV(y, x0, u);
      },
      u0, 4, Ju_diff);

  BOOST_TEST((Jx - Jx_diff).norm() < 1e-5);
  BOOST_TEST((Ju - Ju_diff).norm() < 1e-5);

Now we add the c++ file to the library:

add_library(
  dynobench
  ./src/robot_models.cpp
...
  ./src/integrator2_2d.cpp)

We define double_integrator_2d_v0 with a configuration file models/integrator2_2d_v0.yaml, and one scenario with envs/integrator2_2d_v0/park.yaml

Let's add a viewer in python. We need a new class:

# utils/viewer/integrator2_2d_viewer.py
class Robot :

class Integrator2_2dViewer (RobotViewer):

RobotViewer is a base class that provides default functionality. Robot is the class that draws the robot (e.g. using a rectangle )

# utils/viewer/viewer_cli.py

def get_robot_viewer(robot: str) -> robot_viewer.RobotViewer:
...
    elif robot == "integrator2_2d":
        viewer = double_integrator_2d_viewer.Integrator2_2dViewer()

Now, you can view the robot with (e.g. from build directory):

python3 ../utils/viewer/viewer_cli.py --robot integrator2_2d --env ../envs/integrator2_2d_v0/park.yaml -i

That' s all!

Now we can use Dynoplan to solve the problem!

For example, see test/optimization/test_optimization_1.cpp in Dynoplan

BOOST_AUTO_TEST_CASE(t_opti_integrator2) {

  Options_trajopt options;
  Problem problem(dynobench_base "envs/integrator2_2d_v0/park.yaml");
  problem.models_base_path = dynobench_base "models/";

  Trajectory init_guess, traj_out;
  init_guess.num_time_steps = 50;
  Result_opti opti_out;
  trajectory_optimization(problem, init_guess, options, traj_out, opti_out);
  BOOST_TEST(opti_out.feasible);

  // write down the generated trajectory

  std::string filename = "/tmp/dynoplan/traj_t_opti_integrator2.yaml";
  create_dir_if_necessary(filename.c_str());
  std::ofstream out(filename);
  traj_out.to_yaml_format(out);
}

The planners in Dynoplan that depend on OMPL require to implement a small wrapper to interace with OMPL.

More Motion Primitives

You will find a small set of motion primitives for each system in dynobench.

A large set of primitives for each system can be downloaded from Google Drive. This can be done manually with a web browser or using the command line with gdown. For example:

gdown --fuzzy "https://drive.google.com/file/d/1r_ecGwdfvWnVWxPsvR4d8Hjcayxg5PsB/view?usp=drive_link"

All primitive in two ZIP files: https://drive.google.com/drive/folders/1-Nvctva17I8aFsWvHfdQFWTIDUNWwgcM?usp=drive_link

Primitves per system:

• unicycle1_v0 https://drive.google.com/file/d/15dXqC_OdrI8KjaHRNakYgk9IXLtTeMtt/view?usp=drive_link

• quadrotor_v1 (OMPL-style) https://drive.google.com/file/d/1r_ecGwdfvWnVWxPsvR4d8Hjcayxg5PsB/view?usp=drive_link

• quadrotor_v0 https://drive.google.com/file/d/1j57kwE5hFgO-46LjStv_zqm6S5BFUsY8/view?usp=drive_link

• Acrobot_v0 https://drive.google.com/file/d/1mLiTgcpXSI9UHHss4Qt7AIsRwJPbPC2H/view?usp=drive_link

• Roto_Pole_v0 https://drive.google.com/file/d/1KMb4IDgucHN8uWI9YN_W07AhX59tkph_/view?usp=drive_link

• Planar Rotor_v0 https://drive.google.com/file/d/18kI3qXweA4RgvDxtV3vfxnfc_BhX52j8/view?usp=drive_link

• Car1_v0 https://drive.google.com/file/d/1TPX3c8RvMOy9hiaKL-kUE8M61OknDrDK/view?usp=drive_link

• Unicycle 2 _v0 https://drive.google.com/file/d/1PoK1kbiLRFq_hkv3pVWU0csNr4hap0WX/view?usp=drive_link

• Unicycle 1 v2 https://drive.google.com/file/d/1IvwN-e1jn5P0P1ILaVwSrUnIeBlFxhHI/view?usp=drive_link

• Unicycle 1 v1 https://drive.google.com/file/d/1OLuw5XICTueoZuleXOuD6vNh3PCWfHif/view?usp=drive_link

Pytho Dev

Create a Python Package with:

pip3 wheel . --verbose

Roadmap

Dynobench is still in an alpha stage.

Next steps are:

• Gym interface for RL. Train PPO for unicycle park.
• Use Pinocchio to define the models
• Add a second viewer (e.g. build on top of viewers provided by Pinocchio)
• Interface to Mujoco for simulating problems with contacts.