RSMotion - C++ Library for Reeds-Shepp Cars

Overview

RSMotion - C++ Library for Reeds-Shepp Cars

A C++11 library for calculating optimal paths of car movements using Reeds-Shepp Cars.

Introduction

Let's assume a car has the following freedoms: 1) going forward or in reverse and 2) turning left or right. What is the optimal path for the car to go from point A to point B?

The RSMotion C++ library calculates this optimal path by using Reeds-Shepp Cars. Based on a start position and destination it provides the path segments that define the optimal path. Each path segment is defined by its type (a turn or straight), length and direction (forward or reverse). Typical applications are in robotics, computer games or other simulations.

In the video below you can see what the generated paths look like.

preview

Example of Reeds-Shepp paths. The car goes forwards on green path segments, and in reverse on red segments.

preview

Example of in-game usage of the RSMotion library. The forklift truck is picking up trees and bringing them to storage. The turns and straights are defined by Reeds-Shepp paths.

RSMotion is based on an implementation in the OPML library but has been rewritten to be independent of boost/OPML which makes it easier to embed. And to provide a modern C++ interface.

How to embed?

The library consist of a single source file (.cpp) and a couple of headers (.h). The easiest way to use this library is to simply add the source and header files to your current project:

1. Copy the library/include/rsmotion directory to the include directory of your project
2. Copy library/src/rsmotion.cpp to the source directory in your project and include it into your build.

An alternative would be to create a static library and link that into your application.

RSMotion requires a C++11 compiler and has been tested with Clang 8.0 and Visual Studio 2017.

How to use?

First include the header file:

#include <rsmotion/rsmotion.h>

To calculate a path you must first specify a CarState for the initial position (a 3D vector) and orientation (Quaternion) of the car:

using namespace rsmotion::math;

// set the wheelbase to 1 meter
const float wheelbase = 1.0f;

// set the start position to the origin
const Vec3f startPosition {0.f, 0.f, 0.f};

// set the orientation (yaw; around y axis) to zero degrees (i.e. no rotation)
const Quatf startOrientation { Vec3f{0,1,0}, Anglef::Degrees(0) };

// create the initial CarState
CarState carStart{{startPosition, startOrientation}, wheelbase};

After setting this up, you can calculate the optimal path by specifying the position and orientation of the destination:

const Vec3f finishPosition {3.f, 0.f, 2.f};
const Quatf finishOrientation { Vec3f{0,1,0}, Anglef::Degrees(65) };
const PointState finishPoint {finishPosition, finishOrientation};

const auto path = SearchShortestPath(carStart, finishPoint);

Using the returned path, you can move the car along the path using either a normalized progress value [0.f, 1.0f] :

const float progressNormalized = 0.3f;
CarState movedCar = TraversePathNormalized(progressNormalized, path, carStart);

or with the travelled distance along the path:

const float distanceTravelled = 1.2f;
CarState movedCar = TraversePathDistance(distanceTravelled, path, carStart);

// movedCar now contains the position and orientation
// of both the rear (movedCar.Rear) and front (movedCar.Front) axes.

Coordinate system

The library uses 3D vectors for representing positions as this is likely to be more common in applications. However, it projects onto a 2D plane before calculating the optimal path. The default coordinate system assumes [RIGHT, UP, FORWARD] as the axes. If you use another coordinate system, you'll have to change this in coordinatesystem.h.

Building the example

An example application resides in the example directory. Currently it only builds on Win64 out of the box, as I have only added Win64 binaries of the bgfx library (a low-level graphics API). It should work on other OS-es but you have to build and link the bgfx library (and its dependencies) yourself.

The example requires CMake to build.

Instructions when using VS 2017 (from the root of the project):

msbuild /p:Configuration=Release .\example.sln PS> .\Release\example.exe">
PS> mkdir example/build
PS> cd example/build
PS> cmake ../ -G "Visual Studio 15 2017 Win64"
PS> msbuild /p:Configuration=Release .\example.sln
PS> .\Release\example.exe

Obviously, you can specify another generator for CMake if you have a newer version of Visual Studio or use another build system such as Ninja.

If you see a black screen: ensure that you invoke the executable from a working directory that contains the shaders (fs_cubes.bin/vs_cubes.bin).

How does it work?

Well, to get to know how it really works is to read the paper by Reeds and Shepp. It shows that the calculations and proofs are quite involved. Fortunately we only have to implement it.

Basically, each path is defined by three to five path segments. Each segment can be either a turn (notated as the character C) or a straight (notated as the character S). For example, a path with 1) a turn, 2) a straight, and 3) a turn can be notated as the word CSC. Another example would be three turns: CCC. Note that a turn can either go left or right. Also, a turn can be a of 0 degrees which essentially reduces the turn to a no-op.

As it turns out, all optimal paths can be defined by one of five words. This is reflected in the code:

Path SearchShortestPath(State toState)
{
    // the optimal path is always one of
    // these five path configurations
    return std::min({CSC(toState),
                     CCC(toState),
                     CCCC(toState),
                     CCSC(toState),
                     CCSCC(toState)});
}

For each of these five configurations we test a variety of paths by reflecting and time-flipping (going in reverse). Out of all valid paths (those that reach the destination) we take the path with the minimum length:

template <class R>
Path CalculateMinPath(Path basePath, State toState, R baseFormula)
{
    Path minPath;

    // for each equation, try each operation and return the shortest
    if (baseFormula(toState, basePath))
    {
        minPath = std::min(minPath, basePath);
    }
    if (baseFormula(Reflect(toState), basePath))
    {
        minPath = std::min(minPath, Reflect(basePath));
    }
    if (baseFormula(Timeflip(toState), basePath))
    {
        minPath = std::min(minPath, Timeflip(basePath));
    }
    if (baseFormula(Reflect(Timeflip(toState)), basePath))
    {
        minPath = std::min(minPath, Reflect(Timeflip(basePath)));
    }

    return minPath;
}

As you can see the algorithm is a kind of brute-force approach in that it calculates a lot of alternatives and simply picks the one with the smallest length. However, the result is proven to be optimal and the search space is reduced by recognizing that some paths are isomorphic to others. I have not done measurements on the runtime but I expect it to not vary too much between path searches. It certainly is not dependent on the path length.

Limitations

There are some limitations to consider:

  • There is no collision detection with walls or other inanimate objects. It assumes a total open space.
  • The turn radius is currently equal to the unit circle.
  • The progression along a path does not take into account (de)acceleration and assumes constant speed.

You might want to add this functionality to your application if it demands it. This library will unlikely fulfill all the requirements of your application. As adaptations of the library are to be expected it is okay to take the library as good start and build upon it.

License

The MIT License (MIT)

Copyright (c) 2019 Bas Geertsema [email protected]

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

Issues
  • compile failure

    compile failure

    /home/zhuifei/code/autogox/src/planning/include/rsmotion/math.h:1460:37: error: declaration of ‘using Quaternion = class rsmotion::math::Quaternion’ [-fpermissive] using Quaternion = Quaternion; ^ /home/zhuifei/code/autogox/src/planning/include/rsmotion/math.h:1183:7: error: changes meaning of ‘Quaternion’ from ‘class rsmotion::math::Quaternion’ [-fpermissive] class Quaternion ^ /home/zhuifei/code/autogox/src/planning/include/rsmotion/math.h:1461:32: error: declaration of ‘using Vector = class rsmotion::math::Vector<T, 3ul>’ [-fpermissive] using Vector = Vector<T, 3>; ^ /home/zhuifei/code/autogox/src/planning/include/rsmotion/math.h:846:7: error: changes meaning of ‘Vector’ from ‘class rsmotion::math::Vector<T, 3ul>’ [-fpermissive] class Vector<T, 3> ^

    opened by jmloveyj 3
Owner
Bas Geertsema
Bas Geertsema
Flutter app where you can find your information about your Favorite Super Cars ⚡❤

Super Cars App (Flutter) ⚡ Now you can freely discover and browse your Favourite Super Cars ❤ . Speed! ?? Getting Started This project is a starting p

Shehroz Ali 4 Apr 13, 2022
SSD1306 library and simple graphics core library based on Adafruit GFX Library.

Raspberry Pico SSD1306 + GFX Library Based on Adafruit GFX Library https://github.com/adafruit/Adafruit-GFX-Library Usage Hardware Connect your SSD130

Marcin Bober 27 May 17, 2022
Libft is an individual project at 42 that requires us to re-create some standard C library functions including some additional ones that can be used later to build a library of useful functions for the rest of the program.

Libft is an individual project at 42 that requires us to re-create some standard C library functions including some additional ones that can be used later to build a library of useful functions for the rest of the program.

Paulo Rafael Ramalho 0 Apr 5, 2022
Samir Teymurov 1 Oct 6, 2021
F Graphics Library (FGL) is a small graphics C++ portable library for LCD displays on embedded systems

F Graphics Library (FGL) Full documentation: fgl.docsforge.com (By Filipe Chagas) F Graphics Library is a C++ library that I created for use in embedd

Filipe Chagas 8 Dec 14, 2021
Itpp - IT++ library mirror/fork. C++ library of mathematical, signal processing and communication classes and functions.

Introduction ************ IT++ is a C++ library of mathematical, signal processing and communication classes and functions. Its main use is in simula

null 18 Apr 9, 2022
2D physics header-only library for videogames developed in C using raylib library.

Physac Physac is a small 2D physics engine written in pure C. The engine uses a fixed time-step thread loop to simluate physics. A physics step contai

Víctor Fisac 218 Jun 24, 2022
This is a helper library to abstract away interfacing with floppy disk drives in a cross-platform and open source library.

Adafruit Floppy This is a helper library to abstract away interfacing with floppy disk drives in a cross-platform and open source library. Adafruit Fl

Adafruit Industries 125 Jun 22, 2022
`lv_lib_100ask` is a reference for various out of the box schemes based on lvgl library or an enhanced interface for various components of lvgl library.

Introduction lv_lib_100ask is a reference for various out of the box schemes based on lvgl library or an enhanced interface for various components of

100askTeam 14 Jun 23, 2022
QtVerbalExpressions - This Qt lib is based off of the C++ VerbalExpressions library. [MIT]

QtVerbalExpressions Qt Regular Expressions made easy This Qt lib is based off of the C++ VerbalExpressions library by whackashoe. Testing if we have a

null 53 May 12, 2022
A header-only library for C++(0x) that allows automagic pretty-printing of any container.

cxx-prettyprint =============== A pretty printing library for C++ containers. Synopsis: Simply by including this header-only library in your sourc

Louis Delacroix 526 Jun 9, 2022
A standalone and lightweight C library

Klib: a Generic Library in C Overview Klib is a standalone and lightweight C library distributed under MIT/X11 license. Most components are independen

Attractive Chaos 3.6k Jun 25, 2022
a small C library for x86 CPU detection and feature extraction

libcpuid libcpuid provides CPU identification for the x86 (and x86_64). For details about the programming API, you might want to take a look at the pr

Veselin Georgiev 329 May 31, 2022
NIH Utility Library

libnih is a light-weight "standard library" of C functions to ease the development of other libraries and applications. Its goals are: * despite it

Scott James Remnant 80 Mar 23, 2022
Functional programming style pattern-matching library for C++

Mach7: Pattern Matching for C++ by Yuriy Solodkyy, Gabriel Dos Reis, Bjarne Stroustrup Abstract Pattern matching is an abstraction mechanism that can

Yuriy Solodkyy 1.2k Jun 24, 2022
Cross-platform C++11 header-only library for memory mapped file IO

mio An easy to use header-only cross-platform C++11 memory mapping library with an MIT license. mio has been created with the goal to be easily includ

null 1.3k Jun 24, 2022
Parsing Expression Grammar Template Library

Welcome to the PEGTL The Parsing Expression Grammar Template Library (PEGTL) is a zero-dependency C++ header-only parser combinator library for creati

The Art of C++ 1.5k Jun 23, 2022
Simple Dynamic Strings library for C

Simple Dynamic Strings Notes about version 2: this is an updated version of SDS in an attempt to finally unify Redis, Disque, Hiredis, and the stand a

Salvatore Sanfilippo 3.9k Jul 2, 2022
Semantic version library written in ANSI C

semver.c Semantic version v2.0 parser and render written in ANSI C with zero dependencies. Features Standard compliant (otherwise, open an issue) Vers

Tom 169 May 16, 2022