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 ๐ฆ
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
pip3 wheel . --verbose
and now install,
pip3 install NAME_OF_WHEEL
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!
TODO
Acrobot
Planar Rotor
Planar Rotor Pole
Quadrotor
Unicycle1
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
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
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)
Check the viewers with:
python3 ../utils/viewer/viewer_test.py
and
VISUALIZE=1 python3 ../utils/viewer/viewer_test.py
We provide python bindings for the dynamical systems
Check the example with,
python ../example/test_robot.py
from the build
directory.
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
Control Bounds are
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
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.
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
Create a Python Package with:
pip3 wheel . --verbose
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.