giacomo-b / cpprobotics

Header-only C++ library for robotics, control, and path planning algorithms. Work in progress, contributions are welcome!

C++ Robotics

A header-only, fully-templated C++ library for control algorithms.

Goals • Requirements • Getting Started • Features • TODOs • References

---

Goals

C++ Robotics has the following goals:

• Implement as many robotics algorithms as possible, without sacrificing quality. These include, e.g., control, path planning, and estimation algorithms.
• Be easy to use and to get started with, thus the header-only format and minimal external dependencies.
• Be fast: by making use of templates, most algorithms use static-size data structures whose size is known at compilation time.

This project is inspired by PythonRobotics. Instead of being just an educational repo, this library aims at implementing fast algorithms with a consistent API, to guarantee runtime performance and ease of use.

While this is still a work in progress, I would appreciate it if you left any suggestions or starred the repo. Any help is greatly appreciated!

---

Requirements

• C++11 compiler

• CMake 3.14+ (if using Visual Studio on Windows you should be able to import the project as a CMake project)

• The C++ dependencies will be obtained automatically by CPM.cmake. Note that the library only depends on Eigen, but matplotplusplus is used in the examples folder to plot the results.

---

Getting started

Following are some examples to get started. The examples folder contains several examples that are useful to get started.

CppRobotics aims to be modular, which means:

• Once you define a dynamical system, most algorithms will be readily available for it to use

• Data should flow seamlessly between objects (e.g. estimator -> controller)

• Once you setup an algorithm, you should be able to change the dynamical system and integrate it directly

Clone this repo

git clone https://github.com/giacomo-b/CppRobotics.git

Building and running the examples

Given a generic EXAMPLE that you want to run, the following commands build it:

cmake -S examples/EXAMPLE -B build/EXAMPLE
cmake --build build/EXAMPLE

On Windows, this will default to a Debug configuration. To build the project in release mode, you can add --config=Release after the first command.

To run the example on Linux, macOS, and most Unix-based systems:

./build/EXAMPLE/main

On Windows:

./build/EXAMPLE/CONFIG_TYPE/main

where CONFIG_TYPE is either Debug or Release, depending on how you configured the project.

Using the library in your projects

Importing the library

#include <robotics/robotics.hpp>

System definition

SystemBase represents a generic dynamical system. In most cases, you will be using either a LinearSystem or NonlinearSystem.

Since the library is templated, to define a system you need to define:

• The number of states

• The number of inputs (control actions)

• The number of outputs

e.g.:

static constexpr int N = 2; // Number of states
static constexpr int M = 1; // Number of inputs
static constexpr int P = 2; // Number of outputs

Type aliases can come handy, and prevent the coder from mixing up the wrong dimensions:

using State = Robotics::ColumnVector<N>;
using Input = Robotics::ColumnVector<M>;
using Output = Robotics::ColumnVector<P>;

using StateMatrix = Robotics::SquareMatrix<N>;
using InputMatrix = Robotics::Matrix<N, M>;
using OutputMatrix = Robotics::Matrix<P, N>;
using FeedthroughMatrix = Robotics::Matrix<P, N>;

Let's define a linear system whose state form is

x' = A * x + B * u
y  = C * x + D * u

To set up a LinearSystem:

StateMatrix A;
A << 1, 0,
     0, 1;

InputMatrix B;
B << 1, 0;

OutputMatrix C;
C << 1, 0,
     0, 1;

Note that having templates not only improves runtime performance, but also enforces compile-time checking. If you initialize a matrix with the wrong number of elements, the code will not compile.

Matrices C and D are not required: they are null by default if not provided. In this case, D is null. To define the system:

Robotics::Model::LinearSystem<N, M, P> system(A, B, C);

The initial state is zero by default. You can set a custom initial state as follows:

system.SetInitialState(State(1.0, -1.0));

---

Features

Check out TheLartians/ModernCppStarter if you want to include these features in your project.

Running tests

From the root directory:

cmake -S test -B build/test
cmake --build build/test
CTEST_OUTPUT_ON_FAILURE=1 cmake --build build/test --target test

# or simply call the executable: 
./build/test/RoboticsTests

To also collect code coverage information, run CMake with the -DENABLE_TEST_COVERAGE=1 option.

Running clang-format for autoformatting

This requires clang-format, cmake-format and pyyaml to be installed on the current system.

cmake -S test -B build/test

# view changes
cmake --build build/test --target format

# apply changes
cmake --build build/test --target fix-format

See Format.cmake for details.

Build the documentation

The documentation is automatically built and published whenever a GitHub Release is created. To manually build documentation, call the following command.

cmake -S documentation -B build/doc
cmake --build build/doc --target GenerateDocs
# view the docs
open build/doc/doxygen/html/index.html

To build the documentation locally, you will need Doxygen, jinja2 and Pygments installed in your system.

Build everything at once

The project also includes an all directory that allows building all targets at the same time. This is useful during development, as it exposes all subprojects to your IDE and avoids redundant builds of the library.

cmake -S all -B build
cmake --build build

# run tests
./build/test/RoboticsTests
# format code
cmake --build build --target fix-format
# run standalone
./build/standalone/Robotics --help
# build docs
cmake --build build --target GenerateDocs

Additional tools

The test and standalone subprojects include the tools.cmake file which is used to import additional tools on-demand through CMake configuration arguments. The following are currently supported.

Sanitizers

Sanitizers can be enabled by configuring CMake with -DUSE_SANITIZER=<Address | Memory | MemoryWithOrigins | Undefined | Thread | Leak | 'Address;Undefined'>.

Static Analyzers

Static Analyzers can be enabled by setting -DUSE_STATIC_ANALYZER=<clang-tidy | iwyu | cppcheck>, or a combination of those in quotation marks, separated by semicolons. By default, analyzers will automatically find configuration files such as .clang-format. Additional arguments can be passed to the analyzers by setting the CLANG_TIDY_ARGS, IWYU_ARGS, or CPPCHECK_ARGS variables.

Ccache

Ccache can be enabled by configuring with -DUSE_CCACHE=<ON | OFF>.

---

TODOs

• Add more algorithms
• Add support for nonlinear systems automatic differentiation, so that the Jacobians are automatically computed (see autodiff)
• Add a README.md to each example folder, to explain the theory
• Cache the packages downloaded by CPM.CMake (currently everything is re-downloaded every time a new example is built)
• Many more, feel free to add your ideas!

---

References

As mentioned above, this repo was originally inspired by AtsushiSakai/PythonRobotics. So go check it out if you want to see more algorithms (or if you want to help port a few of them!).

cpprobotics's Issues

Eigen offline

Hey! On building the repository, it throws a fatal error of not being able to clone the eigen repository from gitlab. This is because the main eigen repository is offline due to a technical problem. Can the build process be modified to avoid such things later on? Like maybe maintaining a mirror and cloning that instead?

Mistake in build instructions

This first command here fails to run because the directory examples/EXAMPLE doesn't exist:

cmake -S examples/EXAMPLE -B build/EXAMPLE
cmake --build build/EXAMPLE

Should the library be renamed to Control++?

As per the title, should we use Control++ (acronym C++) instead of CppRobotics?

I like the generality of CppRobotics, but it is also unoriginal and might convey the wrong idea about the library.

Open to suggestions!

CMake Error

On building with cmake with version 3.16.3, build process crashes with this output:

CMake Error at CMakeLists.txt:56 (set_target_properties):
  INTERFACE_LIBRARY targets may only have whitelisted properties.  The
  property "CXX_STANDARD" is not allowed.


-- Configuring incomplete, errors occurred!

Line 56, where the fault points to

The time step should be specified at each call

The time step is currently set with a setter method, it would be better if the user had to specify the time step every time the dynamics is propagated, such as in:

system.PropagateDynamics(input, dt);

Examples of the problem:

CppRobotics/examples/pid/main.cpp

Line 34 in bf1965b

pid.SetTimeStep(dt);

CppRobotics/examples/extended-kalman-filter/main.cpp

Line 26 in bf1965b

system.SetTimeStep(dt);

Why is this a problem?

1. The user must explicitly call the setter, otherwise they might be propagating the dynamics with the wrong (default) time step
2. Variable time steps are not easily supported

Don't Use pragma once

#pragma once isn't part of the c++ standard and isn't supported by all compilers, so I would suggest not using it.

Should classes accept `std::size_t` as template parameters instead of `int`?

All class templates currently require ints as input parameters. This choice was made because Eigen accepts ints as parameters, and it is straightforward to just forward the value.

Since int can be negative, a better solution would be to require std::size_t as template parameters. These inputs can then be cast to ints and then forwarded to Eigen (e.g. static_cast<int>(N)).

Using CRTP instead of standard polymorphism

A Curiously Recurring Template Pattern (CRTP) would probably be a better design choice instead of classic inheritance that makes use of virtual functions.

Advantage in the case of, e.g. LinearSystem and NonlinearSystem (both of which are Systems):

1. There's need to replace System instances at runtime or to work with heterogeneous collections of Systems.
2. CRTP would be more flexible
3. Virtual methods imply vtable lookups at runtime. With CRTP the compiler knows which class gets instantiated, thus guaranteeing better performance.