12345qiupeng
/
mpc_local_planner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

project imported with clean history

master
Christoph Rösmann 2020-02-20 12:32:14 +01:00
commit 238d8293ac
82 changed files with 10144 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
.gitignore vendored Normal file
View File

@ -0,0 +1,18 @@
# Build files
build
# Autosave files
*.autosave
# MacOS temporary files
.DS_Store
# QT project files
*.txt.user*
# kDevelop project files
.kdev4
*.kdev4
# Version control
.svn

61
README.md Normal file
View File

@ -0,0 +1,61 @@
mpc_local_planner ROS Package
=============================
The mpc_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack.
It provides a generic and versatile model predictive control implementation with minimum-time and quadratic-form receding-horizon configurations.
## Authors
* Christoph Rösmann <christoph.roesmann@tu-dortmund.de>
### Citing the Software
*Since a lot of time and effort has gone into the development, please cite at least one of the following publications if you are using the software for published work.*
**Standard MPC and Hypergraph**
- C. Rösmann, M. Krämer, A. Makarow, F. Hoffmann und T. Bertram: Exploiting Sparse Structures in Nonlinear Model Predictive Control with Hypergraphs, IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), New Zealand, July 2018.
**Time-Optimal MPC and Hypergraph**
- C. Rösmann: [Time-optimal nonlinear model predictive control - Direct transcription methods with variable discretization and structural sparsity exploitation](http://dx.doi.org/10.17877/DE290R-20283). Dissertation, TU Dortmund University, Oct. 2019.
- C. Rösmann, F. Hoffmann und T. Bertram: Timed-Elastic-Bands for Time-Optimal Point-to-Point Nonlinear Model Predictive Control, European Control Conference (ECC), Austria, July 2015.
- C. Rösmann, F. Hoffman und T. Bertram: Convergence Analysis of Time-Optimal Model Predictive Control under Limited Computational Resources, European Control Conference (ECC), Denmark, June 2016.
### License
Copyright (c) 2020,
TU Dortmund - Institute of Control Theory and Systems Engineering.
All rights reserved.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <https://www.gnu.org/licenses/>.
The package depends on third-party packages:
- *control_box_rst*, GPLv3, https://github.com/rst-tu-dortmund/control_box_rst
Please check also the dependencies of control_box_rst (not all modules
are included at the moment)
- *Ipopt*, EPL 1.0, https://github.com/coin-or/Ipopt
It depends on other ROS packages, which are listed in the package.xml. They are licensed under BSD resp. Apache 2.0.
### Requirements
Install dependencies (listed in the *package.xml* and *CMakeLists.txt* file) using *rosdep*:
rosdep install mpc_local_planner

95
mpc_local_planner/.clang-format Normal file
View File

@ -0,0 +1,95 @@
---
Language: Cpp
# BasedOnStyle: Google
# Doc: https://clang.llvm.org/docs/ClangFormatStyleOptions.html
AccessModifierOffset: -3
AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: true
AlignConsecutiveDeclarations: false
AlignEscapedNewlinesLeft: true
AlignOperands: true
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortIfStatementsOnASingleLine: true
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterClass: true
AfterControlStatement: true
AfterEnum: false
AfterFunction: true
AfterNamespace: false
AfterObjCDeclaration: false
AfterStruct: true
AfterUnion: true
BeforeCatch: true
BeforeElse: true
IndentBraces: false
BreakBeforeBinaryOperators: None
# Custom: break according to "BraceWrapping"
BreakBeforeBraces: Custom
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
ColumnLimit: 150
CommentPragmas: '^ IWYU pragma:'
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: true
DerivePointerAlignment: false
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
#FixNamespaceComments: true
ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]
IncludeCategories:
- Regex: '^<.*\.h>'
Priority: 1
- Regex: '^<.*'
Priority: 2
- Regex: '.*'
Priority: 3
IndentCaseLabels: true
IndentWidth: 4
IndentWrappedFunctionNames: false
KeepEmptyLinesAtTheStartOfBlocks: true
MacroBlockBegin: ''
MacroBlockEnd: ''
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: false
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Left
ReflowComments: true
SortIncludes: true
SpaceAfterCStyleCast: false
#SpaceAfterTemplateKeyword: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: ControlStatements
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Cpp11
#Standard: Auto
TabWidth: 8
UseTab: Never
...

272
mpc_local_planner/CMakeLists.txt Normal file
View File

@ -0,0 +1,272 @@
cmake_minimum_required(VERSION 2.8.3)
project(mpc_local_planner)
# Set to Release in order to speed up the program significantly
set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
base_local_planner
costmap_2d
costmap_converter
dynamic_reconfigure
roscpp
geometry_msgs
interactive_markers
nav_core
nav_msgs
mbf_costmap_core
mbf_msgs
mpc_local_planner_msgs
pluginlib
std_msgs
teb_local_planner
tf2
tf2_eigen
tf2_geometry_msgs
tf2_ros
visualization_msgs
)
# Find control_box_rst
# TODO: Currently, we use QUIET rather than REQUIRED to avoid warnings
# as some components depend on each other (circularly)
# which is currently not handled well by the exported cmake config
find_package(control_box_rst QUIET COMPONENTS
core
communication
controllers
numerics
systems
optimization
optimal_control
systems
PATH_SUFFIXES share/control_box_rst control_box_rst
PATHS ${CMAKE_BINARY_DIR} /usr/local
)
# Eigen3 FindScript Backward compatibility (ubuntu saucy)
# Since FindEigen.cmake is deprecated starting from jade.
# if (EXISTS "FindEigen3.cmake")
find_package(Eigen3 REQUIRED)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
set(EXTERNAL_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include ${EXTERNAL_INCLUDE_DIRS}
LIBRARIES mpc_local_planner_numerics mpc_local_planner_optimization
CATKIN_DEPENDS roscpp mpc_local_planner_msgs control_box_rst teb_local_planner
# DEPENDS
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${EXTERNAL_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(mpc_local_planner_utils
src/utils/publisher.cpp
src/utils/conversion.cpp
src/utils/time_series_se2.cpp
)
add_library(mpc_local_planner_optimal_control
src/optimal_control/full_discretization_grid_base_se2.cpp
src/optimal_control/finite_differences_grid_se2.cpp
src/optimal_control/finite_differences_variable_grid_se2.cpp
src/optimal_control/stage_inequality_se2.cpp
src/optimal_control/quadratic_cost_se2.cpp
src/optimal_control/final_state_conditions_se2.cpp
src/optimal_control/min_time_via_points_cost.cpp
)
add_library(mpc_local_planner
src/controller.cpp
src/mpc_local_planner_ros.cpp
)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(test_mpc_optim_node src/test_mpc_optim_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(mpc_local_planner_utils corbo_core corbo_systems)
target_link_libraries(mpc_local_planner_utils
corbo_core
corbo_systems
)
target_link_libraries(mpc_local_planner_optimal_control
corbo_optimal_control
)
target_link_libraries(mpc_local_planner
mpc_local_planner_utils
mpc_local_planner_optimal_control
corbo_controllers
${catkin_LIBRARIES}
)
target_link_libraries(test_mpc_optim_node
mpc_local_planner
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_local_planner.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

189
mpc_local_planner/cfg/rviz_test_mpc_optim.rviz Normal file
View File

@ -0,0 +1,189 @@
Panels:
- Class: rviz/Displays
Help Height: 81
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Path1
Splitter Ratio: 0.5
Tree Height: 562
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.100000001
Cell Size: 0.100000001
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Fine Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 100
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.119999997
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 0; 0
Pose Style: Arrows
Radius: 0.0299999993
Shaft Diameter: 0.0199999996
Shaft Length: 0.100000001
Topic: /test_mpc_optim_node/local_plan
Unreliable: false
Value: true
- Alpha: 1
Arrow Length: 0.300000012
Axes Length: 0.300000012
Axes Radius: 0.00999999978
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: true
Head Length: 0.0700000003
Head Radius: 0.0299999993
Name: PoseArray
Shaft Length: 0.230000004
Shaft Radius: 0.00999999978
Shape: Arrow (Flat)
Topic: /test_optim_node/teb_poses
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /test_mpc_optim_node/mpc_markers
Name: Marker
Namespaces:
PointObstacles: true
Queue Size: 100
Value: true
- Class: rviz/InteractiveMarkers
Enable Transparency: true
Enabled: true
Name: InteractiveMarkers
Show Axes: false
Show Descriptions: true
Show Visual Aids: false
Update Topic: /marker_obstacles/update
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 9.30938148
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 2.73932815
Y: 0.504589677
Z: 0.0263484083
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.56479633
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.70042801
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1354
X: 137
Y: 50

113
mpc_local_planner/cfg/test_mpc_optim_node.yaml Normal file
View File

@ -0,0 +1,113 @@
## Robot settings
robot:
type: "unicycle"
unicycle:
max_vel_x: 0.4
max_vel_x_backwards: 0.2
max_vel_theta: 0.3
acc_lim_x: 0.0 # deactive bounds with zero
dec_lim_x: 0.0 # deactive bounds with zero
acc_lim_theta: 0.0 # deactivate bounds with zero
simple_car:
wheelbase: 0.4
front_wheel_driving: False
max_vel_x: 0.4
max_vel_x_backwards: 0.2
max_steering_angle: 1.4
acc_lim_x: 0.0 # deactive bounds with zero
dec_lim_x: 0.0 # deactive bounds with zero
max_steering_rate: 0.5 # deactive bounds with zero
## Footprint model for collision avoidance
footprint_model:
type: "point"
collision_avoidance:
min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_factor: 1.5
cutoff_factor: 5
## Planning grid
grid:
type: "fd_grid"
grid_size_ref: 20
dt_ref: 0.3
xf_fixed: [True, True, True] # E.g., set last one to False in order to unfix the final orientation
warm_start: True
collocation_method: "forward_differences"
cost_integration_method: "left_sum"
variable_grid:
enable: True
min_dt: 0.0;
max_dt: 10.0;
grid_adaptation:
enable: True
dt_hyst_ratio: 0.1
min_grid_size: 2
max_grid_size: 50
## Planning options
planning:
objective:
type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
quadratic_form:
state_weights: [2.0, 2.0, 2.0]
control_weights: [1.0, 1.0]
integral_form: False
minimum_time_via_points:
position_weight: 8.0
orientation_weight: 0.0
via_points_ordered: False
terminal_cost:
type: "quadratic" # can be "none"
quadratic:
final_state_weights: [2.0, 2.0, 2.0]
terminal_constraint:
type: "none" # can be "none"
l2_ball:
weight_matrix: [1.0, 1.0, 1.0]
radius: 5
## Controller options
controller:
outer_ocp_iterations: 1
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
force_reinit_new_goal_dist: 1.0
force_reinit_new_goal_angular: 1.57
force_reinit_num_steps: 0
global_plan_overwrite_orientation: True
global_plan_viapoint_sep: -1
allow_init_with_backward_motion: True
publish_ocp_results: True
print_cpu_time: False
## Solver settings
solver:
type: "ipopt"
ipopt:
iterations: 100
max_cpu_time: -1.0
ipopt_numeric_options:
tol: 1e-4
ipopt_string_options:
linear_solver: "mumps"
hessian_approximation: "limited-memory" # exact or limited-memory
ipopt_integer_options:
print_level: 2
lsq_lm:
iterations: 10
weight_init_eq: 2
weight_init_ineq: 2
weight_init_bounds: 2
weight_adapt_factor_eq: 1.5
weight_adapt_factor_ineq: 1.5
weight_adapt_factor_bounds: 1.5
weight_adapt_max_eq: 500
weight_adapt_max_ineq: 500
weight_adapt_max_bounds: 500

126
mpc_local_planner/include/mpc_local_planner/controller.h Normal file
View File

@ -0,0 +1,126 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef CONTROLLER_H_
#define CONTROLLER_H_
#include <corbo-controllers/predictive_controller.h>
#include <corbo-optimal-control/structured_ocp/structured_optimal_control_problem.h>
#include <mpc_local_planner/optimal_control/stage_inequality_se2.h>
#include <mpc_local_planner/systems/robot_dynamics_interface.h>
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/pose_se2.h>
#include <mpc_local_planner_msgs/StateFeedback.h>
#include <ros/subscriber.h>
#include <ros/time.h>
#include <memory>
#include <mutex>
namespace mpc_local_planner {
/**
* @brief MPC controller for mobile robots
*
* @ingroup controllers
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class Controller : public corbo::PredictiveController
{
public:
using Ptr = std::shared_ptr<Controller>;
using PoseSE2 = teb_local_planner::PoseSE2;
Controller() = default;
bool configure(ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles, teb_local_planner::RobotFootprintModelPtr robot_model,
const std::vector<teb_local_planner::PoseSE2>& via_points);
bool step(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::Twist& vel, double dt, ros::Time t, corbo::TimeSeries::Ptr u_seq,
corbo::TimeSeries::Ptr x_seq);
bool step(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t,
corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq);
// implements interface method
corbo::ControllerInterface::Ptr getInstance() const override { return std::make_shared<Controller>(); }
static corbo::ControllerInterface::Ptr getInstanceStatic() { return std::make_shared<Controller>(); }
void setOptimalControlProblem(corbo::OptimalControlProblemInterface::Ptr ocp) = delete;
RobotDynamicsInterface::Ptr getRobotDynamics() { return _dynamics; }
StageInequalitySE2::Ptr getInequalityConstraint() { return _inequality_constraint; }
void stateFeedbackCallback(const mpc_local_planner_msgs::StateFeedback::ConstPtr& msg);
void publishOptimalControlResult();
void setInitialPlanEstimateOrientation(bool estimate) { _initial_plan_estimate_orientation = estimate; }
// implements interface method
void reset() override;
protected:
corbo::DiscretizationGridInterface::Ptr configureGrid(const ros::NodeHandle& nh);
RobotDynamicsInterface::Ptr configureRobotDynamics(const ros::NodeHandle& nh);
corbo::NlpSolverInterface::Ptr configureSolver(const ros::NodeHandle& nh);
corbo::StructuredOptimalControlProblem::Ptr configureOcp(const ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles,
teb_local_planner::RobotFootprintModelPtr robot_model,
const std::vector<teb_local_planner::PoseSE2>& via_points);
bool generateInitialStateTrajectory(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf,
const std::vector<geometry_msgs::PoseStamped>& initial_plan, bool backward);
std::string _robot_type;
corbo::DiscretizationGridInterface::Ptr _grid;
RobotDynamicsInterface::Ptr _dynamics;
corbo::NlpSolverInterface::Ptr _solver;
StageInequalitySE2::Ptr _inequality_constraint;
corbo::StructuredOptimalControlProblem::Ptr _structured_ocp;
ros::Publisher _ocp_result_pub;
bool _ocp_successful = false;
std::size_t _ocp_seq = 0;
bool _publish_ocp_results = false;
bool _print_cpu_time = false;
bool _prefer_x_feedback = false; // prefer state feedback over odometry feedback
ros::Subscriber _x_feedback_sub;
std::mutex _x_feedback_mutex;
ros::Time _recent_x_time;
Eigen::VectorXd _recent_x_feedback;
teb_local_planner::PoseSE2 _last_goal;
double _force_reinit_new_goal_dist = 1.0;
double _force_reinit_new_goal_angular = 0.5 * M_PI;
corbo::DiscreteTimeReferenceTrajectory _x_seq_init;
bool _initial_plan_estimate_orientation = true;
bool _guess_backwards_motion = true;
int _force_reinit_num_steps = 0;
};
} // namespace mpc_local_planner
#endif // CONTROLLER_H_

430
mpc_local_planner/include/mpc_local_planner/mpc_local_planner_ros.h Normal file
View File

@ -0,0 +1,430 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef MPC_LOCAL_PLANNER_ROS_H_
#define MPC_LOCAL_PLANNER_ROS_H_
#include <ros/ros.h>
// base local planner base class and utilities
#include <base_local_planner/costmap_model.h>
#include <base_local_planner/goal_functions.h>
#include <base_local_planner/odometry_helper_ros.h>
#include <mbf_costmap_core/costmap_controller.h>
#include <nav_core/base_local_planner.h>
// mpc_local_planner related classes
#include <mpc_local_planner/controller.h>
#include <mpc_local_planner/utils/publisher.h>
// teb_local_planner related classes
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/pose_se2.h>
#include <teb_local_planner/robot_footprint_model.h>
// message types
#include <costmap_converter/ObstacleMsg.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
// transforms
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>
// costmap
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_converter/costmap_converter_interface.h>
// dynamic reconfigure
// #include <dynamic_reconfigure/server.h>
// #include <mpc_local_planner/MpcLocalPlannerReconfigureConfig.h>
#include <boost/shared_ptr.hpp>
#include <memory>
#include <mutex>
namespace mpc_local_planner {
/**
* @class MpcLocalPlannerROS
* @brief Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract
* interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF).
* @todo Escape behavior, more efficient obstacle handling
*/
class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController
{
using PoseSE2 = teb_local_planner::PoseSE2;
using RobotFootprintModelPtr = teb_local_planner::RobotFootprintModelPtr;
using Point2dContainer = teb_local_planner::Point2dContainer;
using ObstContainer = teb_local_planner::ObstContainer;
using ViaPointContainer = std::vector<PoseSE2>;
using ObstaclePtr = teb_local_planner::ObstaclePtr;
using PointObstacle = teb_local_planner::PointObstacle;
using CircularObstacle = teb_local_planner::CircularObstacle;
using LineObstacle = teb_local_planner::LineObstacle;
using PolygonObstacle = teb_local_planner::PolygonObstacle;
public:
/**
* @brief Default constructor of the plugin
*/
MpcLocalPlannerROS();
/**
* @brief Destructor of the plugin
*/
~MpcLocalPlannerROS();
/**
* @brief Initializes the teb plugin
* @param name The name of the instance
* @param tf Pointer to a tf buffer
* @param costmap_ros Cost map representing occupied and free space
*/
void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Set the plan that the teb local planner is following
* @param orig_global_plan The plan to pass to the local planner
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.
* @remark Extended version for MBF API
* @param pose the current pose of the robot.
* @param velocity the current velocity of the robot.
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base.
* @param message Optional more detailed outcome as a string
* @return Result code as described on ExePath action result:
* SUCCESS = 0
* 1..9 are reserved as plugin specific non-error results
* FAILURE = 100 Unspecified failure, only used for old, non-mfb_core based plugins
* CANCELED = 101
* NO_VALID_CMD = 102
* PAT_EXCEEDED = 103
* COLLISION = 104
* OSCILLATION = 105
* ROBOT_STUCK = 106
* MISSED_GOAL = 107
* MISSED_PATH = 108
* BLOCKED_PATH = 109
* INVALID_PATH = 110
* TF_ERROR = 111
* NOT_INITIALIZED = 112
* INVALID_PLUGIN = 113
* INTERNAL_ERROR = 114
* 121..149 are reserved as plugin specific errors
*/
uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity,
geometry_msgs::TwistStamped& cmd_vel, std::string& message);
/**
* @brief Check if the goal pose has been achieved
*
* The actual check is performed in computeVelocityCommands().
* Only the status flag is checked here.
* @return True if achieved, false otherwise
*/
bool isGoalReached();
/**
* @brief Dummy version to satisfy MBF API
*/
bool isGoalReached(double xy_tolerance, double yaw_tolerance) { return isGoalReached(); };
/**
* @brief Requests the planner to cancel, e.g. if it takes too much time
* @remark New on MBF API
* @return True if a cancel has been successfully requested, false if not implemented.
*/
bool cancel() { return false; };
/** @name Public utility functions/methods */
//@{
/**
* @brief Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities.
*
* Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component).
* @param tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle)
* @return Translational and angular velocity combined into an Eigen::Vector2d
*/
static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel);
/**
* @brief Get the current robot footprint/contour model
* @param nh const reference to the local ros::NodeHandle
* @return Robot footprint model used for optimization
*/
static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh);
/**
* @brief Set the footprint from the given XmlRpcValue.
* @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
* @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point
* @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the
* sub-arrays should all have exactly 2 elements (x and y coordinates).
* @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came.
* It is used only for reporting errors.
* @return container of vertices describing the polygon
*/
static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name);
/**
* @brief Get a number from the given XmlRpcValue.
* @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros
* @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point
* @param value double value type
* @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came.
* It is used only for reporting errors.
* @returns double value
*/
static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name);
//@}
protected:
/**
* @brief Update internal obstacle vector based on occupied costmap cells
* @remarks All occupied cells will be added as point obstacles.
* @remarks All previous obstacles are cleared.
* @sa updateObstacleContainerWithCostmapConverter
* @todo Include temporal coherence among obstacle msgs (id vector)
* @todo Include properties for dynamic obstacles (e.g. using constant velocity model)
*/
void updateObstacleContainerWithCostmap();
/**
* @brief Update internal obstacle vector based on polygons provided by a costmap_converter plugin
* @remarks Requires a loaded costmap_converter plugin.
* @remarks All previous obstacles are cleared.
* @sa updateObstacleContainerWithCostmap
*/
void updateObstacleContainerWithCostmapConverter();
/**
* @brief Update internal obstacle vector based on custom messages received via subscriber
* @remarks All previous obstacles are NOT cleared. Call this method after other update methods.
* @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter
*/
void updateObstacleContainerWithCustomObstacles();
/**
* @brief Update internal via-point container based on the current reference plan
* @remarks All previous via-points will be cleared.
* @param transformed_plan (local) portion of the global plan (which is already transformed to the planning frame)
* @param min_separation minimum separation between two consecutive via-points
*/
void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped>& transformed_plan, double min_separation);
/**
* @brief Callback for the dynamic_reconfigure node.
*
* This callback allows to modify parameters dynamically at runtime without restarting the node
* @param config Reference to the dynamic reconfigure config
* @param level Dynamic reconfigure level
*/
// void reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level);
/**
* @brief Callback for custom obstacles that are not obtained from the costmap
* @param obst_msg pointer to the message containing a list of polygon shaped obstacles
*/
void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
/**
* @brief Callback for custom via-points
* @param via_points_msg pointer to the message containing a list of via-points
*/
void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg);
/**
* @brief Prune global plan such that already passed poses are cut off
*
* The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform.
* If no valid transformation can be found, the method returns \c false.
* The global plan is pruned until the distance to the robot is at least \c dist_behind_robot.
* If no pose within the specified treshold \c dist_behind_robot can be found,
* nothing will be pruned and the method returns \c false.
* @remarks Do not choose \c dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned.
* @param tf A reference to a tf buffer
* @param global_pose The global pose of the robot
* @param[in,out] global_plan The plan to be transformed
* @param dist_behind_robot Distance behind the robot that should be kept [meters]
* @return \c true if the plan is pruned, \c false in case of a transform exception or if no pose cannot be found inside the threshold
*/
bool pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose,
std::vector<geometry_msgs::PoseStamped>& global_plan, double dist_behind_robot = 1);
/**
* @brief Transforms the global plan of the robot from the planner frame to the local frame (modified).
*
* The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
* such that the index of the current goal pose is returned as well as
* the transformation between the global plan and the planning frame.
* @param tf A reference to a tf buffer
* @param global_plan The plan to be transformed
* @param global_pose The global pose of the robot
* @param costmap A reference to the costmap being used so the window size for transforming can be computed
* @param global_frame The frame to transform the plan to
* @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also
* bounded by the local costmap size!]
* @param[out] transformed_plan Populated with the transformed plan
* @param[out] current_goal_idx Index of the current (local) goal pose in the global plan
* @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
* @return \c true if the global plan is transformed, \c false otherwise
*/
bool transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
const geometry_msgs::PoseStamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame,
double max_plan_length, std::vector<geometry_msgs::PoseStamped>& transformed_plan, int* current_goal_idx = NULL,
geometry_msgs::TransformStamped* tf_plan_to_global = NULL) const;
/**
* @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner.
*
* If the current (local) goal point is not the final one (global)
* substitute the goal orientation by the angle of the direction vector between
* the local goal and the subsequent pose of the global plan.
* This is often helpful, if the global planner does not consider orientations. \n
* A moving average filter is utilized to smooth the orientation.
* @param global_plan The global plan
* @param local_goal Current local goal
* @param current_goal_idx Index of the current (local) goal pose in the global plan
* @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame
* @param moving_average_length number of future poses of the global plan to be taken into account
* @return orientation (yaw-angle) estimate
*/
double estimateLocalGoalOrientation(const std::vector<geometry_msgs::PoseStamped>& global_plan, const geometry_msgs::PoseStamped& local_goal,
int current_goal_idx, const geometry_msgs::TransformStamped& tf_plan_to_global,
int moving_average_length = 3) const;
/**
* @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint
*
* This method prints warnings if validation fails.
* @remarks Currently, we only validate the inscribed radius of the footprints
* @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization
* @param costmap_inscribed_radius Inscribed radius of the footprint model used for the costmap
* @param min_obst_dist desired distance to obstacles
*/
void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist);
private:
// Definition of member variables
// external objects (store weak pointers)
costmap_2d::Costmap2DROS* _costmap_ros; //!< Pointer to the costmap ros wrapper, received from the navigation stack
costmap_2d::Costmap2D* _costmap; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper)
tf2_ros::Buffer* _tf; //!< pointer to tf buffer
// internal objects
Controller _controller;
ObstContainer _obstacles; //!< Obstacle vector that should be considered during local trajectory optimization
Publisher _publisher;
std::shared_ptr<base_local_planner::CostmapModel> _costmap_model;
corbo::TimeSeries::Ptr _x_seq = std::make_shared<corbo::TimeSeries>();
corbo::TimeSeries::Ptr _u_seq = std::make_shared<corbo::TimeSeries>();
std::vector<geometry_msgs::PoseStamped> _global_plan; //!< Store the current global plan
base_local_planner::OdometryHelperRos _odom_helper; //!< Provides an interface to receive the current velocity from the robot
pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> _costmap_converter_loader; //!< Load costmap converter plugins at runtime
boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> _costmap_converter; //!< Store the current costmap_converter
// std::shared_ptr<dynamic_reconfigure::Server<MpcLocalPlannerReconfigureConfig>>
// dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime
ros::Subscriber _custom_obst_sub; //!< Subscriber for custom obstacles received via a ObstacleMsg.
std::mutex _custom_obst_mutex; //!< Mutex that locks the obstacle array (multi-threaded)
costmap_converter::ObstacleArrayMsg _custom_obstacle_msg; //!< Copy of the most recent obstacle message
ViaPointContainer _via_points;
ros::Subscriber _via_points_sub; //!< Subscriber for custom via-points received via a Path msg.
bool _custom_via_points_active = false; //!< Keep track whether valid via-points have been received from via_points_sub_
std::mutex _via_point_mutex; //!< Mutex that locks the via_points container (multi-threaded)
PoseSE2 _robot_pose; //!< Store current robot pose
PoseSE2 _robot_goal; //!< Store current robot goal
geometry_msgs::Twist _robot_vel; //!< Store current robot translational and angular velocity (vx, vy, omega)
bool _goal_reached = false; //!< store whether the goal is reached or not
ros::Time _time_last_infeasible_plan; //!< Store at which time stamp the last infeasible plan was detected
int _no_infeasible_plans = 0; //!< Store how many times in a row the planner failed to find a feasible plan.
geometry_msgs::Twist _last_cmd; //!< Store the last control command generated in computeVelocityCommands()
ros::Time _time_last_cmd;
RobotFootprintModelPtr _robot_model;
std::vector<geometry_msgs::Point> _footprint_spec; //!< Store the footprint of the robot
double _robot_inscribed_radius; //!< The radius of the inscribed circle of the robot (collision possible)
double _robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot
std::string _global_frame; //!< The frame in which the controller will run
std::string _robot_base_frame; //!< Used as the base frame id of the robot
// flags
bool _initialized; //!< Keeps track about the correct initialization of this class
struct Parameters
{
double xy_goal_tolerance = 0.2;
double yaw_goal_tolerance = 0.1;
bool global_plan_overwrite_orientation = true;
double global_plan_prune_distance = 1.0;
double max_global_plan_lookahead_dist = 1.5;
bool is_footprint_dynamic = false;
bool include_costmap_obstacles = true;
double costmap_obstacles_behind_robot_dist = 1.5;
double global_plan_viapoint_sep = -1;
std::string odom_topic = "odom";
double controller_frequency = 10;
} _params;
struct CostmapConverterPlugin
{
std::string costmap_converter_plugin;
double costmap_converter_rate = 5;
bool costmap_converter_spin_thread = true;
} _costmap_conv_params;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}; // end namespace mpc_local_planner
#endif // MPC_LOCAL_PLANNER_ROS_H_

149
mpc_local_planner/include/mpc_local_planner/optimal_control/fd_collocation_se2.h Normal file
View File

@ -0,0 +1,149 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef FINITE_DIFFERENCES_COLLOCATION_H_
#define FINITE_DIFFERENCES_COLLOCATION_H_
#include <corbo-numerics/finite_differences_collocation.h>
#include <mpc_local_planner/utils/math_utils.h>
#include <functional>
#include <memory>
namespace mpc_local_planner {
/**
* @brief Collocation via forward differences (specialized for SE2)
*
* Forward differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner:
* \f[
* \frac{x_{k+1} - x_k}{\delta T} = f(x_k, u_k)
* \f]
*
* @see ForwardDiffCollocation FiniteDifferencesCollocationInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class ForwardDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface
{
public:
// Implements interface method
corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared<ForwardDiffCollocationSE2>(); }
// Implements interface method
void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt,
const corbo::SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> error) override
{
assert(error.size() == x1.size());
assert(x1.size() >= 3);
// assert(dt > 0 && "dt must be greater then zero!");
system.dynamics(x1, u1, error);
error.head(2) -= (x2.head(2) - x1.head(2)) / dt;
error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt;
if (x1.size() > 3)
{
int n = x1.size() - 3;
error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt;
}
}
};
/**
* @brief Collocation via midpoint differences (specialized for SE2)
*
* Midpoint differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner:
* \f[
* \frac{x_{k+1} - x_k}{\delta T} = f(0.5*(x_k + x_{k+1}), u_k)
* \f]
*
* @see FiniteDifferencesCollocationInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class MidpointDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface
{
public:
// Implements interface method
corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared<MidpointDiffCollocationSE2>(); }
// Implements interface method
void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt,
const corbo::SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> error) override
{
assert(error.size() == x1.size());
assert(dt > 0 && "dt must be greater then zero!");
system.dynamics(0.5 * (x1 + x2), u1, error);
error.head(2) -= (x2.head(2) - x1.head(2)) / dt;
error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt;
if (x1.size() > 3)
{
int n = x1.size() - 3;
error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt;
}
}
};
/**
* @brief Collocation via Crank-Nicolson differences (specialized for SE2)
*
* Crank-Nicolson differences approximate \f$ \dot{x} = f(x, u) \f$ in the following manner:
* \f[
* \frac{x_{k+1} - x_k}{\delta T} = 0.5 * ( f(x_k, u_k) + f(x_{k+1}, u_k))
* \f]
*
* @see FiniteDifferencesCollocationInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class CrankNicolsonDiffCollocationSE2 : public corbo::FiniteDifferencesCollocationInterface
{
public:
// Implements interface method
corbo::FiniteDifferencesCollocationInterface::Ptr getInstance() const override { return std::make_shared<CrankNicolsonDiffCollocationSE2>(); }
// Implements interface method
void computeEqualityConstraint(const StateVector& x1, const InputVector& u1, const StateVector& x2, double dt,
const corbo::SystemDynamicsInterface& system, Eigen::Ref<Eigen::VectorXd> error) override
{
assert(error.size() == x1.size());
assert(dt > 0 && "dt must be greater then zero!");
Eigen::VectorXd f1(x1.size());
system.dynamics(x1, u1, f1);
system.dynamics(x2, u1, error);
// error = (x2 - x1) / dt - 0.5 * (f1 + error);
error.head(2) -= (x2.head(2) - x1.head(2)) / dt - 0.5 * (f1.head(2) + error.head(2));
error.coeffRef(2) -= normalize_theta(x2.coeffRef(2) - x1.coeffRef(2)) / dt - 0.5 * (f1.coeffRef(2) + error.coeffRef(2));
if (x1.size() > 3)
{
int n = x1.size() - 3;
error.tail(n) -= (x2.tail(n) - x1.tail(n)) / dt - 0.5 * (f1.tail(n) + error.tail(n));
}
}
};
} // namespace mpc_local_planner
#endif // FINITE_DIFFERENCES_COLLOCATION_H_

88
mpc_local_planner/include/mpc_local_planner/optimal_control/final_state_conditions_se2.h Normal file
View File

@ -0,0 +1,88 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef FINAL_STATE_CONDITIONS_SE2_H_
#define FINAL_STATE_CONDITIONS_SE2_H_
#include <corbo-optimal-control/functions/final_state_constraints.h>
#include <corbo-optimal-control/functions/final_state_cost.h>
#include <cmath>
#include <memory>
namespace mpc_local_planner {
/**
* @brief Quadratic final state cost (specialized for SE2)
*
* This class implements quadratic final costs
* \f[
* J_f(x) = (x_{ref} - x)^T Q_f (x_{xref} - x)
* \f]
* However, this class ensures that the distance (x_{ref} - x) is
* computed properly in SO(2) for the third state component.
*
* @see corbo::QuadraticFinalStateCost
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class QuadraticFinalStateCostSE2 : public corbo::QuadraticFinalStateCost
{
public:
using Ptr = std::shared_ptr<QuadraticFinalStateCostSE2>;
QuadraticFinalStateCostSE2() : corbo::QuadraticFinalStateCost() {}
QuadraticFinalStateCostSE2(const Eigen::Ref<const Eigen::MatrixXd>& Qf, bool lsq_form = false) : corbo::QuadraticFinalStateCost(Qf, lsq_form) {}
corbo::FinalStageCost::Ptr getInstance() const override { return std::make_shared<QuadraticFinalStateCostSE2>(); }
void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;
};
/**
* @brief Terminal ball constraint (specialized for SE2)
*
* This class ensures that the distance (x_{ref} - x) is
* computed properly in SO(2) for the third state component.
*
* @see corbo::TerminalBall
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class TerminalBallSE2 : public corbo::TerminalBall
{
public:
using Ptr = std::shared_ptr<TerminalBallSE2>;
TerminalBallSE2() : corbo::TerminalBall() {}
TerminalBallSE2(const Eigen::Ref<const Eigen::MatrixXd>& S, double gamma) : corbo::TerminalBall(S, gamma) {}
corbo::FinalStageConstraint::Ptr getInstance() const override { return std::make_shared<TerminalBallSE2>(); }
void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;
};
} // namespace mpc_local_planner
#endif // FINAL_STATE_CONDITIONS_SE2_H_

70
mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_grid_se2.h Normal file
View File

@ -0,0 +1,70 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef FINITE_DIFFERENCES_GRID_H_
#define FINITE_DIFFERENCES_GRID_H_
#include <mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h>
#include <memory>
namespace mpc_local_planner {
/**
* @brief Finite differences grid for SE2
*
* This class implements a full discretization grid with finite difference collocation.
* The temporal resolution is fixed.
*
* @see corbo::FullDiscretizationGridBase corbo::DiscretizationGridInterface
* FiniteDifferencesGridSE2
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class FiniteDifferencesGridSE2 : public FullDiscretizationGridBaseSE2
{
public:
using Ptr = std::shared_ptr<FiniteDifferencesGridSE2>;
using BaseEdge = corbo::BaseEdge;
using BaseMixedEdge = corbo::BaseMixedEdge;
//! Default constructor
FiniteDifferencesGridSE2() = default;
//! Default destructor
virtual ~FiniteDifferencesGridSE2() = default;
//! Return a newly created shared instance of the implemented class
corbo::DiscretizationGridInterface::Ptr getInstance() const override { return std::make_shared<FiniteDifferencesGridSE2>(); }
//! Get access to the associated factory
static corbo::Factory<corbo::DiscretizationGridInterface>& getFactory() { return corbo::Factory<corbo::DiscretizationGridInterface>::instance(); }
protected:
void createEdges(NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics) override;
bool isDtFixedIntended() const override { return true; }
};
} // namespace mpc_local_planner
#endif // FINITE_DIFFERENCES_GRID_H_

98
mpc_local_planner/include/mpc_local_planner/optimal_control/finite_differences_variable_grid_se2.h Normal file
View File

@ -0,0 +1,98 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef FINITE_DIFFERENCES_VARIABLE_GRID_H_
#define FINITE_DIFFERENCES_VARIABLE_GRID_H_
#include <mpc_local_planner/optimal_control/finite_differences_grid_se2.h>
#include <memory>
namespace mpc_local_planner {
/**
* @brief Finite differences grid with variable resolution for SE2
*
* This class implements a full discretization grid with finite difference collocation.
* The temporal resolution is free.
* This grid corresponds to the global uniform grid in time-optimal MPC as defined in [1,2].
*
* [1] C. Rösmann, F. Hoffmann und T. Bertram: Timed-Elastic-Bands for Time-Optimal Point-to-Point Nonlinear Model Predictive Control, ECC 2015.
* [2] C. Rösmann: Time-optimal nonlinear model predictive control - Direct transcription methods with variable discretization and structural sparsity
* exploitation. Dissertation, TU Dortmund University, Oct. 2019.
*
* @see FiniteDifferencesGridSE2
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class FiniteDifferencesVariableGridSE2 : public FiniteDifferencesGridSE2
{
public:
using Ptr = std::shared_ptr<FiniteDifferencesVariableGridSE2>;
using UPtr = std::unique_ptr<FiniteDifferencesVariableGridSE2>;
enum class GridAdaptStrategy { NoGridAdapt, TimeBasedSingleStep, TimeBasedAggressiveEstimate, SimpleShrinkingHorizon };
FiniteDifferencesVariableGridSE2() = default;
virtual ~FiniteDifferencesVariableGridSE2() = default;
//! Return a newly created shared instance of the implemented class
corbo::DiscretizationGridInterface::Ptr getInstance() const override { return std::make_shared<FiniteDifferencesVariableGridSE2>(); }
//! Get access to the associated factory
static corbo::Factory<corbo::DiscretizationGridInterface>& getFactory() { return corbo::Factory<corbo::DiscretizationGridInterface>::instance(); }
//! Set lower and upper bounds for the temporal resolution
void setDtBounds(double dt_lb, double dt_ub);
//! Set minium grid size for grid adaptation
void setNmin(int n_min) { _n_min = n_min; }
//! Disable grid adapation
void disableGridAdaptation() { _grid_adapt = GridAdaptStrategy::NoGridAdapt; }
//! Set grid adaptation strategy to 'single step'
void setGridAdaptTimeBasedSingleStep(int n_max, double dt_hyst_ratio = 0.1, bool adapt_first_iter = false);
//! Set grid adaptation strategy to 'aggressive'
void setGridAdaptTimeBasedAggressiveEstimate(int n_max, double dt_hyst_ratio = 0.1, bool adapt_first_iter = false);
//! Set grid adaptation strategy to 'shrinking horizon'
void setGridAdaptSimpleShrinkingHorizon(bool adapt_first_iter = false);
protected:
bool isDtFixedIntended() const override { return false; }
bool adaptGrid(bool new_run, NlpFunctions& nlp_fun) override;
bool adaptGridTimeBasedSingleStep(NlpFunctions& nlp_fun);
bool adaptGridTimeBasedAggressiveEstimate(NlpFunctions& nlp_fun);
bool adaptGridSimpleShrinkingHorizon(NlpFunctions& nlp_fun);
bool isMovingHorizonWarmStartActive() const override { return false; }
bool isGridAdaptActive() const override { return true; }
// auto resize stuff
GridAdaptStrategy _grid_adapt = GridAdaptStrategy::NoGridAdapt;
int _n_max = 1000;
double _dt_hyst_ratio = 0.1;
int _n_min = 2;
bool _adapt_first_iter = false;
};
} // namespace mpc_local_planner
#endif // FINITE_DIFFERENCES_VARIABLE_GRID_H_

232
mpc_local_planner/include/mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h Normal file
View File

@ -0,0 +1,232 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef FULL_DISCRETIZATION_GRID_BASE_H_
#define FULL_DISCRETIZATION_GRID_BASE_H_
#include <corbo-optimal-control/structured_ocp/discretization_grids/discretization_grid_interface.h>
#include <corbo-optimization/hyper_graph/scalar_vertex.h>
#include <corbo-optimization/hyper_graph/vector_vertex.h>
#include <mpc_local_planner/optimal_control/vector_vertex_se2.h>
#include <teb_local_planner/pose_se2.h>
#include <corbo-numerics/finite_differences_collocation.h>
#include <memory>
namespace mpc_local_planner {
/**
* @brief Full discretization grid specialization for SE2
*
* This class defines the basis for full discretization grids
* similar to corbo::FullDiscretizationGridBase.
* However, the main difference is, that this grid
* requires the first three state components to be embedded in SE2.
* Whereas the first two states are ordinary real numbers on (-inf, inf),
* the third component must remain in [-pi, pi).
*
* Therefore, the minimum dimension state dimension is 3.
* However, larger dimensions are allowed, but the related state components
* are considered as standard real numbers on (-inf, inf).
*
* Note the full discretization grid currently supports only finite difference collocation
* to discretize the continuous time dynamics.
* Full discretization is also possible with multiple shooting and (expicit) numerical integration,
* but for this purpose we need to specialize corbo::ShootingGridBase for SE2.
*
* @see corbo::FullDiscretizationGridBase corbo::DiscretizationGridInterface
* FiniteDifferencesGridSE2
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class FullDiscretizationGridBaseSE2 : public corbo::DiscretizationGridInterface
{
public:
using Ptr = std::shared_ptr<FullDiscretizationGridBaseSE2>;
using UPtr = std::unique_ptr<FullDiscretizationGridBaseSE2>;
using ReferenceTrajectoryInterface = corbo::ReferenceTrajectoryInterface;
using SystemDynamicsInterface = corbo::SystemDynamicsInterface;
using NlpFunctions = corbo::NlpFunctions;
using OptimizationEdgeSet = corbo::OptimizationEdgeSet;
using VertexInterface = corbo::VertexInterface;
using ScalarVertex = corbo::ScalarVertex;
using VectorVertex = corbo::VectorVertex;
using PartiallyFixedVectorVertex = corbo::PartiallyFixedVectorVertex;
using TimeSeries = corbo::TimeSeries;
enum class CostIntegrationRule { LeftSum, TrapezoidalRule };
FullDiscretizationGridBaseSE2() = default;
virtual ~FullDiscretizationGridBaseSE2() = default;
//! Return a newly created shared instance of the implemented class
corbo::DiscretizationGridInterface::Ptr getInstance() const override = 0;
// implements interface method
corbo::GridUpdateResult update(const Eigen::VectorXd& x0, ReferenceTrajectoryInterface& xref, ReferenceTrajectoryInterface& uref,
NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics, bool new_run,
const corbo::Time& t, ReferenceTrajectoryInterface* sref = nullptr, const Eigen::VectorXd* prev_u = nullptr,
double prev_u_dt = 0, ReferenceTrajectoryInterface* xinit = nullptr,
ReferenceTrajectoryInterface* uinit = nullptr) override;
// implements interface method
double getFirstDt() const override { return getDt(); }
// implements interface method
double getFinalTime() const override { return double(getN() - 1) * getDt(); }
// implements interface method
bool hasConstantControls() const override { return true; }
// implements interface method
bool hasSingleDt() const override { return true; }
// implements interface method
bool isTimeVariableGrid() const override { return !isDtFixedIntended(); }
// implements interface method
bool isUniformGrid() const override { return true; }
// implements interface method
bool providesStateTrajectory() const override { return true; }
// implements interface method
bool getFirstControlInput(Eigen::VectorXd& u0) override;
//! Return state at time stamp k
const Eigen::VectorXd& getState(int k) const
{
assert(k <= getN());
if (k == _x_seq.size()) return _xf.values();
return _x_seq[k].values();
}
// implements interface method
void getStateAndControlTimeSeries(TimeSeries::Ptr x_sequence, TimeSeries::Ptr u_sequence, double t_max = corbo::CORBO_INF_DBL) const override;
// implements interface method
void clear() override;
// implements interface method
bool isEmpty() const override { return _x_seq.empty() || _u_seq.empty(); }
// implements interface method
virtual bool isValid() const { return (_x_seq.size() == _u_seq.size()); }
// implements interface method
void setN(int n, bool try_resample = true) override;
// implements interface method
void setInitialDt(double dt) override { setDtRef(dt); }
// implements interface method
double getInitialDt() const override { return getDtRef(); }
// implements interface method
int getInitialN() const override { return getNRef(); }
//! get reference horizon length
int getNRef() const { return _n_ref; }
//! get current horizon length
int getN() const override { return _x_seq.size() + 1; }
//! set reference horizon length
void setNRef(int n);
//! set reference temporal resolution
void setDtRef(double dt) { _dt_ref = dt; }
//! get current reference temporal resolution
double getDtRef() const { return _dt_ref; }
//! get current temporal resolution
double getDt() const { return _dt.value(); }
//! activate or deactive warmstart
void setWarmStart(bool active) { _warm_start = active; }
//! Set individual components of the final state to fixed or unfixed
void setXfFixed(const Eigen::Matrix<bool, -1, 1>& xf_fixed)
{
_xf_fixed = xf_fixed;
setModified(true);
}
//! Set finite differences collocation method
void setFiniteDifferencesCollocationMethod(corbo::FiniteDifferencesCollocationInterface::Ptr fd_eval) { _fd_eval = fd_eval; }
//! Set cost integration rule
void setCostIntegrationRule(CostIntegrationRule integration) { _cost_integration = integration; }
/**
* @brief Find the closest pose (first part of the state vector) on the grid w.r.t. to a provided reference point.
*
* @param[in] x_ref X-position of the reference point
* @param[in] y_ref Y-position of the reference point
* @param[in] begin_idx Start search at this time stamp
* @param[out] distance [optional] the resulting minimum distance
* @return Index to the closest state/pose on the grid
*/
int findClosestPose(double x_ref, double y_ref, int start_idx = 0, double* distance = nullptr) const;
// implements interface method
std::vector<VertexInterface*>& getActiveVertices() override { return _active_vertices; }
// implements interface method
void getVertices(std::vector<VertexInterface*>& vertices) override;
protected:
virtual void initializeSequences(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun);
virtual void initializeSequences(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, ReferenceTrajectoryInterface& xref,
ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun);
virtual void warmStartShifting(const Eigen::VectorXd& x0);
virtual bool adaptGrid(bool new_run, NlpFunctions& nlp_fun) { return false; } // A subclass might add a grid adaptation, returns true if adapted
int findNearestState(const Eigen::VectorXd& x0);
void updateBounds(const NlpFunctions& nlp_fun);
virtual void resampleTrajectory(int n_new);
bool checkAndInitializeXfFixedFlags(int dim_x);
virtual void createEdges(NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics) = 0;
virtual bool isDtFixedIntended() const { return true; }
virtual bool isMovingHorizonWarmStartActive() const { return _warm_start; }
virtual bool isGridAdaptActive() const { return false; }
void computeActiveVertices() override;
corbo::FiniteDifferencesCollocationInterface::Ptr _fd_eval = std::make_shared<corbo::CrankNicolsonDiffCollocation>();
std::vector<VectorVertexSE2> _x_seq;
std::vector<VectorVertex> _u_seq;
PartiallyFixedVectorVertexSE2 _xf;
std::vector<VertexInterface*> _active_vertices;
const NlpFunctions* _nlp_fun = nullptr; // cache -> for bounds
int _n_ref = 11;
int _n_adapt = 0; // if adaption is on and warmstart off, we might use this n instead of n_ref (only if n_adapt > 0)
double _dt_ref = 0.1;
ScalarVertex _dt; // we need a ScalarVertex to use the helper methods in stage_functions.cpp
bool _warm_start = false;
bool _first_run = true;
// might be required if the last dt should be fixed or if dt is not fixed
Eigen::Matrix<bool, -1, 1> _xf_fixed;
double _dt_lb = 0;
double _dt_ub = corbo::CORBO_INF_DBL;
CostIntegrationRule _cost_integration;
};
} // namespace mpc_local_planner
#endif // FULL_DISCRETIZATION_GRID_BASE_H_

127
mpc_local_planner/include/mpc_local_planner/optimal_control/min_time_via_points_cost.h Normal file
View File

@ -0,0 +1,127 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef MIN_TIME_VIA_POINTS_COSTS_H_
#define MIN_TIME_VIA_POINTS_COSTS_H_
#include <corbo-core/reference_trajectory.h>
#include <corbo-optimal-control/functions/stage_functions.h>
#include <teb_local_planner/pose_se2.h>
#include <cmath>
#include <memory>
#include <utility>
#include <vector>
namespace mpc_local_planner {
/**
* @brief Hybrid cost function with minimum time and via-point objectives
*
* This class defines a joint objective of minimum time and via-point attraction.
* A user defined weight controls the trade-off between both individual objectives.
*
* The current via-point strategy is borrowed from the teb_local_planner.
*
* @todo we can implement this class as LSQ version as well
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class MinTimeViaPointsCost : public corbo::StageCost
{
public:
using Ptr = std::shared_ptr<MinTimeViaPointsCost>;
using ViaPointContainer = std::vector<teb_local_planner::PoseSE2>;
//! Default constructor
MinTimeViaPointsCost() = default;
/**
* @brief Construct with via-point container and weights
*
* @param[in] via_points Reference to the via-point container (must remain allocated)
* @param[in] position_weight Via-point attraction weight for the position part
* @param[in] orientation_weight Via-point attraction weight for the angular part
* @param[in] via_points_orderedx True, if via-points should be associated w.r.t. the ordering in \c via_points
*/
MinTimeViaPointsCost(const ViaPointContainer& via_points, double position_weight, double orientation_weight, bool via_points_ordered)
: _via_points_ordered(via_points_ordered)
{
setViaPointContainer(via_points);
setViaPointWeights(position_weight, orientation_weight);
}
// implements interface method
corbo::StageCost::Ptr getInstance() const override { return std::make_shared<MinTimeViaPointsCost>(); }
// implements interface method
bool hasNonIntegralTerms(int k) const override { return true; }
// implements interface method
bool hasIntegralTerms(int k) const override { return false; }
// implements interface method
int getNonIntegralDtTermDimension(int k) const override { return (k == 0 || !_single_dt) ? 1 : 0; }
// implements interface method
bool isLsqFormNonIntegralDtTerm(int k) const override { return false; }
// implements interface method
int getNonIntegralStateTermDimension(int k) const override;
// implements interface method
bool update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/,
corbo::ReferenceTrajectoryInterface* /*sref*/, bool single_dt, const Eigen::VectorXd& x0,
corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector<double>& /*dts*/,
const corbo::DiscretizationGridInterface* grid) override;
// implements interface method
void computeNonIntegralDtTerm(int k, double dt, Eigen::Ref<Eigen::VectorXd> cost) const override;
// implements interface method
void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;
//! Set reference to via-point container (warning, object must remain allocated)
void setViaPointContainer(const ViaPointContainer& via_points) { _via_points = &via_points; }
//! Set if the optimzier should try to match the via-point ordering
void setViaPointOrderedMode(bool ordered) { _via_points_ordered = ordered; }
//! Set weights for via-point attraction
void setViaPointWeights(double position_weight, double orientation_weight)
{
_vp_position_weight = position_weight;
_vp_orientation_weight = orientation_weight;
}
protected:
bool _single_dt = false;
double _time_weight = 1;
bool _via_points_ordered = false;
double _vp_position_weight = 1e-3;
double _vp_orientation_weight = 0;
const ViaPointContainer* _via_points = nullptr;
std::vector<std::pair<std::vector<const teb_local_planner::PoseSE2*>, std::size_t>> _vp_association;
// also store previous number of associated points to detect structure changes
};
} // namespace mpc_local_planner
#endif // MIN_TIME_VIA_POINTS_COSTS_H_

140
mpc_local_planner/include/mpc_local_planner/optimal_control/quadratic_cost_se2.h Normal file
View File

@ -0,0 +1,140 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef QUADRATIC_COST_SE2_H_
#define QUADRATIC_COST_SE2_H_
#include <corbo-optimal-control/functions/quadratic_cost.h>
#include <corbo-optimal-control/functions/quadratic_state_cost.h>
#include <cmath>
#include <memory>
namespace mpc_local_planner {
/**
* @brief Quadratic form running cost (specialized for SE2)
*
* This class implements quadratic form running costs
* \f[
* l(x, u) = (x_{ref} - x)^T Q (x_{xref} - x) + (u_{ref} - u)^T R (u_{ref} - u)
* \f]
* However, this class ensures that the state distance (x_{ref} - x) is
* computed properly in SO(2) for the third state component.
*
* Note, the class also provides a least squares form if not in integral form which is:
* \f$ l_{lsq} = \begin{bmatrix} \sqrt{Q} (x_{ref} - x) \\ \sqrt{R} (u_{ref} - u) \end{bmatrix} \f$
* \f$ \sqrt{Q} \f$ \f$ \sqrt{R} \f$ are computed once using the cholesky decomposition.
* This form is later squared by the solver.
*
* @see corbo::QuadraticFinalStateCost
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class QuadraticFormCostSE2 : public corbo::QuadraticFormCost
{
public:
using Ptr = std::shared_ptr<QuadraticFormCostSE2>;
//! Default constructor
QuadraticFormCostSE2() : corbo::QuadraticFormCost() {}
/**
* @brief Construct with weight matrices
*
* @param[in] Q Positive definite state weighting matrix (must match dimensions!)
* @param[in] R Positive definite control weighting matrix (must match dimensions!)
* @param[in] integral_form If true, the running costs are later integrated over the grid interval,
* otherwise, the values are just summed up as in sampled-data MPC.
* @param[in] lsq_form Set to true in order to prefer the least-squares form
*/
QuadraticFormCostSE2(const Eigen::Ref<const Eigen::MatrixXd>& Q, const Eigen::Ref<const Eigen::MatrixXd>& R, bool integral_form = false,
bool lsq_form = false)
: corbo::QuadraticFormCost(Q, R, integral_form, lsq_form)
{
}
// implements interface method
corbo::StageCost::Ptr getInstance() const override { return std::make_shared<QuadraticFormCostSE2>(); }
// implements interface method
void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;
// implements interface method
void computeIntegralStateControlTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, const Eigen::Ref<const Eigen::VectorXd>& u_k,
Eigen::Ref<Eigen::VectorXd> cost) const override;
};
/**
* @brief Quadratic state running cost (specialized for SE2)
*
* This class implements quadratic state running costs
* \f[
* l(x) = (x_{ref} - x)^T Q (x_{xref} - x)
* \f]
* However, this class ensures that the state distance (x_{ref} - x) is
* computed properly in SO(2) for the third state component.
*
* Note, the class also provides a least squares form if not in integral form which is:
* \f$ l_{lsq} = \sqrt{Q} (x_{ref} - x) \f$
* \f$ \sqrt{Q} \f$ is computed once using the cholesky decomposition.
* This form is later squared by the solver.
*
* @see corbo::QuadraticFinalStateCost
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class QuadraticStateCostSE2 : public corbo::QuadraticStateCost
{
public:
using Ptr = std::shared_ptr<QuadraticStateCostSE2>;
//! Default constructor
QuadraticStateCostSE2() : corbo::QuadraticStateCost() {}
/**
* @brief Construct with weight matrices
*
* @param[in] Q Positive definite state weighting matrix (must match dimensions!)
* @param[in] integral_form If true, the running costs are later integrated over the grid interval,
* otherwise, the values are just summed up as in sampled-data MPC.
* @param[in] lsq_form Set to true in order to prefer the least-squares form
*/
QuadraticStateCostSE2(const Eigen::Ref<const Eigen::MatrixXd>& Q, bool integral_form = false, bool lsq_form = false)
: corbo::QuadraticStateCost(Q, integral_form, lsq_form)
{
}
// implements interface method
corbo::StageCost::Ptr getInstance() const override { return std::make_shared<QuadraticStateCostSE2>(); }
// implements interface method
void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;
// implements interface method
void computeIntegralStateControlTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, const Eigen::Ref<const Eigen::VectorXd>& u_k,
Eigen::Ref<Eigen::VectorXd> cost) const override;
};
} // namespace mpc_local_planner
#endif // QUADRATIC_COST_SE2_H_

123
mpc_local_planner/include/mpc_local_planner/optimal_control/stage_inequality_se2.h Normal file
View File

@ -0,0 +1,123 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef STAGE_INEQUALITY_SE2_H_
#define STAGE_INEQUALITY_SE2_H_
#include <corbo-core/reference_trajectory.h>
#include <corbo-optimal-control/functions/stage_functions.h>
#include <teb_local_planner/robot_footprint_model.h>
#include <teb_local_planner/obstacles.h>
#include <cmath>
#include <memory>
namespace mpc_local_planner {
/**
* @brief Stage inequality constraint for obstacle avoidance and control deviation limits
*
* This class defines the inequality constraint for obstacle avoidance and control
* deviation limits (in case limits are active).
*
* The current obstacle association strategy is borrowed from the teb_local_planner.
* It takes the robot footprint model and the geometric obstacle shapes into account.
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class StageInequalitySE2 : public corbo::StageInequalityConstraint
{
public:
using Ptr = std::shared_ptr<StageInequalitySE2>;
using ObstaclePtr = teb_local_planner::ObstaclePtr;
// implements interface method
corbo::StageInequalityConstraint::Ptr getInstance() const override { return std::make_shared<StageInequalitySE2>(); }
// implements interface method
bool hasNonIntegralTerms(int k) const override { return true; }
// implements interface method
bool hasIntegralTerms(int k) const override { return false; }
// implements interface method
int getNonIntegralStateTermDimension(int k) const override;
// implements interface method
int getNonIntegralControlDeviationTermDimension(int k) const override;
// implements interface method
bool update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/,
corbo::ReferenceTrajectoryInterface* /*sref*/, bool /* single_dt*/, const Eigen::VectorXd& x0,
corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector<double>& /*dts*/,
const corbo::DiscretizationGridInterface* grid) override;
// implements interface method
void computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const override;
// implements interface method
void computeNonIntegralControlDeviationTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& u_k, const Eigen::Ref<const Eigen::VectorXd>& u_prev,
double dt, Eigen::Ref<Eigen::VectorXd> cost) const override;
//! Set reference to obstacle vector (must remain allocated)
void setObstacleVector(const teb_local_planner::ObstContainer& obstacles) { _obstacles = &obstacles; }
//! Set robot footprint model
void setRobotFootprintModel(teb_local_planner::RobotFootprintModelPtr robot_model) { _robot_model = robot_model; }
//! Set minimum distance allowed
void setMinimumDistance(double min_dist) { _min_obstacle_dist = min_dist; }
//! Set parameters for prior filtering of obstacles
void setObstacleFilterParameters(double force_inclusion_factor, double cutoff_factor)
{
_obstacle_filter_force_inclusion_factor = force_inclusion_factor;
_obstacle_filter_cutoff_factor = cutoff_factor;
}
//! Set to true to enable dynamic obstacle (constant-velocity prediction)
void setEnableDynamicObstacles(bool enable_dyn_obst) { _enable_dynamic_obstacles = enable_dyn_obst; }
//! Specify lower and upper bounds for limiting the control deviation
void setControlDeviationBounds(const Eigen::VectorXd& du_lb, const Eigen::VectorXd& du_ub);
//! Get current minimum distance parameter value
double getMinimumDistance() const { return _min_obstacle_dist; }
protected:
const teb_local_planner::ObstContainer* _obstacles = nullptr;
std::vector<teb_local_planner::ObstContainer> _relevant_obstacles; // TODO: we can also store raw pointers as _via_points is locked by mutex
teb_local_planner::RobotFootprintModelPtr _robot_model;
double _current_dt = 0.1;
int _num_du_lb_finite = 0;
int _num_du_ub_finite = 0;
double _min_obstacle_dist = 0.1;
double _obstacle_filter_force_inclusion_factor = 1.5;
double _obstacle_filter_cutoff_factor = 5;
bool _enable_dynamic_obstacles = false;
Eigen::VectorXd _du_lb;
Eigen::VectorXd _du_ub;
};
} // namespace mpc_local_planner
#endif // STAGE_INEQUALITY_SE2_H_

316
mpc_local_planner/include/mpc_local_planner/optimal_control/vector_vertex_se2.h Normal file
View File

@ -0,0 +1,316 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef VECTOR_SE_VERTEX_H_
#define VECTOR_SE_VERTEX_H_
#include <corbo-optimization/hyper_graph/vector_vertex.h>
#include <mpc_local_planner/utils/math_utils.h>
#include <corbo-core/types.h>
#include <memory>
#include <vector>
namespace mpc_local_planner {
/**
* @brief Vertex specialization for vectors in SE2
*
* This class defines a vector vertex in which the first
* three components must be defined in SE2.
* Whereas the first two states are ordinary real numbers on (-inf, inf),
* the third component must remain in [-pi, pi).
*
* The minimum dimension of this vertex is 3.
* This vertex allows for larger dimensions of 3,
* but these components are considered as standard real numbers on (-inf, inf).
*
* @see corbo::VertexInterface corbo::PartiallyFixedVectorVertex
* corbo::HyperGraph corbo::EdgeInterface corbo::ScalarVertex
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class VectorVertexSE2 : public corbo::VectorVertex
{
public:
using Ptr = std::shared_ptr<VectorVertexSE2>;
using UPtr = std::unique_ptr<VectorVertexSE2>;
//! Default constructor
VectorVertexSE2() = default;
explicit VectorVertexSE2(bool fixed) : corbo::VectorVertex(fixed) {}
//! Construct and allocate memory for a given dimension
explicit VectorVertexSE2(int dimension, bool fixed = false) : VectorVertex(dimension, fixed) {}
//! Construct vertex with given values
explicit VectorVertexSE2(const Eigen::Ref<const Eigen::VectorXd>& values, bool fixed = false) : VectorVertex(values, fixed) {}
//! Construct vertex with given values, lower and upper bounds
explicit VectorVertexSE2(const Eigen::Ref<const Eigen::VectorXd>& values, const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub, bool fixed = false)
: VectorVertex(values, lb, ub, fixed)
{
}
// implements interface method
void plus(int idx, double inc) override
{
if (idx == 2)
_values[idx] = normalize_theta(_values[idx] + inc);
else
_values[idx] += inc;
}
// implements interface method
void plus(const double* inc) override
{
assert(getDimension() >= 3);
_values[0] += inc[0];
_values[1] += inc[1];
_values[2] = normalize_theta(_values[2] + inc[2]);
if (getDimension() > 3) _values.tail(getDimension() - 3) += Eigen::Map<const Eigen::VectorXd>(inc + 3, getDimension() - 3);
}
// implements interface method
void plusUnfixed(const double* inc) override { plus(inc); }
// implements interface method
void setData(int idx, double data) override
{
if (idx == 2)
_values[idx] = normalize_theta(data);
else
_values[idx] = data;
}
//! Set values and bounds at once
virtual void set(const Eigen::Ref<const Eigen::VectorXd>& values, const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub, bool fixed = false)
{
assert(values.size() == lb.size());
assert(values.size() == ub.size());
assert(values.size() >= 3);
_values = values;
_values[2] = normalize_theta(_values[2]);
setLowerBounds(lb);
setUpperBounds(ub);
setFixed(false);
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* @brief VectorVertexSE2 with support for partially fixed components
*
*
* The vertex extends VectorVertexSE2 by allowing the user to
* partially fix components of the underlying vector.
*
* @see VectorVertexSE2 corbo::VertexInterface corbo::VectorVertex
* corbo::HyperGraph corbo::EdgeInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class PartiallyFixedVectorVertexSE2 : public VectorVertexSE2
{
public:
using Ptr = std::shared_ptr<PartiallyFixedVectorVertexSE2>;
using UPtr = std::unique_ptr<PartiallyFixedVectorVertexSE2>;
//! Default constructor
PartiallyFixedVectorVertexSE2() = default;
explicit PartiallyFixedVectorVertexSE2(int dimension)
: VectorVertexSE2(dimension), _fixed(Eigen::Array<bool, -1, 1>::Constant(dimension, false)), _num_unfixed(dimension)
{
}
//! Construct and allocate memory for a given dimension
explicit PartiallyFixedVectorVertexSE2(int dimension, const Eigen::Ref<const Eigen::Array<bool, -1, 1>>& fixed)
: VectorVertexSE2(dimension), _fixed(fixed), _num_unfixed(dimension)
{
}
//! Construct vertex with given values
explicit PartiallyFixedVectorVertexSE2(const Eigen::Ref<const Eigen::VectorXd>& values)
: VectorVertexSE2(values), _fixed(Eigen::Array<bool, -1, 1>::Constant(values.size(), false)), _num_unfixed(values.size())
{
}
//! Construct vertex with given values and fixed components
explicit PartiallyFixedVectorVertexSE2(const Eigen::Ref<const Eigen::VectorXd>& values, const Eigen::Ref<const Eigen::Array<bool, -1, 1>>& fixed)
: VectorVertexSE2(values), _fixed(fixed), _num_unfixed(fixed.size() - fixed.count())
{
}
//! Construct vertex with given values, lower and upper bounds
explicit PartiallyFixedVectorVertexSE2(const Eigen::Ref<const Eigen::VectorXd>& values, const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub)
: VectorVertexSE2(values, lb, ub), _fixed(Eigen::Array<bool, -1, 1>::Constant(values.size(), false)), _num_unfixed(values.size())
{
}
// implements interface method
int getDimensionUnfixed() const override { return _num_unfixed; }
// implements parent method
void setDimension(int dim) override
{
VectorVertexSE2::setDimension(dim);
_fixed.setConstant(dim, false);
_num_unfixed = dim;
}
//! Set values and bounds at once
void set(const Eigen::Ref<const Eigen::VectorXd>& values, const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub, bool fixed = false) override
{
assert(values.size() == lb.size());
assert(values.size() == ub.size());
assert(values.size() >= 3);
_values = values;
_values[2] = normalize_theta(_values[2]);
setLowerBounds(lb);
setUpperBounds(ub);
setFixed(fixed);
}
//! Set values and bounds at once (overload with fixed vector)
void set(const Eigen::Ref<const Eigen::VectorXd>& values, const Eigen::Ref<const Eigen::VectorXd>& lb,
const Eigen::Ref<const Eigen::VectorXd>& ub, const Eigen::Ref<const Eigen::Array<bool, -1, 1>>& fixed)
{
assert(values.size() == lb.size());
assert(values.size() == ub.size());
assert(values.size() >= 3);
_values = values;
_values[2] = normalize_theta(_values[2]);
setLowerBounds(lb);
setUpperBounds(ub);
setFixed(fixed);
}
//! Set component with idx (0 <= idx < dimension()) to (un)fixed
void setFixed(int idx, bool fixed)
{
_fixed[idx] = fixed;
_num_unfixed = getDimension() - _fixed.count();
}
//! Set logical array [dimension() x 1] in order to fix selected components
void setFixed(const Eigen::Ref<const Eigen::Array<bool, -1, 1>>& fixed)
{
_fixed = fixed;
_num_unfixed = getDimension() - _fixed.count();
}
// implements interface method
void setFixed(bool fixed) override
{
_fixed.setConstant(_values.size(), fixed);
_num_unfixed = fixed ? 0 : getDimension();
}
// implements interface method
void plusUnfixed(const double* inc) override
{
int idx = 0;
for (int i = 0; i < getDimension(); ++i)
{
if (!_fixed(i))
{
plus(i, inc[idx]);
++idx;
}
}
}
// implements interface method
bool hasFixedComponents() const override { return _num_unfixed < getDimension(); }
// implements interface method
bool isFixedComponent(int idx) const override { return _fixed[idx]; }
// implements interface method
int getNumberFiniteLowerBounds(bool unfixed_only) const override
{
if (unfixed_only && _num_unfixed > 0)
{
int num = 0;
for (int i = 0; i < getDimension(); ++i)
{
if (!_fixed[i] && _lb[i] > -corbo::CORBO_INF_DBL) num += 1;
}
return num;
}
else
return (_lb.array() > -corbo::CORBO_INF_DBL).count();
}
// implements interface method
int getNumberFiniteUpperBounds(bool unfixed_only) const override
{
if (unfixed_only && _num_unfixed > 0)
{
int num = 0;
for (int i = 0; i < getDimension(); ++i)
{
if (!_fixed[i] && _ub[i] < corbo::CORBO_INF_DBL) num += 1;
}
return num;
}
else
return (_ub.array() < corbo::CORBO_INF_DBL).count();
}
// implements interface method
int getNumberFiniteBounds(bool unfixed_only) const override
{
if (unfixed_only && _num_unfixed > 0)
{
int num = 0;
for (int i = 0; i < getDimension(); ++i)
{
if (!_fixed[i] && (_ub[i] < corbo::CORBO_INF_DBL || _lb[i] > -corbo::CORBO_INF_DBL)) num += 1;
}
return num;
}
else
return (_ub.array() < corbo::CORBO_INF_DBL || _lb.array() > -corbo::CORBO_INF_DBL).count();
}
//! Read-only access to the underlying logical array for fixed components
const Eigen::Array<bool, -1, 1> fixedArray() const { return _fixed; }
protected:
Eigen::Array<bool, -1, 1> _fixed;
int _num_unfixed;
};
} // namespace mpc_local_planner
#endif // SRC_OPTIMIZATION_INCLUDE_CORBO_OPTIMIZATION_HYPER_GRAPH_VECTOR_VERTEX_H_

108
mpc_local_planner/include/mpc_local_planner/systems/base_robot_se2.h Normal file
View File

@ -0,0 +1,108 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef SYSTEMS_BASE_ROBOT_SE2_H_
#define SYSTEMS_BASE_ROBOT_SE2_H_
#include <mpc_local_planner/systems/robot_dynamics_interface.h>
namespace mpc_local_planner {
/**
* @brief Specialization of RobotDynamicsInterface for mobile robots operating in SE2
*
* This abstract class defines a base class for robots in which the full state vector x
* is embedded in SE2, i.e. x = [pos_x, pos_y, theta] with pos_x, pos_y as real numbers
* and theta in [-pi, pi).
*
* Note, these models allow for simple conversion between state vectors and poses.
*
* @see RobotDynamicsInterface corbo::SystemDynamicsInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class BaseRobotSE2 : public RobotDynamicsInterface
{
public:
using Ptr = std::shared_ptr<BaseRobotSE2>;
//! Default constructor
BaseRobotSE2() = default;
// implements interface method
corbo::SystemDynamicsInterface::Ptr getInstance() const override = 0;
// implements interface method
int getInputDimension() const override = 0;
// implements interface method
int getStateDimension() const override { return 3; }
// implements interface method
bool isContinuousTime() const override { return true; }
// implements interface method
bool isLinear() const override { return false; }
// implements interface method
void getPositionFromState(const Eigen::Ref<const Eigen::VectorXd>& x, double& pos_x, double& pos_y) const override
{
assert(x.size() == getStateDimension());
pos_x = x[0];
pos_y = x[1];
}
// implements interface method
void getPoseSE2FromState(const Eigen::Ref<const Eigen::VectorXd>& x, double& pos_x, double& pos_y, double& theta) const override
{
assert(x.size() == getStateDimension());
pos_x = x[0];
pos_y = x[1];
theta = x[2];
}
// implements interface method
void getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref<Eigen::VectorXd> x) const override
{
assert(x.size() == getStateDimension());
x[0] = pos_x;
x[1] = pos_y;
x[2] = theta;
if (x.size() > 3) x.tail(x.size() - 3).setZero();
}
// implements interface method
virtual bool mergeStateFeedbackAndOdomFeedback(const teb_local_planner::PoseSE2& odom_pose, const geometry_msgs::Twist& odom_twist,
Eigen::Ref<Eigen::VectorXd> x) const override
{
assert(x.size() == getStateDimension());
x[0] = odom_pose.x();
x[1] = odom_pose.y();
x[2] = odom_pose.theta();
}
// implements interface method
void dynamics(const Eigen::Ref<const StateVector>& x, const Eigen::Ref<const ControlVector>& u, Eigen::Ref<StateVector> f) const override = 0;
};
} // namespace mpc_local_planner
#endif // SYSTEMS_BASE_ROBOT_SE2_H_

151
mpc_local_planner/include/mpc_local_planner/systems/robot_dynamics_interface.h Normal file
View File

@ -0,0 +1,151 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_
#define SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_
#include <corbo-core/types.h>
#include <corbo-systems/system_dynamics_interface.h>
#include <geometry_msgs/Twist.h>
#include <teb_local_planner/pose_se2.h>
#include <memory>
namespace mpc_local_planner {
/**
* @brief Specialization of SystemDynamicsInterface for mobile robots
*
* This abstract class extends cobro::SystemDynamicsInterface
* by (abstract) conversion methods between states, poses embedded in SE2,
* controls and twist.
*
* @see BaseRobotSE2 corbo::SystemDynamicsInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class RobotDynamicsInterface : public corbo::SystemDynamicsInterface
{
public:
using Ptr = std::shared_ptr<RobotDynamicsInterface>;
/**
* @brief Convert state vector to position (x,y)
*
* @param[in] x Reference to the state vector [getStateDimension() x 1]
* @param[out] pos_x X-component of the position
* @param[out] pos_y Y-component of the position
*/
virtual void getPositionFromState(const Eigen::Ref<const Eigen::VectorXd>& x, double& pos_x, double& pos_y) const = 0;
/**
* @brief Convert state vector to pose (x,y,theta)
*
* @param[in] x Reference to the state vector [getStateDimension() x 1]
* @param[out] pos_x X-component of the position part
* @param[out] pos_y Y-component of the position part
* @param[out] theta Orientation (yaw angle) in [-pi, pi)
*/
virtual void getPoseSE2FromState(const Eigen::Ref<const Eigen::VectorXd>& x, double& pos_x, double& pos_y, double& theta) const = 0;
/**
* @brief Convert state vector to PoseSE2 object
*
* @param[in] x Reference to the state vector [getStateDimension() x 1]
* @param[out] pose PoseSE2 object
*/
virtual void getPoseSE2FromState(const Eigen::Ref<const Eigen::VectorXd>& x, teb_local_planner::PoseSE2& pose) const
{
getPoseSE2FromState(x, pose.x(), pose.y(), pose.theta());
}
/**
* @brief Convert pose (x,y,theta) to steady state
*
* Converts a pose to state vector with dimensions [getStateDimension() x 1].
* In case getStateDimension() > 3, the state information is incomplete
* and hence it is assumed that a steady state for this particular pose is avaiable,
* which means that there exist a control u such that f(x,u) = 0 (continuous time) or
* f(x,u) = x (discrete time).
*
* For example, in case the remaining state components correspond to just integrators,
* they can be set to zero.
*
* @param[in] pos_x X-component of the position part
* @param[in] pos_y Y-component of the position part
* @param[in] theta Orientation (yaw angle) in [-pi, pi)
* @param[out] x Reference to the state vector [getStateDimension() x 1]
*/
virtual void getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref<Eigen::VectorXd> x) const = 0;
/**
* @brief Convert PoseSE2 to steady state
*
* See description of getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref<Eigen::VectorXd> x)
*
* @param[in] pose PoseSE2 object
* @param[out] x Reference to the state vector [getStateDimension() x 1]
*/
virtual void getSteadyStateFromPoseSE2(const teb_local_planner::PoseSE2& pose, Eigen::Ref<Eigen::VectorXd> x) const
{
getSteadyStateFromPoseSE2(pose.x(), pose.y(), pose.theta(), x);
}
/**
* @brief Convert control vector to twist message
*
* Convert the control vector to a twist message (containing velocity information)
* if possible.
*
* @todo Maybe add current state x as optional input to allow for computing a twist out of the first state and control
*
* @param[in] u Reference to the control vector [getInputDimension() x 1]
* @param[out] twist Reference to the twist message
* @return true, if conversion was successful, false otherwise
*/
virtual bool getTwistFromControl(const Eigen::Ref<const Eigen::VectorXd>& u, geometry_msgs::Twist& twist) const = 0;
/**
* @brief Merge custom state feedback with pose and twist feedback
*
* Many robots in ROS publish their state information (pose, linear and angular velocity)e.g. via an odometry message
* (potentially corrected by localization).
* However, for more complex robot models, it might not be possible to obtain the full state information without any observer
* or further measurements.
* For this purpose, custom state feedback can be provided. But as the complete navigation stack relies on the information obtained by
* odometry, this method allows for merging of both sources.
* In particular, it overrides only state components related to the pose and velocity.
*
* @param[in] odom_pose PoseSE2 object obtained from odometry
* @param[in] odom_twist geometry::Twist message obtained from odometry
* @param[in,out] x Custom state feedback in which related will be overriden [getStateDimension() x 1]
* @return true, if merging was successful, false otherwise
*/
virtual bool mergeStateFeedbackAndOdomFeedback(const teb_local_planner::PoseSE2& odom_pose, const geometry_msgs::Twist& odom_twist,
Eigen::Ref<Eigen::VectorXd> x) const = 0;
virtual void reset() {}
};
} // namespace mpc_local_planner
#endif // SYSTEMS_ROBOT_DYNAMICS_INTERFACE_H_

146
mpc_local_planner/include/mpc_local_planner/systems/simple_car.h Normal file
View File

@ -0,0 +1,146 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef SYSTEMS_SIMPLE_CAR_H_
#define SYSTEMS_SIMPLE_CAR_H_
#include <mpc_local_planner/systems/base_robot_se2.h>
#include <cmath>
namespace mpc_local_planner {
/**
* @brief Simple car model
*
* This class implements the dynamics for a simple car model
* in which the rear wheels are actuated.
* The front wheels are the steering wheels (for wheelbase > 0).
* The state vector [x, y, theta] is defined at the center of the rear axle.
* See [1,2] for a mathematical description and a figure.
*
* [1] S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006.
* (Chapter 13, http://planning.cs.uiuc.edu/)
* [2] A. De Luca et al., Feedback Control of a Nonholonomic Car-like Robot,
* in Robot Motion Planning and Control (Ed. J.-P. Laumond), Springer, 1998.
* (https://homepages.laas.fr/jpl/promotion/chap4.pdf)
*
* @see SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface
* corbo::SystemDynamicsInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class SimpleCarModel : public BaseRobotSE2
{
public:
//! Default constructor
SimpleCarModel() = default;
//! Constructs model with given wheelbase
SimpleCarModel(double wheelbase) : _wheelbase(wheelbase) {}
// implements interface method
SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared<SimpleCarModel>(); }
// implements interface method
int getInputDimension() const override { return 2; }
// implements interface method
void dynamics(const Eigen::Ref<const StateVector>& x, const Eigen::Ref<const ControlVector>& u, Eigen::Ref<StateVector> f) const override
{
assert(x.size() == getStateDimension());
assert(u.size() == getInputDimension());
assert(x.size() == f.size() && "SimpleCarModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f.");
f[0] = u[0] * std::cos(x[2]);
f[1] = u[0] * std::sin(x[2]);
f[2] = u[0] * std::tan(u[1]) / _wheelbase;
}
// implements interface method
bool getTwistFromControl(const Eigen::Ref<const Eigen::VectorXd>& u, geometry_msgs::Twist& twist) const override
{
assert(u.size() == getInputDimension());
twist.linear.x = u[0];
twist.linear.y = twist.linear.y = 0;
twist.angular.z = u[1]; // warning, this is the angle and not the angular vel
twist.angular.x = twist.angular.y = 0;
return true;
}
//! Set wheelbase
void setWheelbase(double wheelbase) { _wheelbase = wheelbase; }
//! Get wheelbase
double getWheelbase() const { return _wheelbase; }
protected:
double _wheelbase = 1.0;
};
/**
* @brief Simple car model with front wheel actuation
*
* This class implements the dynamics for a simple car model
* in which the front wheels are actuated and steered (for wheelbase > 0).
* The state vector [x, y, theta] is defined at the center of the rear axle.
* See [1] for a mathematical description and a figure.
*
* [1] A. De Luca et al., Feedback Control of a Nonholonomic Car-like Robot,
* in Robot Motion Planning and Control (Ed. J.-P. Laumond), Springer, 1998.
* (https://homepages.laas.fr/jpl/promotion/chap4.pdf)
*
* @see SimpleCarFrontWheelDrivingModel BaseRobotSE2 RobotDynamicsInterface
* corbo::SystemDynamicsInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class SimpleCarFrontWheelDrivingModel : public SimpleCarModel
{
public:
//! Default constructor
SimpleCarFrontWheelDrivingModel() = default;
//! Constructs model with given wheelbase
SimpleCarFrontWheelDrivingModel(double wheelbase) : SimpleCarModel(wheelbase) {}
// implements interface method
SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared<SimpleCarFrontWheelDrivingModel>(); }
// implements interface method
void dynamics(const Eigen::Ref<const StateVector>& x, const Eigen::Ref<const ControlVector>& u, Eigen::Ref<StateVector> f) const override
{
assert(x.size() == getStateDimension());
assert(u.size() == getInputDimension());
assert(x.size() == f.size() &&
"SimpleCarFrontWheelDrivingModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f.");
f[0] = u[0] * std::cos(x[2]);
f[1] = u[0] * std::sin(x[2]);
f[2] = u[0] * std::sin(u[1]) / _wheelbase;
}
};
} // namespace mpc_local_planner
#endif // SYSTEMS_SIMPLE_CAR_H_

85
mpc_local_planner/include/mpc_local_planner/systems/unicycle_robot.h Normal file
View File

@ -0,0 +1,85 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef SYSTEMS_UNICYCLE_ROBOT_H_
#define SYSTEMS_UNICYCLE_ROBOT_H_
#include <mpc_local_planner/systems/base_robot_se2.h>
#include <cmath>
namespace mpc_local_planner {
/**
* @brief Unicycle model
*
* This class implements the dynamics for a unicycle model
* which can be used e.g., for differential-drive robots.
* See [1] for a mathematical description and a figure.
*
* [1] S. M. LaValle, Planning Algorithms, Cambridge University Press, 2006.
* (Chapter 13, http://planning.cs.uiuc.edu/)
*
* @see BaseRobotSE2 RobotDynamicsInterface corbo::SystemDynamicsInterface
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class UnicycleModel : public BaseRobotSE2
{
public:
//! Default constructor
UnicycleModel() = default;
// implements interface method
SystemDynamicsInterface::Ptr getInstance() const override { return std::make_shared<UnicycleModel>(); }
// implements interface method
int getInputDimension() const override { return 2; }
// implements interface method
void dynamics(const Eigen::Ref<const StateVector>& x, const Eigen::Ref<const ControlVector>& u, Eigen::Ref<StateVector> f) const override
{
assert(x.size() == getStateDimension());
assert(u.size() == getInputDimension());
assert(x.size() == f.size() && "UnicycleModel::dynamics(): x and f are not of the same size, do not forget to pre-allocate f.");
f[0] = u[0] * std::cos(x[2]);
f[1] = u[0] * std::sin(x[2]);
f[2] = u[1];
}
// implements interface method
bool getTwistFromControl(const Eigen::Ref<const Eigen::VectorXd>& u, geometry_msgs::Twist& twist) const override
{
assert(u.size() == getInputDimension());
twist.linear.x = u[0];
twist.linear.y = twist.linear.y = 0;
twist.angular.z = u[1];
twist.angular.x = twist.angular.y = 0;
return true;
}
};
} // namespace mpc_local_planner
#endif // SYSTEMS_UNICYCLE_ROBOT_H_

56
mpc_local_planner/include/mpc_local_planner/utils/conversion.h Normal file
View File

@ -0,0 +1,56 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef UTILS_CONVERSION_H_
#define UTILS_CONVERSION_H_
#include <corbo-core/time_series.h>
#include <corbo-core/types.h>
#include <mpc_local_planner/systems/robot_dynamics_interface.h>
#include <geometry_msgs/PoseStamped.h>
#include <memory>
#include <string>
namespace mpc_local_planner {
/**
* @brief Convert TimeSeries to pose array
*
* Converts TimeSeries to std::vector<geometry_msgs::PoseStamped>.
*
* @remarks Note, the actual data of the TimeSeries object is copied without interpolation.
*
* @todo We could avoid the system dynamics dependency by specifying a generic getter function for the SE2 poses
*
* @param[in] time_series corbo::TimeSeries object or any child class
* @param[in] dynamics Reference to the robot dynamics interface (to access state-to-SE2 conversion methods)
* @param[out] poses_stamped The resulting pose array (note, the incoming vector will be cleared)
* @param[in] frame_id The planning frame id that is added to the message header
*/
void convert(const corbo::TimeSeries& time_series, const RobotDynamicsInterface& dynamics, std::vector<geometry_msgs::PoseStamped>& poses_stamped,
const std::string& frame_id);
} // namespace mpc_local_planner
#endif // UTILS_CONVERSION_H_

107
mpc_local_planner/include/mpc_local_planner/utils/math_utils.h Normal file
View File

@ -0,0 +1,107 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef UTILS_MATH_UTILS_H_
#define UTILS_MATH_UTILS_H_
#include <cmath>
namespace mpc_local_planner {
/**
* @brief Return the average angle of an arbitrary number of given angles [rad]
* @param angles vector containing all angles
* @return average / mean angle, that is normalized to [-pi, pi]
*/
inline double average_angles(const std::vector<double>& angles)
{
double x = 0, y = 0;
for (std::vector<double>::const_iterator it = angles.begin(); it != angles.end(); ++it)
{
x += std::cos(*it);
y += std::sin(*it);
}
if (x == 0 && y == 0)
return 0;
else
return std::atan2(y, x);
}
/**
* @brief Calculate Euclidean distance between two 2D point datatypes
* @param point1 object containing fields x and y
* @param point2 object containing fields x and y
* @return Euclidean distance: ||point2-point1||
*/
template <typename P1, typename P2>
inline double distance_points2d(const P1& point1, const P2& point2)
{
return std::sqrt(std::pow(point2.x - point1.x, 2) + std::pow(point2.y - point1.y, 2));
}
//! Calculate Euclidean distance between two 2D points
inline double distance_points2d(double x1, double y1, double x2, double y2) { return std::sqrt(std::pow(x2 - x1, 2) + std::pow(y2 - y1, 2)); }
/**
* @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d)
* @param v1 object containing public methods x() and y()
* @param v2 object containing fields x() and y()
* @return magnitude that would result in the 3D case (along the z-axis)
*/
template <typename V1, typename V2>
inline double cross2d(const V1& v1, const V2& v2)
{
return v1.x() * v2.y() - v2.x() * v1.y();
}
/**
* @brief normalize angle to interval [-pi, pi)
* @remark This function is based on normalize_theta from g2o
* see: https://github.com/RainerKuemmerle/g2o/blob/master/g2o/stuff/misc.h
*/
inline double normalize_theta(double theta)
{
if (theta >= -M_PI && theta < M_PI) return theta;
double multiplier = std::floor(theta / (2.0 * M_PI));
theta = theta - multiplier * 2.0 * M_PI;
if (theta >= M_PI) theta -= 2.0 * M_PI;
if (theta < -M_PI) theta += 2.0 * M_PI;
return theta;
}
/**
* @brief Return the interpolated angle between two angles [rad]
* @param angle1
* @param angle2
* @param factor in [0,1], or (1,inf) for extrapolation
* @return average / mean angle, that is normalized to [-pi, pi]
*/
inline double interpolate_angle(double angle1, double angle2, double factor)
{
return normalize_theta(angle1 + factor * normalize_theta(angle2 - angle1));
}
} // namespace mpc_local_planner
#endif // UTILS_MATH_UTILS_H_

150
mpc_local_planner/include/mpc_local_planner/utils/publisher.h Normal file
View File

@ -0,0 +1,150 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef UTILS_PUBLISHER_H_
#define UTILS_PUBLISHER_H_
#include <corbo-core/time_series.h>
#include <corbo-core/types.h>
#include <mpc_local_planner/systems/robot_dynamics_interface.h>
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/robot_footprint_model.h>
#include <geometry_msgs/PoseStamped.h>
#include <ros/publisher.h>
#include <ros/ros.h>
#include <std_msgs/ColorRGBA.h>
#include <memory>
namespace mpc_local_planner {
/**
* @brief This class provides publishing methods for common messages
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class Publisher
{
using ObstaclePtr = teb_local_planner::ObstaclePtr;
using ObstContainer = teb_local_planner::ObstContainer;
using PointObstacle = teb_local_planner::PointObstacle;
using CircularObstacle = teb_local_planner::CircularObstacle;
using LineObstacle = teb_local_planner::LineObstacle;
using PolygonObstacle = teb_local_planner::PolygonObstacle;
using Point2dContainer = teb_local_planner::Point2dContainer;
public:
/**
* @brief Default constructor
* @remarks do not forget to call initialize()
*/
Publisher() = default;
/**
* @brief Constructor that initializes the class and registers topics
* @param[in] nh local ros::NodeHandle
* @param[in] map_frame the planning frame name
*/
Publisher(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame);
/**
* @brief Initializes the class and registers topics.
*
* Call this function if only the default constructor has been called before.
* @param[in] nh local ros::NodeHandle
* @param[in] map_frame the planning frame name
*/
void initialize(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame);
/**
* @brief Publish a given local plan to the ros topic \e ../../local_plan
* @param[in] local_plan Pose array describing the local plan
*/
void publishLocalPlan(const std::vector<geometry_msgs::PoseStamped>& local_plan) const;
/**
* @brief Publish a given local plan to the ros topic \e ../../local_plan
* @param[in] local_plan Pose array describing the local plan
*/
void publishLocalPlan(const corbo::TimeSeries& ts) const;
/**
* @brief Publish a given global plan to the ros topic \e ../../global_plan
* @param[in] global_plan Pose array describing the global plan
*/
void publishGlobalPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan) const;
/**
* @brief Publish the visualization of the robot model
*
* @param[in] current_pose Current pose of the robot
* @param[in] robot_model Subclass of BaseRobotFootprintModel
* @param[in] map_frame frame name for the msg header
* @param[in] ns Namespace for the marker objects
* @param[in] color Color of the footprint
*/
void publishRobotFootprintModel(const teb_local_planner::PoseSE2& current_pose, const teb_local_planner::BaseRobotFootprintModel& robot_model,
const std::string& ns = "RobotFootprintModel", const std_msgs::ColorRGBA& color = toColorMsg(0.5, 0.0, 0.8, 0.0));
/**
* @brief Publish obstacle positions to the ros topic \e ../../mpc_markers
* @todo Move filling of the marker message to polygon class in order to avoid checking types.
* @param[in] obstacles Obstacle container
*/
void publishObstacles(const teb_local_planner::ObstContainer& obstacles) const;
/**
* @brief Publish via-points to the ros topic \e ../../teb_markers
*
* @todo add option to switch between points and poses (including orientation) to be published
*
* @param[in] via_points via-point container
* @param[in] ns marker namespace
*/
void publishViaPoints(const std::vector<teb_local_planner::PoseSE2>& via_points, const std::string& ns = "ViaPoints") const;
/**
* @brief Helper function to generate a color message from single values
* @param[in] a Alpha value
* @param[in] r Red value
* @param[in] g Green value
* @param[in] b Blue value
* @return Color message
*/
static std_msgs::ColorRGBA toColorMsg(float a, float r, float g, float b);
private:
bool _initialized = false;
std::string _map_frame = "map";
RobotDynamicsInterface::Ptr _system;
ros::Publisher _local_plan_pub;
ros::Publisher _global_plan_pub;
ros::Publisher _mpc_marker_pub;
};
} // namespace mpc_local_planner
#endif // UTILS_PUBLISHER_H_

93
mpc_local_planner/include/mpc_local_planner/utils/time_series_se2.h Normal file
View File

@ -0,0 +1,93 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#ifndef UTILS_TIME_SERIES_SE2_H_
#define UTILS_TIME_SERIES_SE2_H_
#include <corbo-core/time_series.h>
#include <Eigen/Core>
namespace mpc_local_planner {
/**
* @brief Time Series with SE2 support
*
* A time series represents a sequence of vectors of floating point numbers
* associated with time stamps. A time series might also be interpreted as a
* discrete-time trajectory.
* This class extends corbo::TimeSeries to support SE2 poses.
* Hereby, the third state component is always treated as orientation in [-pi, pi).
* In particular, whenever values are retrieved by interpolation
* (getValuesInterpolate()), the third state is ensured to remain properly in [-pi, pi).
*
* Note, as the first three state components define the SE2, any further auxiliary
* state component is treated as ordinary real number.
*
* @see corbo::TimeSeries
*
* @author Christoph Rösmann (christoph.roesmann@tu-dortmund.de)
*/
class TimeSeriesSE2 : public corbo::TimeSeries
{
public:
using Ptr = std::shared_ptr<TimeSeriesSE2>;
using ConstPtr = std::shared_ptr<const TimeSeriesSE2>;
//! Default constructor
TimeSeriesSE2() = default;
//! Construct empty time series with a dresired value vector dimension
explicit TimeSeriesSE2(int value_dim) : TimeSeries(value_dim) {}
/**
* @brief Retrieve value vector at a desired time stamp (seconds) via interpolation
*
* This method interpolates the underlying time series object at a desired
* time stamp (given in seconds). Interpolation and extrapolation settings
* can be passed as optional arguments.
*
* @warning This method currently assumes that the values / time pairs are
* stored with monotonically increasing time stamps (in time()).
*
* @param[in] time desired time stamp
* @param[out] values interpolated value vector w.r.t. \c time
* @param[in] interpolation interpolation method according to Interpolation enum
* @param[in] extrapolate specify whether exrapliation should be applied with a similar
* method like interpolation
* @param[in] tolerance specify a tolerance for which two time stamps are treated equally
* @returns true if successfull, false otherwise (e.g. if extrapolate is false but \c time > max(time)
*/
bool getValuesInterpolate(double time, Eigen::Ref<Eigen::VectorXd> values, Interpolation interpolation = Interpolation::Linear,
Extrapolation extrapolate = Extrapolation::NoExtrapolation, double tolerance = 1e-6) const override;
//! Compute and return the mean value of all values among all dimensions
double computeMeanOverall() override;
/**
* @brief Compute and return the component-wise mean values
* @param[out] mean_values Resulting cwise mean values [getValueDimension() x 1] (vector size must be preallocated)
*/
void computeMeanCwise(Eigen::Ref<Eigen::VectorXd> mean_values) override;
};
} // namespace mpc_local_planner
#endif // UTILS_TIME_SERIES_SE2_H_

25
mpc_local_planner/launch/test_mpc_optim_node.launch Normal file
View File

@ -0,0 +1,25 @@
<launch>
<arg name="plot" default="false" />
<arg name="plot_states" default="false" />
<arg name="cpu_time" default="false" />
<!--- Run optimization test node -->
<node pkg="mpc_local_planner" type="test_mpc_optim_node" name="test_mpc_optim_node" output="screen">
<rosparam file="$(find mpc_local_planner)/cfg/test_mpc_optim_node.yaml" command="load" ns=""/>
<param name="controller/print_cpu_time" value="true" if="$(arg cpu_time)"/>
</node>
<!-- RVIZ -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find mpc_local_planner)/cfg/rviz_test_mpc_optim.rviz"/>
<!-- **************** Plot ocp results ************** -->
<node name="ocp_result_plotter" pkg="mpc_local_planner" type="plot_optimal_control_results.py" if="$(arg plot)">
<!-- param name="ocp_result_topic" value="/test_mpc_optim_node/ocp_result"/-->
<param name="plot_rate" value="2"/>
<param name="plot_states" value="$(arg plot_states)"/>
</node>
</launch>

14
mpc_local_planner/mpc_local_planner_plugin.xml Normal file
View File

@ -0,0 +1,14 @@
<library path="lib/libmpc_local_planner">
<class name="mpc_local_planner/MpcLocalPlannerROS" type="mpc_local_planner::MpcLocalPlannerROS" base_class_type="nav_core::BaseLocalPlanner">
<description>
The mpc_local_planner package implements a plugin
to the base_local_planner of the 2D navigation stack.
</description>
</class>
<class name="mpc_local_planner/MpcLocalPlannerROS" type="mpc_local_planner::MpcLocalPlannerROS" base_class_type="mbf_costmap_core::CostmapController">
<description>
Same plugin implemented MBF CostmapController's extended interface.
</description>
</class>
</library>

56
mpc_local_planner/package.xml Normal file
View File

@ -0,0 +1,56 @@
<?xml version="1.0"?>
<package format="2">
<name>mpc_local_planner</name>
<version>0.0.0</version>
<description>The mpc_local_planner package implements a plugin
to the base_local_planner of the 2D navigation stack.
It provides a generic and versatile model predictive control implementation
with minimum-time and quadratic-form receding-horizon configurations.
</description>
<maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
<author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
<license>GPLv3</license>
<url type="website">http://wiki.ros.org/mpc_local_planner</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>tf2_eigen</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
<depend>control_box_rst</depend>
<depend>base_local_planner</depend>
<depend>costmap_2d</depend>
<depend>costmap_converter</depend>
<depend>dynamic_reconfigure</depend>
<depend>eigen</depend>
<depend>geometry_msgs</depend>
<depend>interactive_markers</depend>
<depend>nav_core</depend>
<depend>nav_msgs</depend>
<depend>mbf_costmap_core</depend>
<depend>mbf_msgs</depend>
<depend>mpc_local_planner_msgs</depend>
<depend>pluginlib</depend>
<depend>std_msgs</depend>
<depend>teb_local_planner</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<export>
<nav_core plugin="${prefix}/mpc_local_planner_plugin.xml"/>
<mbf_costmap_core plugin="${prefix}/mpc_local_planner_plugin.xml"/>
</export>
</package>

138
mpc_local_planner/scripts/plot_optimal_control_results.py Executable file
View File

@ -0,0 +1,138 @@
#!/usr/bin/env python
#
#
# Software License Agreement
#
# Copyright (c) 2020, Christoph Rösmann, All rights reserved.
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
#
# Authors: Christoph Rösmann
import rospy, math
from mpc_local_planner_msgs.msg import OptimalControlResult
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.ticker as ticker
from threading import Lock
class OcpResultPlotter:
def __init__(self, plot_states, topic_name):
self.initialized = False
self.plot_states = plot_states
self.dim_states = 0
self.dim_controls = 0
self.x_fig = plt.Figure()
self.x_axes = []
self.u_fig = plt.Figure()
self.u_axes = []
self.tx = []
self.x = []
self.tu = []
self.u = []
self.mutex = Lock()
self.sub = rospy.Subscriber(topic_name, OptimalControlResult, self.ocp_result_callback, queue_size = 1)
rospy.loginfo("Plotting OCP results published on '%s'.",topic_name)
rospy.loginfo("Make sure to enable rosparam 'controller/publish_ocp_results' in the mpc_local_planner.")
def ocp_result_callback(self, data):
rospy.loginfo_once("First message received.")
if not self.initialized:
self.dim_states = data.dim_states
self.dim_controls = data.dim_controls
# Read data
self.mutex.acquire()
if self.plot_states:
self.tx = np.array(data.time_states)
self.x = np.matrix(data.states)
self.x = np.reshape(self.x, (self.tx.size, int(self.dim_states)))
self.tu = np.array(data.time_controls)
self.u = np.matrix(data.controls)
self.u = np.reshape(self.u, (self.tu.size, int(self.dim_controls)))
self.mutex.release()
def initializePlotWindows(self):
if self.plot_states:
self.x_fig, self.x_axes = plt.subplots(self.dim_states, sharex=True)
self.x_axes[0].set_title('States')
for idx, ax in enumerate(self.x_axes):
ax.set_ylabel("x" + str(idx))
self.x_axes[-1].set_xlabel('Time [s]')
self.u_fig, self.u_axes = plt.subplots(self.dim_controls, sharex=True)
self.u_axes[0].set_title('Controls')
for idx, ax in enumerate(self.u_axes):
ax.set_ylabel("u" + str(idx))
self.u_axes[-1].set_xlabel('Time [s]')
plt.ion()
plt.show()
def plot(self):
# We recreate the plot every time, not fast, but okay for now....
self.mutex.acquire()
if self.plot_states:
for idx, ax in enumerate(self.x_axes):
ax.cla()
ax.grid()
ax.plot(self.tx, self.x[:,idx])
ax.get_yaxis().set_major_formatter(ticker.FuncFormatter(lambda x, p: "%.2f" % x))
ax.set_ylabel("x" + str(idx))
self.x_axes[0].set_title('States')
self.x_axes[-1].set_xlabel('Time [s]')
self.x_fig.canvas.draw()
for idx, ax in enumerate(self.u_axes):
ax.cla()
ax.grid()
ax.step(self.tu, self.u[:, idx], where='post')
ax.get_yaxis().set_major_formatter(ticker.FuncFormatter(lambda x, p: "%.2f" % x))
ax.set_ylabel("u" + str(idx))
self.u_axes[0].set_title('Controls')
self.u_axes[-1].set_xlabel('Time [s]')
self.u_fig.canvas.draw()
self.mutex.release()
def start(self, rate):
r = rospy.Rate(rate) # define rate here
while not rospy.is_shutdown():
if not self.initialized and (self.dim_states > 0 or self.dim_controls > 0):
self.initializePlotWindows()
self.initialized = True
if self.initialized:
self.plot()
r.sleep()
if __name__ == '__main__':
try:
rospy.init_node("ocp_result_plotter", anonymous=True)
topic_name = "/test_mpc_optim_node/ocp_result"
topic_name = rospy.get_param('~ocp_result_topic', topic_name)
plot_states = rospy.get_param('~plot_states', False)
result_plotter = OcpResultPlotter(plot_states, topic_name)
rate = 2
topic_name = rospy.get_param('~plot_rate', rate)
result_plotter.start(rate)
except rospy.ROSInterruptException:
pass

790
mpc_local_planner/src/controller.cpp Normal file
View File

@ -0,0 +1,790 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/controller.h>
#include <corbo-optimal-control/functions/minimum_time.h>
#include <corbo-optimal-control/functions/quadratic_control_cost.h>
#include <corbo-optimal-control/structured_ocp/discretization_grids/finite_differences_variable_grid.h>
#include <corbo-optimal-control/structured_ocp/structured_optimal_control_problem.h>
#include <corbo-optimization/hyper_graph/hyper_graph_optimization_problem_edge_based.h>
#include <corbo-optimization/solver/levenberg_marquardt_sparse.h>
#include <corbo-optimization/solver/nlp_solver_ipopt.h>
#include <corbo-optimization/solver/qp_solver_osqp.h>
#include <mpc_local_planner/optimal_control/fd_collocation_se2.h>
#include <mpc_local_planner/optimal_control/final_state_conditions_se2.h>
#include <mpc_local_planner/optimal_control/finite_differences_variable_grid_se2.h>
#include <mpc_local_planner/optimal_control/min_time_via_points_cost.h>
#include <mpc_local_planner/optimal_control/quadratic_cost_se2.h>
#include <mpc_local_planner/optimal_control/stage_inequality_se2.h>
#include <mpc_local_planner/systems/simple_car.h>
#include <mpc_local_planner/systems/unicycle_robot.h>
#include <mpc_local_planner/utils/time_series_se2.h>
#include <mpc_local_planner_msgs/OptimalControlResult.h>
#include <mpc_local_planner_msgs/StateFeedback.h>
#include <corbo-communication/utilities.h>
#include <corbo-core/console.h>
#include <tf2/utils.h>
#include <memory>
#include <mutex>
namespace mpc_local_planner {
bool Controller::configure(ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles,
teb_local_planner::RobotFootprintModelPtr robot_model, const std::vector<teb_local_planner::PoseSE2>& via_points)
{
_dynamics = configureRobotDynamics(nh);
if (!_dynamics) return false; // we may need state and control dimensions to check other parameters
_grid = configureGrid(nh);
_solver = configureSolver(nh);
_structured_ocp = configureOcp(nh, obstacles, robot_model, via_points);
_ocp = _structured_ocp; // copy pointer also to parent member
int outer_ocp_iterations = 1;
nh.param("controller/outer_ocp_iterations", outer_ocp_iterations, outer_ocp_iterations);
setNumOcpIterations(outer_ocp_iterations);
// further goal opions
nh.param("controller/force_reinit_new_goal_dist", _force_reinit_new_goal_dist, _force_reinit_new_goal_dist);
nh.param("controller/force_reinit_new_goal_angular", _force_reinit_new_goal_angular, _force_reinit_new_goal_angular);
nh.param("controller/allow_init_with_backward_motion", _guess_backwards_motion, _guess_backwards_motion);
nh.param("controller/force_reinit_num_steps", _force_reinit_num_steps, _force_reinit_num_steps);
// custom feedback:
nh.param("controller/prefer_x_feedback", _prefer_x_feedback, _prefer_x_feedback);
_x_feedback_sub = nh.subscribe("state_feedback", 1, &Controller::stateFeedbackCallback, this);
// result publisher:
_ocp_result_pub = nh.advertise<mpc_local_planner_msgs::OptimalControlResult>("ocp_result", 100);
nh.param("controller/publish_ocp_results", _publish_ocp_results, _publish_ocp_results);
nh.param("controller/print_cpu_time", _print_cpu_time, _print_cpu_time);
setAutoUpdatePreviousControl(false); // we want to update the previous control value manually
if (_ocp->initialize())
ROS_INFO("OCP initialized.");
else
{
ROS_ERROR("OCP initialization failed");
return false;
}
return _grid && _dynamics && _solver && _structured_ocp;
}
bool Controller::step(const Controller::PoseSE2& start, const Controller::PoseSE2& goal, const geometry_msgs::Twist& vel, double dt, ros::Time t,
corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq)
{
std::vector<geometry_msgs::PoseStamped> initial_plan(2);
start.toPoseMsg(initial_plan.front().pose);
goal.toPoseMsg(initial_plan.back().pose);
return step(initial_plan, vel, dt, t, u_seq, x_seq);
}
bool Controller::step(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t,
corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq)
{
if (!_dynamics || !_grid || !_structured_ocp)
{
ROS_ERROR("Controller must be configured before invoking step().");
return false;
}
if (initial_plan.size() < 2)
{
ROS_ERROR("Controller::step(): initial plan must contain at least two poses.");
return false;
}
PoseSE2 start(initial_plan.front().pose);
PoseSE2 goal(initial_plan.back().pose);
Eigen::VectorXd xf(_dynamics->getStateDimension());
_dynamics->getSteadyStateFromPoseSE2(goal, xf);
// retrieve or estimate current state
Eigen::VectorXd x(_dynamics->getStateDimension());
// check for new measurements
bool new_x = false;
{
std::lock_guard<std::mutex> lock(_x_feedback_mutex);
new_x = _recent_x_feedback.size() > 0 && (t - _recent_x_time).toSec() < 2.0 * dt;
if (new_x) x = _recent_x_feedback;
}
if (!new_x && (!_x_ts || _x_ts->isEmpty() || !_x_ts->getValuesInterpolate(dt, x))) // predict with previous state sequence
{
_dynamics->getSteadyStateFromPoseSE2(start, x); // otherwise initialize steady state
}
if (!new_x || !_prefer_x_feedback)
{
// Merge state feedback with odometry feedback if desired.
// Note, some models like unicycle overwrite the full state by odom feedback unless _prefer_x_measurement is set to true.
_dynamics->mergeStateFeedbackAndOdomFeedback(start, vel, x);
}
// now check goal
if (_force_reinit_num_steps > 0 && _ocp_seq % _force_reinit_num_steps == 0) _grid->clear();
if (!_grid->isEmpty() && ((goal.position() - _last_goal.position()).norm() > _force_reinit_new_goal_dist ||
std::abs(normalize_theta(goal.theta() - _last_goal.theta())) > _force_reinit_new_goal_angular))
{
// goal pose diverges a lot from the previous one, so force reinit
_grid->clear();
}
if (_grid->isEmpty())
{
// generate custom initialization based on initial_plan
// check if the goal is behind the start pose (w.r.t. start orientation)
bool backward = _guess_backwards_motion && (goal.position() - start.position()).dot(start.orientationUnitVec()) < 0;
generateInitialStateTrajectory(x, xf, initial_plan, backward);
}
corbo::Time time(t.toSec());
_x_seq_init.setTimeFromStart(time);
corbo::StaticReference xref(xf); // currently, we only support point-to-point transitions in ros
corbo::ZeroReference uref(_dynamics->getInputDimension());
_ocp_successful = PredictiveController::step(x, xref, uref, corbo::Duration(dt), time, u_seq, x_seq, nullptr, nullptr, &_x_seq_init);
// publish results if desired
if (_publish_ocp_results) publishOptimalControlResult(); // TODO(roesmann): we could also pass time t from above
ROS_INFO_STREAM_COND(_print_cpu_time, "Cpu time: " << _statistics.step_time.toSec() * 1000.0 << " ms.");
++_ocp_seq;
_last_goal = goal;
return _ocp_successful;
}
void Controller::stateFeedbackCallback(const mpc_local_planner_msgs::StateFeedback::ConstPtr& msg)
{
if (!_dynamics) return;
if ((int)msg->state.size() != _dynamics->getStateDimension())
{
ROS_ERROR_STREAM("stateFeedbackCallback(): state feedback dimension does not match robot state dimension: "
<< msg->state.size() << " != " << _dynamics->getStateDimension());
return;
}
std::lock_guard<std::mutex> lock(_x_feedback_mutex);
_recent_x_time = msg->header.stamp;
_recent_x_feedback = Eigen::Map<const Eigen::VectorXd>(msg->state.data(), (int)msg->state.size());
}
void Controller::publishOptimalControlResult()
{
if (!_dynamics) return;
mpc_local_planner_msgs::OptimalControlResult msg;
msg.header.stamp = ros::Time::now();
msg.header.seq = static_cast<unsigned int>(_ocp_seq);
msg.dim_states = _dynamics->getStateDimension();
msg.dim_controls = _dynamics->getInputDimension();
msg.optimal_solution_found = _ocp_successful;
msg.cpu_time = _statistics.step_time.toSec();
if (_x_ts && !_x_ts->isEmpty())
{
msg.time_states = _x_ts->getTime();
msg.states = _x_ts->getValues();
}
if (_u_ts && !_u_ts->isEmpty())
{
msg.time_controls = _u_ts->getTime();
msg.controls = _u_ts->getValues();
}
_ocp_result_pub.publish(msg);
}
void Controller::reset() { PredictiveController::reset(); }
corbo::DiscretizationGridInterface::Ptr Controller::configureGrid(const ros::NodeHandle& nh)
{
if (!_dynamics) return {};
std::string grid_type = "fd_grid";
nh.param("grid/type", grid_type, grid_type);
if (grid_type == "fd_grid")
{
FiniteDifferencesGridSE2::Ptr grid;
bool variable_grid = true;
nh.param("grid/variable_grid/enable", variable_grid, variable_grid);
if (variable_grid)
{
FiniteDifferencesVariableGridSE2::Ptr var_grid = std::make_shared<FiniteDifferencesVariableGridSE2>();
double min_dt = 0.0;
nh.param("grid/variable_grid/min_dt", min_dt, min_dt);
double max_dt = 10.0;
nh.param("grid/variable_grid/max_dt", max_dt, max_dt);
var_grid->setDtBounds(min_dt, max_dt);
bool grid_adaptation = true;
nh.param("grid/variable_grid/grid_adaptation/enable", grid_adaptation, grid_adaptation);
if (grid_adaptation)
{
int max_grid_size = 50;
nh.param("grid/variable_grid/grid_adaptation/max_grid_size", max_grid_size, max_grid_size);
double dt_hyst_ratio = 0.1;
nh.param("grid/variable_grid/grid_adaptation/dt_hyst_ratio", dt_hyst_ratio, dt_hyst_ratio);
var_grid->setGridAdaptTimeBasedSingleStep(max_grid_size, dt_hyst_ratio, true);
int min_grid_size = 2;
nh.param("grid/variable_grid/grid_adaptation/min_grid_size", min_grid_size, min_grid_size);
var_grid->setNmin(min_grid_size);
}
else
{
var_grid->disableGridAdaptation();
}
grid = var_grid;
}
else
{
grid = std::make_shared<FiniteDifferencesGridSE2>();
}
// common grid parameters
int grid_size_ref = 20;
nh.param("grid/grid_size_ref", grid_size_ref, grid_size_ref);
grid->setNRef(grid_size_ref);
double dt_ref = 0.3;
nh.param("grid/dt_ref", dt_ref, dt_ref);
grid->setDtRef(dt_ref);
std::vector<bool> xf_fixed = {true, true, true};
nh.param("grid/xf_fixed", xf_fixed, xf_fixed);
if ((int)xf_fixed.size() != _dynamics->getStateDimension())
{
ROS_ERROR_STREAM("Array size of `xf_fixed` does not match robot state dimension(): " << xf_fixed.size()
<< " != " << _dynamics->getStateDimension());
return {};
}
Eigen::Matrix<bool, -1, 1> xf_fixed_eigen(xf_fixed.size()); // we cannot use Eigen::Map as vector<bool> does not provide raw access
for (int i = 0; i < (int)xf_fixed.size(); ++i) xf_fixed_eigen[i] = xf_fixed[i];
grid->setXfFixed(xf_fixed_eigen);
bool warm_start = true;
nh.param("warm_start", warm_start, warm_start);
grid->setWarmStart(warm_start);
std::string collocation_method = "forward_differences";
nh.param("collocation_method", collocation_method, collocation_method);
if (collocation_method == "forward_differences")
{
grid->setFiniteDifferencesCollocationMethod(std::make_shared<ForwardDiffCollocationSE2>());
}
else if (collocation_method == "midpoint_differences")
{
grid->setFiniteDifferencesCollocationMethod(std::make_shared<MidpointDiffCollocationSE2>());
}
else if (collocation_method == "crank_nicolson_differences")
{
grid->setFiniteDifferencesCollocationMethod(std::make_shared<CrankNicolsonDiffCollocationSE2>());
}
else
{
ROS_ERROR_STREAM("Unknown collocation method '" << collocation_method << "' specified. Falling back to default...");
}
std::string cost_integration_method = "left_sum";
nh.param("cost_integration_method", cost_integration_method, cost_integration_method);
if (cost_integration_method == "left_sum")
{
grid->setCostIntegrationRule(FullDiscretizationGridBaseSE2::CostIntegrationRule::LeftSum);
}
else if (cost_integration_method == "trapezoidal_rule")
{
grid->setCostIntegrationRule(FullDiscretizationGridBaseSE2::CostIntegrationRule::TrapezoidalRule);
}
else
{
ROS_ERROR_STREAM("Unknown cost integration method '" << cost_integration_method << "' specified. Falling back to default...");
}
return std::move(grid);
}
else
{
ROS_ERROR_STREAM("Unknown grid type '" << grid_type << "' specified.");
}
return {};
}
RobotDynamicsInterface::Ptr Controller::configureRobotDynamics(const ros::NodeHandle& nh)
{
_robot_type = "unicycle";
nh.param("robot/type", _robot_type, _robot_type);
if (_robot_type == "unicycle")
{
return std::make_shared<UnicycleModel>();
}
else if (_robot_type == "simple_car")
{
double wheelbase = 0.5;
nh.param("robot/simple_car/wheelbase", wheelbase, wheelbase);
bool front_wheel_driving = false;
nh.param("robot/simple_car/front_wheel_driving", front_wheel_driving, front_wheel_driving);
if (front_wheel_driving)
return std::make_shared<SimpleCarFrontWheelDrivingModel>(wheelbase);
else
return std::make_shared<SimpleCarModel>(wheelbase);
}
else
{
ROS_ERROR_STREAM("Unknown robot type '" << _robot_type << "' specified.");
}
return {};
}
corbo::NlpSolverInterface::Ptr Controller::configureSolver(const ros::NodeHandle& nh)
{
std::string solver_type = "ipopt";
nh.param("solver/type", solver_type, solver_type);
if (solver_type == "ipopt")
{
corbo::SolverIpopt::Ptr solver = std::make_shared<corbo::SolverIpopt>();
solver->initialize(); // requried for setting parameters afterward
int iterations = 100;
nh.param("solver/ipopt/iterations", iterations, iterations);
solver->setIterations(iterations);
double max_cpu_time = -1.0;
nh.param("solver/ipopt/max_cpu_time", max_cpu_time, max_cpu_time);
solver->setMaxCpuTime(max_cpu_time);
// now check for additional ipopt options
std::map<std::string, double> numeric_options;
nh.param("solver/ipopt/ipopt_numeric_options", numeric_options, numeric_options);
for (const auto& item : numeric_options)
{
if (!solver->setIpoptOptionNumeric(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set.");
}
std::map<std::string, std::string> string_options;
nh.param("solver/ipopt/ipopt_string_options", string_options, string_options);
for (const auto& item : string_options)
{
if (!solver->setIpoptOptionString(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set.");
}
std::map<std::string, int> integer_options;
nh.param("solver/ipopt/ipopt_integer_options", integer_options, integer_options);
for (const auto& item : integer_options)
{
if (!solver->setIpoptOptionInt(item.first, item.second)) ROS_WARN_STREAM("Ipopt option " << item.first << " could not be set.");
}
return std::move(solver);
}
// else if (solver_type == "sqp")
// {
// corbo::SolverSQP::Ptr solver = std::make_shared<corbo::SolverSQP>();
// solver->setUseObjectiveHessian(true);
// // solver->setQpZeroWarmstart(false);
// solver->setVerbose(true);
// corbo::SolverOsqp::Ptr qp_solver = std::make_shared<corbo::SolverOsqp>();
// qp_solver->getOsqpSettings()->verbose = 1;
// solver->setQpSolver(qp_solver);
// corbo::LineSearchL1::Ptr ls = std::make_shared<corbo::LineSearchL1>();
// ls->setVerbose(true);
// ls->setEta(1e-6);
// solver->setLineSearchAlgorithm(ls);
// return std::move(solver);
// }
else if (solver_type == "lsq_lm")
{
corbo::LevenbergMarquardtSparse::Ptr solver = std::make_shared<corbo::LevenbergMarquardtSparse>();
int iterations = 10;
nh.param("solver/lsq_lm/iterations", iterations, iterations);
solver->setIterations(iterations);
double weight_init_eq = 2;
nh.param("solver/lsq_lm/weight_init_eq", weight_init_eq, weight_init_eq);
double weight_init_ineq = 2;
nh.param("solver/lsq_lm/weight_init_ineq", weight_init_ineq, weight_init_ineq);
double weight_init_bounds = 2;
nh.param("solver/lsq_lm/weight_init_bounds", weight_init_bounds, weight_init_bounds);
solver->setPenaltyWeights(weight_init_eq, weight_init_ineq, weight_init_bounds);
double weight_adapt_factor_eq = 1;
nh.param("solver/lsq_lm/weight_adapt_factor_eq", weight_adapt_factor_eq, weight_adapt_factor_eq);
double weight_adapt_factor_ineq = 1;
nh.param("solver/lsq_lm/weight_adapt_factor_ineq", weight_adapt_factor_ineq, weight_adapt_factor_ineq);
double weight_adapt_factor_bounds = 1;
nh.param("solver/lsq_lm/weight_adapt_factor_bounds", weight_adapt_factor_bounds, weight_adapt_factor_bounds);
double weight_adapt_max_eq = 500;
nh.param("solver/lsq_lm/weight_adapt_max_eq", weight_adapt_max_eq, weight_adapt_max_eq);
double weight_adapt_max_ineq = 500;
nh.param("solver/lsq_lm/weight_init_eq", weight_adapt_max_ineq, weight_adapt_max_ineq);
double weight_adapt_max_bounds = 500;
nh.param("solver/lsq_lm/weight_adapt_max_bounds", weight_adapt_max_bounds, weight_adapt_max_bounds);
solver->setWeightAdapation(weight_adapt_factor_eq, weight_adapt_factor_ineq, weight_adapt_factor_bounds, weight_adapt_max_eq,
weight_adapt_max_ineq, weight_adapt_max_bounds);
return std::move(solver);
}
else
{
ROS_ERROR_STREAM("Unknown solver type '" << solver_type << "' specified.");
}
return {};
}
corbo::StructuredOptimalControlProblem::Ptr Controller::configureOcp(const ros::NodeHandle& nh, const teb_local_planner::ObstContainer& obstacles,
teb_local_planner::RobotFootprintModelPtr robot_model,
const std::vector<teb_local_planner::PoseSE2>& via_points)
{
corbo::BaseHyperGraphOptimizationProblem::Ptr hg = std::make_shared<corbo::HyperGraphOptimizationProblemEdgeBased>();
corbo::StructuredOptimalControlProblem::Ptr ocp = std::make_shared<corbo::StructuredOptimalControlProblem>(_grid, _dynamics, hg, _solver);
const int x_dim = _dynamics->getStateDimension();
const int u_dim = _dynamics->getInputDimension();
if (_robot_type == "unicycle")
{
double max_vel_x = 0.4;
nh.param("robot/unicycle/max_vel_x", max_vel_x, max_vel_x);
double max_vel_x_backwards = 0.2;
nh.param("robot/unicycle/max_vel_x_backwards", max_vel_x_backwards, max_vel_x_backwards);
if (max_vel_x_backwards < 0)
{
ROS_WARN("max_vel_x_backwards must be >= 0");
max_vel_x_backwards *= -1;
}
double max_vel_theta = 0.3;
nh.param("robot/unicycle/max_vel_theta", max_vel_theta, max_vel_theta);
// ocp->setBounds(Eigen::Vector3d(-corbo::CORBO_INF_DBL, -corbo::CORBO_INF_DBL, -corbo::CORBO_INF_DBL),
// Eigen::Vector3d(corbo::CORBO_INF_DBL, corbo::CORBO_INF_DBL, corbo::CORBO_INF_DBL),
// Eigen::Vector2d(-max_vel_x_backwards, -max_vel_theta), Eigen::Vector2d(max_vel_x, max_vel_theta));
ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_vel_theta), Eigen::Vector2d(max_vel_x, max_vel_theta));
}
else if (_robot_type == "simple_car")
{
double max_vel_x = 0.4;
nh.param("robot/simple_car/max_vel_x", max_vel_x, max_vel_x);
double max_vel_x_backwards = 0.2;
nh.param("robot/simple_car/max_vel_x_backwards", max_vel_x_backwards, max_vel_x_backwards);
if (max_vel_x_backwards < 0)
{
ROS_WARN("max_vel_x_backwards must be >= 0");
max_vel_x_backwards *= -1;
}
double max_steering_angle = 1.5;
nh.param("robot/simple_car/max_steering_angle", max_steering_angle, max_steering_angle);
ocp->setControlBounds(Eigen::Vector2d(-max_vel_x_backwards, -max_steering_angle), Eigen::Vector2d(max_vel_x, max_steering_angle));
}
else
{
ROS_ERROR_STREAM("Cannot configure OCP for unknown robot type " << _robot_type << ".");
return {};
}
std::string objective_type = "minimum_time";
nh.param("planning/objective/type", objective_type, objective_type);
bool lsq_solver = _solver->isLsqSolver();
if (objective_type == "minimum_time")
{
ocp->setStageCost(std::make_shared<corbo::MinimumTime>(lsq_solver));
}
else if (objective_type == "quadratic_form")
{
std::vector<double> state_weights;
nh.param("planning/objective/quadratic_form/state_weights", state_weights, state_weights);
Eigen::MatrixXd Q;
if (state_weights.size() == x_dim)
{
Q = Eigen::Map<Eigen::VectorXd>(state_weights.data(), x_dim).asDiagonal();
}
else if (state_weights.size() == x_dim * x_dim)
{
Q = Eigen::Map<Eigen::MatrixXd>(state_weights.data(), x_dim, x_dim); // Eigens default is column major
}
else
{
ROS_ERROR_STREAM("State weights dimension invalid. Must be either " << x_dim << " x 1 or " << x_dim << " x " << x_dim << ".");
return {};
}
std::vector<double> control_weights;
nh.param("planning/objective/quadratic_form/control_weights", control_weights, control_weights);
Eigen::MatrixXd R;
if (control_weights.size() == u_dim)
{
R = Eigen::Map<Eigen::VectorXd>(control_weights.data(), u_dim).asDiagonal();
}
else if (control_weights.size() == u_dim * u_dim)
{
R = Eigen::Map<Eigen::MatrixXd>(control_weights.data(), u_dim, u_dim); // Eigens default is column major
}
else
{
ROS_ERROR_STREAM("Control weights dimension invalid. Must be either " << u_dim << " x 1 or " << u_dim << " x " << u_dim << ".");
return {};
}
bool integral_form = false;
nh.param("planning/objective/quadratic_form/integral_form", integral_form, integral_form);
bool q_zero = Q.isZero();
bool r_zero = R.isZero();
if (!q_zero && !r_zero)
ocp->setStageCost(std::make_shared<QuadraticFormCostSE2>(Q, R, integral_form, lsq_solver));
else if (!q_zero && r_zero)
ocp->setStageCost(std::make_shared<QuadraticStateCostSE2>(Q, integral_form, lsq_solver));
else if (q_zero && !r_zero)
ocp->setStageCost(std::make_shared<corbo::QuadraticControlCost>(R, integral_form, lsq_solver));
}
else if (objective_type == "minimum_time_via_points")
{
bool via_points_ordered = false;
nh.param("planning/objective/minimum_time_via_points/via_points_ordered", via_points_ordered, via_points_ordered);
double position_weight = 1.0;
nh.param("planning/objective/minimum_time_via_points/position_weight", position_weight, position_weight);
double orientation_weight = 0.0;
nh.param("planning/objective/minimum_time_via_points/orientation_weight", orientation_weight, orientation_weight);
ocp->setStageCost(std::make_shared<MinTimeViaPointsCost>(via_points, position_weight, orientation_weight, via_points_ordered));
// TODO(roesmann): lsq version
}
else
{
ROS_ERROR_STREAM("Unknown objective type '" << objective_type << "' specified ('planning/objective/type').");
return {};
}
std::string terminal_cost = "none";
nh.param("planning/terminal_cost/type", terminal_cost, terminal_cost);
if (terminal_cost == "none")
{
ocp->setFinalStageCost({});
}
else if (terminal_cost == "quadratic")
{
std::vector<double> state_weights;
nh.param("planning/terminal_cost/quadratic/final_state_weights", state_weights, state_weights);
Eigen::MatrixXd Qf;
if (state_weights.size() == x_dim)
{
Qf = Eigen::Map<Eigen::VectorXd>(state_weights.data(), x_dim).asDiagonal();
}
else if (state_weights.size() == x_dim * x_dim)
{
Qf = Eigen::Map<Eigen::MatrixXd>(state_weights.data(), x_dim, x_dim); // Eigens default is column major
}
else
{
ROS_ERROR_STREAM("Final state weights dimension invalid. Must be either " << x_dim << " x 1 or " << x_dim << " x " << x_dim << ".");
return {};
}
ocp->setFinalStageCost(std::make_shared<QuadraticFinalStateCostSE2>(Qf, lsq_solver));
}
else
{
ROS_ERROR_STREAM("Unknown terminal_cost type '" << terminal_cost << "' specified ('planning/terminal_cost/type').");
return {};
}
std::string terminal_constraint = "none";
nh.param("planning/terminal_constraint/type", terminal_constraint, terminal_constraint);
if (terminal_constraint == "none")
{
ocp->setFinalStageConstraint({});
}
else if (terminal_constraint == "l2_ball")
{
std::vector<double> weight_matrix;
nh.param("planning/terminal_constraint/l2_ball/weight_matrix", weight_matrix, weight_matrix);
Eigen::MatrixXd S;
if (weight_matrix.size() == x_dim)
{
S = Eigen::Map<Eigen::VectorXd>(weight_matrix.data(), x_dim).asDiagonal();
}
else if (weight_matrix.size() == x_dim * x_dim)
{
S = Eigen::Map<Eigen::MatrixXd>(weight_matrix.data(), x_dim, x_dim); // Eigens default is column major
}
else
{
ROS_ERROR_STREAM("l2-ball weight_matrix dimensions invalid. Must be either " << x_dim << " x 1 or " << x_dim << " x " << x_dim << ".");
return {};
}
double radius = 1.0;
nh.param("planning/terminal_constraint/l2_ball/radius", radius, radius);
ocp->setFinalStageConstraint(std::make_shared<TerminalBallSE2>(S, radius));
}
else
{
ROS_ERROR_STREAM("Unknown terminal_constraint type '" << terminal_constraint << "' specified ('planning/terminal_constraint/type').");
return {};
}
_inequality_constraint = std::make_shared<StageInequalitySE2>();
_inequality_constraint->setObstacleVector(obstacles);
_inequality_constraint->setRobotFootprintModel(robot_model);
// configure collision avoidance
double min_obstacle_dist = 0.5;
nh.param("collision_avoidance/min_obstacle_dist", min_obstacle_dist, min_obstacle_dist);
_inequality_constraint->setMinimumDistance(min_obstacle_dist);
bool enable_dynamic_obstacles = false;
nh.param("collision_avoidance/enable_dynamic_obstacles", enable_dynamic_obstacles, enable_dynamic_obstacles);
_inequality_constraint->setEnableDynamicObstacles(enable_dynamic_obstacles);
double force_inclusion_factor = 1.5;
nh.param("collision_avoidance/force_inclusion_factor", force_inclusion_factor, force_inclusion_factor);
double cutoff_factor = 5;
nh.param("collision_avoidance/cutoff_factor", cutoff_factor, cutoff_factor);
_inequality_constraint->setObstacleFilterParameters(force_inclusion_factor, cutoff_factor);
// configure control deviation bounds
if (_robot_type == "unicycle")
{
double acc_lim_x = 0.0;
nh.param("robot/unicycle/acc_lim_x", acc_lim_x, acc_lim_x);
double dec_lim_x = 0.0;
nh.param("robot/unicycle/dec_lim_x", dec_lim_x, dec_lim_x);
if (dec_lim_x < 0)
{
ROS_WARN("dec_lim_x must be >= 0");
dec_lim_x *= -1;
}
double acc_lim_theta = 0.0;
nh.param("robot/unicycle/acc_lim_theta", acc_lim_theta, acc_lim_theta);
if (acc_lim_x <= 0) acc_lim_x = corbo::CORBO_INF_DBL;
if (dec_lim_x <= 0) dec_lim_x = corbo::CORBO_INF_DBL;
if (acc_lim_theta <= 0) acc_lim_theta = corbo::CORBO_INF_DBL;
Eigen::Vector2d ud_lb(-dec_lim_x, -acc_lim_theta);
Eigen::Vector2d ud_ub(acc_lim_x, acc_lim_theta);
_inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub);
}
else if (_robot_type == "simple_car")
{
double acc_lim_x = 0.0;
nh.param("robot/simple_car/acc_lim_x", acc_lim_x, acc_lim_x);
double dec_lim_x = 0.0;
nh.param("robot/simple_car/dec_lim_x", dec_lim_x, dec_lim_x);
if (dec_lim_x < 0)
{
ROS_WARN("dec_lim_x must be >= 0");
dec_lim_x *= -1;
}
double max_steering_rate = 0.0;
nh.param("robot/simple_car/max_steering_rate", max_steering_rate, max_steering_rate);
if (acc_lim_x <= 0) acc_lim_x = corbo::CORBO_INF_DBL;
if (dec_lim_x <= 0) dec_lim_x = corbo::CORBO_INF_DBL;
if (max_steering_rate <= 0) max_steering_rate = corbo::CORBO_INF_DBL;
Eigen::Vector2d ud_lb(-dec_lim_x, -max_steering_rate);
Eigen::Vector2d ud_ub(acc_lim_x, max_steering_rate);
_inequality_constraint->setControlDeviationBounds(ud_lb, ud_ub);
}
else
{
ROS_ERROR_STREAM("Cannot configure control deviation bounds for unknown robot type " << _robot_type << ".");
return {};
}
ocp->setStageInequalityConstraint(_inequality_constraint);
return ocp;
}
bool Controller::generateInitialStateTrajectory(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf,
const std::vector<geometry_msgs::PoseStamped>& initial_plan, bool backward)
{
if (initial_plan.size() < 2 || !_dynamics) return false;
TimeSeriesSE2::Ptr ts = std::make_shared<TimeSeriesSE2>();
int n_init = (int)initial_plan.size();
int n_ref = _grid->getInitialN();
if (n_ref < 2)
{
ROS_ERROR("Controller::generateInitialStateTrajectory(): grid not properly initialized");
return false;
}
ts->add(0.0, x0);
double dt_ref = _grid->getInitialDt();
double tf_ref = (double)(n_ref - 1) * dt_ref;
Eigen::VectorXd x(_dynamics->getStateDimension());
// we initialize by assuming equally distributed poses
double dt_init = tf_ref / double(n_init - 1);
double t = dt_init;
for (int i = 1; i < n_init - 1; ++i)
{
// get yaw from the orientation of the distance vector between pose_{i+1} and pose_{i}
double yaw;
if (_initial_plan_estimate_orientation)
{
double dx = initial_plan[i + 1].pose.position.x - initial_plan[i].pose.position.x;
double dy = initial_plan[i + 1].pose.position.y - initial_plan[i].pose.position.y;
yaw = std::atan2(dy, dx);
if (backward) normalize_theta(yaw + M_PI);
}
else
{
yaw = tf2::getYaw(initial_plan[i].pose.orientation);
}
PoseSE2 intermediate_pose(initial_plan[i].pose.position.x, initial_plan[i].pose.position.y, yaw);
_dynamics->getSteadyStateFromPoseSE2(intermediate_pose, x);
ts->add(t, x);
t += dt_init;
}
ts->add(tf_ref, xf);
_x_seq_init.setTrajectory(ts, corbo::TimeSeries::Interpolation::Linear);
return true;
}
} // namespace mpc_local_planner

1031
mpc_local_planner/src/mpc_local_planner_ros.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

66
mpc_local_planner/src/optimal_control/final_state_conditions_se2.cpp Normal file
View File

@ -0,0 +1,66 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/optimal_control/final_state_conditions_se2.h>
#include <mpc_local_planner/utils/math_utils.h>
#include <cmath>
namespace mpc_local_planner {
void QuadraticFinalStateCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k,
Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(cost.size() == getNonIntegralStateTermDimension(k));
Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
xd[2] = normalize_theta(xd[2]);
if (_lsq_form)
{
if (_diagonal_mode)
cost.noalias() = _Qf_diag_sqrt * xd;
else
cost.noalias() = _Qf_sqrt * xd;
}
else
{
if (_diagonal_mode)
cost.noalias() = xd.transpose() * _Qf_diag * xd;
else
cost.noalias() = xd.transpose() * _Qf * xd;
}
}
void TerminalBallSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(cost.size() == getNonIntegralStateTermDimension(k));
Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
xd[2] = normalize_theta(xd[2]);
if (_diagonal_mode)
cost[0] = xd.transpose() * _S_diag * xd - _gamma;
else
cost[0] = xd.transpose() * _S * xd - _gamma;
}
} // namespace mpc_local_planner

156
mpc_local_planner/src/optimal_control/finite_differences_grid_se2.cpp Normal file
View File

@ -0,0 +1,156 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/optimal_control/finite_differences_grid_se2.h>
#include <corbo-optimal-control/structured_ocp/edges/finite_differences_collocation_edges.h>
#include <corbo-communication/utilities.h>
#include <corbo-core/console.h>
#include <algorithm>
#include <cmath>
#include <memory>
namespace mpc_local_planner {
void FiniteDifferencesGridSE2::createEdges(NlpFunctions& nlp_fun, OptimizationEdgeSet& edges, SystemDynamicsInterface::Ptr dynamics)
{
assert(isValid());
// clear edges first
// TODO(roesmann): we could implement a more efficient strategy without recreating the whole edgeset everytime
edges.clear();
int n = getN();
std::vector<BaseEdge::Ptr> cost_terms, eq_terms, ineq_terms;
for (int k = 0; k < n - 1; ++k)
{
VectorVertexSE2& x_next = (k < n - 2) ? _x_seq[k + 1] : _xf;
VectorVertex& u_prev = (k > 0) ? _u_seq[k - 1] : _u_prev;
ScalarVertex& dt_prev = (k > 0) ? _dt : _u_prev_dt;
cost_terms.clear();
eq_terms.clear();
ineq_terms.clear();
nlp_fun.getNonIntegralStageFunctionEdges(k, _x_seq[k], _u_seq[k], _dt, u_prev, dt_prev, cost_terms, eq_terms, ineq_terms);
for (BaseEdge::Ptr& edge : cost_terms) edges.addObjectiveEdge(edge);
for (BaseEdge::Ptr& edge : eq_terms) edges.addEqualityEdge(edge);
for (BaseEdge::Ptr& edge : ineq_terms) edges.addInequalityEdge(edge);
if (nlp_fun.stage_cost && nlp_fun.stage_cost->hasIntegralTerms(k))
{
if (_cost_integration == CostIntegrationRule::TrapezoidalRule)
{
corbo::TrapezoidalIntegralCostEdge::Ptr edge =
std::make_shared<corbo::TrapezoidalIntegralCostEdge>(_x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_cost, k);
edges.addObjectiveEdge(edge);
}
else if (_cost_integration == CostIntegrationRule::LeftSum)
{
corbo::LeftSumCostEdge::Ptr edge = std::make_shared<corbo::LeftSumCostEdge>(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_cost, k);
edges.addObjectiveEdge(edge);
}
else
PRINT_ERROR_NAMED("Cost integration rule not implemented");
}
if (nlp_fun.stage_equalities && nlp_fun.stage_equalities->hasIntegralTerms(k))
{
if (_cost_integration == CostIntegrationRule::TrapezoidalRule)
{
corbo::TrapezoidalIntegralEqualityDynamicsEdge::Ptr edge = std::make_shared<corbo::TrapezoidalIntegralEqualityDynamicsEdge>(
dynamics, _x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_equalities, k);
edge->setFiniteDifferencesCollocationMethod(_fd_eval);
edges.addEqualityEdge(edge);
}
else if (_cost_integration == CostIntegrationRule::LeftSum)
{
corbo::LeftSumEqualityEdge::Ptr edge =
std::make_shared<corbo::LeftSumEqualityEdge>(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_equalities, k);
edges.addEqualityEdge(edge);
// system dynamics edge
corbo::FDCollocationEdge::Ptr sys_edge = std::make_shared<corbo::FDCollocationEdge>(dynamics, _x_seq[k], _u_seq[k], x_next, _dt);
sys_edge->setFiniteDifferencesCollocationMethod(_fd_eval);
edges.addEqualityEdge(sys_edge);
}
else
PRINT_ERROR_NAMED("Cost integration rule not implemented");
}
else
{
// just the system dynamics edge
corbo::FDCollocationEdge::Ptr edge = std::make_shared<corbo::FDCollocationEdge>(dynamics, _x_seq[k], _u_seq[k], x_next, _dt);
edge->setFiniteDifferencesCollocationMethod(_fd_eval);
edges.addEqualityEdge(edge);
}
if (nlp_fun.stage_inequalities && nlp_fun.stage_inequalities->hasIntegralTerms(k))
{
if (_cost_integration == CostIntegrationRule::TrapezoidalRule)
{
corbo::TrapezoidalIntegralInequalityEdge::Ptr edge =
std::make_shared<corbo::TrapezoidalIntegralInequalityEdge>(_x_seq[k], _u_seq[k], x_next, _dt, nlp_fun.stage_inequalities, k);
edges.addInequalityEdge(edge);
}
else if (_cost_integration == CostIntegrationRule::LeftSum)
{
corbo::LeftSumInequalityEdge::Ptr edge =
std::make_shared<corbo::LeftSumInequalityEdge>(_x_seq[k], _u_seq[k], _dt, nlp_fun.stage_inequalities, k);
edges.addInequalityEdge(edge);
}
else
PRINT_ERROR_NAMED("Cost integration rule not implemented");
}
}
// check if we have a separate unfixed final state
if (!_xf.isFixed())
{
// set final state cost
BaseEdge::Ptr cost_edge = nlp_fun.getFinalStateCostEdge(n - 1, _xf);
if (cost_edge) edges.addObjectiveEdge(cost_edge);
// set final state constraint
BaseEdge::Ptr constr_edge = nlp_fun.getFinalStateConstraintEdge(n - 1, _xf);
if (constr_edge)
{
if (nlp_fun.final_stage_constraints->isEqualityConstraint())
edges.addEqualityEdge(constr_edge);
else
edges.addInequalityEdge(constr_edge);
}
}
// add control deviation edges for last control
cost_terms.clear();
eq_terms.clear();
ineq_terms.clear();
nlp_fun.getFinalControlDeviationEdges(n, _u_ref, _u_seq.back(), _dt, cost_terms, eq_terms, ineq_terms);
for (BaseEdge::Ptr& edge : cost_terms) edges.addObjectiveEdge(edge);
for (BaseEdge::Ptr& edge : eq_terms) edges.addEqualityEdge(edge);
for (BaseEdge::Ptr& edge : ineq_terms) edges.addInequalityEdge(edge);
}
} // namespace mpc_local_planner

163
mpc_local_planner/src/optimal_control/finite_differences_variable_grid_se2.cpp Normal file
View File

@ -0,0 +1,163 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/optimal_control/finite_differences_variable_grid_se2.h>
#include <corbo-optimal-control/structured_ocp/edges/finite_differences_collocation_edges.h>
#include <corbo-communication/utilities.h>
#include <corbo-core/console.h>
#include <algorithm>
#include <cmath>
#include <memory>
namespace mpc_local_planner {
void FiniteDifferencesVariableGridSE2::setDtBounds(double dt_lb, double dt_ub)
{
_dt_lb = dt_lb;
_dt_ub = dt_ub;
}
void FiniteDifferencesVariableGridSE2::setGridAdaptTimeBasedSingleStep(int n_max, double dt_hyst_ratio, bool adapt_first_iter)
{
_grid_adapt = GridAdaptStrategy::TimeBasedSingleStep;
_n_max = n_max;
_dt_hyst_ratio = dt_hyst_ratio;
_adapt_first_iter = adapt_first_iter;
}
void FiniteDifferencesVariableGridSE2::setGridAdaptTimeBasedAggressiveEstimate(int n_max, double dt_hyst_ratio, bool adapt_first_iter)
{
_grid_adapt = GridAdaptStrategy::TimeBasedAggressiveEstimate;
_n_max = n_max;
_dt_hyst_ratio = dt_hyst_ratio;
_adapt_first_iter = adapt_first_iter;
}
void FiniteDifferencesVariableGridSE2::setGridAdaptSimpleShrinkingHorizon(bool adapt_first_iter)
{
_grid_adapt = GridAdaptStrategy::SimpleShrinkingHorizon;
_adapt_first_iter = adapt_first_iter;
}
bool FiniteDifferencesVariableGridSE2::adaptGrid(bool new_run, NlpFunctions& nlp_fun)
{
// do not adapt grid in a new run
if (new_run && !_adapt_first_iter) return false;
bool changed = false;
switch (_grid_adapt)
{
case GridAdaptStrategy::NoGridAdapt:
{
break;
}
case GridAdaptStrategy::TimeBasedSingleStep:
{
changed = adaptGridTimeBasedSingleStep(nlp_fun);
break;
}
case GridAdaptStrategy::TimeBasedAggressiveEstimate:
{
changed = adaptGridTimeBasedAggressiveEstimate(nlp_fun);
break;
}
case GridAdaptStrategy::SimpleShrinkingHorizon:
{
changed = adaptGridSimpleShrinkingHorizon(nlp_fun);
break;
}
default:
{
PRINT_ERROR_NAMED("selected grid adaptation strategy not implemented.");
}
}
return changed;
}
bool FiniteDifferencesVariableGridSE2::adaptGridTimeBasedSingleStep(NlpFunctions& nlp_fun)
{
PRINT_WARNING_COND_NAMED(!isTimeVariableGrid(), "time based adaptation might only be used with a fixed dt.");
_nlp_fun = &nlp_fun;
int n = getN();
double dt = getDt();
if (dt > _dt_ref * (1.0 + _dt_hyst_ratio) && n < _n_max)
{
resampleTrajectory(n + 1);
_n_adapt = n + 1;
return true;
}
else if (dt < _dt_ref * (1.0 - _dt_hyst_ratio) && n > _n_min)
{
resampleTrajectory(n - 1);
_n_adapt = n - 1;
return true;
}
return false;
}
bool FiniteDifferencesVariableGridSE2::adaptGridTimeBasedAggressiveEstimate(NlpFunctions& nlp_fun)
{
PRINT_WARNING_COND_NAMED(!isTimeVariableGrid(), "time based adaptation might only be used with a fixed dt.");
_nlp_fun = &nlp_fun;
int n = getN();
double dt = getDt();
// check if hysteresis is satisfied
if (dt >= _dt_ref * (1.0 - _dt_hyst_ratio) && dt <= _dt_ref * (1.0 + _dt_hyst_ratio)) return false;
// estimate number of samples based on the fraction dt/dt_ref.
// dt is the time difference obtained in a previous solution (with a coarser resp. finer trajectory resolution)
int new_n = std::round((double)n * (dt / _dt_ref));
// bound value
if (new_n > _n_max)
new_n = _n_max;
else if (new_n < _n_min)
new_n = _n_min;
if (new_n == n) return false;
// and now resample
resampleTrajectory(new_n);
_n_adapt = new_n;
return true;
}
bool FiniteDifferencesVariableGridSE2::adaptGridSimpleShrinkingHorizon(NlpFunctions& nlp_fun)
{
int n = getN();
if (n > _n_min)
{
resampleTrajectory(n - 1);
_n_adapt = n - 1;
}
return false;
}
} // namespace mpc_local_planner

614
mpc_local_planner/src/optimal_control/full_discretization_grid_base_se2.cpp Normal file
View File

@ -0,0 +1,614 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h>
#include <mpc_local_planner/utils/math_utils.h>
#include <corbo-optimal-control/structured_ocp/edges/finite_differences_collocation_edges.h>
#include <corbo-communication/utilities.h>
#include <corbo-core/console.h>
#include <algorithm>
#include <cmath>
#include <memory>
namespace mpc_local_planner {
corbo::GridUpdateResult FullDiscretizationGridBaseSE2::update(const Eigen::VectorXd& x0, ReferenceTrajectoryInterface& xref,
ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun, OptimizationEdgeSet& edges,
SystemDynamicsInterface::Ptr dynamics, bool new_run, const corbo::Time& t,
ReferenceTrajectoryInterface* sref, const Eigen::VectorXd* prev_u, double prev_u_dt,
ReferenceTrajectoryInterface* xinit, ReferenceTrajectoryInterface* uinit)
{
assert(x0.size() == dynamics->getStateDimension());
assert(xref.getDimension() == x0.size());
assert(uref.getDimension() == dynamics->getInputDimension());
assert(x0.size() >= 3); // SE2 version here
corbo::GridUpdateResult result;
_nlp_fun = &nlp_fun; // for bound access in e.g. resampleTrajectory()
if (isGridAdaptActive() && !_first_run && !isEmpty()) // TODO(roesmann): this might not be efficient in case warm start is deactivated
{
// adapt grid if implemented by subclass
adaptGrid(new_run, nlp_fun); // note, isModified() should return true if something was changed!, we could also use the return value...
}
// check if we need to cache the reference trajectory values // TODO(roesmann): we could restrict this to new_run==true only as long as we do not
// have a grid resize...
int n = std::max(std::max(getNRef(), getN()), _n_adapt);
if (!xref.isCached(getDt(), n, t)) xref.precompute(getDt(), n, t);
if (!uref.isCached(getDt(), n, t)) uref.precompute(getDt(), n, t);
if (sref && !sref->isCached(getDt(), n, t)) sref->precompute(getDt(), n, t);
if (xinit && !xinit->isCached(getDt(), n, t)) xinit->precompute(getDt(), n, t);
if (uinit && !uinit->isCached(getDt(), n, t)) uinit->precompute(getDt(), n, t);
// set previous control which might be utilized by some edges
if (prev_u && prev_u->size() > 0)
setPreviousControl(*prev_u, prev_u_dt);
else
setPreviousControl(Eigen::VectorXd::Zero(dynamics->getInputDimension()), prev_u_dt);
// set last control to uref
setLastControlRef(uref.getReferenceCached(n));
// TODO(roesmann): we do not check if bounds in nlp_fun are updated
// updateBounds(); // calling this everytime is not efficient
// initialize trajectories if empty or if the goal differs from the last one
// reinit if goal is far away
if (isEmpty()) // TODO(roesmann): threshold?: if (_cached_xf.rows() > 0 && (xf - _cached_xf).norm() > _warm_start_goal_dist_reinit)
{
if (xref.isStatic() && !xinit)
{
// TODO(roesmann): optional initialization references x_init, u_init
initializeSequences(x0, xref.getReferenceCached(n - 1), uinit ? *uinit : uref, nlp_fun);
}
else
{
initializeSequences(x0, xref.getReferenceCached(n - 1), xinit ? *xinit : xref, uinit ? *uinit : uref, nlp_fun);
}
}
else
{
if (new_run && isMovingHorizonWarmStartActive())
{
// warm_start
warmStartShifting(x0);
}
if (new_run)
{
// make sure to always overwrite the start with the actual (measured) values
_x_seq.front().values() = x0;
// update fixed goal states
for (int i = 0; i < _xf_fixed.size(); ++i)
{
if (_xf_fixed[i]) _xf.values()[i] = xref.getReferenceCached(getN() - 1)[i];
}
}
}
result.vertices_updated = isModified(); // isModified() returns if the underlying vertex set is updated
if (new_run || result.updated()) // new run -> new t
{
// update NLP functions w.r.t. the current discretization
result.edges_updated =
nlp_fun.update(getN(), t.toSec(), xref, uref, sref, hasSingleDt(), x0, {getDt()}, this); // returns true if edge dimensions changed
// TODO(roesmann): sref is not yet implemented for testing, add this later...
// TODO(roesmann): we do not yet check if dt was updated, this might affect nlp update
}
if (result.updated()) // vertices or eges updated
{
// create grid specific edges
// TODO(roesmann): for now, we assume that we are the only maintainer of the edge set (which might be not true generally!)
// so we clear the edge set whenever we want
createEdges(nlp_fun, edges, dynamics);
result.edges_updated = true; // now we definitely updated the edgeset
}
_first_run = false;
return result;
}
void FullDiscretizationGridBaseSE2::initializeSequences(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, ReferenceTrajectoryInterface& uref,
NlpFunctions& nlp_fun)
{
assert(x0.size() >= 3);
// clear(); // make sure everything is cleared
_x_seq.clear();
_u_seq.clear();
_xf.clear();
_active_vertices.clear();
// check or initalize bounds if empty
nlp_fun.checkAndInitializeBoundDimensions(x0.size(), uref.getDimension());
// check x_fixed
checkAndInitializeXfFixedFlags(x0.size());
int n_init = _n_adapt > 0 ? _n_adapt : _n_ref;
int num_intervals = n_init - 1;
// we initialize the state trajectory linearly
Eigen::VectorXd dir = xf - x0;
double dist = dir.norm();
if (dist != 0) dir /= dist;
double step = dist / num_intervals;
// For the SE2 case here, we set the third state (orientation) to the goal heading
double orient_init = std::atan2(dir[1], dir[0]);
// but check if goal is behind start pose (w.r.t. start orientation)
if (dir.head(2).dot(Eigen::Vector2d(std::cos(x0[2]), std::sin(x0[2]))) < 0) orient_init = normalize_theta(orient_init + M_PI);
for (int k = 0; k < num_intervals; ++k)
{
// add new state by linear interpolation
Eigen::VectorXd new_x = x0 + (double)k * step * dir;
new_x[2] = orient_init;
_x_seq.emplace_back(new_x, nlp_fun.x_lb, nlp_fun.x_ub);
// add new constant control (assume u0 to hold
_u_seq.emplace_back(uref.getReferenceCached(k), nlp_fun.u_lb, nlp_fun.u_ub);
}
// add final state
_xf.set(xf, nlp_fun.x_lb, nlp_fun.x_ub, _xf_fixed);
// set first state as fixed
_x_seq.front().setFixed(true);
// set dt
_dt.set(_dt_ref, _dt_lb, _dt_ub, isDtFixedIntended());
assert(_x_seq.size() > 0);
// notify graph that vertices are changed
setModified(true);
}
void FullDiscretizationGridBaseSE2::initializeSequences(const Eigen::VectorXd& x0, const Eigen::VectorXd& xf, ReferenceTrajectoryInterface& xref,
ReferenceTrajectoryInterface& uref, NlpFunctions& nlp_fun)
{
// clear(); // make sure everything is cleared
_x_seq.clear();
_u_seq.clear();
_xf.clear();
_active_vertices.clear();
// TODO(roesmann): check if this is ever needed in any pracital realization:
// if ((x0 - xref.getReferenceCached(0)).norm() > _non_static_ref_dist_threshold)
// {
// initializeSequences(x0, xref.getReferenceCached(getN() - 1), uref, nlp_fun);
// }
// check or initalize bounds if empty
nlp_fun.checkAndInitializeBoundDimensions(x0.size(), uref.getDimension());
// check x_fixed
checkAndInitializeXfFixedFlags(x0.size());
int n_init = _n_adapt > 0 ? _n_adapt : _n_ref;
int num_intervals = n_init - 1;
_x_seq.emplace_back(x0, nlp_fun.x_lb, nlp_fun.x_ub); // always add the (exact) x0
_u_seq.emplace_back(uref.getReferenceCached(0), nlp_fun.u_lb, nlp_fun.u_ub);
for (int k = 1; k < num_intervals; ++k)
{
// add new state by linear interpolation
_x_seq.emplace_back(xref.getReferenceCached(k), nlp_fun.x_lb, nlp_fun.x_ub);
// add new constant control (assume u0 to hold
_u_seq.emplace_back(uref.getReferenceCached(k), nlp_fun.u_lb, nlp_fun.u_ub);
}
// add final state
_xf.set(xf, nlp_fun.x_lb, nlp_fun.x_ub, _xf_fixed);
// set first state as fixed
_x_seq.front().setFixed(true);
// set dt
_dt.set(_dt_ref, _dt_lb, _dt_ub, isDtFixedIntended());
assert(_x_seq.size() > 1);
// notify graph that vertices are changed
setModified(true);
}
void FullDiscretizationGridBaseSE2::warmStartShifting(const Eigen::VectorXd& x0)
{
// find nearest state to x0 (ideally it is the second one in _x_seq).
int num_shift = findNearestState(x0);
if (num_shift <= 0) return;
if (num_shift > getN() - 2)
{
PRINT_ERROR_NAMED("Cannot shift if num_shift > N-2");
return;
}
// the simplest strategy would be to just remove remove x_seq[0], append xf to x_seq and replace xf (num_shift=1)
// however, this requries to change the structure which always triggers an edge recreation and so the solver must reallocate memory.
// TOOD(roesmann): for time-optimal stuff, if we are sure that we resize the grid, it is then more efficent to implement the strategy above...
// shift from end to front:
for (int i = 0; i < getN() - num_shift; ++i)
{
int idx = i + num_shift;
if (idx == getN() - 1)
{
// final state reached
_x_seq[i].values() = _xf.values();
}
else
{
_x_seq[i].values() = _x_seq[idx].values();
_u_seq[i].values() = _u_seq[idx].values();
}
}
int idx = getN() - num_shift;
if (idx < 0)
{
PRINT_ERROR_NAMED("idx < 0...");
return;
}
for (int i = 0; i < num_shift; ++i, ++idx)
{
// now extrapolate
assert(idx > 1);
// linearly extrapolate states
if (i == num_shift - 1) // consider xf
{
_xf.values() = _x_seq[idx - 2].values() + 2.0 * (_x_seq[idx - 1].values() - _x_seq[idx - 2].values());
// TODO(roesmann): we could avoid computing the third row in the previous command, however, the code
// increases if we support additional states
_xf.values()[2] = interpolate_angle(_x_seq[idx - 2].values()[2], _x_seq[idx - 1].values()[2], 2.0);
}
else
{
// TODO(roesmann) multiply by fraction of last dt and _dt
_x_seq[idx].values() = _x_seq[idx - 2].values() + 2.0 * (_x_seq[idx - 1].values() - _x_seq[idx - 2].values());
// TODO(roesmann): we could avoid computing the third row in the previous command, however, the code
// increases if we support additional states
_x_seq[idx].values()[2] = interpolate_angle(_x_seq[idx - 2].values()[2], _x_seq[idx - 1].values()[2], 2.0);
}
_u_seq[idx - 1].values() = _u_seq[idx - 2].values();
}
}
int FullDiscretizationGridBaseSE2::findNearestState(const Eigen::VectorXd& x0)
{
assert(!isEmpty());
assert(isValid());
// same start as before
double first_dist = (x0 - _x_seq.front().values()).norm();
if (std::abs(first_dist) < 1e-12) return 0;
// find nearest state (using l2-norm) in order to prune the trajectory
// (remove already passed states, should be 1 if the sampling times of the planning and controller call match, but could be higher if rates
// differ)
int num_interv = getN() - 1;
double dist_cache = first_dist;
double dist;
int num_keep_interv = 1; // std::max(1, _num_states_min - 1); // we need to keep at least this number of intervals in the trajectory
int lookahead = std::min(num_interv - num_keep_interv, 20); // max 20 samples
int nearest_idx = 0;
for (int i = 1; i <= lookahead; ++i)
{
dist = (x0 - _x_seq[i].values()).norm();
if (dist < dist_cache)
{
dist_cache = dist;
nearest_idx = i;
}
else
break;
}
return nearest_idx;
}
bool FullDiscretizationGridBaseSE2::getFirstControlInput(Eigen::VectorXd& u0)
{
if (isEmpty() || !isValid()) return false;
assert(!_u_seq.empty());
u0 = _u_seq.front().values();
return true;
}
void FullDiscretizationGridBaseSE2::setNRef(int n)
{
// clear grid if we change n
if (n != getN()) clear();
if (n < 2)
{
PRINT_ERROR_NAMED("Number of states must be n>1.");
_n_ref = 2;
return;
}
_n_ref = n;
_n_adapt = 0;
}
int FullDiscretizationGridBaseSE2::findClosestPose(double x_ref, double y_ref, int start_idx, double* distance) const
{
double min_dist = std::numeric_limits<double>::max();
int min_idx = -1;
for (int i = start_idx; i < (int)_x_seq.size(); ++i)
{
double dist = distance_points2d(x_ref, y_ref, _x_seq[i].values()[0], _x_seq[i].values()[1]);
if (dist < min_dist)
{
min_dist = dist;
min_idx = i;
}
}
double dist = distance_points2d(x_ref, y_ref, _xf.values()[0], _xf.values()[1]);
if (dist < min_dist)
{
min_dist = dist;
min_idx = (int)_x_seq.size();
}
if (distance) *distance = min_dist;
return min_idx;
}
bool FullDiscretizationGridBaseSE2::checkAndInitializeXfFixedFlags(int dim_x)
{
if (_xf_fixed.size() == 0)
{
_xf_fixed.setConstant(dim_x, false);
return true;
}
else if (_xf_fixed.size() == dim_x)
return true;
PRINT_ERROR_NAMED("Dimensions mismatch between xf_fixed and xf. Setting xf_fixed to false.");
_xf_fixed.setConstant(dim_x, false);
return false;
}
void FullDiscretizationGridBaseSE2::updateBounds(const NlpFunctions& nlp_fun)
{
if (isEmpty()) return;
for (VectorVertexSE2& vtx : _x_seq)
{
if (vtx.getDimension() == nlp_fun.x_lb.size())
vtx.setLowerBounds(nlp_fun.x_lb);
else
PRINT_ERROR_NAMED("Cannot update lower state bounds due to dimensions mismatch");
if (vtx.getDimension() == nlp_fun.x_ub.size())
vtx.setUpperBounds(nlp_fun.u_ub);
else
PRINT_ERROR_NAMED("Cannot update upper state bounds due to dimensions mismatch");
}
for (VectorVertex& vtx : _u_seq)
{
if (vtx.getDimension() == nlp_fun.u_lb.size())
vtx.setLowerBounds(nlp_fun.u_lb);
else
PRINT_ERROR_NAMED("Cannot update lower control input bounds due to dimensions mismatch");
if (vtx.getDimension() == nlp_fun.u_ub.size())
vtx.setUpperBounds(nlp_fun.u_ub);
else
PRINT_ERROR_NAMED("Cannot update upper control input bounds due to dimensions mismatch");
}
_dt.setLowerBound(_dt_lb);
_dt.setUpperBound(_dt_ub);
_nlp_fun = &nlp_fun; // for bound access in e.g. resampleTrajectory()
}
void FullDiscretizationGridBaseSE2::resampleTrajectory(int n_new)
{
assert(isValid());
int n = getN();
if (n == n_new) return;
if (!_nlp_fun)
{
PRINT_ERROR_NAMED("We currently need _nlp_fun to be valid in order to retrieve bounds");
}
// copy vertices (vertices itself are shared pointers)
//! @todo(roesmann) More efficient strategy without copying containers at all?
//!
TimeSeries::Ptr ts_states_old = std::make_shared<TimeSeries>();
TimeSeries::Ptr ts_controls_old = std::make_shared<TimeSeries>();
getStateAndControlTimeSeries(ts_states_old, ts_controls_old);
TimeSeries::ValuesMatMap x_seq_old = ts_states_old->getValuesMatrixView();
TimeSeries::ValuesMatMap u_seq_old = ts_controls_old->getValuesMatrixView();
int num_interv = n - 1;
double dt_old = getDt();
// compute new time diff
double dt_new = dt_old * double(n - 1) / double(n_new - 1);
double t_new;
int idx_old = 1;
double t_old_p1 = dt_old; // time for old sample with index idx_old (acutally its the subsequent time step w.r.t t_new)
for (int idx_new = 1; idx_new < n_new - 1; ++idx_new) // n_new-1 since last state is identical and copied later. idx_new=1, since start
// sample is already valid (we do not touch it)
// we allow a small mismatch for the control u_1 and let the optimizer correct it later
{
t_new = dt_new * double(idx_new);
while (t_new > double(idx_old) * dt_old && idx_old < n)
{
++idx_old;
}; // find idx_old that represents the state subsequent to the new one (w.r.t. time)
t_old_p1 = double(idx_old) * dt_old;
const Eigen::VectorXd& x_prev = x_seq_old.col(idx_old - 1);
const Eigen::VectorXd& x_cur = (idx_old < n - 1) ? x_seq_old.col(idx_old) : _xf.values();
if (idx_new < num_interv)
{
// states
_x_seq[idx_new].values() = x_prev + (t_new - (t_old_p1 - dt_old)) / dt_old * (x_cur - x_prev);
// overwrite angle (since we are in SE2, at least the first three states are SE2)
// TODO(roesmann): we could avoid computing the third row in the previous command, however, the code
// increases if we support additional states
_x_seq[idx_new].values()[2] = interpolate_angle(x_prev[2], x_cur[2], (t_new - (t_old_p1 - dt_old)) / dt_old);
// controls
// TODO(roesmann): we do not have a valid control (u_f), so hold last
_u_seq[idx_new].values() = u_seq_old.col(idx_old - 1);
}
else
{
// add new pair
_x_seq.emplace_back(x_prev + (t_new - (t_old_p1 - dt_old)) / dt_old * (x_cur - x_prev), _nlp_fun->x_lb, _nlp_fun->x_ub);
// TODO(roesmann): we could avoid computing the third row in the previous command, however, the code
// increases if we support additional states
_x_seq.back().values()[2] = interpolate_angle(x_prev[2], x_cur[2], (t_new - (t_old_p1 - dt_old)) / dt_old);
_u_seq.emplace_back(u_seq_old.col(idx_old - 1), _nlp_fun->u_lb, _nlp_fun->u_ub);
}
}
// clear invalid states
if (n_new < n)
{
_x_seq.resize(n_new - 1);
_u_seq.resize(n_new - 1);
}
// save new dt
_dt.value() = dt_new;
// set first vertex always to fixed
// _x_seq.front().setFixed(true); // should not be necessary here (we just change values in the first node)
// notify graph that vertices are changed
setModified(true);
}
void FullDiscretizationGridBaseSE2::clear()
{
_x_seq.clear();
_u_seq.clear();
_xf.clear();
_active_vertices.clear();
_nlp_fun = nullptr;
_n_adapt = 0;
_first_run = true;
setModified(true);
}
void FullDiscretizationGridBaseSE2::setN(int n, bool try_resample)
{
if (try_resample && _nlp_fun && !isEmpty())
{
resampleTrajectory(n);
}
else
{
clear(); // resampling not yet implemented
}
setNRef(n);
}
void FullDiscretizationGridBaseSE2::getVertices(std::vector<VertexInterface*>& vertices)
{
vertices.clear();
// order doesn't matter here
for (VectorVertexSE2& vtx : _x_seq) vertices.push_back(&vtx);
for (VectorVertex& vtx : _u_seq) vertices.push_back(&vtx);
vertices.push_back(&_xf);
vertices.push_back(&_dt); // make sure to make it fixed if desired in any subclass
vertices.push_back(&_u_prev); // always fixed...
}
void FullDiscretizationGridBaseSE2::computeActiveVertices()
{
_active_vertices.clear();
assert(isValid());
int n = getN();
for (int i = 0; i < n - 1; ++i)
{
if (!_x_seq[i].isFixed()) _active_vertices.push_back(&_x_seq[i]);
if (!_u_seq[i].isFixed()) _active_vertices.push_back(&_u_seq[i]);
}
if (!_xf.isFixed()) _active_vertices.push_back(&_xf);
if (!_dt.isFixed()) _active_vertices.push_back(&_dt);
}
void FullDiscretizationGridBaseSE2::getStateAndControlTimeSeries(TimeSeries::Ptr x_sequence, TimeSeries::Ptr u_sequence, double t_max) const
{
if (x_sequence) x_sequence->clear();
if (u_sequence) u_sequence->clear();
if (isEmpty()) return;
assert(isValid());
PRINT_ERROR_COND_NAMED(t_max < 0, "t_max >= 0 required");
double dt = getDt();
if (x_sequence)
{
double t = 0;
for (int i = 0; i < _x_seq.size(); ++i)
{
x_sequence->add(t, _x_seq[i].values());
t += dt;
if (t > t_max) break;
}
if (t <= t_max) x_sequence->add(t, _xf.values());
}
if (u_sequence)
{
double t = 0;
for (int i = 0; i < _u_seq.size(); ++i)
{
u_sequence->add(t, _u_seq[i].values());
t += dt;
if (t > t_max) break;
}
// duplicate last u to have the sampe time stamps as x_sequence
if (t <= t_max) u_sequence->add(t, _u_seq.back().values());
}
}
} // namespace mpc_local_planner

147
mpc_local_planner/src/optimal_control/min_time_via_points_cost.cpp Normal file
View File

@ -0,0 +1,147 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/optimal_control/min_time_via_points_cost.h>
#include <mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h>
#include <mpc_local_planner/utils/math_utils.h>
#include <cmath>
#include <memory>
namespace mpc_local_planner {
int MinTimeViaPointsCost::getNonIntegralStateTermDimension(int k) const
{
assert(k < _vp_association.size());
assert(_vp_association[k].first.size() == _vp_association[k].second);
return (int)_vp_association[k].second;
}
bool MinTimeViaPointsCost::update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/,
corbo::ReferenceTrajectoryInterface* /*sref*/, bool single_dt, const Eigen::VectorXd& x0,
corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector<double>& /*dts*/,
const corbo::DiscretizationGridInterface* grid)
{
if (!_via_points)
{
PRINT_WARNING("MinTimeViaPointsCost::update(): no via_point container specified");
return !_vp_association.empty();
}
// setup minimum time cost
_single_dt = single_dt;
if (single_dt)
_time_weight = n - 1;
else
_time_weight = 1.0;
// Setup via points
// we currently require a full discretization grid as we want to have fast access to
// individual states without requiring any new simulation.
// Alternatively, other grids could be used in combination with method getStateAndControlTimeSeries()
const FullDiscretizationGridBaseSE2* fd_grid = static_cast<const FullDiscretizationGridBaseSE2*>(grid);
assert(n == fd_grid->getN());
bool new_structure = (n != _vp_association.size());
if (new_structure)
{
_vp_association.resize(n, std::make_pair<std::vector<const teb_local_planner::PoseSE2*>, std::size_t>({}, 0));
}
// clear previous association
for (auto& item : _vp_association)
{
item.first.clear();
}
int start_pose_idx = 0;
for (const teb_local_planner::PoseSE2& pose : *_via_points)
{
int idx = fd_grid->findClosestPose(pose.x(), pose.y(), start_pose_idx);
if (_via_points_ordered) start_pose_idx = idx + 2; // skip a point to have a DOF inbetween for further via-points
// check if point coincides with final state or is located behind it
if (idx > n - 2) idx = n - 2; // set to a pose before the goal, since it is never fixed
// check if point coincides with start or is located before it
if (idx < 1)
{
if (_via_points_ordered)
idx = 1; // try to connect the via point with the second pose. Grid adaptation might add new poses inbetween later if enabled.
else
{
PRINT_DEBUG("TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose.");
continue; // skip via points really close or behind the current robot pose
}
}
_vp_association[idx].first.push_back(&pose);
}
// check for structure changes
for (auto& item : _vp_association)
{
if (item.first.size() != item.second)
{
item.second = item.first.size();
new_structure = true;
// but continue until end for next time
}
}
// return true if structure requires a changed
return new_structure;
}
void MinTimeViaPointsCost::computeNonIntegralDtTerm(int k, double dt, Eigen::Ref<Eigen::VectorXd> cost) const
{
if (!_single_dt || k == 0)
cost[0] = _time_weight * dt;
else
{
PRINT_DEBUG_NAMED("this method should not be called in single_dt mode and k>0");
}
}
void MinTimeViaPointsCost::computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(_via_points);
assert(k < _vp_association.size());
assert(cost.size() == _vp_association[k].first.size());
assert(_vp_association[k].second == _vp_association[k].first.size());
for (int i = 0; i < (int)_vp_association[k].second; ++i)
{
cost[i] = _vp_position_weight * (_vp_association[k].first[i]->position() - x_k.head(2)).squaredNorm();
if (_vp_orientation_weight > 0)
{
cost[i] += _vp_orientation_weight * normalize_theta(_vp_association[k].first[i]->theta() - x_k[2]);
}
}
}
} // namespace mpc_local_planner

125
mpc_local_planner/src/optimal_control/quadratic_cost_se2.cpp Normal file
View File

@ -0,0 +1,125 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/optimal_control/quadratic_cost_se2.h>
#include <mpc_local_planner/utils/math_utils.h>
#include <cmath>
namespace mpc_local_planner {
void QuadraticFormCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(!_integral_form);
assert(cost.size() == getNonIntegralStateTermDimension(k));
Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
xd[2] = normalize_theta(xd[2]);
if (_lsq_form)
{
if (_Q_diagonal_mode)
cost.noalias() = _Q_diag_sqrt * xd;
else
cost.noalias() = _Q_sqrt * xd;
}
else
{
if (_Q_diagonal_mode)
cost.noalias() = xd.transpose() * _Q_diag * xd;
else
cost.noalias() = xd.transpose() * _Q * xd;
}
}
void QuadraticFormCostSE2::computeIntegralStateControlTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k,
const Eigen::Ref<const Eigen::VectorXd>& u_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(_integral_form);
assert(cost.size() == 1);
cost[0] = 0;
Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
xd[2] = normalize_theta(xd[2]);
if (_Q_diagonal_mode)
cost[0] += xd.transpose() * _Q_diag * xd;
else
cost[0] += xd.transpose() * _Q * xd;
if (_zero_u_ref)
{
if (_R_diagonal_mode)
cost[0] += u_k.transpose() * _R_diag * u_k;
else
cost[0] += u_k.transpose() * _R * u_k;
}
else
{
Eigen::VectorXd ud = u_k - _u_ref->getReferenceCached(k);
if (_R_diagonal_mode)
cost[0] += ud.transpose() * _R_diag * ud;
else
cost[0] += ud.transpose() * _R * ud;
}
}
void QuadraticStateCostSE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(!_integral_form);
assert(cost.size() == getNonIntegralStateTermDimension(k));
Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
xd[2] = normalize_theta(xd[2]);
if (_lsq_form)
{
if (_diagonal_mode)
cost.noalias() = _Q_diag_sqrt * xd;
else
cost.noalias() = _Q_sqrt * xd;
}
else
{
if (_diagonal_mode)
cost.noalias() = xd.transpose() * _Q_diag * xd;
else
cost.noalias() = xd.transpose() * _Q * xd;
}
}
void QuadraticStateCostSE2::computeIntegralStateControlTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k,
const Eigen::Ref<const Eigen::VectorXd>& u_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(_integral_form);
assert(cost.size() == 1);
cost[0] = 0;
Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
xd[2] = normalize_theta(xd[2]);
if (_diagonal_mode)
cost[0] += xd.transpose() * _Q_diag * xd;
else
cost[0] += xd.transpose() * _Q * xd;
}
} // namespace mpc_local_planner

210
mpc_local_planner/src/optimal_control/stage_inequality_se2.cpp Normal file
View File

@ -0,0 +1,210 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/optimal_control/full_discretization_grid_base_se2.h>
#include <mpc_local_planner/optimal_control/stage_inequality_se2.h>
#include <teb_local_planner/obstacles.h>
#include <teb_local_planner/pose_se2.h>
#include <cmath>
#include <memory>
namespace mpc_local_planner {
int StageInequalitySE2::getNonIntegralControlDeviationTermDimension(int k) const { return _num_du_lb_finite + _num_du_ub_finite; }
int StageInequalitySE2::getNonIntegralStateTermDimension(int k) const
{
assert(k < _relevant_obstacles.size());
return (int)_relevant_obstacles[k].size();
}
bool StageInequalitySE2::update(int n, double /*t*/, corbo::ReferenceTrajectoryInterface& /*xref*/, corbo::ReferenceTrajectoryInterface& /*uref*/,
corbo::ReferenceTrajectoryInterface* /*sref*/, bool /* single_dt*/, const Eigen::VectorXd& x0,
corbo::StagePreprocessor::Ptr /*stage_preprocessor*/, const std::vector<double>& /*dts*/,
const corbo::DiscretizationGridInterface* grid)
{
assert(_obstacles);
assert(_robot_model);
// Setup obstacle avoidance
// we currently require a full discretization grid as we want to have fast access to
// individual states without requiring any new simulation.
// Alternatively, other grids could be used in combination with method getStateAndControlTimeSeries()
const FullDiscretizationGridBaseSE2* fd_grid = static_cast<const FullDiscretizationGridBaseSE2*>(grid);
bool new_dimensions = (n != _relevant_obstacles.size());
_relevant_obstacles.resize(n);
teb_local_planner::PoseSE2 pose;
// iterate states but skip start state as it is not subject to optimization
for (int k = 1; k < n; ++k)
{
double left_min_dist = std::numeric_limits<double>::max();
double right_min_dist = std::numeric_limits<double>::max();
ObstaclePtr left_obstacle;
ObstaclePtr right_obstacle;
const Eigen::VectorXd& xk = fd_grid->getState(k);
assert(xk.size() >= 3);
// TODO(roesmann): Overload robot fooprint model functions in teb_local_planner to avoid this copy:
pose.position() = xk.head(2);
pose.theta() = xk.coeffRef(2);
Eigen::Vector2d pose_orient = pose.orientationUnitVec();
// Eigen::Vector2d pose_orient(std::cos(xk[2]), std::sin(xk[2]));
int num_prev_obst = (int)_relevant_obstacles[k].size();
_relevant_obstacles[k].clear();
// iterate obstacles
for (const ObstaclePtr& obst : *_obstacles)
{
// check for dynamic obstacle
if (_enable_dynamic_obstacles && obst->isDynamic())
{
// we consider all dynamic obstacles by now
// TODO(roesmann): we might remove obstacles that "go away" from the trajectory
// or more generally that any intersection in the future is unlikely
_relevant_obstacles[k].push_back(obst);
continue;
}
// calculate distance to robot model
double dist = _robot_model->calculateDistance(pose, obst.get());
// force considering obstacle if really close to the current pose
if (dist < _min_obstacle_dist * _obstacle_filter_force_inclusion_factor)
{
_relevant_obstacles[k].push_back(obst);
continue;
}
// cut-off distance
if (dist > _min_obstacle_dist * _obstacle_filter_cutoff_factor) continue;
// determine side (left or right) and assign obstacle if closer than the previous one
if (cross2d(pose_orient, obst->getCentroid()) > 0) // left
{
if (dist < left_min_dist)
{
left_min_dist = dist;
left_obstacle = obst;
}
}
else
{
if (dist < right_min_dist)
{
right_min_dist = dist;
right_obstacle = obst;
}
}
}
// add left and right obstacle
if (left_obstacle) _relevant_obstacles[k].push_back(left_obstacle);
if (right_obstacle) _relevant_obstacles[k].push_back(right_obstacle);
// check if dimensions changed
new_dimensions = new_dimensions || (_relevant_obstacles[k].size() != num_prev_obst);
}
// update current dt
_current_dt = grid->getFirstDt(); // TODO(roesmann): this does currently not work with non-uniform grids
// setup control deviation constraint
int num_du_lb_finite = _du_lb.size() > 0 ? (_du_lb.array() > -corbo::CORBO_INF_DBL).count() : 0;
int num_du_ub_finite = _du_ub.size() > 0 ? (_du_ub.array() < corbo::CORBO_INF_DBL).count() : 0;
if (num_du_lb_finite != _num_du_lb_finite) new_dimensions = true;
if (num_du_ub_finite != _num_du_ub_finite) new_dimensions = true;
_num_du_lb_finite = num_du_lb_finite;
_num_du_ub_finite = num_du_ub_finite;
// return true if structure requires a changed
return new_dimensions;
}
void StageInequalitySE2::computeNonIntegralStateTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& x_k, Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(_obstacles);
assert(k < _relevant_obstacles.size());
assert(cost.size() == _relevant_obstacles[k].size());
// TODO(roesmann): Overload robot fooprint model functions in teb_local_planner to avoid this copy:
teb_local_planner::PoseSE2 pose(x_k[0], x_k[1], x_k[2]);
for (int i = 0; i < (int)_relevant_obstacles[k].size(); ++i)
{
if (!_enable_dynamic_obstacles || !_relevant_obstacles[k][i]->isDynamic())
{
cost[i] = _min_obstacle_dist - _robot_model->calculateDistance(pose, _relevant_obstacles[k][i].get());
}
else
{
cost[i] =
_min_obstacle_dist - _robot_model->estimateSpatioTemporalDistance(pose, _relevant_obstacles[k][i].get(), (double)k * _current_dt);
}
}
}
void StageInequalitySE2::computeNonIntegralControlDeviationTerm(int k, const Eigen::Ref<const Eigen::VectorXd>& u_k,
const Eigen::Ref<const Eigen::VectorXd>& u_prev, double dt_prev,
Eigen::Ref<Eigen::VectorXd> cost) const
{
assert(cost.size() == _num_du_lb_finite + _num_du_ub_finite);
if (cost.size() == 0) return; // TODO(roesmann): necessary?
if (k == 0 && dt_prev == 0)
{
cost.setZero(); // this is fine as dt_prev for k==0 is not subject to optimization
return;
}
assert(_num_du_lb_finite == 0 || _du_lb.size() == u_k.size());
assert(_num_du_ub_finite == 0 || _du_ub.size() == u_k.size());
assert(dt_prev != 0);
int idx_lb = 0;
int idx_ub = 0;
for (int i = 0; i < u_k.size(); ++i)
{
if (_du_lb[i] > -corbo::CORBO_INF_DBL)
{
cost[idx_lb] = _du_lb[i] - (u_k[i] - u_prev[i]) / dt_prev;
++idx_lb;
}
if (_du_ub[i] < corbo::CORBO_INF_DBL)
{
cost[_num_du_lb_finite + idx_ub] = (u_k[i] - u_prev[i]) / dt_prev - _du_ub[i];
++idx_ub;
}
}
}
void StageInequalitySE2::setControlDeviationBounds(const Eigen::VectorXd& du_lb, const Eigen::VectorXd& du_ub)
{
_du_lb = du_lb;
_du_ub = du_ub;
}
} // namespace mpc_local_planner

262
mpc_local_planner/src/test_mpc_optim_node.cpp Normal file
View File

@ -0,0 +1,262 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <ros/ros.h>
#include <Eigen/Core>
#include <mpc_local_planner/controller.h>
#include <mpc_local_planner/mpc_local_planner_ros.h>
#include <mpc_local_planner/utils/publisher.h>
#include <teb_local_planner/obstacles.h>
#include <interactive_markers/interactive_marker_server.h>
#include <visualization_msgs/Marker.h>
#include <memory>
namespace mpc = mpc_local_planner;
class TestMpcOptimNode
{
public:
TestMpcOptimNode() = default;
void start(ros::NodeHandle& nh);
protected:
void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame,
interactive_markers::InteractiveMarkerServer* marker_server);
void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg);
void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg);
void CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg);
teb_local_planner::ObstContainer _obstacles;
int _no_fixed_obstacles;
std::vector<teb_local_planner::PoseSE2> _via_points;
};
void TestMpcOptimNode::start(ros::NodeHandle& nh)
{
std::string map_frame = "map";
// interactive marker server for simulated dynamic obstacles
interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles");
// configure obstacles
_obstacles.push_back(boost::make_shared<teb_local_planner::PointObstacle>(-3, 1));
_obstacles.push_back(boost::make_shared<teb_local_planner::PointObstacle>(6, 2));
_obstacles.push_back(boost::make_shared<teb_local_planner::PointObstacle>(4, 0.1));
// Add interactive markers
for (int i = 0; i < (int)_obstacles.size(); ++i)
{
// Add interactive markers for all point obstacles
boost::shared_ptr<teb_local_planner::PointObstacle> pobst = boost::dynamic_pointer_cast<teb_local_planner::PointObstacle>(_obstacles[i]);
if (pobst)
{
CreateInteractiveMarker(pobst->x(), pobst->y(), i, map_frame, &marker_server);
}
}
marker_server.applyChanges();
_no_fixed_obstacles = (int)_obstacles.size();
// setup callback for custom obstacles
ros::Subscriber custom_obst_sub = nh.subscribe("obstacles", 1, &TestMpcOptimNode::CB_customObstacle, this);
// setup callback for clicked points (in rviz) that are considered as via-points
ros::Subscriber clicked_points_sub = nh.subscribe("/clicked_point", 5, &TestMpcOptimNode::CB_clicked_points, this);
// setup callback for via-points (callback overwrites previously set via-points)
ros::Subscriber via_points_sub = nh.subscribe("via_points", 1, &TestMpcOptimNode::CB_via_points, this);
// Setup robot shape model
teb_local_planner::RobotFootprintModelPtr robot_model = mpc_local_planner::MpcLocalPlannerROS::getRobotFootprintFromParamServer(nh);
mpc_local_planner::Controller controller;
if (!controller.configure(nh, _obstacles, robot_model, _via_points))
{
ROS_ERROR("Controller configuration failed.");
return;
}
mpc::Publisher publisher(nh, controller.getRobotDynamics(), map_frame);
teb_local_planner::PoseSE2 x0(0, 0, 0);
teb_local_planner::PoseSE2 xf(5, 2, 0);
corbo::TimeSeries::Ptr x_seq = std::make_shared<corbo::TimeSeries>();
corbo::TimeSeries::Ptr u_seq = std::make_shared<corbo::TimeSeries>();
geometry_msgs::Twist vel;
bool success = false;
ros::Rate rate(20);
while (ros::ok())
{
success = controller.step(x0, xf, vel, rate.expectedCycleTime().toSec(), ros::Time::now(), u_seq, x_seq);
if (success)
publisher.publishLocalPlan(*x_seq);
else
ROS_ERROR("OCP solving failed.");
publisher.publishObstacles(_obstacles);
publisher.publishRobotFootprintModel(x0, *robot_model);
publisher.publishViaPoints(_via_points);
ros::spinOnce();
rate.sleep();
}
}
void TestMpcOptimNode::CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame,
interactive_markers::InteractiveMarkerServer* marker_server)
{
// create an interactive marker for our server
visualization_msgs::InteractiveMarker i_marker;
i_marker.header.frame_id = frame;
i_marker.header.stamp = ros::Time::now();
std::ostringstream oss;
// oss << "obstacle" << id;
oss << id;
i_marker.name = oss.str();
i_marker.description = "Obstacle";
i_marker.pose.position.x = init_x;
i_marker.pose.position.y = init_y;
i_marker.pose.orientation.w = 1.0f; // make quaternion normalized
// create a grey box marker
visualization_msgs::Marker box_marker;
box_marker.type = visualization_msgs::Marker::CUBE;
box_marker.id = id;
box_marker.scale.x = 0.2;
box_marker.scale.y = 0.2;
box_marker.scale.z = 0.2;
box_marker.color.r = 0.5;
box_marker.color.g = 0.5;
box_marker.color.b = 0.5;
box_marker.color.a = 1.0;
box_marker.pose.orientation.w = 1.0f; // make quaternion normalized
// create a non-interactive control which contains the box
visualization_msgs::InteractiveMarkerControl box_control;
box_control.always_visible = true;
box_control.markers.push_back(box_marker);
// add the control to the interactive marker
i_marker.controls.push_back(box_control);
// create a control which will move the box, rviz will insert 2 arrows
visualization_msgs::InteractiveMarkerControl move_control;
move_control.name = "move_x";
move_control.orientation.w = 0.707107f;
move_control.orientation.x = 0;
move_control.orientation.y = 0.707107f;
move_control.orientation.z = 0;
move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE;
// add the control to the interactive marker
i_marker.controls.push_back(move_control);
// add the interactive marker to our collection
marker_server->insert(i_marker);
marker_server->setCallback(i_marker.name, boost::bind(&TestMpcOptimNode::CB_obstacle_marker, this, boost::placeholders::_1));
}
void TestMpcOptimNode::CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
{
std::stringstream ss(feedback->marker_name);
unsigned int index;
ss >> index;
if (index >= _no_fixed_obstacles) return;
teb_local_planner::PointObstacle* pobst = static_cast<teb_local_planner::PointObstacle*>(_obstacles.at(index).get());
pobst->position() = Eigen::Vector2d(feedback->pose.position.x, feedback->pose.position.y);
}
void TestMpcOptimNode::CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg)
{
// resize such that the vector contains only the fixed obstacles specified inside the main function
_obstacles.resize(_no_fixed_obstacles);
// Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame)
for (size_t i = 0; i < obst_msg->obstacles.size(); ++i)
{
if (obst_msg->obstacles.at(i).polygon.points.size() == 1)
{
if (obst_msg->obstacles.at(i).radius == 0)
{
_obstacles.push_back(teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(
obst_msg->obstacles.at(i).polygon.points.front().x, obst_msg->obstacles.at(i).polygon.points.front().y)));
}
else
{
_obstacles.push_back(teb_local_planner::ObstaclePtr(
new teb_local_planner::CircularObstacle(obst_msg->obstacles.at(i).polygon.points.front().x,
obst_msg->obstacles.at(i).polygon.points.front().y, obst_msg->obstacles.at(i).radius)));
}
}
else
{
teb_local_planner::PolygonObstacle* polyobst = new teb_local_planner::PolygonObstacle;
for (size_t j = 0; j < obst_msg->obstacles.at(i).polygon.points.size(); ++j)
{
polyobst->pushBackVertex(obst_msg->obstacles.at(i).polygon.points[j].x, obst_msg->obstacles.at(i).polygon.points[j].y);
}
polyobst->finalizePolygon();
_obstacles.push_back(teb_local_planner::ObstaclePtr(polyobst));
}
if (!_obstacles.empty()) _obstacles.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation);
}
}
void TestMpcOptimNode::CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg)
{
// we assume for simplicity that the fixed frame is already the map/planning frame
// consider clicked points as via-points
_via_points.emplace_back(point_msg->point.x, point_msg->point.y, 0.0);
ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added.");
}
void TestMpcOptimNode::CB_via_points(const nav_msgs::Path::ConstPtr& via_points_msg)
{
ROS_INFO_ONCE("Via-points received. This message is printed once.");
_via_points.clear();
for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses)
{
_via_points.emplace_back(pose.pose.position.x, pose.pose.position.y, 0);
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "test_optim_node");
ros::NodeHandle n("~");
TestMpcOptimNode mpc_test;
mpc_test.start(n);
return 0;
}

48
mpc_local_planner/src/utils/conversion.cpp Normal file
View File

@ -0,0 +1,48 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/utils/conversion.h>
#include <tf/tf.h>
namespace mpc_local_planner {
void convert(const corbo::TimeSeries& time_series, const RobotDynamicsInterface& dynamics, std::vector<geometry_msgs::PoseStamped>& poses_stamped,
const std::string& frame_id)
{
poses_stamped.clear();
if (time_series.isEmpty()) return;
for (int i = 0; i < time_series.getTimeDimension(); ++i)
{
poses_stamped.emplace_back();
double theta = 0;
dynamics.getPoseSE2FromState(time_series.getValuesMap(i), poses_stamped.back().pose.position.x, poses_stamped.back().pose.position.y, theta);
poses_stamped.back().pose.orientation = tf::createQuaternionMsgFromYaw(theta);
poses_stamped.back().header.frame_id = frame_id;
poses_stamped.back().header.stamp = ros::Time::now(); // TODO(roesmann) should we use now()?
}
}
} // namespace mpc_local_planner

276
mpc_local_planner/src/utils/publisher.cpp Normal file
View File

@ -0,0 +1,276 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/utils/publisher.h>
#include <mpc_local_planner/utils/conversion.h>
#include <base_local_planner/goal_functions.h>
#include <nav_msgs/Path.h>
#include <ros/console.h>
#include <visualization_msgs/Marker.h>
#include <boost/pointer_cast.hpp>
#include <boost/shared_ptr.hpp>
#include <cmath>
#include <iomanip>
#include <memory>
namespace mpc_local_planner {
Publisher::Publisher(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame) { initialize(nh, system, map_frame); }
void Publisher::initialize(ros::NodeHandle& nh, RobotDynamicsInterface::Ptr system, const std::string& map_frame)
{
if (_initialized) ROS_WARN("mpc_local_planner: Publisher already initialized. Reinitalizing...");
_system = system;
_map_frame = map_frame;
// register topics
_global_plan_pub = nh.advertise<nav_msgs::Path>("global_plan", 1);
_local_plan_pub = nh.advertise<nav_msgs::Path>("local_plan", 1);
_mpc_marker_pub = nh.advertise<visualization_msgs::Marker>("mpc_markers", 1000);
_initialized = true;
}
void Publisher::publishLocalPlan(const std::vector<geometry_msgs::PoseStamped>& local_plan) const
{
if (!_initialized) return;
base_local_planner::publishPlan(local_plan, _local_plan_pub);
}
void Publisher::publishLocalPlan(const corbo::TimeSeries& ts) const
{
if (!_initialized) return;
if (!_system)
{
ROS_ERROR("Publisher::publishLocalPlan(): cannot publish since the system class is not provided.");
return;
}
std::vector<geometry_msgs::PoseStamped> local_plan;
convert(ts, *_system, local_plan, _map_frame);
base_local_planner::publishPlan(local_plan, _local_plan_pub);
}
void Publisher::publishGlobalPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan) const
{
if (!_initialized) return;
base_local_planner::publishPlan(global_plan, _global_plan_pub);
}
void Publisher::publishRobotFootprintModel(const teb_local_planner::PoseSE2& current_pose,
const teb_local_planner::BaseRobotFootprintModel& robot_model, const std::string& ns,
const std_msgs::ColorRGBA& color)
{
if (!_initialized) return;
std::vector<visualization_msgs::Marker> markers;
robot_model.visualizeRobot(current_pose, markers, color);
if (markers.empty()) return;
int idx = 1000000; // avoid overshadowing by obstacles
for (visualization_msgs::Marker& marker : markers)
{
marker.header.frame_id = _map_frame;
marker.header.stamp = ros::Time::now();
marker.action = visualization_msgs::Marker::ADD;
marker.ns = ns;
marker.id = idx++;
marker.lifetime = ros::Duration(2.0);
_mpc_marker_pub.publish(marker);
}
}
void Publisher::publishObstacles(const teb_local_planner::ObstContainer& obstacles) const
{
if (obstacles.empty() || !_initialized) return;
// Visualize point obstacles
{
visualization_msgs::Marker marker;
marker.header.frame_id = _map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "PointObstacles";
marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
for (const ObstaclePtr& obst : obstacles)
{
// std::shared_ptr<PointObstacle> pobst = std::dynamic_pointer_cast<PointObstacle>(obst); // TODO(roesmann): change teb_local_planner
// types to std lib
boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(obst);
if (!pobst) continue;
geometry_msgs::Point point;
point.x = pobst->x();
point.y = pobst->y();
point.z = 0;
marker.points.push_back(point);
}
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
_mpc_marker_pub.publish(marker);
}
// Visualize line obstacles
{
int idx = 0;
for (const ObstaclePtr& obst : obstacles)
{
// LineObstacle::Ptr pobst = std::dynamic_pointer_cast<LineObstacle>(obst);
boost::shared_ptr<LineObstacle> pobst = boost::dynamic_pointer_cast<LineObstacle>(obst);
if (!pobst) continue;
visualization_msgs::Marker marker;
marker.header.frame_id = _map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "LineObstacles";
marker.id = idx++;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
geometry_msgs::Point start;
start.x = pobst->start().x();
start.y = pobst->start().y();
start.z = 0;
marker.points.push_back(start);
geometry_msgs::Point end;
end.x = pobst->end().x();
end.y = pobst->end().y();
end.z = 0;
marker.points.push_back(end);
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
_mpc_marker_pub.publish(marker);
}
}
// Visualize polygon obstacles
{
int idx = 0;
for (const ObstaclePtr& obst : obstacles)
{
// PolygonObstacle::Ptr pobst = std::dynamic_pointer_cast<PolygonObstacle>(obst);
boost::shared_ptr<PolygonObstacle> pobst = boost::dynamic_pointer_cast<PolygonObstacle>(obst);
if (!pobst) continue;
visualization_msgs::Marker marker;
marker.header.frame_id = _map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "PolyObstacles";
marker.id = idx++;
marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
for (Point2dContainer::const_iterator vertex = pobst->vertices().begin(); vertex != pobst->vertices().end(); ++vertex)
{
geometry_msgs::Point point;
point.x = vertex->x();
point.y = vertex->y();
point.z = 0;
marker.points.push_back(point);
}
// Also add last point to close the polygon
// but only if polygon has more than 2 points (it is not a line)
if (pobst->vertices().size() > 2)
{
geometry_msgs::Point point;
point.x = pobst->vertices().front().x();
point.y = pobst->vertices().front().y();
point.z = 0;
marker.points.push_back(point);
}
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
_mpc_marker_pub.publish(marker);
}
}
}
void Publisher::publishViaPoints(const std::vector<teb_local_planner::PoseSE2>& via_points, const std::string& ns) const
{
if (via_points.empty() || !_initialized) return;
visualization_msgs::Marker marker;
marker.header.frame_id = _map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = ns;
marker.id = 55555;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
for (const teb_local_planner::PoseSE2& via_point : via_points)
{
geometry_msgs::Point point;
point.x = via_point.x();
point.y = via_point.y();
point.z = 0;
marker.points.push_back(point);
}
marker.scale.x = 0.1;
marker.scale.y = 0.1;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 0.0;
marker.color.b = 1.0;
_mpc_marker_pub.publish(marker);
}
std_msgs::ColorRGBA Publisher::toColorMsg(float a, float r, float g, float b)
{
std_msgs::ColorRGBA color;
color.a = a;
color.r = r;
color.g = g;
color.b = b;
return color;
}
} // namespace mpc_local_planner

131
mpc_local_planner/src/utils/time_series_se2.cpp Normal file
View File

@ -0,0 +1,131 @@
/*********************************************************************
*
* Software License Agreement
*
* Copyright (c) 2020, Christoph Rösmann, All rights reserved.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
* Authors: Christoph Rösmann
*********************************************************************/
#include <mpc_local_planner/utils/time_series_se2.h>
#include <mpc_local_planner/utils/math_utils.h>
#include <corbo-core/console.h>
#include <cmath>
#include <iomanip>
namespace mpc_local_planner {
bool TimeSeriesSE2::getValuesInterpolate(double time, Eigen::Ref<Eigen::VectorXd> values, Interpolation interpolation, Extrapolation extrapolation,
double tolerance) const
{
if (_time.empty()) return false;
auto it = std::find_if(_time.begin(), _time.end(), [time](double val) { return val >= time; }); // find next time interval
if (it == _time.end())
{
switch (extrapolation)
{
case Extrapolation::NoExtrapolation:
{
break;
}
case Extrapolation::ZeroOrderHold:
{
values = getValuesMap(getTimeDimension() - 1);
return true;
}
default:
{
PRINT_ERROR("TimeSeries::valuesInterpolate(): desired extrapolation method not implemented.");
return false;
}
}
}
// interpolate
// int n = (int)_time.size();
int idx = (int)std::distance(_time.begin(), it);
if (idx >= getTimeDimension()) idx = getTimeDimension() - 1; // this might occure with floating point comparison
if (std::abs(time - _time[idx]) < tolerance)
{
// we are close to the next value, in particular idx
values = getValuesMap(idx);
return true;
}
if (idx < 1)
{
PRINT_ERROR("accessing a time idx in the past which is not this time series");
}
// okay, so now we have the above case val > time -> so idx corresponds to the next sample.
double dt = time - _time[idx - 1];
PRINT_ERROR_COND_NAMED(dt < 0, "dt < 0 in interpolation. This cannot be correct.");
switch (interpolation)
{
case Interpolation::ZeroOrderHold:
{
if (idx < 1) idx = 1;
values = getValuesMap(idx - 1); // since dt > 0 -> we use the previous value
break;
}
case Interpolation::Linear:
{
if (idx < 1)
{
values = getValuesMap(0);
}
else
{
double dt_data = _time[idx] - _time[idx - 1];
double dt_frac = dt / dt_data;
values.noalias() = getValuesMap(idx - 1) + dt_frac * (getValuesMap(idx) - getValuesMap(idx - 1));
// overwrite third component with is an angle in [-pi, pi)
if (values.size() > 2)
{
values[2] = interpolate_angle(getValuesMap(idx - 1)[2], getValuesMap(idx)[2], dt_frac);
}
}
break;
}
default:
{
PRINT_ERROR("TimeSeries::valuesInterpolate(): desired interpolation method not implemented.");
return false;
}
}
return true;
}
double TimeSeriesSE2::computeMeanOverall()
{
PRINT_ERROR_NAMED("SE2 version not yet implemented.");
return getValuesMatrixView().mean();
}
void TimeSeriesSE2::computeMeanCwise(Eigen::Ref<Eigen::VectorXd> mean_values)
{
PRINT_ERROR_NAMED("SE2 version Not yet implemented.");
if (mean_values.size() != getValueDimension())
{
PRINT_ERROR("TimeSeries::computeMeanCwise(): provided mean_values vector does not match value dimension");
return;
}
mean_values = getValuesMatrixView().rowwise().mean();
}
} // namespace mpc_local_planner

179
mpc_local_planner_examples/CMakeLists.txt Normal file
View File

@ -0,0 +1,179 @@
cmake_minimum_required(VERSION 2.8.3)
project(mpc_local_planner_examples)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES teb_local_planner_tutorials
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
## Declare a C++ library
# add_library(teb_local_planner_tutorials
# src/${PROJECT_NAME}/teb_local_planner_tutorials.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(teb_local_planner_tutorials ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
# add_executable(teb_local_planner_tutorials_node src/teb_local_planner_tutorials_node.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(teb_local_planner_tutorials_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(teb_local_planner_tutorials_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS teb_local_planner_tutorials teb_local_planner_tutorials_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
install(DIRECTORY
launch cfg maps stage
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_teb_local_planner_tutorials.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

10
mpc_local_planner_examples/README.md Normal file
View File

@ -0,0 +1,10 @@
# mpc_local_planner_tutorials
This package contains supplementary material and examples for the [mpc_local_planner](http://wiki.ros.org/mpc_local_planner).
The examples package mainly contains fully working robot navigation configurations in combination with the *mpc_local_planner*.
**Dependencies:**
* *navigation stack* and *mpc_local_planner* package
* *stage*: `sudo apt-get install ros-melodic-stage-ros`

38
mpc_local_planner_examples/cfg/amcl_params.yaml Normal file
View File

@ -0,0 +1,38 @@
use_map_topic: true
odom_frame_id: "odom"
base_frame_id: "base_footprint"
global_frame_id: "map"
## Publish scans from best pose at a max of 10 Hz
odom_model_type: "diff"
odom_alpha5: 0.1
gui_publish_rate: 10.0
laser_max_beams: 60
laser_max_range: 12.0
min_particles: 500
max_particles: 2000
kld_err: 0.05
kld_z: 0.99
odom_alpha1: 0.2
odom_alpha2: 0.2
## translation std dev, m
odom_alpha3: 0.2
odom_alpha4: 0.2
laser_z_hit: 0.5
aser_z_short: 0.05
laser_z_max: 0.05
laser_z_rand: 0.5
laser_sigma_hit: 0.2
laser_lambda_short: 0.1
laser_model_type: "likelihood_field" # "likelihood_field" or "beam"
laser_likelihood_max_dist: 2.0
update_min_d: 0.25
update_min_a: 0.2
resample_interval: 1
## Increase tolerance because the computer can get quite busy
transform_tolerance: 1.0
recovery_alpha_slow: 0.001
recovery_alpha_fast: 0.1

28
mpc_local_planner_examples/cfg/carlike/costmap_common_params.yaml Normal file
View File

@ -0,0 +1,28 @@
#---standard pioneer footprint---
#---(in meters)---
#footprint: [ [0.254, -0.0508], [0.1778, -0.0508], [0.1778, -0.1778], [-0.1905, -0.1778], [-0.254, 0], [-0.1905, 0.1778], [0.1778, 0.1778], [0.1778, 0.0508], [0.254, 0.0508] ]
footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ]
transform_tolerance: 0.2
obstacle_layer:
enabled: true
obstacle_range: 3.0
raytrace_range: 3.5
inflation_radius: 0.2
track_unknown_space: false
combination_method: 1
observation_sources: laser_scan_sensor
laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
inflation_layer:
enabled: true
cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10)
inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths.
static_layer:
enabled: true
map_topic: "/map"

15
mpc_local_planner_examples/cfg/carlike/global_costmap_params.yaml Normal file
View File

@ -0,0 +1,15 @@
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 1.0
publish_frequency: 0.5
transform_tolerance: 0.5
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

14
mpc_local_planner_examples/cfg/carlike/local_costmap_params.yaml Normal file
View File

@ -0,0 +1,14 @@
local_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
rolling_window: true
width: 5.5
height: 5.5
resolution: 0.1
transform_tolerance: 0.5
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}

104
mpc_local_planner_examples/cfg/carlike/mpc_local_planner_params.yaml Normal file
View File

@ -0,0 +1,104 @@
MpcLocalPlannerROS:
odom_topic: odom
## Robot settings
robot:
type: "simple_car"
simple_car:
wheelbase: 0.4
front_wheel_driving: False
max_vel_x: 0.4
max_vel_x_backwards: 0.2
max_steering_angle: 1.4
acc_lim_x: 0.5 # deactive bounds with zero
dec_lim_x: 0.5 # deactive bounds with zero
max_steering_rate: 0.5 # deactive bounds with zero
## Footprint model for collision avoidance
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "line"
radius: 0.2 # for type "circular"
line_start: [0.0, 0.0] # for type "line"
line_end: [0.4, 0.0] # for type "line"
front_offset: 0.2 # for type "two_circles"
front_radius: 0.2 # for type "two_circles"
rear_offset: 0.2 # for type "two_circles"
rear_radius: 0.2 # for type "two_circles"
vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
is_footprint_dynamic: False
## Collision avoidance
collision_avoidance:
min_obstacle_dist: 0.25 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_factor: 1.5
cutoff_factor: 5
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
## Planning grid
grid:
type: "fd_grid"
grid_size_ref: 20
dt_ref: 0.3
xf_fixed: [True, True, True]
warm_start: True
collocation_method: "forward_differences"
cost_integration_method: "left_sum"
variable_grid:
enable: True
min_dt: 0.0;
max_dt: 10.0;
grid_adaptation:
enable: True
dt_hyst_ratio: 0.1
min_grid_size: 2
max_grid_size: 50
## Planning options
planning:
objective:
type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
terminal_cost:
type: "none"
terminal_constraint:
type: "none"
## Controller options
controller:
outer_ocp_iterations: 1
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
global_plan_overwrite_orientation: True
global_plan_prune_distance: 1.0
allow_init_with_backward_motion: True
max_global_plan_lookahead_dist: 1.5
force_reinit_new_goal_dist: 1.0
force_reinit_new_goal_angular: 1.57
prefer_x_feedback: False
publish_ocp_results: False
## Solver settings
solver:
type: "ipopt"
ipopt:
iterations: 100
max_cpu_time: -1.0
ipopt_numeric_options:
tol: 1e-4
ipopt_string_options:
linear_solver: "mumps"
hessian_approximation: "limited-memory" # exact/limited-memory, WARNING 'exact' does currently not work well with the carlike model
lsq_lm:
iterations: 10
weight_init_eq: 2
weight_init_ineq: 2
weight_init_bounds: 2
weight_adapt_factor_eq: 1.5
weight_adapt_factor_ineq: 1.5
weight_adapt_factor_bounds: 1.5
weight_adapt_max_eq: 500
weight_adapt_max_ineq: 500
weight_adapt_max_bounds: 500

30
mpc_local_planner_examples/cfg/diff_drive/costmap_common_params.yaml Normal file
View File

@ -0,0 +1,30 @@
#---standard pioneer footprint---
#---(in meters)---
robot_radius: 0.17
footprint_padding: 0.00
transform_tolerance: 0.2
always_send_full_costmap: true
obstacle_layer:
enabled: true
obstacle_range: 3.0
raytrace_range: 4.0
inflation_radius: 0.2
track_unknown_space: true
combination_method: 1
observation_sources: laser_scan_sensor
laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
inflation_layer:
enabled: true
cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10)
inflation_radius: 0.5 # max. distance from an obstacle at which costs are incurred for planning paths.
static_layer:
enabled: true
map_topic: "/map"

31
mpc_local_planner_examples/cfg/diff_drive/costmap_converter_params.yaml Normal file
View File

@ -0,0 +1,31 @@
###########################################################################################
## NOTE: Costmap conversion is experimental. Its purpose is to combine many point ##
## obstales into clusters, computed in a separate thread in order to improve the overall ##
## efficiency of local planning. However, the implemented conversion algorithms are in a ##
## very early stage of development. Contributions are welcome! ##
###########################################################################################
MpcLocalPlannerROS:
## Costmap converter plugin
#costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
#costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
#costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
costmap_converter_spin_thread: True
costmap_converter_rate: 5
## Configure plugins (namespace move_base/costmap_to_lines or move_base/costmap_to_polygons)
## costmap_converter/CostmapToLinesDBSRANSAC, costmap_converter/CostmapToLinesDBSMCCH, costmap_converter/CostmapToPolygonsDBSMCCH
costmap_converter/CostmapToLinesDBSRANSAC:
cluster_max_distance: 0.4
cluster_min_pts: 2
ransac_inlier_distance: 0.15
ransac_min_inliers: 10
ransac_no_iterations: 1500
ransac_remainig_outliers: 3
ransac_convert_outlier_pts: True
ransac_filter_remaining_outlier_pts: False
convex_hull_min_pt_separation: 0.1

15
mpc_local_planner_examples/cfg/diff_drive/global_costmap_params.yaml Normal file
View File

@ -0,0 +1,15 @@
global_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 1.0
publish_frequency: 0.5
transform_tolerance: 0.5
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}

14
mpc_local_planner_examples/cfg/diff_drive/local_costmap_params.yaml Normal file
View File

@ -0,0 +1,14 @@
local_costmap:
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
rolling_window: true
width: 5.5
height: 5.5
resolution: 0.1
transform_tolerance: 0.5
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}

108
mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml Normal file
View File

@ -0,0 +1,108 @@
MpcLocalPlannerROS:
odom_topic: odom
## Robot settings
robot:
type: "unicycle"
unicycle:
max_vel_x: 0.4
max_vel_x_backwards: 0.2
max_vel_theta: 0.3
acc_lim_x: 0.2 # deactive bounds with zero
dec_lim_x: 0.2 # deactive bounds with zero
acc_lim_theta: 0.2 # deactivate bounds with zero
## Footprint model for collision avoidance
footprint_model:
type: "point"
is_footprint_dynamic: False
## Collision avoidance
collision_avoidance:
min_obstacle_dist: 0.2 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_factor: 1.5
cutoff_factor: 5
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.5
## Planning grid
grid:
type: "fd_grid"
grid_size_ref: 20
dt_ref: 0.3
xf_fixed: [True, True, True]
warm_start: True
collocation_method: "forward_differences"
cost_integration_method: "left_sum"
variable_grid:
enable: True
min_dt: 0.0;
max_dt: 10.0;
grid_adaptation:
enable: True
dt_hyst_ratio: 0.1
min_grid_size: 2
max_grid_size: 50
## Planning options
planning:
objective:
type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
quadratic_form:
state_weights: [2.0, 2.0, 2.0]
control_weights: [1.0, 1.0]
integral_form: False
minimum_time_via_points:
position_weight: 10.5
orientation_weight: 0.0
via_points_ordered: False
terminal_cost:
type: "quadratic" # can be "none"
quadratic:
final_state_weights: [2.0, 2.0, 2.0]
terminal_constraint:
type: "none" # can be "none"
l2_ball:
weight_matrix: [1.0, 1.0, 1.0]
radius: 5
## Controller options
controller:
outer_ocp_iterations: 1
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
global_plan_overwrite_orientation: true
global_plan_prune_distance: 1.0
allow_init_with_backward_motion: True
max_global_plan_lookahead_dist: 1.5
global_plan_viapoint_sep: 0.5
force_reinit_new_goal_dist: 1.0
force_reinit_new_goal_angular: 1.57
force_reinit_num_steps: 0
prefer_x_feedback: False
publish_ocp_results: False
## Solver settings
solver:
type: "ipopt"
ipopt:
iterations: 100
max_cpu_time: -1.0
ipopt_numeric_options:
tol: 1e-4
ipopt_string_options:
linear_solver: "mumps"
hessian_approximation: "exact" # exact or limited-memory
lsq_lm:
iterations: 10
weight_init_eq: 2
weight_init_ineq: 2
weight_init_bounds: 2
weight_adapt_factor_eq: 1.5
weight_adapt_factor_ineq: 1.5
weight_adapt_factor_bounds: 1.5
weight_adapt_max_eq: 500
weight_adapt_max_ineq: 500
weight_adapt_max_bounds: 500

103
mpc_local_planner_examples/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml Normal file
View File

@ -0,0 +1,103 @@
MpcLocalPlannerROS:
odom_topic: odom
## Robot settings
robot:
type: "unicycle"
unicycle:
max_vel_x: 0.4
max_vel_x_backwards: 0.2
max_vel_theta: 0.3
acc_lim_x: 0.2 # deactive bounds with zero
dec_lim_x: 0.2 # deactive bounds with zero
acc_lim_theta: 0.2 # deactivate bounds with zero
## Footprint model for collision avoidance
footprint_model:
type: "point"
is_footprint_dynamic: False
## Collision avoidance
collision_avoidance:
min_obstacle_dist: 0.2 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_factor: 1.5
cutoff_factor: 5
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.5
## Planning grid
grid:
type: "fd_grid"
grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist
dt_ref: 0.3 # and here the corresponding temporal resolution
xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below
warm_start: True
collocation_method: "forward_differences"
cost_integration_method: "left_sum"
variable_grid:
enable: False # We want a fixed grid
min_dt: 0.0;
max_dt: 10.0;
grid_adaptation:
enable: True
dt_hyst_ratio: 0.1
min_grid_size: 2
max_grid_size: 50
## Planning options
planning:
objective:
type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
quadratic_form:
state_weights: [2.0, 2.0, 0.25]
control_weights: [0.1, 0.05]
integral_form: False
terminal_cost:
type: "quadratic" # can be "none"
quadratic:
final_state_weights: [10.0, 10.0, 0.5]
terminal_constraint:
type: "none" # can be "none"
l2_ball:
weight_matrix: [1.0, 1.0, 1.0]
radius: 5
## Controller options
controller:
outer_ocp_iterations: 1
xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
global_plan_overwrite_orientation: true
global_plan_prune_distance: 1.0
allow_init_with_backward_motion: True
max_global_plan_lookahead_dist: 1.0 # Check horizon length
force_reinit_new_goal_dist: 1.0
force_reinit_new_goal_angular: 1.57
force_reinit_num_steps: 0
prefer_x_feedback: False
publish_ocp_results: False
## Solver settings
solver:
type: "ipopt"
ipopt:
iterations: 100
max_cpu_time: -1.0
ipopt_numeric_options:
tol: 1e-4
ipopt_string_options:
linear_solver: "mumps"
hessian_approximation: "exact" # exact or limited-memory
lsq_lm:
iterations: 10
weight_init_eq: 2
weight_init_ineq: 2
weight_init_bounds: 2
weight_adapt_factor_eq: 1.5
weight_adapt_factor_ineq: 1.5
weight_adapt_factor_bounds: 1.5
weight_adapt_max_eq: 500
weight_adapt_max_ineq: 500
weight_adapt_max_bounds: 500

317
mpc_local_planner_examples/cfg/rviz_navigation.rviz Normal file
View File

@ -0,0 +1,317 @@
Panels:
- Class: rviz/Displays
Help Height: 81
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
Splitter Ratio: 0.6050959825515747
Tree Height: 546
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 50
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Class: rviz/Group
Displays:
- Class: rviz/Marker
Enabled: true
Marker Topic: /move_base/MpcLocalPlannerROS/mpc_markers
Name: MpcMarker
Namespaces:
PointObstacles: true
Queue Size: 100
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 0; 0
Enabled: true
Head Diameter: 0.05000000074505806
Head Length: 0.10000000149011612
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: LocalPath
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 164; 0; 0
Pose Style: Arrows
Radius: 0.029999999329447746
Shaft Diameter: 0.02500000037252903
Shaft Length: 0.10000000149011612
Topic: /move_base/MpcLocalPlannerROS/local_plan
Unreliable: false
Value: true
- Alpha: 0.4000000059604645
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: LocalCostmap
Topic: /move_base/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
Enabled: true
Name: Local Planner
- Class: rviz/Group
Displays:
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: GlobalPath
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /move_base/GlobalPlanner/plan
Unreliable: false
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: GlobalCostmap
Topic: /move_base/global_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
Enabled: true
Name: Global Planner
- Class: rviz/Group
Displays:
- Alpha: 1
Class: rviz/Polygon
Color: 85; 0; 255
Enabled: true
Name: Robot Footprint
Topic: /move_base/local_costmap/footprint
Unreliable: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 1
Min Color: 0; 0; 0
Min Intensity: 1
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Arrow Length: 0.10000000149011612
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: false
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: AMCL Particles
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic: /particlecloud
Unreliable: false
Value: false
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: false
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic: /odom
Unreliable: false
Value: false
Enabled: true
Name: Robot
Enabled: true
Global Options:
Background Color: 255; 255; 255
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 12.58323860168457
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5.375269889831543
Y: 5.046060085296631
Z: -1.4013299942016602
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.5648000240325928
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.710430145263672
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000363000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1215
X: 705
Y: 37

355
mpc_local_planner_examples/cfg/rviz_navigation_cc.rviz Normal file
View File

@ -0,0 +1,355 @@
Panels:
- Class: rviz/Displays
Help Height: 81
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Local Planner1/TebPoses1
- /Local Planner1/TebMarker1
- /Local Planner1/LocalPath1
- /Local Planner1/LocalCostmap1
- /Global Planner1/GlobalPath1
- /Global Planner1/GlobalCostmap1
- /Robot1/Robot Footprint1
- /Obstacle1
- /Obstacle1/Footprint estimation1
- /Obstacle1/Marker1
Splitter Ratio: 0.6050959825515747
Tree Height: 555
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: LaserScan
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 50
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Class: rviz/Group
Displays:
- Alpha: 1
Arrow Length: 0.30000001192092896
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: true
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: TebPoses
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic: /robot_0/move_base/TebLocalPlannerROS/teb_poses
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /robot_0/move_base/TebLocalPlannerROS/teb_markers
Name: TebMarker
Namespaces:
{}
Queue Size: 100
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 0; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: LocalPath
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /robot_0/move_base/TebLocalPlannerROS/local_plan
Unreliable: false
Value: true
- Alpha: 0.4000000059604645
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: LocalCostmap
Topic: /robot_0/move_base/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
Enabled: true
Name: Local Planner
- Class: rviz/Group
Displays:
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: GlobalPath
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /robot_0/move_base/TebLocalPlannerROS/global_plan
Unreliable: false
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: GlobalCostmap
Topic: /robot_0/move_base/global_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
Enabled: true
Name: Global Planner
- Class: rviz/Group
Displays:
- Alpha: 1
Class: rviz/Polygon
Color: 85; 0; 255
Enabled: true
Name: Robot Footprint
Topic: /robot_0/move_base/local_costmap/footprint
Unreliable: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 1
Min Color: 0; 0; 0
Min Intensity: 1
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /robot_0/scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Arrow Length: 0.10000000149011612
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
Color: 255; 25; 0
Enabled: false
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: AMCL Particles
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic: /robot_0/particlecloud
Unreliable: false
Value: false
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: false
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic: /robot_0/odom
Unreliable: false
Value: false
Enabled: true
Name: Robot
- Class: rviz/Group
Displays:
- Alpha: 1
Class: rviz/Polygon
Color: 25; 255; 0
Enabled: true
Name: Footprint estimation
Topic: /costmap_polygons
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /standalone_converter/costmap_polygon_markers
Name: Marker
Namespaces:
Polygons: true
Queue Size: 100
Value: true
Enabled: true
Name: Obstacle
Enabled: true
Global Options:
Background Color: 255; 255; 255
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 8.012253761291504
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 4.049907684326172
Y: 3.295278549194336
Z: 8.21469497680664
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.5697963237762451
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.715442657470703
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd000000040000000000000176000002b6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000002b6000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c40000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf0000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000343000002b600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1215
X: 705
Y: 37

69
mpc_local_planner_examples/launch/carlike_minimum_time.launch Normal file
View File

@ -0,0 +1,69 @@
<!--
Simulate a carlike robot with the mpc_local_planner in stage:
- stage
- map_server
- move_base
- static map
- amcl
- rviz view
-->
<launch>
<arg name="rviz" default="true" />
<arg name="plot" default="false" />
<arg name="plot_states" default="false" />
<!-- ************** Global Parameters *************** -->
<param name="/use_sim_time" value="true"/>
<!-- ************** Stage Simulator *************** -->
<node pkg="stage_ros" type="stageros" name="stageros" args="$(find mpc_local_planner_examples)/stage/maze_carlike.world">
<remap from="base_scan" to="scan"/>
</node>
<!-- ************** Navigation *************** -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find mpc_local_planner_examples)/cfg/carlike/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/carlike/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/carlike/local_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/carlike/global_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/carlike/mpc_local_planner_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="planner_frequency" value="1.0" />
<param name="planner_patience" value="5.0" />
<param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="15.0" />
<param name="clearing_rotation_allowed" value="false" /> <!-- Our carlike robot is not able to rotate in place -->
<param name="MpcLocalPlannerROS/controller/publish_ocp_results" value="true" if="$(arg plot)" />
</node>
<!-- ****** Maps ***** -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_local_planner_examples)/maps/maze.yaml" output="screen">
<param name="frame_id" value="map"/>
</node>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find mpc_local_planner_examples)/cfg/amcl_params.yaml" command="load" />
<param name="initial_pose_x" value="2"/>
<param name="initial_pose_y" value="2"/>
<param name="initial_pose_a" value="0"/>
</node>
<!-- **************** Visualisation **************** -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_local_planner_examples)/cfg/rviz_navigation.rviz" if="$(arg rviz)"/>
<!-- **************** Plot ocp results ************** -->
<node name="ocp_result_plotter" pkg="mpc_local_planner" type="plot_optimal_control_results.py" output="screen" if="$(arg plot)">
<param name="ocp_result_topic" value="/move_base/MpcLocalPlannerROS/ocp_result"/>
<param name="plot_rate" value="5"/>
<param name="plot_states" value="$(arg plot_states)"/>
</node>
</launch>

68
mpc_local_planner_examples/launch/diff_drive_minimum_time.launch Normal file
View File

@ -0,0 +1,68 @@
<!--
Simulate a differential drive robot with the mpc_local_planner in stage:
- stage
- map_server
- move_base
- static map
- amcl
- rviz view
-->
<launch>
<arg name="rviz" default="true" />
<arg name="plot" default="false" />
<arg name="plot_states" default="false" />
<!-- ************** Global Parameters *************** -->
<param name="/use_sim_time" value="true"/>
<!-- ************** Stage Simulator *************** -->
<node pkg="stage_ros" type="stageros" name="stageros" args="$(find mpc_local_planner_examples)/stage/maze_diff_drive.world">
<remap from="base_scan" to="scan"/>
</node>
<!-- ************** Navigation *************** -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/local_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/global_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="planner_frequency" value="1.0" />
<param name="planner_patience" value="5.0" />
<param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="15.0" />
<param name="MpcLocalPlannerROS/controller/publish_ocp_results" value="true" if="$(arg plot)" />
</node>
<!-- ****** Maps ***** -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_local_planner_examples)/maps/maze.yaml" output="screen">
<param name="frame_id" value="map"/>
</node>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find mpc_local_planner_examples)/cfg/amcl_params.yaml" command="load" />
<param name="initial_pose_x" value="2"/>
<param name="initial_pose_y" value="2"/>
<param name="initial_pose_a" value="0"/>
</node>
<!-- **************** Visualization **************** -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_local_planner_examples)/cfg/rviz_navigation.rviz" if="$(arg rviz)"/>
<!-- **************** Plot ocp results ************** -->
<node name="ocp_result_plotter" pkg="mpc_local_planner" type="plot_optimal_control_results.py" output="screen" if="$(arg plot)">
<param name="ocp_result_topic" value="/move_base/MpcLocalPlannerROS/ocp_result"/>
<param name="plot_rate" value="5"/>
<param name="plot_states" value="$(arg plot_states)"/>
</node>
</launch>

70
mpc_local_planner_examples/launch/diff_drive_minimum_time_costmap_conversion.launch Normal file
View File

@ -0,0 +1,70 @@
<!--
Simulate a differential drive robot with the mpc_local_planner and costmap conversion enabled in stage:
- stage
- map_server
- move_base
- costmap_converter
- static map
- amcl
- rviz view
-->
<launch>
<arg name="rviz" default="true" />
<arg name="plot" default="false" />
<arg name="plot_states" default="false" />
<!-- ************** Global Parameters *************** -->
<param name="/use_sim_time" value="true"/>
<!-- ************** Stage Simulator *************** -->
<node pkg="stage_ros" type="stageros" name="stageros" args="$(find mpc_local_planner_examples)/stage/maze_diff_drive.world">
<remap from="base_scan" to="scan"/>
</node>
<!-- ************** Navigation *************** -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/local_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/global_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/mpc_local_planner_params_minimum_time.yaml" command="load" />
<!-- Here we load our costmap conversion settings -->
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/costmap_converter_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="planner_frequency" value="1.0" />
<param name="planner_patience" value="5.0" />
<param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="15.0" />
<param name="MpcLocalPlannerROS/controller/publish_ocp_results" value="true" if="$(arg plot)" />
</node>
<!-- ****** Maps ***** -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_local_planner_examples)/maps/maze.yaml" output="screen">
<param name="frame_id" value="map"/>
</node>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find mpc_local_planner_examples)/cfg/amcl_params.yaml" command="load" />
<param name="initial_pose_x" value="2"/>
<param name="initial_pose_y" value="2"/>
<param name="initial_pose_a" value="0"/>
</node>
<!-- **************** Visualization **************** -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_local_planner_examples)/cfg/rviz_navigation.rviz" if="$(arg rviz)"/>
<!-- **************** Plot ocp results ************** -->
<node name="ocp_result_plotter" pkg="mpc_local_planner" type="plot_optimal_control_results.py" output="screen" if="$(arg plot)">
<param name="ocp_result_topic" value="/move_base/MpcLocalPlannerROS/ocp_result"/>
<param name="plot_rate" value="5"/>
<param name="plot_states" value="$(arg plot_states)"/>
</node>
</launch>

68
mpc_local_planner_examples/launch/diff_drive_quadratic_form.launch Normal file
View File

@ -0,0 +1,68 @@
<!--
Simulate a differential drive robot with the mpc_local_planner in stage:
- stage
- map_server
- move_base
- static map
- amcl
- rviz view
-->
<launch>
<arg name="rviz" default="true" />
<arg name="plot" default="false" />
<arg name="plot_states" default="false" />
<!-- ************** Global Parameters *************** -->
<param name="/use_sim_time" value="true"/>
<!-- ************** Stage Simulator *************** -->
<node pkg="stage_ros" type="stageros" name="stageros" args="$(find mpc_local_planner_examples)/stage/maze_diff_drive.world">
<remap from="base_scan" to="scan"/>
</node>
<!-- ************** Navigation *************** -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/local_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/global_costmap_params.yaml" command="load" />
<rosparam file="$(find mpc_local_planner_examples)/cfg/diff_drive/mpc_local_planner_params_quadratic_form.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="planner_frequency" value="1.0" />
<param name="planner_patience" value="5.0" />
<param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="15.0" />
<param name="MpcLocalPlannerROS/controller/publish_ocp_results" value="true" if="$(arg plot)" />
</node>
<!-- ****** Maps ***** -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mpc_local_planner_examples)/maps/maze.yaml" output="screen">
<param name="frame_id" value="map"/>
</node>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<rosparam file="$(find mpc_local_planner_examples)/cfg/amcl_params.yaml" command="load" />
<param name="initial_pose_x" value="2"/>
<param name="initial_pose_y" value="2"/>
<param name="initial_pose_a" value="0"/>
</node>
<!-- **************** Visualization **************** -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mpc_local_planner_examples)/cfg/rviz_navigation.rviz" if="$(arg rviz)"/>
<!-- **************** Plot ocp results ************** -->
<node name="ocp_result_plotter" pkg="mpc_local_planner" type="plot_optimal_control_results.py" output="screen" if="$(arg plot)">
<param name="ocp_result_topic" value="/move_base/MpcLocalPlannerROS/ocp_result"/>
<param name="plot_rate" value="5"/>
<param name="plot_states" value="$(arg plot_states)"/>
</node>
</launch>

BIN
mpc_local_planner_examples/maps/corridor.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 457 B

7
mpc_local_planner_examples/maps/corridor.yaml Normal file
View File

@ -0,0 +1,7 @@
image: corridor.png
resolution: 0.050000
origin: [0.000000, 0.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

BIN
mpc_local_planner_examples/maps/empty_box.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 319 B

7
mpc_local_planner_examples/maps/empty_box.yaml Normal file
View File

@ -0,0 +1,7 @@
image: empty_box.png
resolution: 0.050000
origin: [-3.0, -3.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

BIN
mpc_local_planner_examples/maps/maze.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 373 B

6
mpc_local_planner_examples/maps/maze.yaml Normal file
View File

@ -0,0 +1,6 @@
image: maze.png
resolution: 0.05
origin: [0.0, 0.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

29
mpc_local_planner_examples/package.xml Normal file
View File

@ -0,0 +1,29 @@
<?xml version="1.0"?>
<package format="2">
<name>mpc_local_planner_examples</name>
<version>0.0.0</version>
<description>The mpc_local_planner_examples package</description>
<maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
<author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
<license>BSD</license>
<url type="website">http://wiki.ros.org/mpc_local_planner_examples</url>
<buildtool_depend>catkin</buildtool_depend>
<exec_depend>stage_ros</exec_depend>
<exec_depend>mpc_local_planner</exec_depend>
<exec_depend>move_base</exec_depend>
<exec_depend>map_server</exec_depend>
<exec_depend>amcl</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

123
mpc_local_planner_examples/stage/corridor.world Normal file
View File

@ -0,0 +1,123 @@
include "robots/diff_drive_robot_gps.inc"
include "robots/obstacle.inc"
define floorplan model
(
# sombre, sensible, artistic
color "gray30"
# most maps will need a bounding box
boundary 0
gui_nose 0
gui_grid 0
gui_outline 0
gripper_return 0
fiducial_return 0
laser_return 1
)
resolution 0.02
interval_sim 100 # simulation timestep in milliseconds
window
(
size [ 600.0 700.0 ]
center [ 0.0 0.0 ]
rotate [ 0.0 0.0 ]
scale 60
)
floorplan
(
name "corridor"
bitmap "../maps/corridor.png"
size [ 25.000 6.000 2.000 ]
pose [ 12.5 3.0 0.000 0.000 ]
)
# throw in a robot
myrobot
(
pose [ 2.0 3.0 0.0 0.0 ]
name "turtlebot"
)
myobstacle
(
pose [ 4.0 3.77 0.0 0.0 ]
name "obstacle0"
)
myobstacle
(
pose [ 5.5 4.09 0.0 0.0 ]
name "obstacle1"
)
myobstacle
(
pose [ 7.0 5.38 0.0 0.0 ]
name "obstacle2"
)
myobstacle
(
pose [ 8.5 1.47 0.0 0.0 ]
name "obstacle3"
)
myobstacle
(
pose [ 10.0 2.14 0.0 0.0 ]
name "obstacle4"
)
myobstacle
(
pose [ 11.5 3.58 0.0 0.0 ]
name "obstacle5"
)
myobstacle
(
pose [ 13.0 0.26 0.0 0.0 ]
name "obstacle6"
)
myobstacle
(
pose [ 14.5 1.09 0.0 0.0 ]
name "obstacle7"
)
myobstacle
(
pose [ 16 2.94 0.0 0.0 ]
name "obstacle8"
)
myobstacle
(
pose [ 17.5 2.76 0.0 0.0 ]
name "obstacle9"
)
myobstacle
(
pose [ 19 0.79 0.0 0.0 ]
name "obstacle10"
)
myobstacle
(
pose [ 20.5 5.16 0.0 0.0 ]
name "obstacle11"
)
myobstacle
(
pose [ 22 3.62 0.0 0.0 ]
name "obstacle12"
)

32
mpc_local_planner_examples/stage/empty_box.world Normal file
View File

@ -0,0 +1,32 @@
# include our robot and obstacle definitions
include "robots/diff_drive_robot_gps.inc"
include "robots/obstacle.inc"
## Simulation settings
resolution 0.02
interval_sim 100 # simulation timestep in milliseconds
## Load a static map
model
(
# most maps will need a bounding box
name "emptyBox"
bitmap "../maps/empty_box.png"
size [ 6.0 6.0 2.0 ]
pose [ 0.0 0.0 0.0 0.0 ]
laser_return 1
color "gray30"
)
# throw in a robot
myrobot
(
pose [ -2.0 0.0 0.0 -90.0 ]
name "myRobot"
)
myobstacle
(
pose [ 0.0 1.0 0.0 0.0 ]
name "obstacle"
)

44
mpc_local_planner_examples/stage/maze_carlike.world Normal file
View File

@ -0,0 +1,44 @@
include "robots/carlike_robot.inc"
define floorplan model
(
# sombre, sensible, artistic
color "gray30"
# most maps will need a bounding box
boundary 1
gui_nose 0
gui_grid 0
gui_outline 0
gripper_return 0
fiducial_return 0
laser_return 1
)
resolution 0.02
interval_sim 100 # simulation timestep in milliseconds
window
(
size [ 600.0 700.0 ]
center [ 0.0 0.0 ]
rotate [ 0.0 0.0 ]
scale 60
)
floorplan
(
name "maze"
bitmap "../maps/maze.png"
size [ 10.0 10.0 2.0 ]
pose [ 5.0 5.0 0.0 0.0 ]
)
# throw in a robot
carlike_robot
(
pose [ 2.0 2.0 0.0 0.0 ]
name "robot"
)

44
mpc_local_planner_examples/stage/maze_diff_drive.world Normal file
View File

@ -0,0 +1,44 @@
include "robots/diff_drive_robot.inc"
define floorplan model
(
# sombre, sensible, artistic
color "gray30"
# most maps will need a bounding box
boundary 1
gui_nose 0
gui_grid 0
gui_outline 0
gripper_return 0
fiducial_return 0
laser_return 1
)
resolution 0.02
interval_sim 100 # simulation timestep in milliseconds
window
(
size [ 600.0 700.0 ]
center [ 0.0 0.0 ]
rotate [ 0.0 0.0 ]
scale 60
)
floorplan
(
name "maze"
bitmap "../maps/maze.png"
size [ 10.0 10.0 2.0 ]
pose [ 5.0 5.0 0.0 0.0 ]
)
# throw in a robot
diff_drive_robot
(
pose [ 2.0 2.0 0.0 0.0 ]
name "robot"
)

44
mpc_local_planner_examples/stage/maze_omnidir.world Normal file
View File

@ -0,0 +1,44 @@
include "robots/omnidir_robot.inc"
define floorplan model
(
# sombre, sensible, artistic
color "gray30"
# most maps will need a bounding box
boundary 1
gui_nose 0
gui_grid 0
gui_outline 0
gripper_return 0
fiducial_return 0
laser_return 1
)
resolution 0.02
interval_sim 100 # simulation timestep in milliseconds
window
(
size [ 600.0 700.0 ]
center [ 0.0 0.0 ]
rotate [ 0.0 0.0 ]
scale 60
)
floorplan
(
name "maze"
bitmap "../maps/maze.png"
size [ 10.0 10.0 2.0 ]
pose [ 5.0 5.0 0.0 0.0 ]
)
# throw in a robot
omnidir_robot
(
pose [ 2.0 2.0 0.0 0.0 ]
name "robot"
)

38
mpc_local_planner_examples/stage/robots/carlike_robot.inc Normal file
View File

@ -0,0 +1,38 @@
define laser ranger
(
sensor
(
range_max 6.5
fov 58.0
samples 640
)
# generic model properties
color "black"
size [ 0.06 0.15 0.03 ]
)
#
# Robot model:
# footprint (counter-clockwise): [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125]
# center of rotation: [0,0]
# wheelbase: 0.4
define carlike_robot position
(
pose [ 0.0 0.0 0.0 0.0 ]
odom_error [0.03 0.03 999999 999999 999999 0.02]
size [ 0.6 0.25 0.40 ] # this models the footprint (rectangular), but shifted such that the bottom-left corner is in [0,0]. The center of rotation now here at [0.3, 0.125, 0.2]
# correct center of rotation:
origin [ 0.2 0.0 0.0 0.0]
gui_nose 1
color "red"
# kinematics
drive "car"
wheelbase 0.4 # distance between both axles
# spawn sensors
laser(pose [ -0.1 0.0 -0.11 0.0 ])
)

30
mpc_local_planner_examples/stage/robots/diff_drive_robot.inc Normal file
View File

@ -0,0 +1,30 @@
define laser ranger
(
sensor
(
range_max 6.5
fov 58.0
samples 640
)
# generic model properties
color "black"
size [ 0.06 0.15 0.03 ]
)
define diff_drive_robot position
(
pose [ 0.0 0.0 0.0 0.0 ]
odom_error [0.03 0.03 999999 999999 999999 0.02]
size [ 0.25 0.25 0.40 ]
origin [ 0.0 0.0 0.0 0.0 ]
gui_nose 1
color "red"
# kinematics
drive "diff"
# spawn sensors
laser(pose [ -0.1 0.0 -0.11 0.0 ])
)

21
mpc_local_planner_examples/stage/robots/diff_drive_robot_gps.inc Normal file
View File

@ -0,0 +1,21 @@
define mylaser ranger
(
sensor
(
# just for demonstration purposes
range [0.1 25] # minimum and maximum range
fov 360.0 # field of view
samples 1920 # number of samples
)
size [ 0.06 0.15 0.03 ]
)
define myrobot position
(
size [ 0.25 0.25 0.40 ] # (x,y,z)
localization "gps" # exact localization
gui_nose 1 # draw nose showing heading
drive "diff" # diff-drive
color "red" # red model
mylaser(pose [ -0.1 0.0 -0.11 0.0 ]) # has mylaser sensor
)

8
mpc_local_planner_examples/stage/robots/obstacle.inc Normal file
View File

@ -0,0 +1,8 @@
define myobstacle position
(
localization "gps"
size [ 0.25 0.25 0.40 ]
gui_nose 1
drive "omni"
color "blue"
)

30
mpc_local_planner_examples/stage/robots/omnidir_robot.inc Normal file
View File

@ -0,0 +1,30 @@
define laser ranger
(
sensor
(
range_max 6.5
fov 58.0
samples 640
)
# generic model properties
color "black"
size [ 0.06 0.15 0.03 ]
)
define omnidir_robot position
(
pose [ 0.0 0.0 0.0 0.0 ]
odom_error [0.03 0.03 999999 999999 999999 0.02]
size [ 0.25 0.25 0.40 ]
origin [ 0.0 0.0 0.0 0.0 ]
gui_nose 1
color "red"
# kinematics
drive "omni"
# spawn sensors
laser(pose [ -0.1 0.0 -0.11 0.0 ])
)

205
mpc_local_planner_msgs/CMakeLists.txt Normal file
View File

@ -0,0 +1,205 @@
cmake_minimum_required(VERSION 2.8.3)
project(mpc_local_planner_msgs)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
std_msgs
message_generation
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
StateFeedback.msg
OptimalControlResult.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES mpc_local_planner_msgs
CATKIN_DEPENDS std_msgs message_runtime
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
#include_directories(
# include
# ${catkin_INCLUDE_DIRS}
#)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/mpc_local_planner_msgs.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/mpc_local_planner_msgs_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_local_planner_msgs.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

12
mpc_local_planner_msgs/msg/OptimalControlResult.msg Normal file
View File

@ -0,0 +1,12 @@
std_msgs/Header header
int64 dim_states
int64 dim_controls
float64[] time_states
float64[] states # Column Major
float64[] time_controls
float64[] controls # Column Major
bool optimal_solution_found
float64 cpu_time

3
mpc_local_planner_msgs/msg/StateFeedback.msg Normal file
View File

@ -0,0 +1,3 @@
std_msgs/Header header
float64[] state

30
mpc_local_planner_msgs/package.xml Normal file
View File

@ -0,0 +1,30 @@
<?xml version="1.0"?>
<package format="2">
<name>mpc_local_planner_msgs</name>
<version>0.0.0</version>
<description>This package provides message types that are used by the package mpc_local_planner</description>
<maintainer email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</maintainer>
<author email="christoph.roesmann@tu-dortmund.de">Christoph Rösmann</author>
<license>GPLv3</license>
<url type="website">http://wiki.ros.org/mpc_local_planner_msgs</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>