MPL - Motion Planning Libraries

Introduction

This library integrates motion planning libraries mainly for navigation. In general it realizes path planning on a 2D Envire traversability maps. If you want to add your own planning library you have to inheret from AbstractMotionPlanningLibrary and implement the core virtual functions. See Config.hpp to get a list of the available planning libraries, planners and environments. See the module documentation how to set up a small navigation stack in ROCK. In the \ref sec_libraries section the available planning libraries and their configuration are described.

Frames

Three types of coordinate frames are used within this library:

First the world frame which defines the position of the system in meter within the root node of the received traversability map.
Second the grid local frame which defines the position within the traversability map in meters.
And third the grid frame in which the position is described within the traversability map in grid cells.

In general the user just works with the world frame. The following code example shows the complete transformation from world2grid.

      Eigen::Affine3d world2local = trav->getEnvironment()->relativeTransform(
          trav->getEnvironment()->getRootNode(),
          trav->getFrameNode());
      local_pose.setTransform(world2local * world_pose.getTransform());
      trav->toGrid(local_pose.position.x(), local_pose.position.y(), x_grid, y_grid, ...);

After the traversability map has been set (MotionPlanningLibraries::setTravGrid) the start and goal position/pose within the world can be defined (MotionPlanningLibraries::setStartState, MotionPlanningLibraries::setGoalState). The integrated planning libraries will just work within the grid frame. If the planning library requires grid local cordinates (e.g. SBPL) the cell size in meters can be extracted from the received traversability map. Each library has to implement AbstractMotionPlanningLibrary::fillPath to support the found path which can be specified within the grid or the grid local frame.

Libraries

OMPL Sample-Based Planning

OMPL [http://ompl.kavrakilab.org/] is a very flexible library for Sample-Based Planning which is mainly be used for high-dimensional planning. Additional spaces, validators and objectives (costs) can be added without any great effort. The following environments have been implemted:

ENV_XY: Simple point-based optimizing planning.
ENV_XYTHETA: Control based planning using a simple ODE or a car like ODE to build up the navigation tree. OMPL does not support optimization for this environment, so no usable results are produced yet. Instead SBPL ENV_XYTHETA should be used for planning which regards the orientation of the system.
ENV_SHERPA: This environment allows circle based planning regarding an adaptive footprint. The planner tries to maximise the footprint for a safe stand, just shrinks it for narrow passages. This is a first test implementation for body embedded planning which will be extended in the future.
ENV_ARM: In general OMPL is used for manipulation / arm movement. This has been integrated just for testing. For a meaningful use a robot model and octomaps have to be integrated into MPL.

SBPL Search-Based Planning

SBPL [http://www.sbpl.net/] is a library mainly used for orientation based navigation within a discrete map supporting powerful planning algorithms like Anytime-DStar. This is done by using so called motion primitives. This primitives are small defined motions which are combined and optimized to the overall trajectory by the planner. The SBPL library supports a MATLAB script to generate the motion primitives. For that the user has to define all motion primites for the first three discrete angles by hand (if overall 16 discrete angles are used) which are used to create all primitives for all angles. This has been replaced by a complete automatic primitive generation for which only the speed, the multipliers and the turning radius of the system have to be defined. Each movement type (forward, backward, lateral, forward-turn, backward-turn, pointturn) got its own multiplier.

ENV_XY: Simple point-based optimizing planning.
ENV_XYTHETA: Orientation based optimized planning. The forward- and turning-speed, the turning radius and the multipliers have to be defined which are used to generate the primitive which are used to generate the final path. An internal epsilion is used by SBPL to describe the optimality of the current solution. An optimal path has been found if the epsilon reaches 1.0.

Configuration

The following section describes the minimal required configuration for each environment. \subsection General	Parameter	Description
mPlanningLibType	Defines the planning library, see motion_planning_libraries::PlanningLibraryType
mEnvType	Defines the environment, see motion_planning_libraries::EnvType

\subsection OMPL	Environment	Parameter
ENV_XYTHETA	mMobilty	mSpeed and mTurningSpeed are used to define the control space and to calculate the cost traversing a grid cell.
	mFootprintLengthMinMax	Used for the car ODE.
ENV_SHERPA	mFootprintRadiusMinMax	Footprint is defined as a circle here.
	mNumFootprintClasses	To reduce the plannign dimension the footprint radius is descretized.
	mTimeToAdaptFootprint	Time to change the system from min to max footprint.
	mAdaptFootprintPenalty	Additional costs which are added if the footprint changes between two states.
ENV_ARM	mJointBorders	Borders of the arm joints.

\subsection SBPL	Environment	Parameter
ENV_XY	mSBPLEnvFile	(optional) Allows to load an SBPL environment instead of using the Envire traversability map.
ENV_XYTHETA	mMobilty	Speeds are used together with the multipliers for the cost calculation. In addition the multipliers are used to activates the movement types (>0). mMinTurnignRadius takes care that the curve primitives are driveable for the system.
	mSBPLEnvFile	(optional) Allows to load an SBPL environment instead of using the Envire traversability map.
	mSBPLMotionPrimitivesFile	(optional) Allows to use an existing SBPL primitive file instead of creating one based on the mMobility parameters.
	mFootprintLengthMinMax	The max value is used to define the robot length in SBPL.
	mFootprintWidthMinMax	The max value is used to define the robot width in SBPL.
	mNumIntermediatePoints	Sets the number of intermediate points which are added to each primitive to create smoother trajectories.
	mNumPrimPartition	Defines how much primitive for each movement type should be created. More primitives will optimize the result but increase the planning time.
	mPrimAccuracy	Defines how close a primitive has to reach a discrete end position. If this parameter is reduced towards 0, the discretization error will be reduced but the length of the primitives will be increased and the overall number of primitive for each movement type could also be reduced.

TODOs

"Adds method to remove obstacles within the start pose."
"Optional: If the goal lies on an obstacle use next valid goal position."
"Split State and Config to classes using inheritance. Possible?"
"Calculate correct center of rotation in SBPL MotionPrimitives."
"Add struct to combine all replanning parameters."
"Config: Add boolean whether a new map should initiate a replanning."
"Config: Add boolean whether a new goal pose should initiate a replanning."
"Config: Add boolean if only optimal trajectories (epsilon == 1) should be supported."
"Adds new trajectory format to integrate all type of movements."
"Escape trajectory cannt be created correctly anymore since Janoschs cahnges."

Structure

This directory structure follows some simple rules, to allow for generic build processes and simplify reuse of this project.

For build automation the project structure should be parsed and validated

-- src/ Contains all header (.h/.hpp) and source files -- build/ The target directory for the build process, temporary content -- bindings/ Language bindings for this package, e.g. put into subfolders such as |-- ruby/ Ruby language bindings -- viz/ Source files for a vizkit plugin / widget related to this library -- resources/ General resources such as images that are needed by the program -- configuration/ Configuration files for running the program -- external/ When including software that needs a non standard installation process, or one that can be easily embedded include the external software directly here -- doc/ should contain the existing doxygen file: doxygen.conf

rock-planning / planning-motion_planning_libraries

readme