forked from rock-planning/planning-motion_planning_libraries
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathREADME
142 lines (133 loc) · 9.01 KB
/
README
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
# 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 | Description |
| ----------- | ---------------------- | ----------- |
| 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 | Description |
| ----------- | ------------------------- | ----------- |
| 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