GTDynamics

This library is still under very active development, hence bleeding edge, and not "supported" in the way GTSAM is. In particular, we are still actively re-factoring the way we deal with time and time intervals.

Full kinodynamics constraints for arbitrary robot configurations with factor graphs.

GTDynamics is a library that allows the user to express the full kinodynamics constraints of an arbitrary robot configuration on a factor graph. These constraints can be used to solve the forward and inverse dynamics problems.

Dependencies

• GTSAM4
• gtwrap
• sdformat15

Installing SDFormat

GTDynamics uses the SDFormat parser to parse SDF/URDF files containing robot descriptions.

Homebrew

Using Homebrew is the easiest way to get SDFormat installed and it also makes switching versions straightforward.

$ # Install homebrew.
$ /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/HEAD/install.sh)"
$ # Set up the tap and install sdformat15
$ brew tap osrf/simulation
$ brew install sdformat15

Source

Alternatively, you can install from source if you are on Linux or want more fine-tuned control.

We provide an Ubuntu-based process below. Please reference this tutorial for complete details on installing from source.

# Install basic dependencies
sudo apt-get install -y ruby-dev build-essential libboost-all-dev cmake pkg-config wget lsb-release

# Setup the repo
sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
sudo apt-get update

# Install SDFormat dependencies
sudo apt-get install -y libtinyxml2-dev liburdfdom-dev libgz-cmake2-dev libgz-tools-dev libgz-math6-dev

# Set the version to install
export GTD_SDFormat_VERSION="15.0.0"

# Download specific version of SDFormat
wget http://osrf-distributions.s3.amazonaws.com/sdformat/releases/sdformat-${GTD_SDFormat_VERSION}.tar.bz2

tar -xvjf sdformat-${GTD_SDFormat_VERSION}.tar.bz2

cd sdformat-${GTD_SDFormat_VERSION}
mkdir build && cd build

cmake -DCMAKE_INSTALL_PREFIX=../install ..
make -j4
sudo make install

Installing GTDynamics

$ git clone https://github.com/borglab/GTDynamics.git
$ cd GTDynamics
$ mkdir build; cd build
# We can specify the install path with -DCMAKE_INSTALL_PREFIX
# To compile with Python wrapper support, ensure that GTSAM was built with Python
#   and use -DGTDYNAMICS_BUILD_PYTHON=ON
$ cmake -DCMAKE_INSTALL_PREFIX=../install ..
$ make
$ sudo make install
# Run make python-install for installation with Python

Running Tests

$ make check

Running Examples

The examples directory contains various full example projects demonstrating the use of GTDynamics for various robotic applications.

We recommend going through the examples to get a better understanding of how to use GTDynamics for your own use cases.

NOTE The examples are made to run within the GTDynamics source folder and are not standalone projects.

1. Build GTDynamics and navigate to the build folder:

   $ cd build

2. Run the example using:

   $ make example_XXX.run

   where XXX corresponds to the example name. Example names align with folder names in examples, but some of them have added suffixes. For example, make example_forward_dynamics.run or make example_spider_walking_forward.

3. Run the simulation using:

   $ make example_XXX.sim

   where XXX corresponds to the folder name in examples. For example, make example_quadruped_mp.sim or make example_spider_walking.sim. Make sure you have pybullet and matplotlib installed in your Python environment.

Including GTDynamics With CMake

The examples/cmake_project_example directory contains an example CMake-based project that demonstrates how to include GTDynamics in your application.

Use this as a template when you want to set up your own project that uses GTDynamics (e.g. separate git repo, ROS, personal libraries, etc).

To build the project:

1. Navigate to the example's subdirectory and create a build directory. e.g.

   cd GTDynamics/examples/cmake_project_example
   mkdir build; cd build

2. Make the example.

   If GTDynamics was installed to ~/JohnDoe/GTDynamics/install, then run the cmake command with:

   cmake -DCMAKE_PREFIX_PATH=~/JohnDoe/GTDynamics/install ..
   make

3. Run the example!

   ./example

Python Wrapper (Recommended use case)

GTDynamics now supports a Pybind11-based Python API.

GTWrap comes bundled with GTSAM, which generates a corresponding GTSAM Python API. The same GTWrap package can be used to generate python bindings for GTDynamics (i.e. it is not necessary to manually install a separate GTWrap).

Note: when using CMake, it is ideal for GTSAM and GTDynamics to have the same, non /usr/local prefix for installing packages. To update the CMake prefix from a system directory, use the flag CMAKE_INSTALL_PREFIX=/path/to/install/dir when running cmake.

To compile and install the GTDynamics python library:

1. Ensure that GTSAM is built with generated python bindings. If not, go to the build directory and run cmake with the flag GTSAM_BUILD_PYTHON=ON. It is highly advised to specify a non user/local CMake prefix for installing packages. Afterwards, install the GTSAM python package.

   cmake -DGTSAM_BUILD_PYTHON=ON -DCMAKE_INSTALL_PREFIX=/path/to/install/dir ..
   make && make install && make python-install

2. In the GTDynamics build directory, run cmake with the flag GTDYNAMICS_BUILD_PYTHON=ON. It is highly advised for the GTDynamics CMake prefix to match the CMake prefix used for GTSAM. Again, use the CMAKE_INSTALL_PREFIX=/path/to/install/dir flag to specify the updated prefix.

   cmake -DGTDYNAMICS_BUILD_PYTHON=ON -DCMAKE_INSTALL_PREFIX=/path/to/install/dir ..

3. Build as normal and install the python package.

   make && make python-install

4. To run the Python tests, you can simply run:

   make python-test

   You can also run individual test suites, e.g. with:

   make python-test.base
   make python-test.cablerobot

(Alternative) Python Wrapper installation

If preferred, GTWrap can be downloaded and installed separately. Afterwards, follow the instructions above from step 2 for building and installing the GTDynamics python bindings.

Citing This Work

Please cite the following paper if you use this code as part of any published research:

@misc{2011.06194,
    Author = {Mandy Xie, Alejandro Escontrela, and Frank Dellaert},
    Title = {A Factor-Graph Approach for Optimization Problems with Dynamics Constraints},
    Year = {2020},
    Eprint = {arXiv:2011.06194},
}