Generalized Rigid Body Dynamics Algorithms

This repository contains a C++ library of generalized rigid-body dynamics algorithms. The algorithms are based on the recursive algorithms of Featherstone, but modified such that they can efficiently compute the constrained dynamics of robotic systems with kinematic loops between small groups of bodies, also known as "local loop constraints." Full details of the algorithms can be found in our accompanying paper. Local loop constraints are common in robotic systems that use actuation sub-mechanisms. Actuation sub-mechanisms (e.g., geared transmissions, differential drives, and linkages) are increasingly common in robotic systems, yet to date there has been no efficient method for computing their dynamics. The reason for this is that the local kinematic loops induced by the actuation sub-mechanisms violate the assumptions of conventional recursive dynamics algorithms. The key insight of our generalized algorithms is the clustering of bodies involved in local loop constraints. Clustering bodies enables loop constraints to be resolved locally, i.e., only when that group of bodies is encountered during a forward or backward pass. Our approach is functionally similar to Jain's Local Constraint Embedding, but the derivation is decidedly different, placing an emphasis on generalizing the concept of an articulated body to the case of body clusters, which should increase accessibility for roboticists.

Installation
Usage
Configuration
Benchmarking
Testing
License
Contact Information
Citation

Installation

Dependencies

C++17 or higher
CMake 3.15 or higher
Eigen 3.3.7 or higher
Casadi 3.6.3 or higher

They can be installed using the provided install script.

Supported Operating Systems

Ubuntu 18.04 or higher
MacOS 10.15 or higher (Note: When compiling on a machine with Apple M1 chip, you can optimize the performance by using the following command: cmake -DM1_BUILD=ON ..)

Building

To run the unit tests and bencmarks

Create a build directory: mkdir build && cd build
Run CMake: cmake ..
Build: make
Unit tests can be run all at once with ctest or individually with ./bin/<name-of-unit-test-binary>
Benchmarks can be run with the shell scripts ../Benchmarking/run_time_benchmark.sh or ../Benchmarking/run_accuracy_benchmarks.sh

To incorporate the library into your own project, see Configuration.

Usage

As a general purpose rigid-body dynamics library, the project can be used in a variety of ways. The most common uses are as follows:

Dynamic Simulation: Using the constrained forward dynamics algorithms and the contact dynamics algorithms, the repository can be used as part of a simulator for contact-rich robotic systems.
Model-Based Control: Using the inverse dynamics algorithms, the repository can be used to as part of a whole-body controller to compute the control torques required to achieve a desired state.

At this time, we do not offer support for URDF parsing, so the model has to be constructed manually. The process for building a model proceeds as follows:

Register all of the bodies contained in a cluster by providing their connectivity and inertia information.
Append those bodies to the model by providing information about the cluster joint that connects them.
Repeat the above steps for each cluster in the system. Specific examples of how to build a model can be found in the Robots folder, along with a more detailed README detailing the process.

Configuration

You can use this repository in your project using CMake by:

Adding the include directory in your project's include path.
Linking against the generalized_rbda library.

The following example shows how to do this in CMake:

set(GRBDA_INCLUDE_DIR /path/to/generalized_rbda/include)
find_library(GRBDA_LIBRARY
    NAMES generalized_rbda
    HINTS ${GRBDA_INCLUDE_DIR}/../build)
if(GRBDA_LIBRARY)
    set(GRBDA_LIBRARIES ${GRBDA_LIBRARIES} ${GRBDA_LIBRARY})
endif()

include_directories(`${GRBDA_INCLUDE_DIR}`)
target_link_libraries(${PROJECT_NAME} ${GRBDA_LIBRARIES})

Benchmarking

We have included a set of benchmarks to compare the performance of our algorithms against existing algorithms. We test the algorithms on the following robots:

Forward Dynamics

We compare our constrained forward dynamics algorithm, the Cluster-based Articulated Body Algorithm (C-ABA) against the following algorithms:

Articulated Body Algorithm (ABA): The conventional recursive algorithm for computing the forward dynamics of kinematic chains and branched models. This algorithm is not capable of handling actuation sub-mechanisms.
Projection Method: A non-recursive algorithm that computes the constrained forward dynamics by projecting the equations of motion for the spanning tree coordinates onto the independent coordinates. See Method 3 in Chapter 8.5 of Rigid Body Dynamics Algorithms for more details.
Lagrange Multiplier Method: A non-recursive algorithm that first computes the constraint forces necessary to satisfy closed loop constraints and then uses those forces to compute the forward dynamics. See Method 2 in Chapter 8.5 of Rigid Body Dynamics Algorithms for more details.

Inverse Dynamics

We compare our constrained inverse dynamics algorithm, the Cluster-based Recursive Newton-Euler Algorithm (C-RNEA) against the following algorithms:

Recursive Newton-Euler Algorithm (RNEA): The conventional recursive algorithm for computing the inverse dynamics of kinematic chains and branched models. This algorithm is not capable of handling actuation sub-mechanisms.
Projected RNEA: Computes the inverse dynamics of the spanning tree via RNEA by ignoring the closed loop constraints, and then projects the resulting generalized forces $\tau$ onto the independent coordinates. See Chapter 8.12 of Rigid Body Dynamics Algorithms for more details.

Inverse Operational-Space Inertia Matrix

We compare our constrained inverse operational-space inertia matrix algorithm, the Cluster-based Extended Force Propagator Algorithm (C-EFPA) against the following algorithms:

Extended Force Propagator Algorithm (EFPA): The conventional recursive algorithm for computing the inverse operational-space inertia matrix of kinematic chains and branched models. This algorithm is not capable of handling actuation sub-mechanisms.
Projected EFPA: Computes the inverse operational-space inertia matrix of the spanning tree via EFPA by ignoring the closed loop constraints, and then projects the resulting matrix onto the independent coordinates.

License

The code is licensed under the MIT license. See the LICENSE file for more details.

Contact Information:

For further questions or collaboration inquiries, please contact the developers at:

Matthew Chignoli

Citation

To cite this library in your research, please use the following citation for the accompanying paper:

@article{chignoli2023recursive,
  title={Recursive rigid-body dynamics algorithms for systems with kinematic loops},
  author={Chignoli, Matthew and Adrian, Nicholas and Kim, Sangbae and Wensing, Patrick M.},
  journal={arXiv preprint arXiv:2311.13732},
  year={2023}
}

and the following citation for the codebase:

@misc{grbdaweb,
   author = {Matthew Chignoli, Nicholas Adrian, Patrick M. Wensing, and others},
   title = {GRBDA: Generalized Rigid-Body Dynamics Algorithms},
   howpublished = {https://github.com/ROAM-Lab-ND/generalized_rbda},
   year = {2023}
}

ROAM-Lab-ND / generalized_rbda