hongshaorou/DPtraj
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add backend

Browse Source
This commit is contained in:
Han-Sin
2025-07-20 12:46:40 +08:00
parent 3a6c6a9a97
commit a9c0a5975c
108 changed files with 576799 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
LICENSE Normal file
View File

@@ -0,0 +1,21 @@
MIT License
Copyright (c) 2022 GaoLon
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

75
README.md
View File

@@ -1,36 +1,55 @@
# DPtraj
A double-polynomial discription for trajectory interfaced with learning-based front end.
# Bilayer-based Trajectory Optimization
This work is presented in the paper: Hierarchically Depicting Vehicle Trajectory with Stability in Complex Environments, published in Science Robotics.
## 1. Setup
The backend trajectory optimizer improvements build upon our previous work (available at https://github.com/ZJU-FAST-Lab/Dftpav), where singularity issues were addressed.
All the tests are conducted in the Linux environment on a computer equipped with an Intel Core i7-10700 CPU and a GeForce RTX 2060 GPU.
Moreover, the approach has recently been extended and applied to more complex multi-joint robotic platforms (see https://github.com/Tracailer/Tracailer).
Moreover, our software is developed and tested in Ubuntu 18.04, 20.04 with ROS installed.
ROS can be installed here: [ROS Installation](http://wiki.ros.org/ROS/Installation).
If you find this repository helpful, please consider citing at least one of the following papers:
To build this project, ensure that you have the following dependencies installed: 
```bibtex
@article{han2025hierarchically,
title={Hierarchically depicting vehicle trajectory with stability in complex environments},
author={Han, Zhichao and Tian, Mengze and Gongye, Zaitian and Xue, Donglai and Xing, Jiaxi and Wang, Qianhao and Gao, Yuman and Wang, Jingping and Xu, Chao and Gao, Fei},
journal={Science Robotics},
volume={10},
number={103},
pages={eads4551},
year={2025},
publisher={American Association for the Advancement of Science}
}
@article{han2023efficient,
title={An efficient spatial-temporal trajectory planner for autonomous vehicles in unstructured environments},
author={Han, Zhichao and Wu, Yuwei and Li, Tong and Zhang, Lu and Pei, Liuao and Xu, Long and Li, Chengyang and Ma, Changjia and Xu, Chao and Shen, Shaojie and others},
journal={IEEE Transactions on Intelligent Transportation Systems},
volume={25},
number={2},
pages={1797--1814},
year={2023},
publisher={IEEE}
}
-[Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page): A high-level C++ library for linear algebra operations. 
-The Open Motion Planning Library ([OMPL](https://ompl.kavrakilab.org/)): A comprehensive library for motion planning and control.
## 2. Build on ROS
1. Unzipping the compressed file and compile it.
```bash
cd ~/trajectoryOpt
catkin_make -DCMAKE_BUILD_TYPE=Release
```
The code will be divided into several modules and gradually open-sourced in different branches. Currently, you can switch to the `backend` branch to try our efficient singularity-free backend optimization. This branch includes a README to guide you through quick deployment.
## 3. Run
Open a new terminal window, `cd ` to `~/trajectoryOpt/` and type:
```bash
source devel/setup.bash
```
Then, run the script:
```bash
./run.sh
```
Then, you can use the **2D Nav Goal** in **RVIZ** to trigger the planning.
Here is an example:
![](README_md_files/1.gif)
Here, the blue curve represents the trajectory generated by our method, whereas the red curve depicts the trajectory generated by the differential flattening-based method employed for comparative analysis. 
The program evaluates the kinematic state at each point along both trajectories and computes the maximum value for each metric. If successful, the terminal will output the maximum curvature and maximum steering angle rate ($\omega$) for each method.
The results indicate that our approach adheres to the kinematic constraint standards, whereas the comparative method violates constraints by more than an order of magnitude.
Note: Due to computational performance and solver randomness, slight deviations in results may occur.
## 4. Contact
If you have any questions, please feel free to contact Zhichao HAN (<zhichaohan@zju.edu.cn>) or Mengze TIAN(<mengze.tian@epfl.ch>).

BIN
README_md_files/1.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 12 MiB

1
trajectoryOpt/.catkin_workspace Normal file
View File

@@ -0,0 +1 @@
# This file currently only serves to mark the location of a catkin workspace for tool integration

7
trajectoryOpt/.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,7 @@
{
"files.associations": {
"*.ipp": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp"
}
}

4
trajectoryOpt/run.sh Executable file
View File

@@ -0,0 +1,4 @@
roslaunch random_map_generator test.launch & sleep 1; #not sending map now
roslaunch plan_manage plan_node.launch & sleep 1;
wait

84
trajectoryOpt/src/.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,84 @@
{
"files.associations": {
"random": "cpp",
"core": "cpp",
"sparsecore": "cpp",
"cmath": "cpp",
"complex": "cpp",
"valarray": "cpp",
"__locale": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"array": "cpp",
"atomic": "cpp",
"hash_map": "cpp",
"hash_set": "cpp",
"strstream": "cpp",
"bit": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"codecvt": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"list": "cpp",
"map": "cpp",
"set": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"ratio": "cpp",
"string": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"fstream": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"ostream": "cpp",
"shared_mutex": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp",
"__nullptr": "cpp",
"__split_buffer": "cpp",
"ios": "cpp",
"__hash_table": "cpp",
"__tree": "cpp",
"queue": "cpp",
"stack": "cpp",
"filesystem": "cpp",
"eigen": "cpp",
"csignal": "cpp"
}
}

1
trajectoryOpt/src/CMakeLists.txt Symbolic link
View File

@@ -0,0 +1 @@
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

48
trajectoryOpt/src/path_search/CMakeLists.txt Normal file
View File

@@ -0,0 +1,48 @@
cmake_minimum_required(VERSION 2.8.3)
project(path_search)
set(CMAKE_VERBOSE_MAKEFILE "true")
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wall")
find_package(Eigen3 REQUIRED)
find_package(ompl REQUIRED)
find_package(PCL REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
visualization_msgs
std_msgs
tools
cv_bridge
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
#if this catkin packge's header is used by other packages, use catkin_package to
#declare the include directories of this package.
catkin_package(
INCLUDE_DIRS include
)
add_library(kino_astar
src/kino_astar.cpp
)
target_link_libraries(kino_astar
ompl
${catkin_LIBRARIES}
)

275
trajectoryOpt/src/path_search/include/path_searching/kino_astar.h Executable file
View File

@@ -0,0 +1,275 @@
#ifndef _KINODYNAMIC_ASTAR_H
#define _KINODYNAMIC_ASTAR_H
#include <Eigen/Eigen>
#include <iostream>
#include <map>
#include <ros/console.h>
#include <ros/ros.h>
#include <string>
#include <unordered_map>
#include <boost/functional/hash.hpp>
#include <queue>
#include <math.h>
#include <utility>
#include <pcl/point_cloud.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <tools/config.hpp>
#include <tools/gridmap.hpp>
#include "kino_model.hpp"
namespace path_searching{
using namespace std;
#define IN_CLOSE_SET 'a'
#define IN_OPEN_SET 'b'
#define NOT_EXPAND 'c'
#define inf 1 >> 30
class PathNode {
public:
/* -------------------- */
Eigen::Vector2i index;
int yaw_idx;
/* --- the state is x y theta(orientation) */
Eigen::Vector3d state;
double g_score, f_score;
/* control input should be steer and arc */
Eigen::Vector2d input;
PathNode* parent;
char node_state;
int singul = 0;
/* -------------------- */
PathNode() {
parent = NULL;
node_state = NOT_EXPAND;
}
~PathNode(){};
};
typedef PathNode* PathNodePtr;
class NodeComparator {
public:
template <class NodePtr>
bool operator()(NodePtr node1, NodePtr node2) {
return node1->f_score > node2->f_score;
}
};
template <typename T>
struct matrix_hash : std::unary_function<T, size_t> {
std::size_t operator()(T const& matrix) const {
size_t seed = 0;
for (long int i = 0; i < matrix.size(); ++i) {
auto elem = *(matrix.data() + i);
seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
}
return seed;
}
};
template <class NodePtr>
class NodeHashTable {
private:
/* data */
std::unordered_map<Eigen::Vector2i, NodePtr, matrix_hash<Eigen::Vector2i>> data_2d_;
std::unordered_map<Eigen::Vector3i, NodePtr, matrix_hash<Eigen::Vector3i>> data_3d_;
public:
NodeHashTable(/* args */) {
}
~NodeHashTable() {
}
// : for 2d vehicle planning
void insert(Eigen::Vector2i idx, NodePtr node) {
data_2d_.insert(std::make_pair(idx, node));
}
//for 3d vehicle planning
void insert(Eigen::Vector2i idx, int yaw_idx, NodePtr node) {
data_3d_.insert(std::make_pair(Eigen::Vector3i(idx(0), idx(1), yaw_idx), node));
}
void insert(Eigen::Vector3i idx,NodePtr node ){
data_3d_.insert(std::make_pair(idx,node));
}
NodePtr find(Eigen::Vector2i idx) {
auto iter = data_2d_.find(idx);
return iter == data_2d_.end() ? NULL : iter->second;
}
NodePtr find(Eigen::Vector2i idx, int yaw_idx) {
auto iter = data_3d_.find(Eigen::Vector3i(idx(0), idx(1), yaw_idx));
return iter == data_3d_.end() ? NULL : iter->second;
}
void clear() {
data_2d_.clear();
data_3d_.clear();
}
};
// struct FlatTrajData
// {
// int singul;
// std::vector<Eigen::Vector3d> traj_pts; // 3, N x,y dur
// std::vector<double> thetas;
// Eigen::MatrixXd start_state; // start flat state (2, 3)
// Eigen::MatrixXd final_state; // end flat state (2, 3)
// };
// typedef std::vector<FlatTrajData> KinoTrajData;
class KinoAstar {
private:
/* ---------- main data structure ---------- */
vector<PathNodePtr> path_node_pool_;
int use_node_num_, iter_num_;
NodeHashTable<PathNodePtr> expanded_nodes_;
std::priority_queue<PathNodePtr, std::vector<PathNodePtr>, NodeComparator> open_set_;
std::vector<PathNodePtr> path_nodes_;
/* ---------- record data ---------- */
Eigen::Vector4d start_state_, end_state_;
Eigen::Vector2d start_ctrl;
bool is_shot_succ_ = false;
bool has_path_ = false;
/* ---------- parameter ---------- */
/* search */
double step_arc = 1.0;
double max_cur_ = 0.35;
double max_steer_ = 0.78539815;
double max_seach_time = 0.1;
double max_forward_vel = 4.0;
double max_forward_acc = 2.0;
double max_backward_vel = 4.0;
double max_backward_acc = 2.0;
double wheel_base = 0.4;
double steer_res;
double traj_forward_penalty = 1.0;
double traj_back_penalty = 2.0;
double traj_gear_switch_penalty = 15.0;
double traj_steer_penalty = 0.5;
double traj_steer_change_penalty = 0.0;
double horizon_;
double lambda_heu_;
int allocate_num_;
int check_num_;
double tie_breaker_ = 1.0 + 1.0 / 10000;
/* map */
double resolution_, inv_resolution_, yaw_resolution_, inv_yaw_resolution_;
Eigen::Vector2d origin_, map_size_3d_;
double yaw_origin_ = -M_PI;
/*shot hzc*/
std::vector<double> shot_timeList;
std::vector<double> shot_lengthList;
std::vector<int> shotindex;
std::vector<int> shot_SList;
std::vector<Eigen::Vector3d> SampleTraj;
/* helper */
Eigen::Vector2i posToIndex(Eigen::Vector2d pt);
int yawToIndex(double yaw);
void retrievePath(PathNodePtr end_node);
void getFlatState(Eigen::Vector4d state, Eigen::Vector2d control_input,
Eigen::MatrixXd &flat_state, int singul);
bool is_shot_sucess(Eigen::Vector3d state1,Eigen::Vector3d state2);
void computeShotTraj(Eigen::Vector3d &state1, Eigen::Vector3d &state2,
std::vector<Eigen::Vector3d> &path_list,
double& len);
void stateTransit(Eigen::Vector3d &state0, Eigen::Vector3d &state1,
Eigen::Vector2d &ctrl_input);
double evaluateLength(double curt,double locallength,double localtime, double max_vel, double max_acc, double startV = 0.0, double endV = 0.0);
double evaluateDuration(double length, double max_vel, double max_acc, double startV = 0.0, double endV = 0.0);
map_util::OccMapUtil* frontend_map_itf_;
double non_siguav;
ompl::base::StateSpacePtr shotptr;
inline int getSingularity(double vel)
{
int singul = 0;
if (fabs(vel) > 1e-2){
if (vel >= 0.0){singul = 1;}
else{singul = -1;}
}
return singul;
}
// getHeu
inline double getHeu(Eigen::VectorXd x1, Eigen::VectorXd x2)
{
double dx = abs(x1(0) - x2(0));
double dy = abs(x1(1) - x2(1));
return tie_breaker_* sqrt(dx * dx + dy * dy);
}
public:
KinoAstar(){};
~KinoAstar();
enum { REACH_HORIZON = 1, REACH_END = 2, NO_PATH = 3, REACH_END_BUT_SHOT_FAILS = 4};
/* main API */
void setParam(ros::NodeHandle& nh);
void init(ConfigPtr cfg_, ros::NodeHandle nh);
void reset();
int search(Eigen::Vector4d start_state, Eigen::Vector2d init_ctrl,
Eigen::Vector4d end_state,double& model_time, bool use3d = false);
// inital semantic map
void intialMap(map_util::OccMapUtil *map_itf);
// get kino traj for optimization
void getKinoNode(KinoTrajData &flat_trajs);
void getKinoNode(KinoTrajData &flat_trajs, std::vector<Eigen::Vector3d> inputSamples);
void NodeVis(Eigen::Vector3d state);
/*hzchzc*/
Eigen::Vector3d evaluatePos(double t);
std::vector<Eigen::Vector4d> SamplePosList(int N); //px py yaw t
std::vector<Eigen::Vector3d> getSampleTraj();
double totalTrajTime;
double checkl = 0.2;
ros::Publisher expandNodesVis;
std::vector<float> nm;
std::vector<Eigen::Vector3d> getRoughSamples();
std::vector<PathNodePtr> getVisitedNodes();
std::vector<Eigen::Vector4d> state_list;
std::vector<Eigen::Vector3d> acc_list;
typedef shared_ptr<KinoAstar> KinoAstarPtr;
};
} // namespace
#endif

32
trajectoryOpt/src/path_search/include/path_searching/kino_model.hpp Normal file
View File

@@ -0,0 +1,32 @@
#ifndef _KINOMODEL_H
#define _KINOMODEL_H
#include <boost/array.hpp>
#include <boost/bind.hpp>
#include <iostream>
#include <boost/array.hpp>
#include <boost/bind.hpp>
#include <iostream>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
namespace path_searching{
struct FlatTrajData
{
int singul;
std::vector<Eigen::Vector3d> traj_pts; // 3, N x,y dur
std::vector<double> thetas;
Eigen::MatrixXd start_state; // start flat state (2, 3)
Eigen::MatrixXd final_state; // end flat state (2, 3)
double duration;
};
typedef std::vector<FlatTrajData> KinoTrajData;
}
#endif

28
trajectoryOpt/src/path_search/package.xml Normal file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<package>
<name>path_search</name>
<maintainer email="zhichaohan@zju.edu.cn">zhichaohan</maintainer>
<version>0.1.0</version>
<description> the path_search package</description>
<license>GPLV3</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>tools</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>tools</run_depend>
</package>

1071
trajectoryOpt/src/path_search/src/kino_astar.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

55
trajectoryOpt/src/plan_manage/CMakeLists.txt Normal file
View File

@@ -0,0 +1,55 @@
cmake_minimum_required(VERSION 2.8.3)
project(plan_manage)
set(CMAKE_VERBOSE_MAKEFILE "true")
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -fPIC")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -Wall" )
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
visualization_msgs
std_msgs
tools
path_search
cv_bridge
decomp_ros_utils
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
#if this catkin packge's header is used by other packages, use catkin_package to
#declare the include directories of this package.
catkin_package(
INCLUDE_DIRS include
)
#singua:plan_manage plan_manage_benchmark
add_library(plan_manage
src/plan_manage.cpp
)
target_link_libraries(plan_manage
kino_astar
${catkin_LIBRARIES}
)
add_executable(plan_node
src/plan_node.cpp
)
target_link_libraries(plan_node
plan_manage
${catkin_LIBRARIES}
${PCL_LIBRARIES}
)

77
trajectoryOpt/src/plan_manage/config/planning copy.yaml Normal file
View File

@@ -0,0 +1,77 @@
# trajectory Optimizer
mini_T: 0.05
wei_time_: 100.0
kmax: 1.67 #2.0+0.1 1.5
kdotmax: 10000.0
cons_eps_: 0.1
vmax: 2.0
latAccmax: 1.5
lonAccmax: 1.5
accRatemax: 1.5
phimax: 0.785
omegamax: 300000.14 #0.5-0.02
traj_res: 32
past: 20
rho: 1.0
rho_max: 1000.0
gamma: 1.0
mem_size: 256
g_epsilon: 1.0e-3
min_step: 1.0e-32
delta: 1.0e-4
max_iterations: 1000
amlMaxIter: 100
penaWei: 1000.0
esdfWei: 10000.0
pieceTime: 0.7
miniS: 0.1
safeMargin: 0.3 #0.15+0.05
conpts: [[0.15, 0.0], [0.45, -0.00]]
#
#map
mapRes: 0.1
mapX: 20.0
mapY: 20.0
expandSize: 0
wheel_base: 0.6
horizon_: 50.0
#res res res
# yaw_resolution_: 0.4
# steer_res: 0.5
# allocate_num_: 1000000
yaw_resolution_: 0.2
steer_res: 0.25
allocate_num_: 2000000
#800000
#
# yaw_resolution_: 0.785
# steer_res: 0.5
# yaw_resolution_: 1.57
# steer_res: 0.5
lambda_heu_: 1.0
#0:48134 1:21376 2:19494 3:5794 4:4041 5:3064
check_num_: 3
step_arc: 0.3
checkl: 0.1
max_seach_time: 1000.0
non_siguav: 0.0
backW: 2.0
useDP: true
isdebug: false
isfixGear: false
isVis: false
enable_shot: false

78
trajectoryOpt/src/plan_manage/config/planning.yaml Normal file
View File

@@ -0,0 +1,78 @@
# trajectory Optimizer
mini_T: 0.05
wei_time_: 100.0
kmax: 1.67 #2.0+0.1 1.5
kdotmax: 10000.0
cons_eps_: 0.1
vmax: 2.0
latAccmax: 1.5
lonAccmax: 1.5
accRatemax: 1.5
phimax: 0.785
omegamax: 0.35 #0.5-0.02
traj_res: 8
past: 1
rho: 1.0
rho_max: 1000.0
gamma: 1.0
mem_size: 256
g_epsilon: 1.0e-3
min_step: 1.0e-32
delta: 1.0e-3
max_iterations: 1000
amlMaxIter: 100
penaWei: 1000.0
esdfWei: 10000.0
pieceTime: 0.8
miniS: 0.1
# safeMargin: 0.25 #0.15+0.05
# conpts: [[0.15, 0.0], [0.3, -0.00]]
safeMargin: 0.2 #0.15+0.05
conpts: [[0.15, 0.0], [0.3, -0.00]]
#
#map
mapRes: 0.1
mapX: 20.0
mapY: 20.0
expandSize: 0
wheel_base: 0.6
horizon_: 50.0
#res res res
yaw_resolution_: 3.15
lambda_heu_: 1.0
steer_res: 0.5
allocate_num_: 1000000
# datageneration
# yaw_resolution_: 0.2
# steer_res: 0.25
# allocate_num_: 2000000
#800000
#
# yaw_resolution_: 0.785
# steer_res: 0.5
# yaw_resolution_: 1.57
# steer_res: 0.5
#0:48134 1:21376 2:19494 3:5794 4:4041 5:3064
check_num_: 3
step_arc: 0.3
checkl: 0.1
max_seach_time: 1000.0
non_siguav: 0.0
backW: 2.0
useDP: true
isdebug: false
isfixGear: false
isVis: false
enable_shot: true

67
trajectoryOpt/src/plan_manage/config/planning_v1.yaml Normal file
View File

@@ -0,0 +1,67 @@
# trajectory Optimizer
mini_T: 0.05
wei_time_: 100.0
#wheelbase = 0.5
kmax: 2.0 #2.0+0.1 1.5
kdotmax: 1.0
cons_eps_: 0.1
vmax: 2.0
latAccmax: 1.5
lonAccmax: 1.5
accRatemax: 1.5
phimax: 0.785
omegamax: 0.4 #0.5-0.02
traj_res: 16
past: 0
rho: 1.0
rho_max: 1000.0
gamma: 1.0
mem_size: 256
g_epsilon: 1.0
min_step: 1.0e-32
delta: 1.0e-1
max_iterations: 1000
amlMaxIter: 20
useDP: false
penaWei: 5000.0
esdfWei: 10000.0
pieceTime: 1.2
miniS: 0.1
safeMargin: 0.1 #0.15+0.05
conpts: [[0.0, 0.0], [0.2, -0.00]]
non_siguav: 1.0
#map
mapRes: 0.1
mapX: 20.0
mapY: 20.0
expandSize: 0
wheel_base: 0.6
horizon_: 50.0
yaw_resolution_: 3.15
lambda_heu_: 1.0
#0:48134 1:21376 2:19494 3:5794 4:4041 5:3064
allocate_num_: 1000000
check_num_: 8
step_arc: 0.3
checkl: 0.15
max_seach_time: 20.0
backW: 1.0
isdebug: false
isfixGear: true
isVis: false
enable_shot: false

67
trajectoryOpt/src/plan_manage/config/planning_v2.yaml Normal file
View File

@@ -0,0 +1,67 @@
# trajectory Optimizer
mini_T: 0.05
wei_time_: 100.0
#wheelbase = 0.5
kmax: 2.0 #2.0+0.1 1.5
kdotmax: 1.0
cons_eps_: 0.1
vmax: 2.0
latAccmax: 1.5
lonAccmax: 1.5
accRatemax: 1.5
phimax: 0.785
omegamax: 0.4 #0.5-0.02
traj_res: 8
past: 0
rho: 1.0
rho_max: 1000.0
gamma: 1.0
mem_size: 256
g_epsilon: 1.0
min_step: 1.0e-32
delta: 1.0e-1
max_iterations: 1000
amlMaxIter: 20
useDP: false
penaWei: 1000.0
esdfWei: 10000.0
pieceTime: 0.8
miniS: 0.1
safeMargin: 0.1 #0.15+0.05
conpts: [[0.0, 0.0], [0.2, -0.00]]
non_siguav: 0.1
#map
mapRes: 0.1
mapX: 20.0
mapY: 20.0
expandSize: 0
wheel_base: 0.6
horizon_: 50.0
yaw_resolution_: 3.15
lambda_heu_: 1.0
#0:48134 1:21376 2:19494 3:5794 4:4041 5:3064
allocate_num_: 1000000
check_num_: 8
step_arc: 0.3
checkl: 0.15
max_seach_time: 20.0
backW: 1.0
isdebug: false
isfixGear: true
isVis: false
enable_shot: false

67
trajectoryOpt/src/plan_manage/config/planning_v3.yaml Normal file
View File

@@ -0,0 +1,67 @@
# trajectory Optimizer
mini_T: 0.05
wei_time_: 100.0
#wheelbase = 0.5
kmax: 1.67 #2.0+0.1 1.5
kdotmax: 1.0
cons_eps_: 0.1
vmax: 2.0
latAccmax: 1.5
lonAccmax: 1.5
accRatemax: 1.5
phimax: 0.785
omegamax: 0.35 #0.4-0.05
traj_res: 8
past: 0
rho: 1.0
rho_max: 1000.0
gamma: 1.0
mem_size: 256
g_epsilon: 1.0e-8
min_step: 1.0e-32
delta: 1.0e-1
max_iterations: 1000
amlMaxIter: 20
useDP: false
penaWei: 10000.0
esdfWei: 10000.0
pieceTime: 0.8
miniS: 0.1
safeMargin: 0.2 #0.15+0.05
conpts: [[0.15, 0.0], [0.3, -0.00]]
non_siguav: 0.01
#map
mapRes: 0.1
mapX: 20.0
mapY: 20.0
expandSize: 0
wheel_base: 0.6
horizon_: 50.0
yaw_resolution_: 3.15
lambda_heu_: 1.0
steer_res: 0.5
#0:48134 1:21376 2:19494 3:5794 4:4041 5:3064
allocate_num_: 1000000
check_num_: 8
step_arc: 0.3
checkl: 0.15
max_seach_time: 20.0
backW: 1.0
isdebug: false
isfixGear: true
isVis: false
enable_shot: false

1497
trajectoryOpt/src/plan_manage/include/arcPlan/Polytraj.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

1517
trajectoryOpt/src/plan_manage/include/arcPlan/Trajopt_alm.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

1069
trajectoryOpt/src/plan_manage/include/arcPlan/Trajopt_penalty.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

129
trajectoryOpt/src/plan_manage/include/arcPlan/geoutils2d.hpp Normal file
View File

@@ -0,0 +1,129 @@
#ifndef GEOUTILS_HPP
#define GEOUTILS_HPP
#include "sdlp.hpp"
#include "quickhull.hpp"
#include <Eigen/Eigen>
#include <cfloat>
#include <cstdint>
#include <set>
#include <chrono>
namespace geoutils
{
// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
// The outter_normal is assumed to be NORMALIZED
inline bool findInterior(const Eigen::MatrixXd &hPoly,
Eigen::Vector3d &interior)
{
int m = hPoly.cols();
Eigen::MatrixXd A(m, 4);
Eigen::VectorXd b(m), c(4), x(4);
A.leftCols<3>() = hPoly.topRows<3>().transpose();
A.rightCols<1>().setConstant(1.0);
b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum().transpose();
c.setZero();
c(3) = -1.0;
double minmaxsd = sdlp::linprog(c, A, b, x);
interior = x.head<3>();
return minmaxsd < 0.0 && !std::isinf(minmaxsd);
}
struct filterLess
{
inline bool operator()(const Eigen::Vector3d &l,
const Eigen::Vector3d &r)
{
return l(0) < r(0) ||
(l(0) == r(0) &&
(l(1) < r(1) ||
(l(1) == r(1) &&
l(2) < r(2))));
}
};
inline void filterVs(const Eigen::MatrixXd &rV,
const double &epsilon,
Eigen::MatrixXd &fV)
{
double mag = std::max(fabs(rV.maxCoeff()), fabs(rV.minCoeff()));
double res = mag * std::max(fabs(epsilon) / mag, DBL_EPSILON);
std::set<Eigen::Vector3d, filterLess> filter;
fV = rV;
int offset = 0;
Eigen::Vector3d quanti;
for (int i = 0; i < rV.cols(); i++)
{
quanti = (rV.col(i) / res).array().round();
if (filter.find(quanti) == filter.end())
{
filter.insert(quanti);
fV.col(offset) = rV.col(i);
offset++;
}
}
fV = fV.leftCols(offset).eval();
return;
}
// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
// The outter_normal is assumed to be NORMALIZED
// proposed epsilon is 1.0e-6
inline void enumerateVs(const Eigen::MatrixXd &hPoly,
const Eigen::Vector3d &inner,
Eigen::MatrixXd &vPoly,
const double epsilon = 1.0e-6)
{
Eigen::RowVectorXd b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum() -
inner.transpose() * hPoly.topRows<3>();
Eigen::MatrixXd A = hPoly.topRows<3>().array().rowwise() / b.array();
quickhull::QuickHull<double> qh;
double qhullEps = std::min(epsilon, quickhull::defaultEps<double>());
// CCW is false because the normal in quickhull towards interior
const auto cvxHull = qh.getConvexHull(A.data(), A.cols(), false, true, qhullEps);
const auto &idBuffer = cvxHull.getIndexBuffer();
int hNum = idBuffer.size() / 3;
Eigen::MatrixXd rV(3, hNum);
Eigen::Vector3d normal, point, edge0, edge1;
for (int i = 0; i < hNum; i++)
{
point = A.col(idBuffer[3 * i + 1]);
edge0 = point - A.col(idBuffer[3 * i]);
edge1 = A.col(idBuffer[3 * i + 2]) - point;
normal = edge0.cross(edge1); //cross in CW gives an outter normal
rV.col(i) = normal / normal.dot(point);
}
filterVs(rV, epsilon, vPoly);
vPoly = (vPoly.array().colwise() + inner.array()).eval();
return;
}
// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
// The outter_normal is assumed to be NORMALIZED
// proposed epsilon is 1.0e-6
inline bool enumerateVs(const Eigen::MatrixXd &hPoly,
Eigen::MatrixXd &vPoly,
const double epsilon = 1.0e-6)
{
Eigen::Vector3d inner;
if (findInterior(hPoly, inner))
{
enumerateVs(hPoly, inner, vPoly, epsilon);
return true;
}
else
{
return false;
}
}
} // namespace geoutils
#endif

838
trajectoryOpt/src/plan_manage/include/arcPlan/lbfgs.hpp Normal file
View File

@@ -0,0 +1,838 @@
#ifndef LBFGS_HPP
#define LBFGS_HPP
#include <Eigen/Eigen>
#include <cmath>
#include <algorithm>
namespace lbfgs
{
// ----------------------- Data Type Part -----------------------
/**
* L-BFGS optimization parameters.
*/
struct lbfgs_parameter_t
{
/**
* The number of corrections to approximate the inverse hessian matrix.
* The L-BFGS routine stores the computation results of previous m
* iterations to approximate the inverse hessian matrix of the current
* iteration. This parameter controls the size of the limited memories
* (corrections). The default value is 8. Values less than 3 are
* not recommended. Large values will result in excessive computing time.
*/
int mem_size = 8;
/**
* Epsilon for grad convergence test. DO NOT USE IT in nonsmooth cases!
* Set it to 0.0 and use past-delta-based test for nonsmooth functions.
* This parameter determines the accuracy with which the solution is to
* be found. A minimization terminates when
* ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon,
* where ||.||_inf is the infinity norm. The default value is 1.0e-5.
* This should be greater than 1.0e-6 in practice because L-BFGS does
* not directly reduce first-order residual. It still needs the function
* value which can be corrupted by machine_prec when ||g|| is small.
*/
double g_epsilon = 1.0e-5;
/**
* Distance for delta-based convergence test.
* This parameter determines the distance, in iterations, to compute
* the rate of decrease of the cost function. If the value of this
* parameter is zero, the library does not perform the delta-based
* convergence test. The default value is 3.
*/
int past = 3;
/**
* Delta for convergence test.
* This parameter determines the minimum rate of decrease of the
* cost function. The library stops iterations when the following
* condition is met:
* |f' - f| / max(1, |f|) < delta,
* where f' is the cost value of past iterations ago, and f is
* the cost value of the current iteration.
* The default value is 1.0e-6.
*/
double delta = 1.0e-6;
/**
* The maximum number of iterations.
* The lbfgs_optimize() function terminates an minimization process with
* ::LBFGSERR_MAXIMUMITERATION status code when the iteration count
* exceedes this parameter. Setting this parameter to zero continues an
* minimization process until a convergence or error. The default value
* is 0.
*/
int max_iterations = 0;
/**
* The maximum number of trials for the line search.
* This parameter controls the number of function and gradients evaluations
* per iteration for the line search routine. The default value is 64.
*/
int max_linesearch = 64;
/**
* The minimum step of the line search routine.
* The default value is 1.0e-20. This value need not be modified unless
* the exponents are too large for the machine being used, or unless the
* problem is extremely badly scaled (in which case the exponents should
* be increased).
*/
double min_step = 1.0e-20;
/**
* The maximum step of the line search.
* The default value is 1.0e+20. This value need not be modified unless
* the exponents are too large for the machine being used, or unless the
* problem is extremely badly scaled (in which case the exponents should
* be increased).
*/
double max_step = 1.0e+20;
/**
* A parameter to control the accuracy of the line search routine.
* The default value is 1.0e-4. This parameter should be greater
* than zero and smaller than 1.0.
*/
double f_dec_coeff = 1.0e-4;
/**
* A parameter to control the accuracy of the line search routine.
* The default value is 0.9. If the function and gradient
* evaluations are inexpensive with respect to the cost of the
* iteration (which is sometimes the case when solving very large
* problems) it may be advantageous to set this parameter to a small
* value. A typical small value is 0.1. This parameter should be
* greater than the f_dec_coeff parameter and smaller than 1.0.
*/
double s_curv_coeff = 0.9;
/**
* A parameter to ensure the global convergence for nonconvex functions.
* The default value is 1.0e-6. The parameter performs the so called
* cautious update for L-BFGS, especially when the convergence is
* not sufficient. The parameter must be positive but might as well
* be less than 1.0e-3 in practice.
*/
double cautious_factor = 1.0e-6;
/**
* The machine precision for floating-point values. The default is 1.0e-16.
* This parameter must be a positive value set by a client program to
* estimate the machine precision.
*/
double machine_prec = 1.0e-16;
};
/**
* Return values of lbfgs_optimize().
* Roughly speaking, a negative value indicates an error.
*/
enum
{
/** L-BFGS reaches convergence. */
LBFGS_CONVERGENCE = 0,
/** L-BFGS satisfies stopping criteria. */
LBFGS_STOP,
/** The iteration has been canceled by the monitor callback. */
LBFGS_CANCELED,
/** Unknown error. */
LBFGSERR_UNKNOWNERROR = -1024,
/** Invalid number of variables specified. */
LBFGSERR_INVALID_N,
/** Invalid parameter lbfgs_parameter_t::mem_size specified. */
LBFGSERR_INVALID_MEMSIZE,
/** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */
LBFGSERR_INVALID_GEPSILON,
/** Invalid parameter lbfgs_parameter_t::past specified. */
LBFGSERR_INVALID_TESTPERIOD,
/** Invalid parameter lbfgs_parameter_t::delta specified. */
LBFGSERR_INVALID_DELTA,
/** Invalid parameter lbfgs_parameter_t::min_step specified. */
LBFGSERR_INVALID_MINSTEP,
/** Invalid parameter lbfgs_parameter_t::max_step specified. */
LBFGSERR_INVALID_MAXSTEP,
/** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */
LBFGSERR_INVALID_FDECCOEFF,
/** Invalid parameter lbfgs_parameter_t::s_curv_coeff specified. */
LBFGSERR_INVALID_SCURVCOEFF,
/** Invalid parameter lbfgs_parameter_t::machine_prec specified. */
LBFGSERR_INVALID_MACHINEPREC,
/** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */
LBFGSERR_INVALID_MAXLINESEARCH,
/** The function value became NaN or Inf. */
LBFGSERR_INVALID_FUNCVAL,
/** The line-search step became smaller than lbfgs_parameter_t::min_step. */
LBFGSERR_MINIMUMSTEP,
/** The line-search step became larger than lbfgs_parameter_t::max_step. */
LBFGSERR_MAXIMUMSTEP,
/** Line search reaches the maximum, assumptions not satisfied or precision not achievable.*/
LBFGSERR_MAXIMUMLINESEARCH,
/** The algorithm routine reaches the maximum number of iterations. */
LBFGSERR_MAXIMUMITERATION,
/** Relative search interval width is at least lbfgs_parameter_t::machine_prec. */
LBFGSERR_WIDTHTOOSMALL,
/** A logic error (negative line-search step) occurred. */
LBFGSERR_INVALIDPARAMETERS,
/** The current search direction increases the cost function value. */
LBFGSERR_INCREASEGRADIENT,
};
/**
* Callback interface to provide cost function and gradient evaluations.
*
* The lbfgs_optimize() function call this function to obtain the values of cost
* function and its gradients when needed. A client program must implement
* this function to evaluate the values of the cost function and its
* gradients, given current values of variables.
*
* @param instance The user data sent for lbfgs_optimize() function by the client.
* @param x The current values of variables.
* @param g The gradient vector. The callback function must compute
* the gradient values for the current variables.
* @retval double The value of the cost function for the current variables.
*/
typedef double (*lbfgs_evaluate_t)(void *instance,
const Eigen::VectorXd &x,
Eigen::VectorXd &g);
/**
* Callback interface to provide an upper bound at the beginning of the current line search.
*
* The lbfgs_optimize() function call this function to obtain the values of the
* upperbound of the stepsize to search in, provided with the beginning values of
* variables before the line search, and the current step vector (can be descent direction).
* A client program can implement this function for more efficient linesearch. Any step
* larger than this bound should not be considered. For example, it has a very large or even
* inf function value. Note that the function value at the provided bound should be FINITE!
* If it is not used, just set it nullptr.
*
* @param instance The user data sent for lbfgs_optimize() function by the client.
* @param xp The values of variables before current line search.
* @param d The step vector. It can be the descent direction.
* @retval double The upperboud of the step in current line search routine,
* such that (stpbound * d) is the maximum reasonable step.
*/
typedef double (*lbfgs_stepbound_t)(void *instance,
const Eigen::VectorXd &xp,
const Eigen::VectorXd &d);
/**
* Callback interface to monitor the progress of the minimization process.
*
* The lbfgs_optimize() function call this function for each iteration. Implementing
* this function, a client program can store or display the current progress
* of the minimization process. If it is not used, just set it nullptr.
*
* @param instance The user data sent for lbfgs_optimize() function by the client.
* @param x The current values of variables.
* @param g The current gradient values of variables.
* @param fx The current value of the cost function.
* @param step The line-search step used for this iteration.
* @param k The iteration count.
* @param ls The number of evaluations called for this iteration.
* @retval int Zero to continue the minimization process. Returning a
* non-zero value will cancel the minimization process.
*/
typedef int (*lbfgs_progress_t)(void *instance,
const Eigen::VectorXd &x,
const Eigen::VectorXd &g,
const double fx,
const double step,
const int k,
const int ls);
/**
* Callback data struct
*/
struct callback_data_t
{
void *instance = nullptr;
lbfgs_evaluate_t proc_evaluate = nullptr;
lbfgs_stepbound_t proc_stepbound = nullptr;
lbfgs_progress_t proc_progress = nullptr;
};
// ----------------------- L-BFGS Part -----------------------
/**
* Line search method for smooth or nonsmooth functions.
* This function performs line search to find a point that satisfy
* both the Armijo condition and the weak Wolfe condition. It is
* as robust as the backtracking line search but further applies
* to continuous and piecewise smooth functions where the strong
* Wolfe condition usually does not hold.
*
* @see
* Adrian S. Lewis and Michael L. Overton. Nonsmooth optimization
* via quasi-Newton methods. Mathematical Programming, Vol 141,
* No 1, pp. 135-163, 2013.
*/
inline int line_search_lewisoverton(Eigen::VectorXd &x,
double &f,
Eigen::VectorXd &g,
double &stp,
const Eigen::VectorXd &s,
const Eigen::VectorXd &xp,
const Eigen::VectorXd &gp,
const double stpmin,
const double stpmax,
const callback_data_t &cd,
const lbfgs_parameter_t &param)
{
int count = 0;
bool brackt = false, touched = false;
double finit, dginit, dgtest, dstest;
double mu = 0.0, nu = stpmax;
/* Check the input parameters for errors. */
if (!(stp > 0.0))
{
return LBFGSERR_INVALIDPARAMETERS;
}
/* Compute the initial gradient in the search direction. */
dginit = gp.dot(s);
/* Make sure that s points to a descent direction. */
if (0.0 < dginit)
{
return LBFGSERR_INCREASEGRADIENT;
}
/* The initial value of the cost function. */
finit = f;
dgtest = param.f_dec_coeff * dginit;
dstest = param.s_curv_coeff * dginit;
while (true)
{
x = xp + stp * s;
/* Evaluate the function and gradient values. */
f = cd.proc_evaluate(cd.instance, x, g);
++count;
/* Test for errors. */
if (std::isinf(f) || std::isnan(f))
{
return LBFGSERR_INVALID_FUNCVAL;
}
if(param.past > 0 && fabs(finit-f)/(fabs(finit)+1.0)<param.delta/param.past)
{
return count;
}
/* Check the Armijo condition. */
if (f > finit + stp * dgtest)
{
nu = stp;
brackt = true;
}
else
{
/* Check the weak Wolfe condition. */
if (g.dot(s) < dstest)
{
mu = stp;
}
else
{
return count;
}
}
if (param.max_linesearch <= count)
{
// std::cout<< fabs(dginit)/finit<<std::endl;
// std::cout << fabs(finit-f)/(fabs(finit)+1.0) << std::endl;
/* Maximum number of iteration. */
return LBFGSERR_MAXIMUMLINESEARCH;
}
if (brackt && (nu - mu) < param.machine_prec * nu)
{
/* Relative interval width is at least machine_prec. */
return LBFGSERR_WIDTHTOOSMALL;
}
if (brackt)
{
stp = 0.5 * (mu + nu);
}
else
{
stp *= 2.0;
}
if (stp < stpmin)
{
/* The step is the minimum value. */
return LBFGSERR_MINIMUMSTEP;
}
if (stp > stpmax)
{
if (touched)
{
/* The step is the maximum value. */
return LBFGSERR_MAXIMUMSTEP;
}
else
{
/* The maximum value should be tried once. */
touched = true;
stp = stpmax;
}
}
}
}
/**
* Start a L-BFGS optimization.
* Assumptions: 1. f(x) is either C2 or C0 but piecewise C2;
* 2. f(x) is lower bounded;
* 3. f(x) has bounded level sets;
* 4. g(x) is either the gradient or subgradient;
* 5. The gradient exists at the initial guess x0.
* A user must implement a function compatible with ::lbfgs_evaluate_t (evaluation
* callback) and pass the pointer to the callback function to lbfgs_optimize()
* arguments. Similarly, a user can implement a function compatible with
* ::lbfgs_stepbound_t to provide an external upper bound for stepsize, and
* ::lbfgs_progress_t (progress callback) to obtain the current progress
* (e.g., variables, function, and gradient, etc) and to cancel the iteration
* process if necessary. Implementation of the stepbound and the progress callback
* is optional: a user can pass nullptr if progress notification is not necessary.
*
*
* @param x The vector of decision variables.
* THE INITIAL GUESS x0 SHOULD BE SET BEFORE THE CALL!
* A client program can receive decision variables
* through this vector, at which the cost and its
* gradient are queried during minimization.
* @param f The ref to the variable that receives the final
* value of the cost function for the variables.
* @param proc_evaluate The callback function to provide function f(x) and
* gradient g(x) evaluations given a current values of
* variables x. A client program must implement a
* callback function compatible with lbfgs_evaluate_t
* and pass the pointer to the callback function.
* @param proc_stepbound The callback function to provide values of the
* upperbound of the stepsize to search in, provided
* with the beginning values of variables before the
* line search, and the current step vector (can be
* negative gradient). A client program can implement
* this function for more efficient linesearch. If it is
* not used, just set it nullptr.
* @param proc_progress The callback function to receive the progress
* (the number of iterations, the current value of
* the cost function) of the minimization
* process. This argument can be set to nullptr if
* a progress report is unnecessary.
* @param instance A user data pointer for client programs. The callback
* functions will receive the value of this argument.
* @param param The parameters for L-BFGS optimization.
* @retval int The status code. This function returns a nonnegative
* integer if the minimization process terminates without
* an error. A negative integer indicates an error.
*/
inline int lbfgs_optimize(Eigen::VectorXd &x,
double &f,
lbfgs_evaluate_t proc_evaluate,
lbfgs_stepbound_t proc_stepbound,
lbfgs_progress_t proc_progress,
void *instance,
const lbfgs_parameter_t &param)
{
int ret, i, j, k, ls, end, bound;
double step, step_min, step_max, fx, ys, yy;
double gnorm_inf, xnorm_inf, beta, rate, cau;
const int n = x.size();
const int m = param.mem_size;
/* Check the input parameters for errors. */
if (n <= 0)
{
return LBFGSERR_INVALID_N;
}
if (m <= 0)
{
return LBFGSERR_INVALID_MEMSIZE;
}
if (param.g_epsilon < 0.0)
{
return LBFGSERR_INVALID_GEPSILON;
}
if (param.past < 0)
{
return LBFGSERR_INVALID_TESTPERIOD;
}
if (param.delta < 0.0)
{
return LBFGSERR_INVALID_DELTA;
}
if (param.min_step < 0.0)
{
return LBFGSERR_INVALID_MINSTEP;
}
if (param.max_step < param.min_step)
{
return LBFGSERR_INVALID_MAXSTEP;
}
if (!(param.f_dec_coeff > 0.0 &&
param.f_dec_coeff < 1.0))
{
return LBFGSERR_INVALID_FDECCOEFF;
}
if (!(param.s_curv_coeff < 1.0 &&
param.s_curv_coeff > param.f_dec_coeff))
{
return LBFGSERR_INVALID_SCURVCOEFF;
}
if (!(param.machine_prec > 0.0))
{
return LBFGSERR_INVALID_MACHINEPREC;
}
if (param.max_linesearch <= 0)
{
return LBFGSERR_INVALID_MAXLINESEARCH;
}
/* Prepare intermediate variables. */
Eigen::VectorXd xp(n);
Eigen::VectorXd g(n);
Eigen::VectorXd gp(n);
Eigen::VectorXd d(n);
Eigen::VectorXd pf(std::max(1, param.past));
/* Initialize the limited memory. */
Eigen::VectorXd lm_alpha = Eigen::VectorXd::Zero(m);
Eigen::MatrixXd lm_s = Eigen::MatrixXd::Zero(n, m);
Eigen::MatrixXd lm_y = Eigen::MatrixXd::Zero(n, m);
Eigen::VectorXd lm_ys = Eigen::VectorXd::Zero(m);
/* Construct a callback data. */
callback_data_t cd;
cd.instance = instance;
cd.proc_evaluate = proc_evaluate;
cd.proc_stepbound = proc_stepbound;
cd.proc_progress = proc_progress;
/* Evaluate the function value and its gradient. */
fx = cd.proc_evaluate(cd.instance, x, g);
/* Store the initial value of the cost function. */
pf(0) = fx;
/*
Compute the direction;
we assume the initial hessian matrix H_0 as the identity matrix.
*/
d = -g;
/*
Make sure that the initial variables are not a stationary point.
*/
gnorm_inf = g.cwiseAbs().maxCoeff();
xnorm_inf = x.cwiseAbs().maxCoeff();
if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon)
{
/* The initial guess is already a stationary point. */
ret = LBFGS_CONVERGENCE;
}
else
{
/*
Compute the initial step:
*/
step = 1.0 / d.norm();
k = 1;
end = 0;
bound = 0;
while (true)
{
/* Store the current position and gradient vectors. */
xp = x;
gp = g;
/* If the step bound can be provied dynamically, then apply it. */
step_min = param.min_step;
step_max = param.max_step;
if (cd.proc_stepbound)
{
step_max = cd.proc_stepbound(cd.instance, xp, d);
step_max = step_max < param.max_step ? step_max : param.max_step;
step = step < step_max ? step : 0.5 * step_max;
}
/* Search for an optimal step. */
ls = line_search_lewisoverton(x, fx, g, step, d, xp, gp, step_min, step_max, cd, param);
// int idx = 1;
// step = 1.0;
// if(d.norm()>1.0){
// d = d.normalized().eval()*1.0;
// }
// while(1){
// // std::cout<<"step * d"<<(step * d).transpose()<<std::endl;
// x = xp + step * d;
// /* Evaluate the function and gradient values. */
// double f = cd.proc_evaluate(cd.instance, x, g);
// if(g.norm()>1.0){
// g = g.normalized().eval()*1.0;
// }
// std::cout<<"fx: "<<fx<<" f: "<<f<<"step: "<<step<<std::endl;
// if(f < fx || step <= step_min){
// fx = f;
// break;
// }
// step = 1.0 / (++idx);
// }
// ls = 1;
if (ls < 0)
{
/* Revert to the previous point. */
x = xp;
g = gp;
ret = ls;
break;
}
/* Report the progress. */
if (cd.proc_progress)
{
if (cd.proc_progress(cd.instance, x, g, fx, step, k, ls))
{
ret = LBFGS_CANCELED;
break;
}
}
/*
Convergence test.
The criterion is given by the following formula:
||g(x)||_inf / max(1, ||x||_inf) < g_epsilon
*/
gnorm_inf = g.cwiseAbs().maxCoeff();
xnorm_inf = x.cwiseAbs().maxCoeff();
if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon)
{
/* Convergence. */
ret = LBFGS_CONVERGENCE;
break;
}
/*
Test for stopping criterion.
The criterion is given by the following formula:
|f(past_x) - f(x)| / max(1, |f(x)|) < \delta.
*/
if (0 < param.past)
{
/* We don't test the stopping criterion while k < past. */
if (param.past <= k)
{
/* The stopping criterion. */
rate = std::fabs(pf(k % param.past) - fx) / std::max(1.0, std::fabs(fx));
if (rate < param.delta)
{
ret = LBFGS_STOP;
break;
}
}
/* Store the current value of the cost function. */
pf(k % param.past) = fx;
}
if (param.max_iterations != 0 && param.max_iterations <= k)
{
/* Maximum number of iterations. */
ret = LBFGSERR_MAXIMUMITERATION;
break;
}
/* Count the iteration number. */
++k;
/*
Update vectors s and y:
s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}.
y_{k+1} = g_{k+1} - g_{k}.
*/
lm_s.col(end) = x - xp;
lm_y.col(end) = g - gp;
/*
Compute scalars ys and yy:
ys = y^t \cdot s = 1 / \rho.
yy = y^t \cdot y.
Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor).
*/
ys = lm_y.col(end).dot(lm_s.col(end));
yy = lm_y.col(end).squaredNorm();
lm_ys(end) = ys;
/* Compute the negative of gradients. */
d = -g;
/*
Only cautious update is performed here as long as
(y^t \cdot s) / ||s_{k+1}||^2 > \epsilon * ||g_{k}||^\alpha,
where \epsilon is the cautious factor and a proposed value
for \alpha is 1.
This is not for enforcing the PD of the approxomated Hessian
since ys > 0 is already ensured by the weak Wolfe condition.
This is to ensure the global convergence as described in:
Dong-Hui Li and Masao Fukushima. On the global convergence of
the BFGS method for nonconvex unconstrained optimization problems.
SIAM Journal on Optimization, Vol 11, No 4, pp. 1054-1064, 2011.
*/
cau = lm_s.col(end).squaredNorm() * gp.norm() * param.cautious_factor;
if (ys > cau)
{
/*
Recursive formula to compute dir = -(H \cdot g).
This is described in page 779 of:
Jorge Nocedal.
Updating Quasi-Newton Matrices with Limited Storage.
Mathematics of Computation, Vol. 35, No. 151,
pp. 773--782, 1980.
*/
++bound;
bound = m < bound ? m : bound;
end = (end + 1) % m;
j = end;
for (i = 0; i < bound; ++i)
{
j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */
/* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */
lm_alpha(j) = lm_s.col(j).dot(d) / lm_ys(j);
/* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */
d += (-lm_alpha(j)) * lm_y.col(j);
}
d *= ys / yy;
for (i = 0; i < bound; ++i)
{
/* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamm_{i}. */
beta = lm_y.col(j).dot(d) / lm_ys(j);
/* \gamm_{i+1} = \gamm_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */
d += (lm_alpha(j) - beta) * lm_s.col(j);
j = (j + 1) % m; /* if (++j == m) j = 0; */
}
}
/* The search direction d is ready. We try step = 1 first. */
step = 1.0;
}
}
/* Return the final value of the cost function. */
f = fx;
return ret;
}
/**
* Get string description of an lbfgs_optimize() return code.
*
* @param err A value returned by lbfgs_optimize().
*/
inline const char *lbfgs_strerror(const int err)
{
switch (err)
{
case LBFGS_CONVERGENCE:
return "Success: reached convergence (g_epsilon).";
case LBFGS_STOP:
return "Success: met stopping criteria (past f decrease less than delta).";
case LBFGS_CANCELED:
return "The iteration has been canceled by the monitor callback.";
case LBFGSERR_UNKNOWNERROR:
return "Unknown error.";
case LBFGSERR_INVALID_N:
return "Invalid number of variables specified.";
case LBFGSERR_INVALID_MEMSIZE:
return "Invalid parameter lbfgs_parameter_t::mem_size specified.";
case LBFGSERR_INVALID_GEPSILON:
return "Invalid parameter lbfgs_parameter_t::g_epsilon specified.";
case LBFGSERR_INVALID_TESTPERIOD:
return "Invalid parameter lbfgs_parameter_t::past specified.";
case LBFGSERR_INVALID_DELTA:
return "Invalid parameter lbfgs_parameter_t::delta specified.";
case LBFGSERR_INVALID_MINSTEP:
return "Invalid parameter lbfgs_parameter_t::min_step specified.";
case LBFGSERR_INVALID_MAXSTEP:
return "Invalid parameter lbfgs_parameter_t::max_step specified.";
case LBFGSERR_INVALID_FDECCOEFF:
return "Invalid parameter lbfgs_parameter_t::f_dec_coeff specified.";
case LBFGSERR_INVALID_SCURVCOEFF:
return "Invalid parameter lbfgs_parameter_t::s_curv_coeff specified.";
case LBFGSERR_INVALID_MACHINEPREC:
return "Invalid parameter lbfgs_parameter_t::machine_prec specified.";
case LBFGSERR_INVALID_MAXLINESEARCH:
return "Invalid parameter lbfgs_parameter_t::max_linesearch specified.";
case LBFGSERR_INVALID_FUNCVAL:
return "The function value became NaN or Inf.";
case LBFGSERR_MINIMUMSTEP:
return "The line-search step became smaller than lbfgs_parameter_t::min_step.";
case LBFGSERR_MAXIMUMSTEP:
return "The line-search step became larger than lbfgs_parameter_t::max_step.";
case LBFGSERR_MAXIMUMLINESEARCH:
return "Line search reaches the maximum try number, assumptions not satisfied or precision not achievable.";
case LBFGSERR_MAXIMUMITERATION:
return "The algorithm routine reaches the maximum number of iterations.";
case LBFGSERR_WIDTHTOOSMALL:
return "Relative search interval width is at least lbfgs_parameter_t::machine_prec.";
case LBFGSERR_INVALIDPARAMETERS:
return "A logic error (negative line-search step) occurred.";
case LBFGSERR_INCREASEGRADIENT:
return "The current search direction increases the cost function value.";
default:
return "(unknown)";
}
}
} // namespace lbfgs
#endif

1697
trajectoryOpt/src/plan_manage/include/arcPlan/quickhull.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

1114
trajectoryOpt/src/plan_manage/include/arcPlan/root_finder.hpp Executable file
View File

File diff suppressed because it is too large Load Diff

801
trajectoryOpt/src/plan_manage/include/arcPlan/sdlp.hpp Normal file
View File

@@ -0,0 +1,801 @@
/*
* Copyright (c) 1990 Michael E. Hohmeyer,
* hohmeyer@icemcfd.com
* Permission is granted to modify and re-distribute this code in any manner
* as long as this notice is preserved. All standard disclaimers apply.
*
* R. Seidel's algorithm for solving LPs (linear programs.)
*/
/*
* Copyright (c) 2021 Zhepei Wang,
* wangzhepei@live.com
* 1. Bug fix in "move_to_front" function that "prev[m]" is illegally accessed
* while "prev" originally has only m ints. It is fixed by allocating a
* "prev" with m + 1 ints.
* 2. Add Eigen interface.
* Permission is granted to modify and re-distribute this code in any manner
* as long as this notice is preserved. All standard disclaimers apply.
*
* Reference: Seidel, R. (1991), "Small-dimensional linear programming and convex hulls made easy",
* Discrete & Computational Geometry 6 (1): 423–434, doi:10.1007/BF02574699
*/
#ifndef SDLP_HPP
#define SDLP_HPP
#include <Eigen/Eigen>
#include <cmath>
#include <random>
namespace sdlp
{
constexpr double prog_epsilon = 2.0e-16;
enum PROG_STATE
{
/* minimum attained */
MINIMUM = 0,
/* no feasible region */
INFEASIBLE,
/* unbounded solution */
UNBOUNDED,
/* only a vertex in the solution set */
AMBIGUOUS,
};
inline double dot2(const double a[2], const double b[2])
{
return a[0] * b[0] + a[1] * b[1];
}
inline double cross2(const double a[2], const double b[2])
{
return a[0] * b[1] - a[1] * b[0];
}
inline bool unit2(const double a[2], double b[2], double eps)
{
double size;
size = sqrt(a[0] * a[0] + a[1] * a[1]);
if (size < 2 * eps)
{
return true;
}
b[0] = a[0] / size;
b[1] = a[1] / size;
return false;
}
/* unitize a d+1 dimensional point */
inline bool lp_d_unit(int d, const double *a, double *b)
{
int i;
double size;
size = 0.0;
for (i = 0; i <= d; i++)
{
size += a[i] * a[i];
}
if (size < (d + 1) * prog_epsilon * prog_epsilon)
{
return true;
}
size = 1.0 / sqrt(size);
for (i = 0; i <= d; i++)
{
b[i] = a[i] * size;
}
return false;
}
/* optimize the objective function when there are no contraints */
inline int lp_no_constraints(int d, const double *n_vec, const double *d_vec, double *opt)
{
int i;
double n_dot_d, d_dot_d;
n_dot_d = 0.0;
d_dot_d = 0.0;
for (i = 0; i <= d; i++)
{
n_dot_d += n_vec[i] * d_vec[i];
d_dot_d += d_vec[i] * d_vec[i];
}
if (d_dot_d < prog_epsilon * prog_epsilon)
{
d_dot_d = 1.0;
n_dot_d = 0.0;
}
for (i = 0; i <= d; i++)
{
opt[i] = -n_vec[i] + d_vec[i] * n_dot_d / d_dot_d;
}
/* normalize the optimal point */
if (lp_d_unit(d, opt, opt))
{
opt[d] = 1.0;
return AMBIGUOUS;
}
else
{
return MINIMUM;
}
}
/* returns the index of the plane that is in i's place */
inline int move_to_front(int i, int *next, int *prev)
{
int previ;
if (i == 0 || i == next[0])
{
return i;
}
previ = prev[i];
/* remove i from it's current position */
next[prev[i]] = next[i];
prev[next[i]] = prev[i];
/* put i at the front */
next[i] = next[0];
prev[i] = 0;
prev[next[i]] = i;
next[0] = i;
return previ;
}
inline void lp_min_lin_rat(int degen,
const double cw_vec[2],
const double ccw_vec[2],
const double n_vec[2],
const double d_vec[2],
double opt[2])
{
double d_cw, d_ccw, n_cw, n_ccw;
/* linear rational function case */
d_cw = dot2(cw_vec, d_vec);
d_ccw = dot2(ccw_vec, d_vec);
n_cw = dot2(cw_vec, n_vec);
n_ccw = dot2(ccw_vec, n_vec);
if (degen)
{
/* if degenerate simply compare values */
if (n_cw / d_cw < n_ccw / d_ccw)
{
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
/* check that the clock-wise and counter clockwise bounds are not near a poles */
}
else if (fabs(d_cw) > 2.0 * prog_epsilon &&
fabs(d_ccw) > 2.0 * prog_epsilon)
{
/* the valid region does not contain a poles */
if (d_cw * d_ccw > 0.0)
{
/* find which end has the minimum value */
if (n_cw / d_cw < n_ccw / d_ccw)
{
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
}
else
{
/* the valid region does contain a poles */
if (d_cw > 0.0)
{
opt[0] = -d_vec[1];
opt[1] = d_vec[0];
}
else
{
opt[0] = d_vec[1];
opt[1] = -d_vec[0];
}
}
}
else if (fabs(d_cw) > 2.0 * prog_epsilon)
{
/* the counter clockwise bound is near a pole */
if (n_ccw * d_cw > 0.0)
{
/* counter clockwise bound is a positive pole */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
/* counter clockwise bound is a negative pole */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
}
else if (fabs(d_ccw) > 2.0 * prog_epsilon)
{
/* the clockwise bound is near a pole */
if (n_cw * d_ccw > 2 * prog_epsilon)
{
/* clockwise bound is at a positive pole */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
else
{
/* clockwise bound is at a negative pole */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
}
else
{
/* both bounds are near poles */
if (cross2(d_vec, n_vec) > 0.0)
{
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
}
}
inline int wedge(const double (*halves)[2],
int m,
int *next,
int *prev,
double cw_vec[2],
double ccw_vec[2],
int *degen)
{
int i;
double d_cw, d_ccw;
int offensive;
*degen = 0;
for (i = 0; i != m; i = next[i])
{
if (!unit2(halves[i], ccw_vec, prog_epsilon))
{
/* clock-wise */
cw_vec[0] = ccw_vec[1];
cw_vec[1] = -ccw_vec[0];
/* counter-clockwise */
ccw_vec[0] = -cw_vec[0];
ccw_vec[1] = -cw_vec[1];
break;
}
}
if (i == m)
{
return UNBOUNDED;
}
i = 0;
while (i != m)
{
offensive = 0;
d_cw = dot2(cw_vec, halves[i]);
d_ccw = dot2(ccw_vec, halves[i]);
if (d_ccw >= 2 * prog_epsilon)
{
if (d_cw <= -2 * prog_epsilon)
{
cw_vec[0] = halves[i][1];
cw_vec[1] = -halves[i][0];
unit2(cw_vec, cw_vec, prog_epsilon);
offensive = 1;
}
}
else if (d_cw >= 2 * prog_epsilon)
{
if (d_ccw <= -2 * prog_epsilon)
{
ccw_vec[0] = -halves[i][1];
ccw_vec[1] = halves[i][0];
unit2(ccw_vec, ccw_vec, prog_epsilon);
offensive = 1;
}
}
else if (d_ccw <= -2 * prog_epsilon && d_cw <= -2 * prog_epsilon)
{
return INFEASIBLE;
}
else if ((d_cw <= -2 * prog_epsilon) ||
(d_ccw <= -2 * prog_epsilon) ||
(cross2(cw_vec, halves[i]) < 0.0))
{
/* degenerate */
if (d_cw <= -2 * prog_epsilon)
{
unit2(ccw_vec, cw_vec, prog_epsilon);
}
else if (d_ccw <= -2 * prog_epsilon)
{
unit2(cw_vec, ccw_vec, prog_epsilon);
}
*degen = 1;
offensive = 1;
}
/* place this offensive plane in second place */
if (offensive)
{
i = move_to_front(i, next, prev);
}
i = next[i];
if (*degen)
{
break;
}
}
if (*degen)
{
while (i != m)
{
d_cw = dot2(cw_vec, halves[i]);
d_ccw = dot2(ccw_vec, halves[i]);
if (d_cw < -2 * prog_epsilon)
{
if (d_ccw < -2 * prog_epsilon)
{
return INFEASIBLE;
}
else
{
cw_vec[0] = ccw_vec[0];
cw_vec[1] = ccw_vec[1];
}
}
else if (d_ccw < -2 * prog_epsilon)
{
ccw_vec[0] = cw_vec[0];
ccw_vec[1] = cw_vec[1];
}
i = next[i];
}
}
return MINIMUM;
}
/*
* return the minimum on the projective line
*/
inline int lp_base_case(const double (*halves)[2], /* halves --- half lines */
int m, /* m --- terminal marker */
const double n_vec[2], /* n_vec --- numerator funciton */
const double d_vec[2], /* d_vec --- denominator function */
double opt[2], /* opt --- optimum */
int *next, /* next, prev --- double linked list of indices */
int *prev)
{
double cw_vec[2], ccw_vec[2];
int degen;
int status;
double ab;
/* find the feasible region of the line */
status = wedge(halves, m, next, prev, cw_vec, ccw_vec, &degen);
if (status == INFEASIBLE)
{
return status;
}
/* no non-trivial constraints one the plane: return the unconstrained optimum */
if (status == UNBOUNDED)
{
return lp_no_constraints(1, n_vec, d_vec, opt);
}
ab = fabs(cross2(n_vec, d_vec));
if (ab < 2 * prog_epsilon * prog_epsilon)
{
if (dot2(n_vec, n_vec) < 2 * prog_epsilon * prog_epsilon ||
dot2(d_vec, d_vec) > 2 * prog_epsilon * prog_epsilon)
{
/* numerator is zero or numerator and denominator are linearly dependent */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
status = AMBIGUOUS;
}
else
{
/* numerator is non-zero and denominator is zero minimize linear functional on circle */
if (!degen && cross2(cw_vec, n_vec) <= 0.0 &&
cross2(n_vec, ccw_vec) <= 0.0)
{
/* optimum is in interior of feasible region */
opt[0] = -n_vec[0];
opt[1] = -n_vec[1];
}
else if (dot2(n_vec, cw_vec) > dot2(n_vec, ccw_vec))
{
/* optimum is at counter-clockwise boundary */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
else
{
/* optimum is at clockwise boundary */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
status = MINIMUM;
}
}
else
{
/* niether numerator nor denominator is zero */
lp_min_lin_rat(degen, cw_vec, ccw_vec, n_vec, d_vec, opt);
status = MINIMUM;
}
return status;
}
/* find the largest coefficient in a plane */
inline void findimax(const double *pln, int idim, int *imax)
{
double rmax;
int i;
*imax = 0;
rmax = fabs(pln[0]);
for (i = 1; i <= idim; i++)
{
double ab;
ab = fabs(pln[i]);
if (ab > rmax)
{
*imax = i;
rmax = ab;
}
}
}
inline void vector_up(const double *equation, int ivar, int idim,
const double *low_vector, double *vector)
{
int i;
vector[ivar] = 0.0;
for (i = 0; i < ivar; i++)
{
vector[i] = low_vector[i];
vector[ivar] -= equation[i] * low_vector[i];
}
for (i = ivar + 1; i <= idim; i++)
{
vector[i] = low_vector[i - 1];
vector[ivar] -= equation[i] * low_vector[i - 1];
}
vector[ivar] /= equation[ivar];
}
inline void vector_down(const double *elim_eqn, int ivar, int idim,
const double *old_vec, double *new_vec)
{
int i;
double fac, ve, ee;
ve = 0.0;
ee = 0.0;
for (i = 0; i <= idim; i++)
{
ve += old_vec[i] * elim_eqn[i];
ee += elim_eqn[i] * elim_eqn[i];
}
fac = ve / ee;
for (i = 0; i < ivar; i++)
{
new_vec[i] = old_vec[i] - elim_eqn[i] * fac;
}
for (i = ivar + 1; i <= idim; i++)
{
new_vec[i - 1] = old_vec[i] - elim_eqn[i] * fac;
}
}
inline void plane_down(const double *elim_eqn, int ivar, int idim,
const double *old_plane, double *new_plane)
{
double crit;
int i;
crit = old_plane[ivar] / elim_eqn[ivar];
for (i = 0; i < ivar; i++)
{
new_plane[i] = old_plane[i] - elim_eqn[i] * crit;
}
for (i = ivar + 1; i <= idim; i++)
{
new_plane[i - 1] = old_plane[i] - elim_eqn[i] * crit;
}
}
inline int linfracprog(const double *halves, /* halves --- half spaces */
int istart, /* istart --- should be zero unless doing incremental algorithm */
int m, /* m --- terminal marker */
const double *n_vec, /* n_vec --- numerator vector */
const double *d_vec, /* d_vec --- denominator vector */
int d, /* d --- projective dimension */
double *opt, /* opt --- optimum */
double *work, /* work --- work space (see below) */
int *next, /* next --- array of indices into halves */
int *prev, /* prev --- array of indices into halves */
int max_size) /* max_size --- size of halves array */
/*
**
** half-spaces are in the form
** halves[i][0]*x[0] + halves[i][1]*x[1] +
** ... + halves[i][d-1]*x[d-1] + halves[i][d]*x[d] >= 0
**
** coefficients should be normalized
** half-spaces should be in random order
** the order of the half spaces is 0, next[0] next[next[0]] ...
** and prev[next[i]] = i
**
** halves: (max_size)x(d+1)
**
** the optimum has been computed for the half spaces
** 0 , next[0], next[next[0]] , ... , prev[istart]
** the next plane that needs to be tested is istart
**
** m is the index of the first plane that is NOT on the list
** i.e. m is the terminal marker for the linked list.
**
** the objective function is dot(x,nvec)/dot(x,dvec)
** if you want the program to solve standard d dimensional linear programming
** problems then n_vec = ( x0, x1, x2, ..., xd-1, 0)
** and d_vec = ( 0, 0, 0, ..., 0, 1)
** and halves[0] = (0, 0, ... , 1)
**
** work points to (max_size+3)*(d+2)*(d-1)/2 double space
*/
{
int status;
int i, j, imax;
double *new_opt, *new_n_vec, *new_d_vec, *new_halves, *new_work;
const double *plane_i;
double val;
if (d == 1 && m != 0)
{
return lp_base_case((const double(*)[2])halves, m, n_vec,
d_vec, opt, next, prev);
}
else
{
int d_vec_zero;
val = 0.0;
for (j = 0; j <= d; j++)
{
val += d_vec[j] * d_vec[j];
}
d_vec_zero = (val < (d + 1) * prog_epsilon * prog_epsilon);
/* find the unconstrained minimum */
if (!istart)
{
status = lp_no_constraints(d, n_vec, d_vec, opt);
}
else
{
status = MINIMUM;
}
if (m == 0)
{
return status;
}
/* allocate memory for next level of recursion */
new_opt = work;
new_n_vec = new_opt + d;
new_d_vec = new_n_vec + d;
new_halves = new_d_vec + d;
new_work = new_halves + max_size * d;
for (i = istart; i != m; i = next[i])
{
/* if the optimum is not in half space i then project the problem onto that plane */
plane_i = halves + i * (d + 1);
/* determine if the optimum is on the correct side of plane_i */
val = 0.0;
for (j = 0; j <= d; j++)
{
val += opt[j] * plane_i[j];
}
if (val < -(d + 1) * prog_epsilon)
{
/* find the largest of the coefficients to eliminate */
findimax(plane_i, d, &imax);
/* eliminate that variable */
if (i != 0)
{
double fac;
fac = 1.0 / plane_i[imax];
for (j = 0; j != i; j = next[j])
{
const double *old_plane;
double *new_plane;
int k;
double crit;
old_plane = halves + j * (d + 1);
new_plane = new_halves + j * d;
crit = old_plane[imax] * fac;
for (k = 0; k < imax; k++)
{
new_plane[k] = old_plane[k] - plane_i[k] * crit;
}
for (k = imax + 1; k <= d; k++)
{
new_plane[k - 1] = old_plane[k] - plane_i[k] * crit;
}
}
}
/* project the objective function to lower dimension */
if (d_vec_zero)
{
vector_down(plane_i, imax, d, n_vec, new_n_vec);
for (j = 0; j < d; j++)
{
new_d_vec[j] = 0.0;
}
}
else
{
plane_down(plane_i, imax, d, n_vec, new_n_vec);
plane_down(plane_i, imax, d, d_vec, new_d_vec);
}
/* solve sub problem */
status = linfracprog(new_halves, 0, i, new_n_vec,
new_d_vec, d - 1, new_opt, new_work, next, prev, max_size);
/* back substitution */
if (status != INFEASIBLE)
{
vector_up(plane_i, imax, d, new_opt, opt);
/* in line code for unit */
double size;
size = 0.0;
for (j = 0; j <= d; j++)
size += opt[j] * opt[j];
size = 1.0 / sqrt(size);
for (j = 0; j <= d; j++)
opt[j] *= size;
}
else
{
return status;
}
/* place this offensive plane in second place */
i = move_to_front(i, next, prev);
}
}
return status;
}
}
inline void rand_permutation(int n, int *p)
{
typedef std::uniform_int_distribution<int> rand_int;
typedef rand_int::param_type rand_range;
static std::mt19937_64 gen;
static rand_int rdi(0, 1);
int i, j, t;
for (i = 0; i < n; i++)
{
p[i] = i;
}
for (i = 0; i < n; i++)
{
rdi.param(rand_range(0, n - i - 1));
j = rdi(gen) + i;
t = p[j];
p[j] = p[i];
p[i] = t;
}
}
inline double linprog(const Eigen::VectorXd &c,
const Eigen::MatrixXd &A,
const Eigen::VectorXd &b,
Eigen::VectorXd &x)
/*
** min cTx, s.t. Ax<=b
** dim(x) << dim(b)
*/
{
int d = c.size();
int m = b.size() + 1;
x = Eigen::VectorXd::Zero(d);
double minimum = INFINITY;
int *perm, *next, *prev;
double *halves, *n_vec, *d_vec, *work, *opt;
int i, status = sdlp::AMBIGUOUS;
perm = (int *)malloc((m - 1) * sizeof(int));
next = (int *)malloc(m * sizeof(int));
/* original allocated size is m, here changed by m + 1 for legal tail accessing */
prev = (int *)malloc((m + 1) * sizeof(int));
halves = (double *)malloc(m * (d + 1) * sizeof(double));
n_vec = (double *)malloc((d + 1) * sizeof(double));
d_vec = (double *)malloc((d + 1) * sizeof(double));
work = (double *)malloc((m + 3) * (d + 2) * (d - 1) / 2 * sizeof(double));
opt = (double *)malloc((d + 1) * sizeof(double));
Eigen::Map<Eigen::MatrixXd> Af(halves, d + 1, m);
Eigen::Map<Eigen::VectorXd> nv(n_vec, d + 1);
Eigen::Map<Eigen::VectorXd> dv(d_vec, d + 1);
Eigen::Map<Eigen::VectorXd> xf(opt, d + 1);
Af.col(0).setZero();
Af(d, 0) = 1.0;
Af.topRightCorner(d, m - 1) = -A.transpose();
Af.bottomRightCorner(1, m - 1) = b.transpose();
nv.head(d) = c;
nv(d) = 0.0;
dv.setZero();
dv(d) = 1.0;
/* randomize the input planes */
rand_permutation(m - 1, perm);
/* previous to 0 is actually never used */
prev[0] = 0;
/* link the zero position in at the beginning */
next[0] = perm[0] + 1;
prev[perm[0] + 1] = 0;
/* link the other planes */
for (i = 0; i < m - 2; i++)
{
next[perm[i] + 1] = perm[i + 1] + 1;
prev[perm[i + 1] + 1] = perm[i] + 1;
}
/* flag the last plane */
next[perm[m - 2] + 1] = m;
status = sdlp::linfracprog(halves, 0, m, n_vec, d_vec,
d, opt, work, next, prev, m);
/* handle states for linprog whose definitions differ from linfracprog */
if (status != sdlp::INFEASIBLE)
{
if (xf(d) != 0.0 && status != sdlp::UNBOUNDED)
{
x = xf.head(d) / xf(d);
minimum = c.dot(x);
}
if (xf(d) == 0.0 || status == sdlp::UNBOUNDED)
{
x = xf.head(d);
minimum = -INFINITY;
}
}
free(perm);
free(next);
free(prev);
free(halves);
free(n_vec);
free(d_vec);
free(work);
free(opt);
return minimum;
}
} // namespace sdlp
#endif

129
trajectoryOpt/src/plan_manage/include/difPlan/geoutils2d.hpp Normal file
View File

@@ -0,0 +1,129 @@
#ifndef GEOUTILS_HPP
#define GEOUTILS_HPP
#include "sdlp.hpp"
#include "quickhull.hpp"
#include <Eigen/Eigen>
#include <cfloat>
#include <cstdint>
#include <set>
#include <chrono>
namespace geoutils
{
// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
// The outter_normal is assumed to be NORMALIZED
inline bool findInterior(const Eigen::MatrixXd &hPoly,
Eigen::Vector3d &interior)
{
int m = hPoly.cols();
Eigen::MatrixXd A(m, 4);
Eigen::VectorXd b(m), c(4), x(4);
A.leftCols<3>() = hPoly.topRows<3>().transpose();
A.rightCols<1>().setConstant(1.0);
b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum().transpose();
c.setZero();
c(3) = -1.0;
double minmaxsd = sdlp::linprog(c, A, b, x);
interior = x.head<3>();
return minmaxsd < 0.0 && !std::isinf(minmaxsd);
}
struct filterLess
{
inline bool operator()(const Eigen::Vector3d &l,
const Eigen::Vector3d &r)
{
return l(0) < r(0) ||
(l(0) == r(0) &&
(l(1) < r(1) ||
(l(1) == r(1) &&
l(2) < r(2))));
}
};
inline void filterVs(const Eigen::MatrixXd &rV,
const double &epsilon,
Eigen::MatrixXd &fV)
{
double mag = std::max(fabs(rV.maxCoeff()), fabs(rV.minCoeff()));
double res = mag * std::max(fabs(epsilon) / mag, DBL_EPSILON);
std::set<Eigen::Vector3d, filterLess> filter;
fV = rV;
int offset = 0;
Eigen::Vector3d quanti;
for (int i = 0; i < rV.cols(); i++)
{
quanti = (rV.col(i) / res).array().round();
if (filter.find(quanti) == filter.end())
{
filter.insert(quanti);
fV.col(offset) = rV.col(i);
offset++;
}
}
fV = fV.leftCols(offset).eval();
return;
}
// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
// The outter_normal is assumed to be NORMALIZED
// proposed epsilon is 1.0e-6
inline void enumerateVs(const Eigen::MatrixXd &hPoly,
const Eigen::Vector3d &inner,
Eigen::MatrixXd &vPoly,
const double epsilon = 1.0e-6)
{
Eigen::RowVectorXd b = hPoly.topRows<3>().cwiseProduct(hPoly.bottomRows<3>()).colwise().sum() -
inner.transpose() * hPoly.topRows<3>();
Eigen::MatrixXd A = hPoly.topRows<3>().array().rowwise() / b.array();
quickhull::QuickHull<double> qh;
double qhullEps = std::min(epsilon, quickhull::defaultEps<double>());
// CCW is false because the normal in quickhull towards interior
const auto cvxHull = qh.getConvexHull(A.data(), A.cols(), false, true, qhullEps);
const auto &idBuffer = cvxHull.getIndexBuffer();
int hNum = idBuffer.size() / 3;
Eigen::MatrixXd rV(3, hNum);
Eigen::Vector3d normal, point, edge0, edge1;
for (int i = 0; i < hNum; i++)
{
point = A.col(idBuffer[3 * i + 1]);
edge0 = point - A.col(idBuffer[3 * i]);
edge1 = A.col(idBuffer[3 * i + 2]) - point;
normal = edge0.cross(edge1); //cross in CW gives an outter normal
rV.col(i) = normal / normal.dot(point);
}
filterVs(rV, epsilon, vPoly);
vPoly = (vPoly.array().colwise() + inner.array()).eval();
return;
}
// Each col of hPoly denotes a facet (outter_normal^T,point^T)^T
// The outter_normal is assumed to be NORMALIZED
// proposed epsilon is 1.0e-6
inline bool enumerateVs(const Eigen::MatrixXd &hPoly,
Eigen::MatrixXd &vPoly,
const double epsilon = 1.0e-6)
{
Eigen::Vector3d inner;
if (findInterior(hPoly, inner))
{
enumerateVs(hPoly, inner, vPoly, epsilon);
return true;
}
else
{
return false;
}
}
} // namespace geoutils
#endif

838
trajectoryOpt/src/plan_manage/include/difPlan/lbfgs.hpp Normal file
View File

@@ -0,0 +1,838 @@
#ifndef LBFGS_HPP
#define LBFGS_HPP
#include <Eigen/Eigen>
#include <cmath>
#include <algorithm>
namespace lbfgs
{
// ----------------------- Data Type Part -----------------------
/**
* L-BFGS optimization parameters.
*/
struct lbfgs_parameter_t
{
/**
* The number of corrections to approximate the inverse hessian matrix.
* The L-BFGS routine stores the computation results of previous m
* iterations to approximate the inverse hessian matrix of the current
* iteration. This parameter controls the size of the limited memories
* (corrections). The default value is 8. Values less than 3 are
* not recommended. Large values will result in excessive computing time.
*/
int mem_size = 8;
/**
* Epsilon for grad convergence test. DO NOT USE IT in nonsmooth cases!
* Set it to 0.0 and use past-delta-based test for nonsmooth functions.
* This parameter determines the accuracy with which the solution is to
* be found. A minimization terminates when
* ||g(x)||_inf / max(1, ||x||_inf) < g_epsilon,
* where ||.||_inf is the infinity norm. The default value is 1.0e-5.
* This should be greater than 1.0e-6 in practice because L-BFGS does
* not directly reduce first-order residual. It still needs the function
* value which can be corrupted by machine_prec when ||g|| is small.
*/
double g_epsilon = 1.0e-5;
/**
* Distance for delta-based convergence test.
* This parameter determines the distance, in iterations, to compute
* the rate of decrease of the cost function. If the value of this
* parameter is zero, the library does not perform the delta-based
* convergence test. The default value is 3.
*/
int past = 3;
/**
* Delta for convergence test.
* This parameter determines the minimum rate of decrease of the
* cost function. The library stops iterations when the following
* condition is met:
* |f' - f| / max(1, |f|) < delta,
* where f' is the cost value of past iterations ago, and f is
* the cost value of the current iteration.
* The default value is 1.0e-6.
*/
double delta = 1.0e-6;
/**
* The maximum number of iterations.
* The lbfgs_optimize() function terminates an minimization process with
* ::LBFGSERR_MAXIMUMITERATION status code when the iteration count
* exceedes this parameter. Setting this parameter to zero continues an
* minimization process until a convergence or error. The default value
* is 0.
*/
int max_iterations = 0;
/**
* The maximum number of trials for the line search.
* This parameter controls the number of function and gradients evaluations
* per iteration for the line search routine. The default value is 64.
*/
int max_linesearch = 64;
/**
* The minimum step of the line search routine.
* The default value is 1.0e-20. This value need not be modified unless
* the exponents are too large for the machine being used, or unless the
* problem is extremely badly scaled (in which case the exponents should
* be increased).
*/
double min_step = 1.0e-20;
/**
* The maximum step of the line search.
* The default value is 1.0e+20. This value need not be modified unless
* the exponents are too large for the machine being used, or unless the
* problem is extremely badly scaled (in which case the exponents should
* be increased).
*/
double max_step = 1.0e+20;
/**
* A parameter to control the accuracy of the line search routine.
* The default value is 1.0e-4. This parameter should be greater
* than zero and smaller than 1.0.
*/
double f_dec_coeff = 1.0e-4;
/**
* A parameter to control the accuracy of the line search routine.
* The default value is 0.9. If the function and gradient
* evaluations are inexpensive with respect to the cost of the
* iteration (which is sometimes the case when solving very large
* problems) it may be advantageous to set this parameter to a small
* value. A typical small value is 0.1. This parameter should be
* greater than the f_dec_coeff parameter and smaller than 1.0.
*/
double s_curv_coeff = 0.9;
/**
* A parameter to ensure the global convergence for nonconvex functions.
* The default value is 1.0e-6. The parameter performs the so called
* cautious update for L-BFGS, especially when the convergence is
* not sufficient. The parameter must be positive but might as well
* be less than 1.0e-3 in practice.
*/
double cautious_factor = 1.0e-6;
/**
* The machine precision for floating-point values. The default is 1.0e-16.
* This parameter must be a positive value set by a client program to
* estimate the machine precision.
*/
double machine_prec = 1.0e-16;
};
/**
* Return values of lbfgs_optimize().
* Roughly speaking, a negative value indicates an error.
*/
enum
{
/** L-BFGS reaches convergence. */
LBFGS_CONVERGENCE = 0,
/** L-BFGS satisfies stopping criteria. */
LBFGS_STOP,
/** The iteration has been canceled by the monitor callback. */
LBFGS_CANCELED,
/** Unknown error. */
LBFGSERR_UNKNOWNERROR = -1024,
/** Invalid number of variables specified. */
LBFGSERR_INVALID_N,
/** Invalid parameter lbfgs_parameter_t::mem_size specified. */
LBFGSERR_INVALID_MEMSIZE,
/** Invalid parameter lbfgs_parameter_t::g_epsilon specified. */
LBFGSERR_INVALID_GEPSILON,
/** Invalid parameter lbfgs_parameter_t::past specified. */
LBFGSERR_INVALID_TESTPERIOD,
/** Invalid parameter lbfgs_parameter_t::delta specified. */
LBFGSERR_INVALID_DELTA,
/** Invalid parameter lbfgs_parameter_t::min_step specified. */
LBFGSERR_INVALID_MINSTEP,
/** Invalid parameter lbfgs_parameter_t::max_step specified. */
LBFGSERR_INVALID_MAXSTEP,
/** Invalid parameter lbfgs_parameter_t::f_dec_coeff specified. */
LBFGSERR_INVALID_FDECCOEFF,
/** Invalid parameter lbfgs_parameter_t::s_curv_coeff specified. */
LBFGSERR_INVALID_SCURVCOEFF,
/** Invalid parameter lbfgs_parameter_t::machine_prec specified. */
LBFGSERR_INVALID_MACHINEPREC,
/** Invalid parameter lbfgs_parameter_t::max_linesearch specified. */
LBFGSERR_INVALID_MAXLINESEARCH,
/** The function value became NaN or Inf. */
LBFGSERR_INVALID_FUNCVAL,
/** The line-search step became smaller than lbfgs_parameter_t::min_step. */
LBFGSERR_MINIMUMSTEP,
/** The line-search step became larger than lbfgs_parameter_t::max_step. */
LBFGSERR_MAXIMUMSTEP,
/** Line search reaches the maximum, assumptions not satisfied or precision not achievable.*/
LBFGSERR_MAXIMUMLINESEARCH,
/** The algorithm routine reaches the maximum number of iterations. */
LBFGSERR_MAXIMUMITERATION,
/** Relative search interval width is at least lbfgs_parameter_t::machine_prec. */
LBFGSERR_WIDTHTOOSMALL,
/** A logic error (negative line-search step) occurred. */
LBFGSERR_INVALIDPARAMETERS,
/** The current search direction increases the cost function value. */
LBFGSERR_INCREASEGRADIENT,
};
/**
* Callback interface to provide cost function and gradient evaluations.
*
* The lbfgs_optimize() function call this function to obtain the values of cost
* function and its gradients when needed. A client program must implement
* this function to evaluate the values of the cost function and its
* gradients, given current values of variables.
*
* @param instance The user data sent for lbfgs_optimize() function by the client.
* @param x The current values of variables.
* @param g The gradient vector. The callback function must compute
* the gradient values for the current variables.
* @retval double The value of the cost function for the current variables.
*/
typedef double (*lbfgs_evaluate_t)(void *instance,
const Eigen::VectorXd &x,
Eigen::VectorXd &g);
/**
* Callback interface to provide an upper bound at the beginning of the current line search.
*
* The lbfgs_optimize() function call this function to obtain the values of the
* upperbound of the stepsize to search in, provided with the beginning values of
* variables before the line search, and the current step vector (can be descent direction).
* A client program can implement this function for more efficient linesearch. Any step
* larger than this bound should not be considered. For example, it has a very large or even
* inf function value. Note that the function value at the provided bound should be FINITE!
* If it is not used, just set it nullptr.
*
* @param instance The user data sent for lbfgs_optimize() function by the client.
* @param xp The values of variables before current line search.
* @param d The step vector. It can be the descent direction.
* @retval double The upperboud of the step in current line search routine,
* such that (stpbound * d) is the maximum reasonable step.
*/
typedef double (*lbfgs_stepbound_t)(void *instance,
const Eigen::VectorXd &xp,
const Eigen::VectorXd &d);
/**
* Callback interface to monitor the progress of the minimization process.
*
* The lbfgs_optimize() function call this function for each iteration. Implementing
* this function, a client program can store or display the current progress
* of the minimization process. If it is not used, just set it nullptr.
*
* @param instance The user data sent for lbfgs_optimize() function by the client.
* @param x The current values of variables.
* @param g The current gradient values of variables.
* @param fx The current value of the cost function.
* @param step The line-search step used for this iteration.
* @param k The iteration count.
* @param ls The number of evaluations called for this iteration.
* @retval int Zero to continue the minimization process. Returning a
* non-zero value will cancel the minimization process.
*/
typedef int (*lbfgs_progress_t)(void *instance,
const Eigen::VectorXd &x,
const Eigen::VectorXd &g,
const double fx,
const double step,
const int k,
const int ls);
/**
* Callback data struct
*/
struct callback_data_t
{
void *instance = nullptr;
lbfgs_evaluate_t proc_evaluate = nullptr;
lbfgs_stepbound_t proc_stepbound = nullptr;
lbfgs_progress_t proc_progress = nullptr;
};
// ----------------------- L-BFGS Part -----------------------
/**
* Line search method for smooth or nonsmooth functions.
* This function performs line search to find a point that satisfy
* both the Armijo condition and the weak Wolfe condition. It is
* as robust as the backtracking line search but further applies
* to continuous and piecewise smooth functions where the strong
* Wolfe condition usually does not hold.
*
* @see
* Adrian S. Lewis and Michael L. Overton. Nonsmooth optimization
* via quasi-Newton methods. Mathematical Programming, Vol 141,
* No 1, pp. 135-163, 2013.
*/
inline int line_search_lewisoverton(Eigen::VectorXd &x,
double &f,
Eigen::VectorXd &g,
double &stp,
const Eigen::VectorXd &s,
const Eigen::VectorXd &xp,
const Eigen::VectorXd &gp,
const double stpmin,
const double stpmax,
const callback_data_t &cd,
const lbfgs_parameter_t &param)
{
int count = 0;
bool brackt = false, touched = false;
double finit, dginit, dgtest, dstest;
double mu = 0.0, nu = stpmax;
/* Check the input parameters for errors. */
if (!(stp > 0.0))
{
return LBFGSERR_INVALIDPARAMETERS;
}
/* Compute the initial gradient in the search direction. */
dginit = gp.dot(s);
/* Make sure that s points to a descent direction. */
if (0.0 < dginit)
{
return LBFGSERR_INCREASEGRADIENT;
}
/* The initial value of the cost function. */
finit = f;
dgtest = param.f_dec_coeff * dginit;
dstest = param.s_curv_coeff * dginit;
while (true)
{
x = xp + stp * s;
/* Evaluate the function and gradient values. */
f = cd.proc_evaluate(cd.instance, x, g);
++count;
/* Test for errors. */
if (std::isinf(f) || std::isnan(f))
{
return LBFGSERR_INVALID_FUNCVAL;
}
if(param.past > 0 && fabs(finit-f)/(fabs(finit)+1.0)<param.delta/param.past)
{
return count;
}
/* Check the Armijo condition. */
if (f > finit + stp * dgtest)
{
nu = stp;
brackt = true;
}
else
{
/* Check the weak Wolfe condition. */
if (g.dot(s) < dstest)
{
mu = stp;
}
else
{
return count;
}
}
if (param.max_linesearch <= count)
{
// std::cout<< fabs(dginit)/finit<<std::endl;
// std::cout << fabs(finit-f)/(fabs(finit)+1.0) << std::endl;
/* Maximum number of iteration. */
return LBFGSERR_MAXIMUMLINESEARCH;
}
if (brackt && (nu - mu) < param.machine_prec * nu)
{
/* Relative interval width is at least machine_prec. */
return LBFGSERR_WIDTHTOOSMALL;
}
if (brackt)
{
stp = 0.5 * (mu + nu);
}
else
{
stp *= 2.0;
}
if (stp < stpmin)
{
/* The step is the minimum value. */
return LBFGSERR_MINIMUMSTEP;
}
if (stp > stpmax)
{
if (touched)
{
/* The step is the maximum value. */
return LBFGSERR_MAXIMUMSTEP;
}
else
{
/* The maximum value should be tried once. */
touched = true;
stp = stpmax;
}
}
}
}
/**
* Start a L-BFGS optimization.
* Assumptions: 1. f(x) is either C2 or C0 but piecewise C2;
* 2. f(x) is lower bounded;
* 3. f(x) has bounded level sets;
* 4. g(x) is either the gradient or subgradient;
* 5. The gradient exists at the initial guess x0.
* A user must implement a function compatible with ::lbfgs_evaluate_t (evaluation
* callback) and pass the pointer to the callback function to lbfgs_optimize()
* arguments. Similarly, a user can implement a function compatible with
* ::lbfgs_stepbound_t to provide an external upper bound for stepsize, and
* ::lbfgs_progress_t (progress callback) to obtain the current progress
* (e.g., variables, function, and gradient, etc) and to cancel the iteration
* process if necessary. Implementation of the stepbound and the progress callback
* is optional: a user can pass nullptr if progress notification is not necessary.
*
*
* @param x The vector of decision variables.
* THE INITIAL GUESS x0 SHOULD BE SET BEFORE THE CALL!
* A client program can receive decision variables
* through this vector, at which the cost and its
* gradient are queried during minimization.
* @param f The ref to the variable that receives the final
* value of the cost function for the variables.
* @param proc_evaluate The callback function to provide function f(x) and
* gradient g(x) evaluations given a current values of
* variables x. A client program must implement a
* callback function compatible with lbfgs_evaluate_t
* and pass the pointer to the callback function.
* @param proc_stepbound The callback function to provide values of the
* upperbound of the stepsize to search in, provided
* with the beginning values of variables before the
* line search, and the current step vector (can be
* negative gradient). A client program can implement
* this function for more efficient linesearch. If it is
* not used, just set it nullptr.
* @param proc_progress The callback function to receive the progress
* (the number of iterations, the current value of
* the cost function) of the minimization
* process. This argument can be set to nullptr if
* a progress report is unnecessary.
* @param instance A user data pointer for client programs. The callback
* functions will receive the value of this argument.
* @param param The parameters for L-BFGS optimization.
* @retval int The status code. This function returns a nonnegative
* integer if the minimization process terminates without
* an error. A negative integer indicates an error.
*/
inline int lbfgs_optimize(Eigen::VectorXd &x,
double &f,
lbfgs_evaluate_t proc_evaluate,
lbfgs_stepbound_t proc_stepbound,
lbfgs_progress_t proc_progress,
void *instance,
const lbfgs_parameter_t &param)
{
int ret, i, j, k, ls, end, bound;
double step, step_min, step_max, fx, ys, yy;
double gnorm_inf, xnorm_inf, beta, rate, cau;
const int n = x.size();
const int m = param.mem_size;
/* Check the input parameters for errors. */
if (n <= 0)
{
return LBFGSERR_INVALID_N;
}
if (m <= 0)
{
return LBFGSERR_INVALID_MEMSIZE;
}
if (param.g_epsilon < 0.0)
{
return LBFGSERR_INVALID_GEPSILON;
}
if (param.past < 0)
{
return LBFGSERR_INVALID_TESTPERIOD;
}
if (param.delta < 0.0)
{
return LBFGSERR_INVALID_DELTA;
}
if (param.min_step < 0.0)
{
return LBFGSERR_INVALID_MINSTEP;
}
if (param.max_step < param.min_step)
{
return LBFGSERR_INVALID_MAXSTEP;
}
if (!(param.f_dec_coeff > 0.0 &&
param.f_dec_coeff < 1.0))
{
return LBFGSERR_INVALID_FDECCOEFF;
}
if (!(param.s_curv_coeff < 1.0 &&
param.s_curv_coeff > param.f_dec_coeff))
{
return LBFGSERR_INVALID_SCURVCOEFF;
}
if (!(param.machine_prec > 0.0))
{
return LBFGSERR_INVALID_MACHINEPREC;
}
if (param.max_linesearch <= 0)
{
return LBFGSERR_INVALID_MAXLINESEARCH;
}
/* Prepare intermediate variables. */
Eigen::VectorXd xp(n);
Eigen::VectorXd g(n);
Eigen::VectorXd gp(n);
Eigen::VectorXd d(n);
Eigen::VectorXd pf(std::max(1, param.past));
/* Initialize the limited memory. */
Eigen::VectorXd lm_alpha = Eigen::VectorXd::Zero(m);
Eigen::MatrixXd lm_s = Eigen::MatrixXd::Zero(n, m);
Eigen::MatrixXd lm_y = Eigen::MatrixXd::Zero(n, m);
Eigen::VectorXd lm_ys = Eigen::VectorXd::Zero(m);
/* Construct a callback data. */
callback_data_t cd;
cd.instance = instance;
cd.proc_evaluate = proc_evaluate;
cd.proc_stepbound = proc_stepbound;
cd.proc_progress = proc_progress;
/* Evaluate the function value and its gradient. */
fx = cd.proc_evaluate(cd.instance, x, g);
/* Store the initial value of the cost function. */
pf(0) = fx;
/*
Compute the direction;
we assume the initial hessian matrix H_0 as the identity matrix.
*/
d = -g;
/*
Make sure that the initial variables are not a stationary point.
*/
gnorm_inf = g.cwiseAbs().maxCoeff();
xnorm_inf = x.cwiseAbs().maxCoeff();
if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon)
{
/* The initial guess is already a stationary point. */
ret = LBFGS_CONVERGENCE;
}
else
{
/*
Compute the initial step:
*/
step = 1.0 / d.norm();
k = 1;
end = 0;
bound = 0;
while (true)
{
/* Store the current position and gradient vectors. */
xp = x;
gp = g;
/* If the step bound can be provied dynamically, then apply it. */
step_min = param.min_step;
step_max = param.max_step;
if (cd.proc_stepbound)
{
step_max = cd.proc_stepbound(cd.instance, xp, d);
step_max = step_max < param.max_step ? step_max : param.max_step;
step = step < step_max ? step : 0.5 * step_max;
}
/* Search for an optimal step. */
ls = line_search_lewisoverton(x, fx, g, step, d, xp, gp, step_min, step_max, cd, param);
// int idx = 1;
// step = 1.0;
// if(d.norm()>1.0){
// d = d.normalized().eval()*1.0;
// }
// while(1){
// // std::cout<<"step * d"<<(step * d).transpose()<<std::endl;
// x = xp + step * d;
// /* Evaluate the function and gradient values. */
// double f = cd.proc_evaluate(cd.instance, x, g);
// if(g.norm()>1.0){
// g = g.normalized().eval()*1.0;
// }
// std::cout<<"fx: "<<fx<<" f: "<<f<<"step: "<<step<<std::endl;
// if(f < fx || step <= step_min){
// fx = f;
// break;
// }
// step = 1.0 / (++idx);
// }
// ls = 1;
if (ls < 0)
{
/* Revert to the previous point. */
x = xp;
g = gp;
ret = ls;
break;
}
/* Report the progress. */
if (cd.proc_progress)
{
if (cd.proc_progress(cd.instance, x, g, fx, step, k, ls))
{
ret = LBFGS_CANCELED;
break;
}
}
/*
Convergence test.
The criterion is given by the following formula:
||g(x)||_inf / max(1, ||x||_inf) < g_epsilon
*/
gnorm_inf = g.cwiseAbs().maxCoeff();
xnorm_inf = x.cwiseAbs().maxCoeff();
if (gnorm_inf / std::max(1.0, xnorm_inf) < param.g_epsilon)
{
/* Convergence. */
ret = LBFGS_CONVERGENCE;
break;
}
/*
Test for stopping criterion.
The criterion is given by the following formula:
|f(past_x) - f(x)| / max(1, |f(x)|) < \delta.
*/
if (0 < param.past)
{
/* We don't test the stopping criterion while k < past. */
if (param.past <= k)
{
/* The stopping criterion. */
rate = std::fabs(pf(k % param.past) - fx) / std::max(1.0, std::fabs(fx));
if (rate < param.delta)
{
ret = LBFGS_STOP;
break;
}
}
/* Store the current value of the cost function. */
pf(k % param.past) = fx;
}
if (param.max_iterations != 0 && param.max_iterations <= k)
{
/* Maximum number of iterations. */
ret = LBFGSERR_MAXIMUMITERATION;
break;
}
/* Count the iteration number. */
++k;
/*
Update vectors s and y:
s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}.
y_{k+1} = g_{k+1} - g_{k}.
*/
lm_s.col(end) = x - xp;
lm_y.col(end) = g - gp;
/*
Compute scalars ys and yy:
ys = y^t \cdot s = 1 / \rho.
yy = y^t \cdot y.
Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor).
*/
ys = lm_y.col(end).dot(lm_s.col(end));
yy = lm_y.col(end).squaredNorm();
lm_ys(end) = ys;
/* Compute the negative of gradients. */
d = -g;
/*
Only cautious update is performed here as long as
(y^t \cdot s) / ||s_{k+1}||^2 > \epsilon * ||g_{k}||^\alpha,
where \epsilon is the cautious factor and a proposed value
for \alpha is 1.
This is not for enforcing the PD of the approxomated Hessian
since ys > 0 is already ensured by the weak Wolfe condition.
This is to ensure the global convergence as described in:
Dong-Hui Li and Masao Fukushima. On the global convergence of
the BFGS method for nonconvex unconstrained optimization problems.
SIAM Journal on Optimization, Vol 11, No 4, pp. 1054-1064, 2011.
*/
cau = lm_s.col(end).squaredNorm() * gp.norm() * param.cautious_factor;
if (ys > cau)
{
/*
Recursive formula to compute dir = -(H \cdot g).
This is described in page 779 of:
Jorge Nocedal.
Updating Quasi-Newton Matrices with Limited Storage.
Mathematics of Computation, Vol. 35, No. 151,
pp. 773--782, 1980.
*/
++bound;
bound = m < bound ? m : bound;
end = (end + 1) % m;
j = end;
for (i = 0; i < bound; ++i)
{
j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */
/* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */
lm_alpha(j) = lm_s.col(j).dot(d) / lm_ys(j);
/* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */
d += (-lm_alpha(j)) * lm_y.col(j);
}
d *= ys / yy;
for (i = 0; i < bound; ++i)
{
/* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamm_{i}. */
beta = lm_y.col(j).dot(d) / lm_ys(j);
/* \gamm_{i+1} = \gamm_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */
d += (lm_alpha(j) - beta) * lm_s.col(j);
j = (j + 1) % m; /* if (++j == m) j = 0; */
}
}
/* The search direction d is ready. We try step = 1 first. */
step = 1.0;
}
}
/* Return the final value of the cost function. */
f = fx;
return ret;
}
/**
* Get string description of an lbfgs_optimize() return code.
*
* @param err A value returned by lbfgs_optimize().
*/
inline const char *lbfgs_strerror(const int err)
{
switch (err)
{
case LBFGS_CONVERGENCE:
return "Success: reached convergence (g_epsilon).";
case LBFGS_STOP:
return "Success: met stopping criteria (past f decrease less than delta).";
case LBFGS_CANCELED:
return "The iteration has been canceled by the monitor callback.";
case LBFGSERR_UNKNOWNERROR:
return "Unknown error.";
case LBFGSERR_INVALID_N:
return "Invalid number of variables specified.";
case LBFGSERR_INVALID_MEMSIZE:
return "Invalid parameter lbfgs_parameter_t::mem_size specified.";
case LBFGSERR_INVALID_GEPSILON:
return "Invalid parameter lbfgs_parameter_t::g_epsilon specified.";
case LBFGSERR_INVALID_TESTPERIOD:
return "Invalid parameter lbfgs_parameter_t::past specified.";
case LBFGSERR_INVALID_DELTA:
return "Invalid parameter lbfgs_parameter_t::delta specified.";
case LBFGSERR_INVALID_MINSTEP:
return "Invalid parameter lbfgs_parameter_t::min_step specified.";
case LBFGSERR_INVALID_MAXSTEP:
return "Invalid parameter lbfgs_parameter_t::max_step specified.";
case LBFGSERR_INVALID_FDECCOEFF:
return "Invalid parameter lbfgs_parameter_t::f_dec_coeff specified.";
case LBFGSERR_INVALID_SCURVCOEFF:
return "Invalid parameter lbfgs_parameter_t::s_curv_coeff specified.";
case LBFGSERR_INVALID_MACHINEPREC:
return "Invalid parameter lbfgs_parameter_t::machine_prec specified.";
case LBFGSERR_INVALID_MAXLINESEARCH:
return "Invalid parameter lbfgs_parameter_t::max_linesearch specified.";
case LBFGSERR_INVALID_FUNCVAL:
return "The function value became NaN or Inf.";
case LBFGSERR_MINIMUMSTEP:
return "The line-search step became smaller than lbfgs_parameter_t::min_step.";
case LBFGSERR_MAXIMUMSTEP:
return "The line-search step became larger than lbfgs_parameter_t::max_step.";
case LBFGSERR_MAXIMUMLINESEARCH:
return "Line search reaches the maximum try number, assumptions not satisfied or precision not achievable.";
case LBFGSERR_MAXIMUMITERATION:
return "The algorithm routine reaches the maximum number of iterations.";
case LBFGSERR_WIDTHTOOSMALL:
return "Relative search interval width is at least lbfgs_parameter_t::machine_prec.";
case LBFGSERR_INVALIDPARAMETERS:
return "A logic error (negative line-search step) occurred.";
case LBFGSERR_INCREASEGRADIENT:
return "The current search direction increases the cost function value.";
default:
return "(unknown)";
}
}
} // namespace lbfgs
#endif

941
trajectoryOpt/src/plan_manage/include/difPlan/poly_traj_utils.hpp Executable file
View File

@@ -0,0 +1,941 @@
#pragma once
#include <ros/ros.h>
#include <iostream>
#include <cmath>
#include <cfloat>
#include <vector>
#include <Eigen/Eigen>
#include "root_finder.hpp"
namespace dftpav
{
constexpr double PI = 3.1415926;
typedef Eigen::Matrix<double, 2, 6> CoefficientMat;
typedef Eigen::Matrix<double, 2, 5> VelCoefficientMat;
typedef Eigen::Matrix<double, 2, 4> AccCoefficientMat;
class Piece // component from poly
{
private: // duration + coeffMat
double duration;
CoefficientMat coeffMat;
int dim = 2;
int order = 5;
int singul;
public:
Piece() = default;
Piece(double dur, const CoefficientMat &cMat, int s)
: duration(dur), coeffMat(cMat), singul(s) {}
//@yuwei
inline int getDim() const
{
return dim;
}
//@yuwei
inline int getOrder() const
{
return order;
}
inline double getDuration() const
{
return duration;
}
inline const CoefficientMat &getCoeffMat() const
{
return coeffMat;
}
inline VelCoefficientMat getVelCoeffMat() const
{
VelCoefficientMat velCoeffMat;
int n = 1;
for (int i = 4; i >= 0; i--)
{
velCoeffMat.col(i) = n * coeffMat.col(i);
n++;
}
return velCoeffMat;
}
// the point in the rear axle center
inline Eigen::Vector2d getSigma(const double &t) const
{
Eigen::Vector2d pos(0.0, 0.0);
double tn = 1.0;
for (int i = order; i >= 0; i--)
{
pos += tn * coeffMat.col(i);
tn *= t;
}
return pos;
}
inline Eigen::Matrix2d getR(const double &t) const
{
Eigen::Vector2d current_v = getdSigma(t);
Eigen::Matrix2d rotation_matrix;
rotation_matrix << current_v(0), -current_v(1),
current_v(1), current_v(0);
rotation_matrix = singul * rotation_matrix / current_v.norm();
return rotation_matrix;
}
inline Eigen::Matrix2d getRdot(const double &t) const
{
Eigen::Vector2d current_v = getdSigma(t);
Eigen::Vector2d current_a = getddSigma(t);
Eigen::Matrix2d temp_a_ba, temp_v_bv;
temp_a_ba << current_a(0), -current_a(1),
current_a(1), current_a(0);
temp_v_bv << current_v(0), -current_v(1),
current_v(1), current_v(0);
Eigen::Matrix2d R_dot = singul * (temp_a_ba / current_v.norm() - temp_v_bv / pow(current_v.norm(), 3) * (current_v.transpose() * current_a));
return R_dot;
}
inline Eigen::Vector2d getdSigma(const double &t) const
{
Eigen::Vector2d dsigma(0.0, 0.0);
double tn = 1.0;
int n = 1;
for (int i = order-1; i >= 0; i--)
{
dsigma += n * tn * coeffMat.col(i);
tn *= t;
n++;
}
return dsigma;
}
inline Eigen::Vector2d getddSigma(const double &t) const
{
Eigen::Vector2d ddsigma(0.0, 0.0);
double tn = 1.0;
int m = 1;
int n = 2;
for (int i = order-2; i >= 0; i--)
{
ddsigma += m * n * tn * coeffMat.col(i);
tn *= t;
m++;
n++;
}
return ddsigma;
}
inline Eigen::Vector2d getdddSigma(const double &t) const
{
Eigen::Vector2d dddsigma(0.0, 0.0);
double tn = 1.0;
int l = 1;
int m = 2;
int n = 3;
for (int i = order-3; i >= 0; i--)
{
dddsigma += l * m * n * tn * coeffMat.col(i);
tn *= t;
l++;
m++;
n++;
}
return dddsigma;
}
inline CoefficientMat normalizePosCoeffMat() const
{
CoefficientMat nPosCoeffsMat;
double t = 1.0;
for (int i = order; i >= 0; i--)
{
nPosCoeffsMat.col(i) = coeffMat.col(i) * t;
t *= duration;
}
return nPosCoeffsMat;
}
};
class Trajectory
{
private:
typedef std::vector<Piece> Pieces;
Pieces pieces;
public:
Trajectory() = default;
Trajectory(const std::vector<double> &durs,
const std::vector<CoefficientMat> &cMats, int s)
{
int N = std::min(durs.size(), cMats.size());
pieces.reserve(N);
for (int i = 0; i < N; i++)
{
pieces.emplace_back(durs[i], cMats[i], s);
}
}
inline int getPieceNum() const
{
return pieces.size();
}
inline Eigen::VectorXd getDurations() const
{
int N = getPieceNum();
Eigen::VectorXd durations(N);
for (int i = 0; i < N; i++)
{
durations(i) = pieces[i].getDuration();
}
return durations;
}
inline double getTotalDuration() const
{
int N = getPieceNum();
double totalDuration = 0.0;
for (int i = 0; i < N; i++)
{
totalDuration += pieces[i].getDuration();
}
return totalDuration;
}
inline const Piece &operator[](int i) const
{
return pieces[i];
}
inline Piece &operator[](int i)
{
return pieces[i];
}
inline void clear(void)
{
pieces.clear();
return;
}
inline Pieces::const_iterator begin() const
{
return pieces.begin();
}
inline Pieces::const_iterator end() const
{
return pieces.end();
}
inline Pieces::iterator begin()
{
return pieces.begin();
}
inline Pieces::iterator end()
{
return pieces.end();
}
inline void reserve(const int &n)
{
pieces.reserve(n);
return;
}
inline void emplace_back(const Piece &piece)
{
pieces.emplace_back(piece);
return;
}
inline void emplace_back(const double &dur,
const CoefficientMat &cMat, int s)
{
pieces.emplace_back(dur, cMat, s);
return;
}
inline void append(const Trajectory &traj)
{
pieces.insert(pieces.end(), traj.begin(), traj.end());
return;
}
inline int locatePieceIdx(double &t) const
{
int N = getPieceNum();
int idx;
double dur;
for (idx = 0;
idx < N &&
t > (dur = pieces[idx].getDuration());
idx++)
{
t -= dur;
}
if (idx == N)
{
idx--;
t += pieces[idx].getDuration();
}
return idx;
}
inline int locatePieceIdx2(double t) const
{
int N = getPieceNum();
std::cout << "N = getPieceNum() is"<< N << std::endl;
int idx;
double dur;
for (idx = 0;
idx < N &&
t > (dur = pieces[idx].getDuration());
idx++)
{
t -= dur;
}
if (idx == N)
{
idx--;
t += pieces[idx].getDuration();
}
return idx;
}
inline Eigen::Vector2d getSigma(double t) const
{
int pieceIdx = locatePieceIdx(t);
return pieces[pieceIdx].getSigma(t);
}
inline Eigen::Vector2d getdSigma(double t) const
{
int pieceIdx = locatePieceIdx(t);
return pieces[pieceIdx].getdSigma(t);
}
double getVelNorm(double t) const{
return getdSigma(t).norm();
}
inline Eigen::Vector2d getddSigma(double t) const
{
int pieceIdx = locatePieceIdx(t);
// std::cout << "pieceIdx is"<< pieceIdx << std::endl;
// std::cout << "t is"<< t << std::endl;
return pieces[pieceIdx].getddSigma(t);
}
inline Eigen::Vector2d getdddSigma(double t) const
{
int pieceIdx = locatePieceIdx(t);
return pieces[pieceIdx].getdddSigma(t);
}
// hzc
inline std::pair<int, double> locatePieceIdxWithRatio(double &t) const
{
int N = getPieceNum();
int idx;
double dur;
for (idx = 0;
idx < N &&
t > (dur = pieces[idx].getDuration());
idx++)
{
t -= dur;
}
if (idx == N)
{
idx--;
t += pieces[idx].getDuration();
}
std::pair<int, double> idx_ratio;
idx_ratio.first = idx;
idx_ratio.second = t / dur;
return idx_ratio;
}
};
class DifTrajectory{
private:
public:
DifTrajectory() = default;
std::vector<Trajectory> Traj_container;
std::vector<int> etas;
inline int getSegNum() const { return etas.size(); }
inline int locateTrajIdx(double &t) const
{
int N = etas.size();
int idx;
double dur;
for (idx = 0;
idx < N &&
t > (dur = Traj_container[idx].getTotalDuration()+1.0e-6);
idx++)
{
t -= dur;
}
if (idx == N)
{
idx--;
t += Traj_container[idx].getTotalDuration();
}
return idx;
}
inline int getDirection(double t) const{
int idx = locateTrajIdx(t);
if(etas[idx] > 0){
return 1;
}
else{
return -1;
}
}
inline double getTotalDuration() const
{
double totalT = 0.0;
for(const auto traj : Traj_container){
totalT += traj.getTotalDuration();
}
return totalT;
}
double getVelItem(double t) const{
int idx = locateTrajIdx(t);
int eta = etas[idx];
return Traj_container[idx].getVelNorm(t) * eta;
}
inline double getTotalArc() const{
double length = 0.0;
for(double t = 0.0; t < getTotalDuration()-1.0e-3; t+=0.01){
length += 0.01 * fabs(getVelItem(t));
}
return length;
}
inline Eigen::Vector2d getPos(double t) const
{
int idx = locateTrajIdx(t);
return Traj_container[idx].getSigma(t);
}
double getCur(double t) const{
int idx = locateTrajIdx(t);
int eta = etas[idx];
Eigen::Vector2d vel = Traj_container[idx].getdSigma(t);
Eigen::Vector2d acc = Traj_container[idx].getddSigma(t);
return eta*(vel[0]*acc[1]-vel[1]*acc[0])/(vel.norm()*vel.norm()*vel.norm());
}
double getCurDot(double t) const{
int idx = locateTrajIdx(t);
int eta = etas[idx];
Eigen::Vector2d vel = Traj_container[idx].getdSigma(t);
Eigen::Vector2d acc = Traj_container[idx].getddSigma(t);
Eigen::Vector2d jerk = Traj_container[idx].getdddSigma(t);
Eigen::Matrix2d B;
B << 0,-1,
1, 0;
double tp1 = jerk.transpose()*B*vel;
double v3 = pow(vel.norm(),3);
double tp2 = acc.transpose()*B*vel;
double tp3 = acc.dot(vel);
double v1 = vel.norm();
double v6 = pow(vel.norm(),6);
double denor = tp1*v3-3.0*tp2*tp3*v1;
double kdot = denor / v6;
return kdot * eta;
}
double getPhi(double t, double wheelbase = 0.6){
double c = getCur(t);
return atan(c * wheelbase);
}
double getOmega(double t, double wheelbase = 0.6) const{
int idx = locateTrajIdx(t);
int eta = etas[idx];
Eigen::Vector2d vel = Traj_container[idx].getdSigma(t);
Eigen::Vector2d acc = Traj_container[idx].getddSigma(t);
Eigen::Vector2d jerk = Traj_container[idx].getdddSigma(t);
Eigen::Matrix2d B;
B << 0,-1,
1, 0;
double tp1 = jerk.transpose()*B*vel;
double v3 = pow(vel.norm(),3);
double tp2 = acc.transpose()*B*vel;
double tp3 = acc.dot(vel);
double v1 = vel.norm();
double v6 = pow(vel.norm(),6);
double denor = tp1*v3-3.0*tp2*tp3*v1;
double nor = v6 + pow(tp2*wheelbase,2);
double omega = wheelbase * denor / nor;
return omega * eta;
}
inline double getYaw(double t) const{
int idx = locateTrajIdx(t);
Eigen::Vector2d vel = Traj_container[idx].getdSigma(t);
double yaw = atan2(etas[idx] * vel[1], etas[idx] * vel[0]);
return yaw;
}
inline Eigen::VectorXd getDurations() const{
std::vector<double> stdDurations;
for(const auto traj : Traj_container){
Eigen::VectorXd tmpDurations = traj.getDurations();
for(int i = 0; i < tmpDurations.size(); i++){
stdDurations.push_back(tmpDurations[i]);
}
}
Eigen::VectorXd durations;
durations.resize(stdDurations.size());
for(int i = 0; i < stdDurations.size(); i++){
durations[i] = stdDurations[i];
}
return durations;
}
inline int getTotalPieceNum() const{
int pieceNum = 0;
for(const auto traj : Traj_container){
pieceNum += traj.getPieceNum();
}
return pieceNum;
}
inline const Trajectory &operator[](int i) const
{
return Traj_container[i];
}
inline Trajectory &operator[](int i)
{
return Traj_container[i];
}
};
// The banded system class is used for solving
// banded linear system Ax=b efficiently.
// A is an N*N band matrix with lower band width lowerBw
// and upper band width upperBw.
// Banded LU factorization has O(N) time complexity.
class BandedSystem {
public:
// The size of A, as well as the lower/upper
// banded width p/q are needed
inline void create(const int &n, const int &p, const int &q) {
// In case of re-creating before destroying
destroy();
N = n;
lowerBw = p;
upperBw = q;
int actualSize = N * (lowerBw + upperBw + 1);
ptrData = new double[actualSize];
std::fill_n(ptrData, actualSize, 0.0);
return;
}
inline void destroy() {
if (ptrData != nullptr) {
delete[] ptrData;
ptrData = nullptr;
}
return;
}
private:
int N;
int lowerBw;
int upperBw;
// Compulsory nullptr initialization here
double *ptrData = nullptr;
public:
// Reset the matrix to zero
inline void reset(void) {
std::fill_n(ptrData, N * (lowerBw + upperBw + 1), 0.0);
return;
}
// The band matrix is stored as suggested in "Matrix Computation"
inline const double &operator()(const int &i, const int &j) const {
return ptrData[(i - j + upperBw) * N + j];
}
inline double &operator()(const int &i, const int &j) {
return ptrData[(i - j + upperBw) * N + j];
}
// This function conducts banded LU factorization in place
// Note that NO PIVOT is applied on the matrix "A" for efficiency!!!
inline void factorizeLU() {
int iM, jM;
double cVl;
for (int k = 0; k <= N - 2; k++) {
iM = std::min(k + lowerBw, N - 1);
cVl = operator()(k, k);
for (int i = k + 1; i <= iM; i++) {
if (operator()(i, k) != 0.0) {
operator()(i, k) /= cVl;
}
}
jM = std::min(k + upperBw, N - 1);
for (int j = k + 1; j <= jM; j++) {
cVl = operator()(k, j);
if (cVl != 0.0) {
for (int i = k + 1; i <= iM; i++) {
if (operator()(i, k) != 0.0) {
operator()(i, j) -= operator()(i, k) * cVl;
}
}
}
}
}
return;
}
// This function solves Ax=b, then stores x in b
// The input b is required to be N*m, i.e.,
// m vectors to be solved.
template <typename EIGENMAT>
inline void solve(EIGENMAT &b) const {
int iM;
for (int j = 0; j <= N - 1; j++) {
iM = std::min(j + lowerBw, N - 1);
for (int i = j + 1; i <= iM; i++) {
if (operator()(i, j) != 0.0) {
b.row(i) -= operator()(i, j) * b.row(j);
}
}
}
for (int j = N - 1; j >= 0; j--) {
b.row(j) /= operator()(j, j);
iM = std::max(0, j - upperBw);
for (int i = iM; i <= j - 1; i++) {
if (operator()(i, j) != 0.0) {
b.row(i) -= operator()(i, j) * b.row(j);
}
}
}
return;
}
// This function solves ATx=b, then stores x in b
// The input b is required to be N*m, i.e.,
// m vectors to be solved.
template <typename EIGENMAT>
inline void solveAdj(EIGENMAT &b) const {
int iM;
for (int j = 0; j <= N - 1; j++) {
b.row(j) /= operator()(j, j);
iM = std::min(j + upperBw, N - 1);
for (int i = j + 1; i <= iM; i++) {
if (operator()(j, i) != 0.0) {
b.row(i) -= operator()(j, i) * b.row(j);
}
}
}
for (int j = N - 1; j >= 0; j--) {
iM = std::max(0, j - lowerBw);
for (int i = iM; i <= j - 1; i++) {
if (operator()(j, i) != 0.0) {
b.row(i) -= operator()(j, i) * b.row(j);
}
}
}
return;
}
};
class MinJerkOpt
{
public:
MinJerkOpt() = default;
~MinJerkOpt() { A.destroy(); }
private:
int N; // pieceNum
Eigen::MatrixXd headPVA, tailPVA;
Eigen::MatrixXd b, c, adjScaledGrad; // 6*N, 2
BandedSystem A; // 6 * N, 6 * N
Eigen::Matrix<double, 6, 1> t, tInv;
// for outside use
/*polynomial descrips*/
Eigen::MatrixXd gdC;
double gdT;
/*MINCO descrips*/
Eigen::MatrixXd gdHead;
Eigen::MatrixXd gdTail;
Eigen::MatrixXd gdP;
public:
inline void reset(const int &pieceNum)
{
N = pieceNum;
A.create(6 * N, 6, 6);
b.resize(6 * N, 2);
c.resize(6 * N, 2);
adjScaledGrad.resize(6 * N, 2);
gdC.resize(6 * N, 2);
gdP.resize(2, N - 1);
gdHead.resize(2, 3);
gdTail.resize(2, 3);
t(0) = 1.0;
A(0, 0) = 1.0;
A(1, 1) = 1.0;
A(2, 2) = 2.0;
for (int i = 0; i < N - 1; i++) {
A(6 * i + 3, 6 * i + 3) = 6.0;
A(6 * i + 3, 6 * i + 4) = 24.0;
A(6 * i + 3, 6 * i + 5) = 60.0;
A(6 * i + 3, 6 * i + 9) = -6.0;
A(6 * i + 4, 6 * i + 4) = 24.0;
A(6 * i + 4, 6 * i + 5) = 120.0;
A(6 * i + 4, 6 * i + 10) = -24.0;
A(6 * i + 5, 6 * i) = 1.0;
A(6 * i + 5, 6 * i + 1) = 1.0;
A(6 * i + 5, 6 * i + 2) = 1.0;
A(6 * i + 5, 6 * i + 3) = 1.0;
A(6 * i + 5, 6 * i + 4) = 1.0;
A(6 * i + 5, 6 * i + 5) = 1.0;
A(6 * i + 6, 6 * i) = 1.0;
A(6 * i + 6, 6 * i + 1) = 1.0;
A(6 * i + 6, 6 * i + 2) = 1.0;
A(6 * i + 6, 6 * i + 3) = 1.0;
A(6 * i + 6, 6 * i + 4) = 1.0;
A(6 * i + 6, 6 * i + 5) = 1.0;
A(6 * i + 6, 6 * i + 6) = -1.0;
A(6 * i + 7, 6 * i + 1) = 1.0;
A(6 * i + 7, 6 * i + 2) = 2.0;
A(6 * i + 7, 6 * i + 3) = 3.0;
A(6 * i + 7, 6 * i + 4) = 4.0;
A(6 * i + 7, 6 * i + 5) = 5.0;
A(6 * i + 7, 6 * i + 7) = -1.0;
A(6 * i + 8, 6 * i + 2) = 2.0;
A(6 * i + 8, 6 * i + 3) = 6.0;
A(6 * i + 8, 6 * i + 4) = 12.0;
A(6 * i + 8, 6 * i + 5) = 20.0;
A(6 * i + 8, 6 * i + 8) = -2.0;
}
A(6 * N - 3, 6 * N - 6) = 1.0;
A(6 * N - 3, 6 * N - 5) = 1.0;
A(6 * N - 3, 6 * N - 4) = 1.0;
A(6 * N - 3, 6 * N - 3) = 1.0;
A(6 * N - 3, 6 * N - 2) = 1.0;
A(6 * N - 3, 6 * N - 1) = 1.0;
A(6 * N - 2, 6 * N - 5) = 1.0;
A(6 * N - 2, 6 * N - 4) = 2.0;
A(6 * N - 2, 6 * N - 3) = 3.0;
A(6 * N - 2, 6 * N - 2) = 4.0;
A(6 * N - 2, 6 * N - 1) = 5.0;
A(6 * N - 1, 6 * N - 4) = 2.0;
A(6 * N - 1, 6 * N - 3) = 6.0;
A(6 * N - 1, 6 * N - 2) = 12.0;
A(6 * N - 1, 6 * N - 1) = 20.0;
A.factorizeLU();
return;
}
inline void generate(const Eigen::MatrixXd &inPs,
const double &dT,
const Eigen::MatrixXd &headState,
const Eigen::MatrixXd &tailState)
{
headPVA = headState;
tailPVA = tailState;
t(1) = dT;
t(2) = t(1) * t(1);
t(3) = t(2) * t(1);
t(4) = t(2) * t(2);
t(5) = t(4) * t(1);
tInv = t.cwiseInverse();
b.setZero();
b.row(0) = headPVA.col(0).transpose();
b.row(1) = headPVA.col(1).transpose() * t(1);
b.row(2) = headPVA.col(2).transpose() * t(2);
for (int i = 0; i < N - 1; i++) {
b.row(6 * i + 5) = inPs.col(i).transpose();
}
b.row(6 * N - 3) = tailPVA.col(0).transpose();
b.row(6 * N - 2) = tailPVA.col(1).transpose() * t(1);
b.row(6 * N - 1) = tailPVA.col(2).transpose() * t(2);
A.solve(b);
for (int i = 0; i < N; i++) {
c.block<6, 2>(6 * i, 0) =
b.block<6, 2>(6 * i, 0).array().colwise() * tInv.array();
}
return;
}
inline Trajectory getTraj(int s) const
{
Trajectory traj;
traj.reserve(N);
for (int i = 0; i < N; i++)
{
traj.emplace_back(t(1), c.block<6, 2>(6 * i, 0).transpose().rowwise().reverse(), s);
}
return traj;
}
inline double getTrajJerkCost() const {
double energy = 0.0;
for (int i = 0; i < N; i++) {
energy += 36.0 * c.row(6 * i + 3).squaredNorm() * t(1) +
144.0 * c.row(6 * i + 4).dot(c.row(6 * i + 3)) * t(2) +
192.0 * c.row(6 * i + 4).squaredNorm() * t(3) +
240.0 * c.row(6 * i + 5).dot(c.row(6 * i + 3)) * t(3) +
720.0 * c.row(6 * i + 5).dot(c.row(6 * i + 4)) * t(4) +
720.0 * c.row(6 * i + 5).squaredNorm() * t(5);
}
return energy;
}
inline void initSmGradCost() {
for (int i = 0; i < N; i++) {
gdC.row(6 * i + 5) = 240.0 * c.row(6 * i + 3) * t(3) +
720.0 * c.row(6 * i + 4) * t(4) +
1440.0 * c.row(6 * i + 5) * t(5);
gdC.row(6 * i + 4) = 144.0 * c.row(6 * i + 3) * t(2) +
384.0 * c.row(6 * i + 4) * t(3) +
720.0 * c.row(6 * i + 5) * t(4);
gdC.row(6 * i + 3) = 72.0 * c.row(6 * i + 3) * t(1) +
144.0 * c.row(6 * i + 4) * t(2) +
240.0 * c.row(6 * i + 5) * t(3);
gdC.block<3, 2>(6 * i, 0).setZero();
}
gdT = 0.0;
for (int i = 0; i < N; i++) {
gdT += 36.0 * c.row(6 * i + 3).squaredNorm() +
288.0 * c.row(6 * i + 4).dot(c.row(6 * i + 3)) * t(1) +
576.0 * c.row(6 * i + 4).squaredNorm() * t(2) +
720.0 * c.row(6 * i + 5).dot(c.row(6 * i + 3)) * t(2) +
2880.0 * c.row(6 * i + 5).dot(c.row(6 * i + 4)) * t(3) +
3600.0 * c.row(6 * i + 5).squaredNorm() * t(4);
}
return;
}
inline void calGrads_PT() {
for (int i = 0; i < N; i++) {
adjScaledGrad.block<6, 2>(6 * i, 0) =
gdC.block<6, 2>(6 * i, 0).array().colwise() * tInv.array();
}
A.solveAdj(adjScaledGrad);
for (int i = 0; i < N - 1; i++) {
gdP.col(i) = adjScaledGrad.row(6 * i + 5).transpose();
}
gdHead = adjScaledGrad.topRows(3).transpose() * t.head<3>().asDiagonal();
gdTail = adjScaledGrad.bottomRows(3).transpose() * t.head<3>().asDiagonal();
gdT += headPVA.col(1).dot(adjScaledGrad.row(1));
gdT += headPVA.col(2).dot(adjScaledGrad.row(2)) * 2.0 * t(1);
gdT += tailPVA.col(1).dot(adjScaledGrad.row(6 * N - 2));
gdT += tailPVA.col(2).dot(adjScaledGrad.row(6 * N - 1)) * 2.0 * t(1);
Eigen::Matrix<double, 6, 1> gdtInv;
gdtInv(0) = 0.0;
gdtInv(1) = -1.0 * tInv(2);
gdtInv(2) = -2.0 * tInv(3);
gdtInv(3) = -3.0 * tInv(4);
gdtInv(4) = -4.0 * tInv(5);
gdtInv(5) = -5.0 * tInv(5) * tInv(1);
const Eigen::VectorXd gdcol = gdC.cwiseProduct(b).rowwise().sum();
for (int i = 0; i < N; i++) {
gdT += gdtInv.dot(gdcol.segment<6>(6 * i));
}
return;
}
inline const Eigen::MatrixXd &getCoeffs(void) const {
return c;
}
inline const double &getDt(void) const {
return t(1);
}
inline Eigen::MatrixXd &get_gdC()
{
return gdC;
}
inline double &get_gdT()
{
return gdT;
}
inline Eigen::MatrixXd get_gdHead(void) {
return gdHead;
}
inline Eigen::MatrixXd get_gdTail(void) {
return gdTail;
}
inline Eigen::MatrixXd get_gdP(void) {
return gdP;
}
};
} //namespace plan_utils

1697
trajectoryOpt/src/plan_manage/include/difPlan/quickhull.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

1090
trajectoryOpt/src/plan_manage/include/difPlan/root_finder.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

801
trajectoryOpt/src/plan_manage/include/difPlan/sdlp.hpp Normal file
View File

@@ -0,0 +1,801 @@
/*
* Copyright (c) 1990 Michael E. Hohmeyer,
* hohmeyer@icemcfd.com
* Permission is granted to modify and re-distribute this code in any manner
* as long as this notice is preserved. All standard disclaimers apply.
*
* R. Seidel's algorithm for solving LPs (linear programs.)
*/
/*
* Copyright (c) 2021 Zhepei Wang,
* wangzhepei@live.com
* 1. Bug fix in "move_to_front" function that "prev[m]" is illegally accessed
* while "prev" originally has only m ints. It is fixed by allocating a
* "prev" with m + 1 ints.
* 2. Add Eigen interface.
* Permission is granted to modify and re-distribute this code in any manner
* as long as this notice is preserved. All standard disclaimers apply.
*
* Reference: Seidel, R. (1991), "Small-dimensional linear programming and convex hulls made easy",
* Discrete & Computational Geometry 6 (1): 423–434, doi:10.1007/BF02574699
*/
#ifndef SDLP_HPP
#define SDLP_HPP
#include <Eigen/Eigen>
#include <cmath>
#include <random>
namespace sdlp
{
constexpr double prog_epsilon = 2.0e-16;
enum PROG_STATE
{
/* minimum attained */
MINIMUM = 0,
/* no feasible region */
INFEASIBLE,
/* unbounded solution */
UNBOUNDED,
/* only a vertex in the solution set */
AMBIGUOUS,
};
inline double dot2(const double a[2], const double b[2])
{
return a[0] * b[0] + a[1] * b[1];
}
inline double cross2(const double a[2], const double b[2])
{
return a[0] * b[1] - a[1] * b[0];
}
inline bool unit2(const double a[2], double b[2], double eps)
{
double size;
size = sqrt(a[0] * a[0] + a[1] * a[1]);
if (size < 2 * eps)
{
return true;
}
b[0] = a[0] / size;
b[1] = a[1] / size;
return false;
}
/* unitize a d+1 dimensional point */
inline bool lp_d_unit(int d, const double *a, double *b)
{
int i;
double size;
size = 0.0;
for (i = 0; i <= d; i++)
{
size += a[i] * a[i];
}
if (size < (d + 1) * prog_epsilon * prog_epsilon)
{
return true;
}
size = 1.0 / sqrt(size);
for (i = 0; i <= d; i++)
{
b[i] = a[i] * size;
}
return false;
}
/* optimize the objective function when there are no contraints */
inline int lp_no_constraints(int d, const double *n_vec, const double *d_vec, double *opt)
{
int i;
double n_dot_d, d_dot_d;
n_dot_d = 0.0;
d_dot_d = 0.0;
for (i = 0; i <= d; i++)
{
n_dot_d += n_vec[i] * d_vec[i];
d_dot_d += d_vec[i] * d_vec[i];
}
if (d_dot_d < prog_epsilon * prog_epsilon)
{
d_dot_d = 1.0;
n_dot_d = 0.0;
}
for (i = 0; i <= d; i++)
{
opt[i] = -n_vec[i] + d_vec[i] * n_dot_d / d_dot_d;
}
/* normalize the optimal point */
if (lp_d_unit(d, opt, opt))
{
opt[d] = 1.0;
return AMBIGUOUS;
}
else
{
return MINIMUM;
}
}
/* returns the index of the plane that is in i's place */
inline int move_to_front(int i, int *next, int *prev)
{
int previ;
if (i == 0 || i == next[0])
{
return i;
}
previ = prev[i];
/* remove i from it's current position */
next[prev[i]] = next[i];
prev[next[i]] = prev[i];
/* put i at the front */
next[i] = next[0];
prev[i] = 0;
prev[next[i]] = i;
next[0] = i;
return previ;
}
inline void lp_min_lin_rat(int degen,
const double cw_vec[2],
const double ccw_vec[2],
const double n_vec[2],
const double d_vec[2],
double opt[2])
{
double d_cw, d_ccw, n_cw, n_ccw;
/* linear rational function case */
d_cw = dot2(cw_vec, d_vec);
d_ccw = dot2(ccw_vec, d_vec);
n_cw = dot2(cw_vec, n_vec);
n_ccw = dot2(ccw_vec, n_vec);
if (degen)
{
/* if degenerate simply compare values */
if (n_cw / d_cw < n_ccw / d_ccw)
{
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
/* check that the clock-wise and counter clockwise bounds are not near a poles */
}
else if (fabs(d_cw) > 2.0 * prog_epsilon &&
fabs(d_ccw) > 2.0 * prog_epsilon)
{
/* the valid region does not contain a poles */
if (d_cw * d_ccw > 0.0)
{
/* find which end has the minimum value */
if (n_cw / d_cw < n_ccw / d_ccw)
{
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
}
else
{
/* the valid region does contain a poles */
if (d_cw > 0.0)
{
opt[0] = -d_vec[1];
opt[1] = d_vec[0];
}
else
{
opt[0] = d_vec[1];
opt[1] = -d_vec[0];
}
}
}
else if (fabs(d_cw) > 2.0 * prog_epsilon)
{
/* the counter clockwise bound is near a pole */
if (n_ccw * d_cw > 0.0)
{
/* counter clockwise bound is a positive pole */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
/* counter clockwise bound is a negative pole */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
}
else if (fabs(d_ccw) > 2.0 * prog_epsilon)
{
/* the clockwise bound is near a pole */
if (n_cw * d_ccw > 2 * prog_epsilon)
{
/* clockwise bound is at a positive pole */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
else
{
/* clockwise bound is at a negative pole */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
}
else
{
/* both bounds are near poles */
if (cross2(d_vec, n_vec) > 0.0)
{
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
else
{
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
}
}
inline int wedge(const double (*halves)[2],
int m,
int *next,
int *prev,
double cw_vec[2],
double ccw_vec[2],
int *degen)
{
int i;
double d_cw, d_ccw;
int offensive;
*degen = 0;
for (i = 0; i != m; i = next[i])
{
if (!unit2(halves[i], ccw_vec, prog_epsilon))
{
/* clock-wise */
cw_vec[0] = ccw_vec[1];
cw_vec[1] = -ccw_vec[0];
/* counter-clockwise */
ccw_vec[0] = -cw_vec[0];
ccw_vec[1] = -cw_vec[1];
break;
}
}
if (i == m)
{
return UNBOUNDED;
}
i = 0;
while (i != m)
{
offensive = 0;
d_cw = dot2(cw_vec, halves[i]);
d_ccw = dot2(ccw_vec, halves[i]);
if (d_ccw >= 2 * prog_epsilon)
{
if (d_cw <= -2 * prog_epsilon)
{
cw_vec[0] = halves[i][1];
cw_vec[1] = -halves[i][0];
unit2(cw_vec, cw_vec, prog_epsilon);
offensive = 1;
}
}
else if (d_cw >= 2 * prog_epsilon)
{
if (d_ccw <= -2 * prog_epsilon)
{
ccw_vec[0] = -halves[i][1];
ccw_vec[1] = halves[i][0];
unit2(ccw_vec, ccw_vec, prog_epsilon);
offensive = 1;
}
}
else if (d_ccw <= -2 * prog_epsilon && d_cw <= -2 * prog_epsilon)
{
return INFEASIBLE;
}
else if ((d_cw <= -2 * prog_epsilon) ||
(d_ccw <= -2 * prog_epsilon) ||
(cross2(cw_vec, halves[i]) < 0.0))
{
/* degenerate */
if (d_cw <= -2 * prog_epsilon)
{
unit2(ccw_vec, cw_vec, prog_epsilon);
}
else if (d_ccw <= -2 * prog_epsilon)
{
unit2(cw_vec, ccw_vec, prog_epsilon);
}
*degen = 1;
offensive = 1;
}
/* place this offensive plane in second place */
if (offensive)
{
i = move_to_front(i, next, prev);
}
i = next[i];
if (*degen)
{
break;
}
}
if (*degen)
{
while (i != m)
{
d_cw = dot2(cw_vec, halves[i]);
d_ccw = dot2(ccw_vec, halves[i]);
if (d_cw < -2 * prog_epsilon)
{
if (d_ccw < -2 * prog_epsilon)
{
return INFEASIBLE;
}
else
{
cw_vec[0] = ccw_vec[0];
cw_vec[1] = ccw_vec[1];
}
}
else if (d_ccw < -2 * prog_epsilon)
{
ccw_vec[0] = cw_vec[0];
ccw_vec[1] = cw_vec[1];
}
i = next[i];
}
}
return MINIMUM;
}
/*
* return the minimum on the projective line
*/
inline int lp_base_case(const double (*halves)[2], /* halves --- half lines */
int m, /* m --- terminal marker */
const double n_vec[2], /* n_vec --- numerator funciton */
const double d_vec[2], /* d_vec --- denominator function */
double opt[2], /* opt --- optimum */
int *next, /* next, prev --- double linked list of indices */
int *prev)
{
double cw_vec[2], ccw_vec[2];
int degen;
int status;
double ab;
/* find the feasible region of the line */
status = wedge(halves, m, next, prev, cw_vec, ccw_vec, &degen);
if (status == INFEASIBLE)
{
return status;
}
/* no non-trivial constraints one the plane: return the unconstrained optimum */
if (status == UNBOUNDED)
{
return lp_no_constraints(1, n_vec, d_vec, opt);
}
ab = fabs(cross2(n_vec, d_vec));
if (ab < 2 * prog_epsilon * prog_epsilon)
{
if (dot2(n_vec, n_vec) < 2 * prog_epsilon * prog_epsilon ||
dot2(d_vec, d_vec) > 2 * prog_epsilon * prog_epsilon)
{
/* numerator is zero or numerator and denominator are linearly dependent */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
status = AMBIGUOUS;
}
else
{
/* numerator is non-zero and denominator is zero minimize linear functional on circle */
if (!degen && cross2(cw_vec, n_vec) <= 0.0 &&
cross2(n_vec, ccw_vec) <= 0.0)
{
/* optimum is in interior of feasible region */
opt[0] = -n_vec[0];
opt[1] = -n_vec[1];
}
else if (dot2(n_vec, cw_vec) > dot2(n_vec, ccw_vec))
{
/* optimum is at counter-clockwise boundary */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
else
{
/* optimum is at clockwise boundary */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
status = MINIMUM;
}
}
else
{
/* niether numerator nor denominator is zero */
lp_min_lin_rat(degen, cw_vec, ccw_vec, n_vec, d_vec, opt);
status = MINIMUM;
}
return status;
}
/* find the largest coefficient in a plane */
inline void findimax(const double *pln, int idim, int *imax)
{
double rmax;
int i;
*imax = 0;
rmax = fabs(pln[0]);
for (i = 1; i <= idim; i++)
{
double ab;
ab = fabs(pln[i]);
if (ab > rmax)
{
*imax = i;
rmax = ab;
}
}
}
inline void vector_up(const double *equation, int ivar, int idim,
const double *low_vector, double *vector)
{
int i;
vector[ivar] = 0.0;
for (i = 0; i < ivar; i++)
{
vector[i] = low_vector[i];
vector[ivar] -= equation[i] * low_vector[i];
}
for (i = ivar + 1; i <= idim; i++)
{
vector[i] = low_vector[i - 1];
vector[ivar] -= equation[i] * low_vector[i - 1];
}
vector[ivar] /= equation[ivar];
}
inline void vector_down(const double *elim_eqn, int ivar, int idim,
const double *old_vec, double *new_vec)
{
int i;
double fac, ve, ee;
ve = 0.0;
ee = 0.0;
for (i = 0; i <= idim; i++)
{
ve += old_vec[i] * elim_eqn[i];
ee += elim_eqn[i] * elim_eqn[i];
}
fac = ve / ee;
for (i = 0; i < ivar; i++)
{
new_vec[i] = old_vec[i] - elim_eqn[i] * fac;
}
for (i = ivar + 1; i <= idim; i++)
{
new_vec[i - 1] = old_vec[i] - elim_eqn[i] * fac;
}
}
inline void plane_down(const double *elim_eqn, int ivar, int idim,
const double *old_plane, double *new_plane)
{
double crit;
int i;
crit = old_plane[ivar] / elim_eqn[ivar];
for (i = 0; i < ivar; i++)
{
new_plane[i] = old_plane[i] - elim_eqn[i] * crit;
}
for (i = ivar + 1; i <= idim; i++)
{
new_plane[i - 1] = old_plane[i] - elim_eqn[i] * crit;
}
}
inline int linfracprog(const double *halves, /* halves --- half spaces */
int istart, /* istart --- should be zero unless doing incremental algorithm */
int m, /* m --- terminal marker */
const double *n_vec, /* n_vec --- numerator vector */
const double *d_vec, /* d_vec --- denominator vector */
int d, /* d --- projective dimension */
double *opt, /* opt --- optimum */
double *work, /* work --- work space (see below) */
int *next, /* next --- array of indices into halves */
int *prev, /* prev --- array of indices into halves */
int max_size) /* max_size --- size of halves array */
/*
**
** half-spaces are in the form
** halves[i][0]*x[0] + halves[i][1]*x[1] +
** ... + halves[i][d-1]*x[d-1] + halves[i][d]*x[d] >= 0
**
** coefficients should be normalized
** half-spaces should be in random order
** the order of the half spaces is 0, next[0] next[next[0]] ...
** and prev[next[i]] = i
**
** halves: (max_size)x(d+1)
**
** the optimum has been computed for the half spaces
** 0 , next[0], next[next[0]] , ... , prev[istart]
** the next plane that needs to be tested is istart
**
** m is the index of the first plane that is NOT on the list
** i.e. m is the terminal marker for the linked list.
**
** the objective function is dot(x,nvec)/dot(x,dvec)
** if you want the program to solve standard d dimensional linear programming
** problems then n_vec = ( x0, x1, x2, ..., xd-1, 0)
** and d_vec = ( 0, 0, 0, ..., 0, 1)
** and halves[0] = (0, 0, ... , 1)
**
** work points to (max_size+3)*(d+2)*(d-1)/2 double space
*/
{
int status;
int i, j, imax;
double *new_opt, *new_n_vec, *new_d_vec, *new_halves, *new_work;
const double *plane_i;
double val;
if (d == 1 && m != 0)
{
return lp_base_case((const double(*)[2])halves, m, n_vec,
d_vec, opt, next, prev);
}
else
{
int d_vec_zero;
val = 0.0;
for (j = 0; j <= d; j++)
{
val += d_vec[j] * d_vec[j];
}
d_vec_zero = (val < (d + 1) * prog_epsilon * prog_epsilon);
/* find the unconstrained minimum */
if (!istart)
{
status = lp_no_constraints(d, n_vec, d_vec, opt);
}
else
{
status = MINIMUM;
}
if (m == 0)
{
return status;
}
/* allocate memory for next level of recursion */
new_opt = work;
new_n_vec = new_opt + d;
new_d_vec = new_n_vec + d;
new_halves = new_d_vec + d;
new_work = new_halves + max_size * d;
for (i = istart; i != m; i = next[i])
{
/* if the optimum is not in half space i then project the problem onto that plane */
plane_i = halves + i * (d + 1);
/* determine if the optimum is on the correct side of plane_i */
val = 0.0;
for (j = 0; j <= d; j++)
{
val += opt[j] * plane_i[j];
}
if (val < -(d + 1) * prog_epsilon)
{
/* find the largest of the coefficients to eliminate */
findimax(plane_i, d, &imax);
/* eliminate that variable */
if (i != 0)
{
double fac;
fac = 1.0 / plane_i[imax];
for (j = 0; j != i; j = next[j])
{
const double *old_plane;
double *new_plane;
int k;
double crit;
old_plane = halves + j * (d + 1);
new_plane = new_halves + j * d;
crit = old_plane[imax] * fac;
for (k = 0; k < imax; k++)
{
new_plane[k] = old_plane[k] - plane_i[k] * crit;
}
for (k = imax + 1; k <= d; k++)
{
new_plane[k - 1] = old_plane[k] - plane_i[k] * crit;
}
}
}
/* project the objective function to lower dimension */
if (d_vec_zero)
{
vector_down(plane_i, imax, d, n_vec, new_n_vec);
for (j = 0; j < d; j++)
{
new_d_vec[j] = 0.0;
}
}
else
{
plane_down(plane_i, imax, d, n_vec, new_n_vec);
plane_down(plane_i, imax, d, d_vec, new_d_vec);
}
/* solve sub problem */
status = linfracprog(new_halves, 0, i, new_n_vec,
new_d_vec, d - 1, new_opt, new_work, next, prev, max_size);
/* back substitution */
if (status != INFEASIBLE)
{
vector_up(plane_i, imax, d, new_opt, opt);
/* in line code for unit */
double size;
size = 0.0;
for (j = 0; j <= d; j++)
size += opt[j] * opt[j];
size = 1.0 / sqrt(size);
for (j = 0; j <= d; j++)
opt[j] *= size;
}
else
{
return status;
}
/* place this offensive plane in second place */
i = move_to_front(i, next, prev);
}
}
return status;
}
}
inline void rand_permutation(int n, int *p)
{
typedef std::uniform_int_distribution<int> rand_int;
typedef rand_int::param_type rand_range;
static std::mt19937_64 gen;
static rand_int rdi(0, 1);
int i, j, t;
for (i = 0; i < n; i++)
{
p[i] = i;
}
for (i = 0; i < n; i++)
{
rdi.param(rand_range(0, n - i - 1));
j = rdi(gen) + i;
t = p[j];
p[j] = p[i];
p[i] = t;
}
}
inline double linprog(const Eigen::VectorXd &c,
const Eigen::MatrixXd &A,
const Eigen::VectorXd &b,
Eigen::VectorXd &x)
/*
** min cTx, s.t. Ax<=b
** dim(x) << dim(b)
*/
{
int d = c.size();
int m = b.size() + 1;
x = Eigen::VectorXd::Zero(d);
double minimum = INFINITY;
int *perm, *next, *prev;
double *halves, *n_vec, *d_vec, *work, *opt;
int i, status = sdlp::AMBIGUOUS;
perm = (int *)malloc((m - 1) * sizeof(int));
next = (int *)malloc(m * sizeof(int));
/* original allocated size is m, here changed by m + 1 for legal tail accessing */
prev = (int *)malloc((m + 1) * sizeof(int));
halves = (double *)malloc(m * (d + 1) * sizeof(double));
n_vec = (double *)malloc((d + 1) * sizeof(double));
d_vec = (double *)malloc((d + 1) * sizeof(double));
work = (double *)malloc((m + 3) * (d + 2) * (d - 1) / 2 * sizeof(double));
opt = (double *)malloc((d + 1) * sizeof(double));
Eigen::Map<Eigen::MatrixXd> Af(halves, d + 1, m);
Eigen::Map<Eigen::VectorXd> nv(n_vec, d + 1);
Eigen::Map<Eigen::VectorXd> dv(d_vec, d + 1);
Eigen::Map<Eigen::VectorXd> xf(opt, d + 1);
Af.col(0).setZero();
Af(d, 0) = 1.0;
Af.topRightCorner(d, m - 1) = -A.transpose();
Af.bottomRightCorner(1, m - 1) = b.transpose();
nv.head(d) = c;
nv(d) = 0.0;
dv.setZero();
dv(d) = 1.0;
/* randomize the input planes */
rand_permutation(m - 1, perm);
/* previous to 0 is actually never used */
prev[0] = 0;
/* link the zero position in at the beginning */
next[0] = perm[0] + 1;
prev[perm[0] + 1] = 0;
/* link the other planes */
for (i = 0; i < m - 2; i++)
{
next[perm[i] + 1] = perm[i + 1] + 1;
prev[perm[i + 1] + 1] = perm[i] + 1;
}
/* flag the last plane */
next[perm[m - 2] + 1] = m;
status = sdlp::linfracprog(halves, 0, m, n_vec, d_vec,
d, opt, work, next, prev, m);
/* handle states for linprog whose definitions differ from linfracprog */
if (status != sdlp::INFEASIBLE)
{
if (xf(d) != 0.0 && status != sdlp::UNBOUNDED)
{
x = xf.head(d) / xf(d);
minimum = c.dot(x);
}
if (xf(d) == 0.0 || status == sdlp::UNBOUNDED)
{
x = xf.head(d);
minimum = -INFINITY;
}
}
free(perm);
free(next);
free(prev);
free(halves);
free(n_vec);
free(d_vec);
free(work);
free(opt);
return minimum;
}
} // namespace sdlp
#endif

1015
trajectoryOpt/src/plan_manage/include/difPlan/trajDif_opt.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

53
trajectoryOpt/src/plan_manage/include/plan_manage/plan_manage.h Normal file
View File

@@ -0,0 +1,53 @@
#ifndef PLAN_MANAGE_HPP
#define PLAN_MANAGE_HPP
#include <memory>
#include <set>
#include <string>
#include <thread>
#include <iostream>
#include <sstream>
#include <ros/ros.h>
#include <path_searching/kino_astar.h>
#include <tools/visualization.hpp>
#include <nav_msgs/Odometry.h>
// #include <mpc_controller/SE2Traj.h>
// #include <mpc_controller/PolyTraj.h>
// #include <mpc_controller/SinglePoly.h>
// #include <mpc_controller/SinglePolyAC.h>
// #include <mpc_controller/PolyTrajAC.h>
// #include <mpc_controller/DPtrajContainer.h>
#define BUDGET 0.1
namespace plan_manage{
class PlanManager{
public:
PlanManager(){};
void init(ros::NodeHandle & nh);
private:
bool hasTarget = false, hasOdom = false;
Eigen::Vector3d targetPose, odom;
std::shared_ptr<visualization::Visualization> vis_tool;
std::shared_ptr<Config> config_;
std::unique_ptr<path_searching::KinoAstar> kino_path_finder_;
map_util::OccMapUtil gridMap;
Eigen::Matrix<double, 2, 3> iniState2d, finState2d;
Eigen::MatrixXd initInnerPts2d;
std::vector<Eigen::MatrixXd> hPolys;
void process(const ros::TimerEvent &);
void targetCallback(const geometry_msgs::PoseStamped &msg);
void odomCallback(const nav_msgs::OdometryPtr &msg);
double pieceTime;
/*ros related*/
ros::Timer processTimer;
ros::Subscriber targetSub, odomSub;
ros::Publisher trajCmdPub;
int startid = 10000, pathid = 1;
};
}
#endif

12
trajectoryOpt/src/plan_manage/launch/plan_node.launch Normal file
View File

@@ -0,0 +1,12 @@
<launch>
<node pkg="plan_manage" type="plan_node" name="plan_node0" output="screen">
<rosparam file="$(find plan_manage)/config/planning.yaml" command="load" />
</node>
<node pkg="plan_manage" type="plan_node" name="plan_node1" output="screen">
<rosparam file="$(find plan_manage)/config/planning_v3.yaml" command="load" />
</node>
</launch>

36
trajectoryOpt/src/plan_manage/package.xml Normal file
View File

@@ -0,0 +1,36 @@
<?xml version="1.0"?>
<package>
<name>plan_manage</name>
<maintainer email="zhichaohan@zju.edu.cn">zhichaohan</maintainer>
<version>0.1.0</version>
<description> the plan_manage package</description>
<license>GPLV3</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>tools</build_depend>
<build_depend>path_search</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>decomp_ros_utils</build_depend>
<build_depend>mpc_controller</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>tools</run_depend>
<run_depend>path_search</run_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>decomp_ros_utils</run_depend>
<run_depend>mpc_controller</run_depend>
</package>

157
trajectoryOpt/src/plan_manage/rviz/default.rviz Normal file
View File

@@ -0,0 +1,157 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud21
Splitter Ratio: 0.5
Tree Height: 719
- 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
Name: Time
SyncMode: 0
SyncSource: PointCloud2
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: 30
Reference Frame: <Fixed Frame>
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/PointCloud2
Color: 85; 87; 83
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Boxes
Topic: /global_pcl
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /ugv/mesh
Name: Marker
Namespaces:
"": true
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 238; 238; 236
Default Light: true
Fixed Frame: world
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: 16.154529571533203
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -3.8151516914367676
Y: 5.991873264312744
Z: 0.005227289162576199
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.5647963285446167
Target Frame: <Fixed Frame>
Yaw: 4.7135844230651855
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1848
X: 72
Y: 27

385
trajectoryOpt/src/plan_manage/src/plan_manage.cpp Normal file
View File

@@ -0,0 +1,385 @@
#include <plan_manage/plan_manage.h>
#include <tf/tf.h>
#include <tools/tic_toc.hpp>
#include <arcPlan/Trajopt_alm.hpp>
#include <difPlan/trajDif_opt.hpp>
// #include <arcPlan/Trajopt_penalty.hpp>
#include <fstream>
#include <random>
using namespace plan_manage;
using namespace std;
void PlanManager::init(ros::NodeHandle& nh){
vis_tool.reset(new visualization::Visualization(nh));
config_.reset(new Config(nh));
gridMap.setParam(config_, nh);
//read map
hasTarget = false;
hasOdom = false;
processTimer = nh.createTimer(ros::Duration(0.02), &PlanManager::process, this);
targetSub = nh.subscribe("/move_base_simple/goal", 1, &PlanManager::targetCallback, this);
odomSub = nh.subscribe("/ugv/odometry", 1, &PlanManager::odomCallback, this);
vis_tool->registe<nav_msgs::Path>("/visualization/kinoPath");
vis_tool->registe<nav_msgs::Path>("/visualization/AstarPath");
vis_tool->registe<nav_msgs::Path>("/visualization/optTraj");
vis_tool->registe<nav_msgs::Path>("/visualization/optTraj_kino");
vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/debugTraj");
vis_tool->registe<nav_msgs::Path>("/visualization/NewTraj");
vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/waittoRefine");
vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/optArrowTraj");
vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/optArrowTraj2");
vis_tool->registe<decomp_ros_msgs::PolyhedronArray>("/visualization/sfc");
vis_tool->registe<nav_msgs::Path>("/visualization/refinedTraj2");
vis_tool->registe<nav_msgs::Path>("/visualization/debugRefinedTraj2");
vis_tool->registe<nav_msgs::Path>("/visualization/debugSe2Traj");
vis_tool->registe<visualization_msgs::MarkerArray>("/visualization/arrowTraj");
vis_tool->registe<sensor_msgs::PointCloud2>("/visualization/wapoints_nn");
vis_tool->registe<nav_msgs::Path>("/visualization/refinedTraj_nn");
vis_tool->registe<visualization_msgs::Marker>("/visualization/fullshapeTraj_nn");
vis_tool->registe<sensor_msgs::PointCloud2>("/visualization/wapoints_kinoastar");
vis_tool->registe<nav_msgs::Path>("/visualization/refinedTraj_dptraj");
vis_tool->registe<nav_msgs::Path>("/visualization/refinedTraj_dftpav");
vis_tool->registe<visualization_msgs::Marker>("/visualization/fullshapeTraj_kinoastar");
pieceTime = config_->pieceTime;
kino_path_finder_.reset(new path_searching::KinoAstar);
kino_path_finder_->init(config_, nh);
kino_path_finder_->intialMap(&gridMap);
// if(config_->useDP){
// trajCmdPub = nh.advertise<mpc_controller::DPtrajContainer>("/planner/dptrajectory", 1);
// }
// else{
// trajCmdPub = nh.advertise<mpc_controller::PolyTraj>("/planner/trajectory", 1);
// }
}
void PlanManager::odomCallback(const nav_msgs::OdometryPtr &msg)
{
odom[0] = msg->pose.pose.position.x;
odom[1] = msg->pose.pose.position.y;
Eigen::Quaterniond q(msg->pose.pose.orientation.w,
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z);
Eigen::Matrix3d R(q);
odom[2] = atan2(R.col(0)[1],R.col(0)[0]);
hasOdom = true;
return;
}
void PlanManager::targetCallback(const geometry_msgs::PoseStamped &msg){
ROS_INFO("Recieved target!");
targetPose << msg.pose.position.x, msg.pose.position.y,
tf::getYaw(msg.pose.orientation);
// targetPose << (6.0+5.3) / 2, 5.5, -M_PI / 2.0;
// targetPose << 11.701, 4.11058, -1.84895;
std::cout<<"targetPose: "<<targetPose.transpose()<<std::endl;
hasTarget = true;
return;
}
void PlanManager::process(const ros::TimerEvent &){
if(!gridMap.has_map_()) return;
// neural_path_finder_->intialMap(&gridMap);
gridMap.publishPCL();
// std::cout << "pcl number: "<<gridMap.getCloud().size()<<std::endl;
if(!hasTarget || !hasOdom) return;
ROS_WARN("Triggering------------------------------------ we begin to plan a trajectory!");
hasTarget = false;
{
//-9.5,0,0 0,9.5,pi/2 9.5,0,pi 0,-9.5,-pi/2
//-9.5 5 -2.5 9.5 4.5 5 -2.5 0.5
vector<Eigen::Vector3d> sampleTraj;
path_searching::KinoTrajData kino_trajs_;
std::vector<Eigen::Vector3d> visKinoPath;
{
kino_path_finder_->reset();
Eigen::Vector4d iniFs, finFs;
iniFs << -9.5,5, 0, 0.0;
finFs << -2.5,9.5,M_PI_2, 0.0;
// iniFs << 0,9.5,-M_PI_2, 0.0;
// finFs << 9.5,0,0, 0.0;
TicToc time_profile_tool_;
time_profile_tool_.tic();
double model_time;
double t1 = ros::Time::now().toSec();
int status = kino_path_finder_->search(iniFs, Eigen::Vector2d::Zero(), finFs, model_time, true);
double t2 = ros::Time::now().toSec();
std::vector<Eigen::Vector3d> ts = kino_path_finder_->getRoughSamples();
sampleTraj.insert(sampleTraj.end(), ts.begin(), ts.end());
}
{
kino_path_finder_->reset();
Eigen::Vector4d iniFs, finFs;
iniFs << -2.5,9.5,M_PI_2, 0.0;
finFs << 4.5,5,M_PI, 0.0;
TicToc time_profile_tool_;
time_profile_tool_.tic();
double model_time;
double t1 = ros::Time::now().toSec();
int status = kino_path_finder_->search(iniFs, Eigen::Vector2d::Zero(), finFs, model_time, true);
double t2 = ros::Time::now().toSec();
std::vector<Eigen::Vector3d> ts = kino_path_finder_->getRoughSamples();
sampleTraj.insert(sampleTraj.end(), ts.begin(), ts.end());
}
double ddd = -2.5, sss = 0.5;
{
kino_path_finder_->reset();
Eigen::Vector4d iniFs, finFs;
iniFs << 4.5,5,M_PI, 0.0;
finFs << ddd ,sss,M_PI_2, 0.0;
TicToc time_profile_tool_;
time_profile_tool_.tic();
double model_time;
double t1 = ros::Time::now().toSec();
int status = kino_path_finder_->search(iniFs, Eigen::Vector2d::Zero(), finFs, model_time, true);
double t2 = ros::Time::now().toSec();
std::vector<Eigen::Vector3d> ts = kino_path_finder_->getRoughSamples();
sampleTraj.insert(sampleTraj.end(), ts.begin(), ts.end());
}
// {
// kino_path_finder_->reset();
// Eigen::Vector4d iniFs, finFs;
// iniFs << 0,-9.5,-M_PI_2, 0.0;
// finFs << -9.5,0,0, 0.0;
// TicToc time_profile_tool_;
// time_profile_tool_.tic();
// double model_time;
// double t1 = ros::Time::now().toSec();
// int status = kino_path_finder_->search(iniFs, Eigen::Vector2d::Zero(), finFs, model_time, true);
// double t2 = ros::Time::now().toSec();
// std::vector<Eigen::Vector3d> ts = kino_path_finder_->getRoughSamples();
// sampleTraj.insert(sampleTraj.end(), ts.begin(), ts.end());
// }
kino_path_finder_->getKinoNode(kino_trajs_, sampleTraj);
// kino_path_finder_->getKinoNode(kino_trajs_);
for(int i = 0; i < sampleTraj.size(); i++){
Eigen::Vector3d pos;
pos.head(2) = sampleTraj[i].head(2);
pos[2] = 0.2;
visKinoPath.push_back(pos);
}
vis_tool->visualize_path(visKinoPath, "/visualization/kinoPath");
//use neural network to get initial guess？？？
int segnum = kino_trajs_.size();
std::vector<int> refined_singuals; refined_singuals.resize(segnum);
Eigen::VectorXd refined_rt;
refined_rt.resize(segnum);
std::vector<Eigen::MatrixXd> refined_inPs_container;
refined_inPs_container.resize(segnum);
std::vector<Eigen::Vector2d> refined_gearPos;
std::vector<double> refined_angles;
refined_gearPos.resize(segnum - 1); refined_angles.resize(segnum - 1);
double basetime = 0.0;
std::vector<int> pnums;
for(int i = 0; i < segnum; i++){
double timePerPiece = pieceTime;
path_searching::FlatTrajData kino_traj = kino_trajs_.at(i);
refined_singuals[i] = kino_traj.singul;
std::vector<Eigen::Vector3d> pts = kino_traj.traj_pts;
int piece_nums;
double initTotalduration = 0.0;
for(const auto pt : pts){
initTotalduration += pt[2];
}
piece_nums = std::max(int(initTotalduration / timePerPiece + 0.5),1);
double dt = initTotalduration / piece_nums;
refined_rt[i] = (initTotalduration / piece_nums);
pnums.push_back(piece_nums);
refined_inPs_container[i].resize(2, piece_nums - 1);
for(int j = 0; j < piece_nums - 1; j++ ){
double t = basetime + (j+1)*dt;
Eigen::Vector3d pos = kino_path_finder_->evaluatePos(t);
refined_inPs_container[i].col(j) = pos.head(2);
}
if(i >=1){
Eigen::Vector3d pos = kino_path_finder_->evaluatePos(basetime);
refined_gearPos[i-1] = pos.head(2);
refined_angles[i-1] = pos[2];
}
basetime += initTotalduration;
}
Eigen::Vector3d ini;ini << -9.5,5.0, 0.0;
Eigen::Vector3d fin;fin << ddd,sss,M_PI_2;
// std::cout <<"11111111111111111\n";
iniState2d << ini[0], refined_singuals[0] * cos(ini[2]), 0.0,
ini[1], refined_singuals[0] * sin(ini[2]), 0.0;
finState2d << fin[0], refined_singuals[segnum-1] * cos(fin[2]), 0.0,
fin[1], refined_singuals[segnum-1] * sin(fin[2]), 0.0;
// ROS_INFO("begin to refine");
// PolyTrajOpt::TrajOpt refinedOpt;
// refined_rt[0] = 6.4 / pnums[0];
// refined_rt[1] = 6.59023 / pnums[1];
// refined_rt[2] = 6.97846 / pnums[2];
// refined_rt[3] = 2.83716 / pnums[3];
refined_rt[0] = 6.6 / pnums[0];
refined_rt[1] = 7.0 / pnums[1];
refined_rt[2] = 7.3 / pnums[2];
refined_rt[3] = 3.6 / pnums[3];
if(config_->useDP){
PolyTrajOpt::TrajOpt refinedOpt;
// 25ms
double t3 = ros::Time::now().toSec();
int flagSs = refinedOpt.OptimizeSe2Trajectory(
iniState2d, finState2d, refined_rt,
refined_inPs_container, refined_gearPos,
refined_angles, &gridMap, config_, refined_singuals, vis_tool,"dptraj");
PolyTrajOpt::UgvTrajectory optTraj = refinedOpt.getOptTraj();
// ROS_WARN_STREAM("dptraj total arc: "<<optTraj.getTotalArc());
// ROS_WARN_STREAM("dptraj traj time: "<<optTraj.getTotalDuration());
// std::cout << "durations: "<<optTraj.getDurations().transpose()<<std::endl;
// for(int i = 0; i < optTraj.etas.size(); i++){
// std::cout <<"i: "<<i<<" "<<optTraj.Traj_container[i].getTotalDuration()<<std::endl;
// }
// ofstream f_dp("/home/han/2023codes/NeuralTraj/dp_traj.txt",ios::out);
// for(double t = 0.0; t <= optTraj.getTotalDuration(); t+= 0.01){
// Eigen::Vector2d pos = optTraj.getPos(t);
// double yaw = optTraj.getYaw(t);
// f_dp << t <<" "<<pos[0]<<" "<<pos[1]<<" "<<yaw<<std::endl;
// }
/*
ofstream f_omega("/home/han/2023codes/NeuralTraj/data/omega.txt",ios::out);
ofstream f_phi("/home/han/2023codes/NeuralTraj/data/phi.txt",ios::out);
ofstream f_t("/home/han/2023codes/NeuralTraj/data/t.txt",ios::out);
ofstream f_v("/home/han/2023codes/NeuralTraj/data/v.txt",ios::out);
ofstream f_a("/home/han/2023codes/NeuralTraj/data/a.txt",ios::out);
double maxomega = 0.0;
for(double t = 0.0; t <= optTraj.getTotalDuration(); t+=0.01){
double phi = optTraj.getPhi(t);
double omega = optTraj.getOmega(t);
double lonv = optTraj.getVelItem(t);
f_omega << omega << " ";
f_phi << phi << " ";
f_v << lonv << " ";
f_t << t <<" ";
if(fabs(omega) > fabs(maxomega)){
maxomega = omega;
}
}
f_omega.close();
f_phi.close();
f_v.close();
f_a.close();
f_t.close();*/
// {
// mpc_controller::DPtrajContainer trajmsg;
// for(int i = 0; i < optTraj.getSegNum(); i++){
// int singual = optTraj.etas[i];
// mpc_controller::PolyTrajAC trajSegment;
// for(int j = 0; j < optTraj.Traj_container[i].getPieceNum(); j++){
// mpc_controller::SinglePolyAC piece;
// piece.dt = optTraj.Traj_container[i].tTraj[j].getDuration();
// piece.ds = optTraj.Traj_container[i].posTraj[j].getDuration();
// Eigen::Matrix<double, 2, 6> c = optTraj.Traj_container[i].posTraj[j].getCoeffMat();
// Eigen::Matrix<double, 1, 6> c1 = optTraj.Traj_container[i].tTraj[j].getCoeffMat();
// for (int k=0; k<6; k++)
// {
// piece.coef_x.push_back(c(0, k));
// piece.coef_y.push_back(c(1, k));
// piece.coef_s.push_back(c1(0, k));
// }
// trajSegment.trajs.push_back(piece);
// }
// trajmsg.traj_container.push_back(trajSegment);
// // trajmsg.reverse
// if(singual > 0) trajmsg.reverse.push_back(false);
// else trajmsg.reverse.push_back(true);
// }
// trajmsg.start_time = ros::Time::now();
// trajCmdPub.publish(trajmsg);
// }
}
else{
dftpav::PolyTrajOptimizer refinedOpt;
// 25ms
double t3 = ros::Time::now().toSec();
int flagSs = refinedOpt.OptimizeSe2Trajectory(
iniState2d, finState2d, refined_rt,
refined_inPs_container, refined_gearPos,
refined_angles, &gridMap, config_, refined_singuals, vis_tool,"dftpav");
dftpav::DifTrajectory optTraj = refinedOpt.getOptTraj();
// ROS_WARN_STREAM("dftpav total arc: "<<optTraj.getTotalArc());
// ROS_WARN_STREAM("dftpav traj time: "<<optTraj.getTotalDuration());
// ofstream f_dfp("/home/han/2023codes/NeuralTraj/dfp_traj.txt",ios::out);
// for(double t = 0.0; t <= optTraj.getTotalDuration(); t+= 0.01){
// Eigen::Vector2d pos = optTraj.getPos(t);
// double yaw = optTraj.getYaw(t);
// f_dfp << t <<" "<<pos[0]<<" "<<pos[1]<<" "<<yaw<<std::endl;
// }
/*
ofstream f_omega("/home/han/2023codes/NeuralTraj/omega.txt",ios::out);
ofstream f_phi("/home/han/2023codes/NeuralTraj/phi.txt",ios::out);
ofstream f_t("/home/han/2023codes/NeuralTraj/t.txt",ios::out);
ofstream f_v("/home/han/2023codes/NeuralTraj/v.txt",ios::out);
ofstream f_a("/home/han/2023codes/NeuralTraj/a.txt",ios::out);
double maxomega = 0.0;
for(double t = 0.0; t <= optTraj.getTotalDuration(); t+=0.01){
double phi = optTraj.getPhi(t);
double omega = optTraj.getOmega(t);
double lonv = optTraj.getVelItem(t);
f_omega << omega << " ";
f_phi << phi << " ";
f_v << lonv << " ";
f_t << t <<" ";
if(fabs(omega) > fabs(maxomega)){
maxomega = omega;
}
}
f_omega.close();
f_phi.close();
f_v.close();
f_a.close();
f_t.close();
*/
// {
// mpc_controller::PolyTraj trajmsg;
// for(int i = 0; i < optTraj.getSegNum(); i++){
// int singual = optTraj.etas[i];
// for(int j = 0; j < optTraj[i].getPieceNum(); j++){
// mpc_controller::SinglePoly piece;
// if(singual > 0) piece.reverse = false;
// else piece.reverse = true;
// piece.duration = optTraj[i][j].getDuration();
// Eigen::Matrix<double, 2, 6> c = optTraj[i][j].getCoeffMat();
// for (int k=0; k<6; k++)
// {
// piece.coef_x.push_back(c(0, k));
// piece.coef_y.push_back(c(1, k));
// }
// trajmsg.trajs.push_back(piece);
// }
// }
// trajmsg.start_time = ros::Time::now();
// trajCmdPub.publish(trajmsg);
// }
}
}
}

19
trajectoryOpt/src/plan_manage/src/plan_node.cpp Normal file
View File

@@ -0,0 +1,19 @@
#include<plan_manage/plan_manage.h>
#include <ros/ros.h>
#include <ros/console.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "global_planning_node");
ros::NodeHandle nh("~");
plan_manage::PlanManager manager;
manager.init(nh);
// ros::AsyncSpinner spinner(8); // Use4 threads
// spinner.start();
// ros::waitForShutdown();
ros::spin();
return 0;
}

37
trajectoryOpt/src/simulator/kinematics_simulator/CMakeLists.txt Executable file
View File

@@ -0,0 +1,37 @@
cmake_minimum_required(VERSION 2.8.3)
project(kinematics_simulator)
set(CMAKE_BUILD_TYPE "Release")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
nav_msgs
std_msgs
visualization_msgs
)
find_package(Eigen3 REQUIRED)
add_message_files(FILES
ControlCmd.msg
)
generate_messages(DEPENDENCIES
std_msgs
)
catkin_package(
CATKIN_DEPENDS roscpp std_msgs message_runtime
)
include_directories(
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)
add_executable (kinematics_simulator_node src/kinematics_sim.cpp )
target_link_libraries(kinematics_simulator_node
${catkin_LIBRARIES})

13
trajectoryOpt/src/simulator/kinematics_simulator/launch/run_simulator.launch Normal file
View File

@@ -0,0 +1,13 @@
<launch>
<arg name="odom_topic" value="/ugv/odometry" />
<arg name="cmd_topic" value="/ugv/command" />
<rosparam command="load" file="$(find kinematics_simulator)/params/simulator.yaml" />
<node pkg="kinematics_simulator" type="kinematics_simulator_node" name="kinematics_simulator_node" output="screen" required="true">
<remap from ="~cmd" to="$(arg cmd_topic)"/>
<remap from="~odom" to="$(arg odom_topic)"/>
</node>
</launch>

BIN
trajectoryOpt/src/simulator/kinematics_simulator/meshes/ackermann_model.STL Normal file
View File

Binary file not shown.

3
trajectoryOpt/src/simulator/kinematics_simulator/msg/ControlCmd.msg Normal file
View File

@@ -0,0 +1,3 @@
# ackermann robots: longitude_acc, steer_vel
float64 longitude_acc
float64 steer_vel

30
trajectoryOpt/src/simulator/kinematics_simulator/package.xml Executable file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<package format="2">
<name>kinematics_simulator</name>
<version>0.0.1</version>
<description>a kinematics simulator package</description>
<maintainer email="xulon666@gmail.com">xulong</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>visualization_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>message_runtime</exec_depend>
</package>

23
trajectoryOpt/src/simulator/kinematics_simulator/params/simulator.yaml Normal file
View File

@@ -0,0 +1,23 @@
kinematics_simulator_node:
simulator:
ugv_type: 2 # 0: omnidirectional robots; 1: differential robots; 2: ackermann robots;
init_x: -9.5
init_y: 5.0
init_yaw: 0.0
time_resolution: 0.01
max_longitude_vel: 2.1
max_longitude_acc: 1.6
max_steer: 0.785
max_steer_vel: 0.4
wheel_base: 0.6
time_delay: 0.0
noise_percent: 0.0
#0 0
#0.02 10.0
#0.05 15.0
#0.07 20.0
noise_std_longitude_speed: 0.0
noise_std_steer: 0.0

225
trajectoryOpt/src/simulator/kinematics_simulator/src/kinematics_sim.cpp Executable file
View File

@@ -0,0 +1,225 @@
#include <iostream>
#include <math.h>
#include <random>
#include <eigen3/Eigen/Dense>
#include <ros/ros.h>
#include <vector>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include "kinematics_simulator/ControlCmd.h"
#define ACKERMANN 2
using namespace std;
// ros interface
ros::Subscriber command_sub;
ros::Publisher odom_pub;
ros::Publisher mesh_pub;
ros::Timer simulate_timer;
ros::Time get_cmdtime;
visualization_msgs::Marker marker;
// simulator variables
std::default_random_engine generator;
std::normal_distribution<double> distribution{0.0, 1.0};
// std::uniform_real_distribution<double> real_uniform{-1.0, 1.0};
std::uniform_real_distribution<double> real_uniform{-0.5, 1.5};
kinematics_simulator::ControlCmd immediate_cmd;
vector<kinematics_simulator::ControlCmd> cmd_buff;
double x = 0.0;
double y = 0.0;
double v = 0.0;
double steer = 0.0;
double yaw = 0.0;
bool rcv_cmd = false;
double vx = 0.0;
double vy = 0.0;
double w = 0.0;
// simulator parameters
double init_x = 0.0;
double init_y = 0.0;
double init_v = 0.0;
double init_steer = 0.0;
double init_yaw = 0.0;
double time_resolution = 0.01;
double max_longitude_acc = 15.0;
double max_longitude_vel = 1.5;
double max_steer = 0.7;
double max_steer_vel = 7.0;
double time_delay = 0.0;
double wheel_base = 0.6;
double noise_percent = 0.0;
// double noise_percent = 0.1;
double noise_std_longitude_speed = 0.1;
double noise_std_steer = 0.1;
Eigen::Quaterniond q_mesh;
Eigen::Vector3d pos_mesh;
// utils
void normYaw(double& th)
{
while (th > M_PI)
th -= M_PI * 2;
while (th < -M_PI)
th += M_PI * 2;
}
Eigen::Vector2d guassRandom2d(double std)
{
return std * Eigen::Vector2d(distribution(generator), distribution(generator));
}
Eigen::Vector3d guassRandom3d(double std)
{
return std * Eigen::Vector3d(distribution(generator), distribution(generator), distribution(generator));
}
double guassRandom(double std)
{
return std * distribution(generator);
}
double uniformRandom(double limit)
{
return limit * real_uniform(generator);
return limit;
}
// callBackes
void rcvCmdCallBack(const kinematics_simulator::ControlCmdConstPtr cmd)
{
if (rcv_cmd==false)
{
rcv_cmd = true;
cmd_buff.push_back(*cmd);
get_cmdtime = ros::Time::now();
}
else
{
cmd_buff.push_back(*cmd);
if ((ros::Time::now() - get_cmdtime).toSec() > time_delay)
{
immediate_cmd = cmd_buff[0];
cmd_buff.erase(cmd_buff.begin());
}
}
}
void simCallback(const ros::TimerEvent &e)
{
nav_msgs::Odometry new_odom;
new_odom.header.stamp = ros::Time::now();
new_odom.header.frame_id = "world";
vx = v * cos(yaw);
vy = v * sin(yaw);
w = v * tan(steer) / wheel_base;
vx += uniformRandom(noise_percent*fabs(vx));
vy += uniformRandom(noise_percent*fabs(vy));
w += uniformRandom(noise_percent*fabs(w));
x = x + vx * time_resolution;
y = y + vy * time_resolution;
yaw = yaw + w * time_resolution;
immediate_cmd.steer_vel += uniformRandom(noise_percent*fabs(immediate_cmd.steer_vel));
immediate_cmd.longitude_acc += uniformRandom(noise_percent*fabs(immediate_cmd.longitude_acc));
steer = steer + max(min(immediate_cmd.steer_vel, max_steer_vel), -max_steer_vel) * time_resolution;
steer = max(min(steer, max_steer), -max_steer);
v = v + max(min(immediate_cmd.longitude_acc, max_longitude_acc), -max_longitude_acc) * time_resolution;
v = max(min(v, max_longitude_vel), -max_longitude_vel);
normYaw(yaw);
new_odom.pose.pose.position.x = x;
new_odom.pose.pose.position.y = y;
new_odom.pose.pose.position.z = 0;
new_odom.pose.pose.orientation.w = cos(yaw/2.0);
new_odom.pose.pose.orientation.x = 0;
new_odom.pose.pose.orientation.y = 0;
new_odom.pose.pose.orientation.z = sin(yaw/2.0);
new_odom.twist.twist.linear.x = vx;
new_odom.twist.twist.linear.y = vy;
new_odom.twist.twist.linear.z = 0;
new_odom.twist.twist.angular.x = v;
new_odom.twist.twist.angular.y = steer;
new_odom.twist.twist.angular.z = w;
Eigen::Quaterniond qyaw(cos(yaw/2.0), 0.0, 0.0, sin(yaw/2.0));
Eigen::Quaterniond q = (qyaw * q_mesh).normalized();
Eigen::Matrix3d R(qyaw);
Eigen::Vector3d dp = R*pos_mesh;
marker.pose.position.x = x - dp.x();
marker.pose.position.y = y - dp.y();
marker.pose.position.z = 0.0;
marker.pose.orientation.w = q.w();
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
odom_pub.publish(new_odom);
mesh_pub.publish(marker);
}
// main loop
int main (int argc, char** argv)
{
ros::init (argc, argv, "simulator_node");
ros::NodeHandle nh("~");
nh.getParam("simulator/init_x", init_x);
nh.getParam("simulator/init_y", init_y);
nh.getParam("simulator/init_yaw", init_yaw);
nh.getParam("simulator/time_resolution", time_resolution);
nh.getParam("simulator/max_longitude_vel", max_longitude_vel);
nh.getParam("simulator/max_longitude_acc", max_longitude_acc);
nh.getParam("simulator/max_steer", max_steer);
nh.getParam("simulator/max_steer_vel", max_steer_vel);
nh.getParam("simulator/time_delay", time_delay);
nh.getParam("simulator/wheel_base", wheel_base);
nh.getParam("simulator/noise_percent", noise_percent);
nh.getParam("simulator/noise_std_longitude_speed", noise_std_longitude_speed);
nh.getParam("simulator/noise_std_steer", noise_std_steer);
noise_percent /= 100.0;
command_sub = nh.subscribe("cmd", 1000, rcvCmdCallBack);
odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
mesh_pub = nh.advertise<visualization_msgs::Marker>("mesh", 10);
immediate_cmd.longitude_acc = 0.0;
immediate_cmd.steer_vel = 0.0;
marker.header.frame_id = "world";
marker.id = 0;
marker.type = visualization_msgs::Marker::MESH_RESOURCE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = x = init_x;
marker.pose.position.y = y = init_y;
marker.pose.position.z = 0.0;
marker.pose.orientation.w = 0.5;
marker.pose.orientation.x = 0.5;
marker.pose.orientation.y = 0.5;
marker.pose.orientation.z = 0.5;
marker.color.a = 1.0;
marker.color.r = 0.5;
marker.color.g = 0.5;
marker.color.b = 0.5;
marker.scale.x = 0.00025;
marker.scale.y = 0.00025;
marker.scale.z = 0.00025;
q_mesh = Eigen::Quaterniond(1.0/sqrt(2), 0.0, 0.0, 1.0/sqrt(2));
pos_mesh = Eigen::Vector3d(-0.75, 0.35, 0.0);
marker.mesh_resource = "package://kinematics_simulator/meshes/ackermann_model.STL";
simulate_timer = nh.createTimer(ros::Duration(time_resolution), simCallback);
ros::spin();
return 0;
}

37
trajectoryOpt/src/simulator/random_map_generator/CMakeLists.txt Executable file
View File

@@ -0,0 +1,37 @@
cmake_minimum_required(VERSION 2.8.3)
project(random_map_generator)
set(CMAKE_BUILD_TYPE "Release")
ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
sensor_msgs
std_msgs
visualization_msgs
pcl_conversions
tools
cv_bridge
roslib
)
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
catkin_package(
CATKIN_DEPENDS roscpp std_msgs sensor_msgs visualization_msgs roslib
)
include_directories(
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
)
add_executable (random_map src/random_map_node.cpp )
target_link_libraries(random_map
${catkin_LIBRARIES}
${PCL_LIBRARIES}
)

36
trajectoryOpt/src/simulator/random_map_generator/launch/test.launch Normal file
View File

@@ -0,0 +1,36 @@
<launch>
<arg name="odom_topic" value="/ugv/odometry" />
<arg name="cmd_topic" value="/ugv/command" />
<arg name="sensor_topic" value="/ugv/laser" />
<arg name="mesh_topic" value="/ugv/mesh" />
<arg name="map_topic" value="/global_map" />
<!-- <rosparam command="load" file="$(find mpc_controller)/params/controller.yaml" /> -->
<rosparam command="load" file="$(find kinematics_simulator)/params/simulator.yaml" />
<rosparam command="load" file="$(find random_map_generator)/params/map.yaml" />
<!-- <node pkg="mpc_controller" name="mpc_controller_node" type="mpc_controller_node" output="screen" required="true">
<remap from="cmd" to="$(arg cmd_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="traj" to="/planner/trajectory"/>
<remap from="dptraj" to="/planner/dptrajectory"/>
</node>
-->
<node pkg="kinematics_simulator" type="kinematics_simulator_node" name="kinematics_simulator_node" output="screen" required="true">
<remap from ="~cmd" to="$(arg cmd_topic)"/>
<remap from="~odom" to="$(arg odom_topic)"/>
<remap from="~mesh" to="$(arg mesh_topic)"/>
</node>
<node pkg="random_map_generator" type="random_map" name="random_map" output="screen" required="true">
<rosparam file="$(find plan_manage)/config/planning.yaml" command="load" />
<remap from ="~odom" to="$(arg odom_topic)"/>
<remap from="~local_cloud" to="$(arg sensor_topic)"/>
<remap from="~global_cloud" to="$(arg map_topic)"/>
</node>
<!-- rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find random_map_generator)/rviz/vis.rviz" required="true" />
</launch>

8
trajectoryOpt/src/simulator/random_map_generator/launch/vis.launch Normal file
View File

@@ -0,0 +1,8 @@
<launch>
<!-- rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find random_map_generator)/rviz/default.rviz" required="true" />
</launch>

30
trajectoryOpt/src/simulator/random_map_generator/package.xml Executable file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<package format="2">
<name>random_map_generator</name>
<version>0.0.1</version>
<description>a random map generator package</description>
<maintainer email="xulon666@gmail.com">xulong</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>roslib</build_depend>
<build_depend>tools</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>visualization_msgs</build_export_depend>
<build_export_depend>tools</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<exec_depend>tools</exec_depend>
<exec_depend>roslib</exec_depend>
</package>

47
trajectoryOpt/src/simulator/random_map_generator/params/map.yaml Normal file
View File

@@ -0,0 +1,47 @@
random_map:
map:
obs_num: [0, 50, 55] # num of Triangle, Quadrangle, Pentagon, ... 20~30
resolution: 0.01
size_x: 20.0
size_y: 20.0
min_width: 0.4
max_width: 0.5
min_dis: 1.5
sensor_rate: 10.0
sensor_range: 5.0
egeneration_node:
map:
obs_num: [0, 25, 25] # num of Triangle, Quadrangle, Pentagon, ... 20~30
resolution: 0.01
size_x: 20.0
size_y: 20.0
min_width: 0.4
max_width: 0.5
min_dis: 1.8
sensor_rate: 10.0
sensor_range: 5.0
# image_node:
# map:
# obs_num: [23, 23, 23] # num of Triangle, Quadrangle, Pentagon, ... 20~30
# resolution: 0.01
# size_x: 20.0
# size_y: 20.0
# min_width: 0.8
# max_width: 0.9
# min_dis: 1.7
# sensor_rate: 10.0
# sensor_range: 5.0
# corridor_node:
# map:
# obs_num: [23, 23, 23] # num of Triangle, Quadrangle, Pentagon, ... 20~30
# resolution: 0.01
# size_x: 20.0
# size_y: 20.0
# min_width: 0.8
# max_width: 0.9
# min_dis: 1.7
# sensor_rate: 10.0
# sensor_range: 5.0

209
trajectoryOpt/src/simulator/random_map_generator/rviz/vis.rviz Normal file
View File

@@ -0,0 +1,209 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud21
- /dptraj1
- /Path1
Splitter Ratio: 0.5
Tree Height: 719
- 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
Name: Time
SyncMode: 0
SyncSource: PointCloud2
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: 100
Reference Frame: <Fixed Frame>
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/PointCloud2
Color: 85; 87; 83
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 85; 87; 83
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Boxes
Topic: /global_pcl
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /ugv/mesh
Name: Marker
Namespaces:
"": true
Queue Size: 100
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 32; 74; 135
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.029999999329447746
Name: dptraj
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /visualization/refinedTraj_dptraj
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 164; 0; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /visualization/refinedTraj_dftpav
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: false
Marker Topic: /visualization/fullshapeTraj_kinoastar
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: true
Global Options:
Background Color: 238; 238; 236
Default Light: true
Fixed Frame: world
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:
Angle: -0.009999876841902733
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: 66.69723510742188
Target Frame: <Fixed Frame>
X: -2.9230759143829346
Y: 5.27029275894165
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1848
X: 72
Y: 27

552409
trajectoryOpt/src/simulator/random_map_generator/src/env.pcd Normal file
View File

File diff suppressed because it is too large Load Diff

1098
trajectoryOpt/src/simulator/random_map_generator/src/random_map_node.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

29
trajectoryOpt/src/utils/DecompROS/LICENSE Normal file
View File

@@ -0,0 +1,29 @@
BSD 3-Clause License
Copyright (c) 2018, sikang
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

10
trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/CMakeLists.txt Normal file
View File

@@ -0,0 +1,10 @@
cmake_minimum_required(VERSION 2.8.3)
project(decomp_ros_msgs)
find_package(catkin_simple REQUIRED)
catkin_simple()
cs_install()
cs_export()

2
trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/msg/Ellipsoid.msg Normal file
View File

@@ -0,0 +1,2 @@
float64[3] d
float64[9] E

2
trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/msg/EllipsoidArray.msg Normal file
View File

@@ -0,0 +1,2 @@
Header header
Ellipsoid[] ellipsoids

2
trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/msg/Polyhedron.msg Normal file
View File

@@ -0,0 +1,2 @@
geometry_msgs/Point[] points
geometry_msgs/Point[] normals #norm is an outer vector

2
trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/msg/PolyhedronArray.msg Normal file
View File

@@ -0,0 +1,2 @@
Header header
Polyhedron[] polyhedrons

26
trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/package.xml Normal file
View File

@@ -0,0 +1,26 @@
<?xml version="1.0"?>
<package format="2">
<name>decomp_ros_msgs</name>
<version>0.0.0</version>
<description>The decomp_ros_msgs package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="sikang@seas.upenn.edu">sikang</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>catkin_simple</build_depend>
<depend>geometry_msgs</depend>
<depend>message_generation</depend>
<depend>message_runtime</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

47
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/CMakeLists.txt Normal file
View File

@@ -0,0 +1,47 @@
cmake_minimum_required(VERSION 2.8.3)
project(decomp_ros_utils)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3 -Wall -Wno-deprecated-declarations")
find_package(catkin REQUIRED COMPONENTS rviz roscpp)
find_package(catkin_simple REQUIRED)
find_package(cmake_modules)
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS} ${DECOMP_UTIL_INCLUDE_DIRS})
add_definitions(-DQT_NO_KEYWORDS)
## This setting causes Qt's "MOC" generation to happen automatically.
## this does not moc things in include!!!!!!! only in src
set(CMAKE_AUTOMOC ON)
## We'll use the version that rviz used so they are compatible.
if(rviz_QT_VERSION VERSION_LESS "5")
message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
include(${QT_USE_FILE})
else()
message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
set(QT_LIBRARIES Qt5::Widgets)
endif()
catkin_simple()
set(SOURCE_FILES
src/bound_visual.cpp
src/mesh_visual.cpp
src/vector_visual.cpp
src/ellipsoid_array_visual.cpp
src/ellipsoid_array_display.cpp
src/polyhedron_array_display.cpp
${MOC_FILES})
cs_add_library(decomp_rviz_plugins ${SOURCE_FILES})
target_link_libraries(decomp_rviz_plugins ${QT_LIBRARIES} ${catkin_LIBRARIES} ${rviz_DEFAULT_PLUGIN_LIBRARIES})
cs_install()
cs_export()

BIN
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/icons/classes/EllipsoidArray.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 145 KiB

BIN
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/icons/classes/PolyhedronArray.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 259 KiB

130
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_basis/data_type.h Normal file
View File

@@ -0,0 +1,130 @@
/**
* @file data_type.h
* @brief Defines all data types used in this lib
* Mostly alias from Eigen Library.
*/
#include <stdio.h>
#include <math.h>
#include <limits>
#include <vector>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
///Set red font in printf funtion
#ifndef ANSI_COLOR_RED
#define ANSI_COLOR_RED "\x1b[1;31m"
#endif
///Set green font in printf funtion
#ifndef ANSI_COLOR_GREEN
#define ANSI_COLOR_GREEN "\x1b[1;32m"
#endif
///Set yellow font in printf funtion
#ifndef ANSI_COLOR_YELLOW
#define ANSI_COLOR_YELLOW "\x1b[1;33m"
#endif
///Set blue font in printf funtion
#ifndef ANSI_COLOR_BLUE
#define ANSI_COLOR_BLUE "\x1b[1;34m"
#endif
///Set magenta font in printf funtion
#ifndef ANSI_COLOR_MAGENTA
#define ANSI_COLOR_MAGENTA "\x1b[1;35m"
#endif
///Set cyan font in printf funtion
#ifndef ANSI_COLOR_CYAN
#define ANSI_COLOR_CYAN "\x1b[1;36m"
#endif
///Reset font color in printf funtion
#ifndef ANSI_COLOR_RESET
#define ANSI_COLOR_RESET "\x1b[0m"
#endif
#ifndef DATA_TYPE_H
#define DATA_TYPE_H
/*! \brief Rename the float type used in lib
Default is set to be double, but user can change it to float.
*/
typedef double decimal_t;
///Pre-allocated std::vector for Eigen using vec_E
template <typename T>
using vec_E = std::vector<T, Eigen::aligned_allocator<T>>;
///Eigen 1D float vector
template <int N>
using Vecf = Eigen::Matrix<decimal_t, N, 1>;
///Eigen 1D int vector
template <int N>
using Veci = Eigen::Matrix<int, N, 1>;
///MxN Eigen matrix
template <int M, int N>
using Matf = Eigen::Matrix<decimal_t, M, N>;
///MxN Eigen matrix with M unknown
template <int N>
using MatDNf = Eigen::Matrix<decimal_t, Eigen::Dynamic, N>;
///Vector of Eigen 1D float vector
template <int N>
using vec_Vecf = vec_E<Vecf<N>>;
///Vector of Eigen 1D int vector
template <int N>
using vec_Veci = vec_E<Veci<N>>;
///Eigen 1D float vector of size 2
typedef Vecf<2> Vec2f;
///Eigen 1D int vector of size 2
typedef Veci<2> Vec2i;
///Eigen 1D float vector of size 3
typedef Vecf<3> Vec3f;
///Eigen 1D int vector of size 3
typedef Veci<3> Vec3i;
///Eigen 1D float vector of size 4
typedef Vecf<4> Vec4f;
///Column vector in float of size 6
typedef Vecf<6> Vec6f;
///Vector of type Vec2f.
typedef vec_E<Vec2f> vec_Vec2f;
///Vector of type Vec2i.
typedef vec_E<Vec2i> vec_Vec2i;
///Vector of type Vec3f.
typedef vec_E<Vec3f> vec_Vec3f;
///Vector of type Vec3i.
typedef vec_E<Vec3i> vec_Vec3i;
///2x2 Matrix in float
typedef Matf<2, 2> Mat2f;
///3x3 Matrix in float
typedef Matf<3, 3> Mat3f;
///4x4 Matrix in float
typedef Matf<4, 4> Mat4f;
///6x6 Matrix in float
typedef Matf<6, 6> Mat6f;
///Dynamic Nx1 Eigen float vector
typedef Vecf<Eigen::Dynamic> VecDf;
///Nx2 Eigen float matrix
typedef MatDNf<2> MatD2f;
///Nx3 Eigen float matrix
typedef MatDNf<3> MatD3f;
///Dynamic MxN Eigen float matrix
typedef Matf<Eigen::Dynamic, Eigen::Dynamic> MatDf;
///Allias of Eigen::Affine2d
typedef Eigen::Transform<decimal_t, 2, Eigen::Affine> Aff2f;
///Allias of Eigen::Affine3d
typedef Eigen::Transform<decimal_t, 3, Eigen::Affine> Aff3f;
#endif
#ifndef EIGEN_QUAT
#define EIGEN_QUAT
///Allias of Eigen::Quaterniond
typedef Eigen::Quaternion<decimal_t> Quatf;
#endif
#ifndef EIGEN_EPSILON
#define EIGEN_EPSILON
///Compensate for numerical error
constexpr decimal_t epsilon_ = 1e-10; // numerical calculation error
#endif

36
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_basis/data_utils.h Normal file
View File

@@ -0,0 +1,36 @@
/**
* @file data_utils.h
* @brief Provide a few widely used function for basic type
*/
#ifndef DATA_UTILS_H
#define DATA_UTILS_H
#include <decomp_basis/data_type.h>
///Template for transforming a vector
template <class T, class TF>
vec_E<T> transform_vec(const vec_E<T> &t, const TF &tf) {
vec_E<T> new_t;
for (const auto &it : t)
new_t.push_back(tf * it);
return new_t;
}
///Template for calculating distance
template <class T>
decimal_t total_distance(const vec_E<T>& vs){
decimal_t dist = 0;
for(unsigned int i = 1; i < vs.size(); i++)
dist += (vs[i] - vs[i-1]).norm();
return dist;
}
///Transform all entries in a vector using given TF
#define transform_vec3 transform_vec<Vec3f, Aff3f>
///Sum up total distance for Vec3f
#define total_distance3f total_distance<Vec3f>
///Sum up total distance for Vec3i
#define total_distance3i total_distance<Vec3i>
#endif

102
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_geometry/ellipsoid.h Normal file
View File

@@ -0,0 +1,102 @@
/**
* @file ellipsoid.h
* @brief Ellipsoid class
*/
#ifndef DECOMP_ELLIPSOID_H
#define DECOMP_ELLIPSOID_H
#include <iostream>
#include <decomp_basis/data_type.h>
#include <decomp_geometry/polyhedron.h>
template <int Dim>
struct Ellipsoid {
Ellipsoid() {}
Ellipsoid(const Matf<Dim, Dim>& C, const Vecf<Dim>& d) : C_(C), d_(d) {}
/// Calculate distance to the center
decimal_t dist(const Vecf<Dim>& pt) const {
return (C_.inverse() * (pt - d_)).norm();
}
/// Check if the point is inside, non-exclusive
bool inside(const Vecf<Dim>& pt) const {
return dist(pt) <= 1;
}
/// Calculate points inside ellipsoid, non-exclusive
vec_Vecf<Dim> points_inside(const vec_Vecf<Dim> &O) const {
vec_Vecf<Dim> new_O;
for (const auto &it : O) {
if (inside(it))
new_O.push_back(it);
}
return new_O;
}
///Find the closest point
Vecf<Dim> closest_point(const vec_Vecf<Dim> &O) const {
Vecf<Dim> pt = Vecf<Dim>::Zero();
decimal_t min_dist = std::numeric_limits<decimal_t>::max();
for (const auto &it : O) {
decimal_t d = dist(it);
if (d < min_dist) {
min_dist = d;
pt = it;
}
}
return pt;
}
///Find the closest hyperplane from the closest point
Hyperplane<Dim> closest_hyperplane(const vec_Vecf<Dim> &O) const {
const auto closest_pt = closest_point(O);
const auto n = C_.inverse() * C_.inverse().transpose() *
(closest_pt - d_);
return Hyperplane<Dim>(closest_pt, n.normalized());
}
/// Sample n points along the contour
template<int U = Dim>
typename std::enable_if<U == 2, vec_Vecf<U>>::type
sample(int num) const {
vec_Vecf<Dim> pts;
decimal_t dyaw = M_PI*2/num;
for(decimal_t yaw = 0; yaw < M_PI*2; yaw+=dyaw) {
Vecf<Dim> pt;
pt << cos(yaw), sin(yaw);
pts.push_back(C_ * pt + d_);
}
return pts;
}
void print() const {
std::cout << "C: " << C_ << std::endl;
std::cout << "d: " << d_ << std::endl;
}
/// Get ellipsoid volume
decimal_t volume() const {
return C_.determinant();
}
/// Get C matrix
Matf<Dim, Dim> C() const {
return C_;
}
/// Get center
Vecf<Dim> d() const {
return d_;
}
Matf<Dim, Dim> C_;
Vecf<Dim> d_;
};
typedef Ellipsoid<2> Ellipsoid2D;
typedef Ellipsoid<3> Ellipsoid3D;
#endif

258
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_geometry/geometric_utils.h Normal file
View File

@@ -0,0 +1,258 @@
/**
* @file geometric_utils.h
* @brief basic geometry utils
*/
#ifndef DECOMP_GEOMETRIC_UTILS_H
#define DECOMP_GEOMETRIC_UTILS_H
#include <Eigen/Eigenvalues>
#include <decomp_basis/data_utils.h>
#include <decomp_geometry/polyhedron.h>
#include <iostream>
/// Calculate eigen values
template <int Dim> Vecf<Dim> eigen_value(const Matf<Dim, Dim> &A) {
Eigen::SelfAdjointEigenSolver<Matf<Dim, Dim>> es(A);
return es.eigenvalues();
}
/// Calculate rotation matrix from a vector (aligned with x-axis)
inline Mat2f vec2_to_rotation(const Vec2f &v) {
decimal_t yaw = std::atan2(v(1), v(0));
Mat2f R;
R << cos(yaw), -sin(yaw), sin(yaw), cos(yaw);
return R;
}
inline Mat3f vec3_to_rotation(const Vec3f &v) {
// zero roll
Vec3f rpy(0, std::atan2(-v(2), v.topRows<2>().norm()),
std::atan2(v(1), v(0)));
Quatf qx(cos(rpy(0) / 2), sin(rpy(0) / 2), 0, 0);
Quatf qy(cos(rpy(1) / 2), 0, sin(rpy(1) / 2), 0);
Quatf qz(cos(rpy(2) / 2), 0, 0, sin(rpy(2) / 2));
return Mat3f(qz * qy * qx);
}
/// Sort plannar points in the counter-clockwise order
inline vec_Vec2f sort_pts(const vec_Vec2f &pts) {
/// if empty, dont sort
if (pts.empty())
return pts;
/// calculate center point
Vec2f avg = Vec2f::Zero();
for (const auto &pt : pts)
avg += pt;
avg /= pts.size();
/// sort in body frame
vec_E<std::pair<decimal_t, Vec2f>> pts_valued;
pts_valued.resize(pts.size());
for (unsigned int i = 0; i < pts.size(); i++) {
decimal_t theta = atan2(pts[i](1) - avg(1), pts[i](0) - avg(0));
pts_valued[i] = std::make_pair(theta, pts[i]);
}
std::sort(
pts_valued.begin(), pts_valued.end(),
[](const std::pair<decimal_t, Vec2f> &i,
const std::pair<decimal_t, Vec2f> &j) { return i.first < j.first; });
vec_Vec2f pts_sorted(pts_valued.size());
for (size_t i = 0; i < pts_valued.size(); i++)
pts_sorted[i] = pts_valued[i].second;
return pts_sorted;
}
/// Find intersection between two lines on the same plane, return false if they
/// are not intersected
inline bool line_intersect(const std::pair<Vec2f, Vec2f> &v1,
const std::pair<Vec2f, Vec2f> &v2, Vec2f &pi) {
decimal_t a1 = -v1.first(1);
decimal_t b1 = v1.first(0);
decimal_t c1 = a1 * v1.second(0) + b1 * v1.second(1);
decimal_t a2 = -v2.first(1);
decimal_t b2 = v2.first(0);
decimal_t c2 = a2 * v2.second(0) + b2 * v2.second(1);
decimal_t x = (c1 * b2 - c2 * b1) / (a1 * b2 - a2 * b1);
decimal_t y = (c1 * a2 - c2 * a1) / (a2 * b1 - a1 * b2);
if (std::isnan(x) || std::isnan(y) || std::isinf(x) || std::isinf(y))
return false;
else {
pi << x, y;
return true;
}
}
/// Find intersection between multiple lines
inline vec_Vec2f line_intersects(const vec_E<std::pair<Vec2f, Vec2f>> &lines) {
vec_Vec2f pts;
for (unsigned int i = 0; i < lines.size(); i++) {
for (unsigned int j = i + 1; j < lines.size(); j++) {
Vec2f pi;
if (line_intersect(lines[i], lines[j], pi)) {
pts.push_back(pi);
}
}
}
return pts;
}
/// Find extreme points of Polyhedron2D
inline vec_Vec2f cal_vertices(const Polyhedron2D &poly) {
vec_E<std::pair<Vec2f, Vec2f>> lines;
const auto vs = poly.hyperplanes();
for (unsigned int i = 0; i < vs.size(); i++) {
Vec2f n = vs[i].n_;
Vec2f v(-n(1), n(0));
v = v.normalized();
lines.push_back(std::make_pair(v, vs[i].p_));
/*
std::cout << "add p: " << lines.back().second.transpose() <<
" v: " << lines.back().first.transpose() << std::endl;
*/
}
auto vts = line_intersects(lines);
// for(const auto& it: vts)
// std::cout << "vertice: " << it.transpose() << std::endl;
vec_Vec2f vts_inside = poly.points_inside(vts);
vts_inside = sort_pts(vts_inside);
return vts_inside;
}
/// Find extreme points of Polyhedron3D
inline vec_E<vec_Vec3f> cal_vertices(const Polyhedron3D &poly) {
vec_E<vec_Vec3f> bds;
const auto vts = poly.hyperplanes();
//**** for each plane, find lines on it
for (unsigned int i = 0; i < vts.size(); i++) {
const Vec3f t = vts[i].p_;
const Vec3f n = vts[i].n_;
const Quatf q = Quatf::FromTwoVectors(Vec3f(0, 0, 1), n);
const Mat3f R(q); // body to world
vec_E<std::pair<Vec2f, Vec2f>> lines;
for (unsigned int j = 0; j < vts.size(); j++) {
if (j == i)
continue;
Vec3f nw = vts[j].n_;
Vec3f nb = R.transpose() * nw;
decimal_t bb = vts[j].p_.dot(nw) - nw.dot(t);
Vec2f v = Vec3f(0, 0, 1).cross(nb).topRows<2>(); // line direction
Vec2f p; // point on the line
if (nb(1) != 0)
p << 0, bb / nb(1);
else if (nb(0) != 0)
p << bb / nb(0), 0;
else
continue;
lines.push_back(std::make_pair(v, p));
}
//**** find all intersect points
vec_Vec2f pts = line_intersects(lines);
//**** filter out points inside polytope
vec_Vec2f pts_inside;
for (const auto &it : pts) {
Vec3f p = R * Vec3f(it(0), it(1), 0) + t; // convert to world frame
if (poly.inside(p))
pts_inside.push_back(it);
}
if (pts_inside.size() > 2) {
//**** sort in plane frame
pts_inside = sort_pts(pts_inside);
//**** transform to world frame
vec_Vec3f points_valid;
for (auto &it : pts_inside)
points_valid.push_back(R * Vec3f(it(0), it(1), 0) + t);
//**** insert resulting polygon
bds.push_back(points_valid);
}
}
return bds;
}
/// Get the convex hull of a 2D points array, use wrapping method
inline vec_Vec2f cal_convex_hull(const vec_Vec2f &pts) {
/// find left most point
Vec2f p0;
decimal_t min_x = std::numeric_limits<decimal_t>::infinity();
for (const auto &it : pts) {
if (min_x > it(0) || (min_x == it(0) && it(1) < p0(1))) {
min_x = it(0);
p0 = it;
}
}
vec_Vec2f vs;
vs.push_back(p0);
while (vs.back() != p0 || vs.size() == 1) {
const auto ref_pt = vs.back();
Vec2f end_pt = p0;
for (size_t i = 0; i < pts.size(); i++) {
if (pts[i] == ref_pt)
continue;
Vec2f dir = (pts[i] - ref_pt).normalized();
Hyperplane2D hp(ref_pt, Vec2f(-dir(1), dir(0)));
bool most_left_hp = true;
for (size_t j = 0; j < pts.size(); j++) {
if (hp.signed_dist(pts[j]) > 0 && pts[j] != pts[i] &&
pts[j] != ref_pt) {
// if(hp.signed_dist(pts[j]) > 0) {
most_left_hp = false;
break;
}
}
if (most_left_hp) {
end_pt = pts[i];
break;
}
}
// std::cout << "add: " << end_pt.transpose() << std::endl;
vs.push_back(end_pt);
}
return vs;
}
inline Polyhedron2D get_convex_hull(const vec_Vec2f &pts) {
Polyhedron2D poly;
Vec2f prev_dir(-1, -1);
for (size_t i = 0; i < pts.size() - 1; i++) {
size_t j = i + 1;
Vec2f dir = (pts[j] - pts[i]).normalized();
if (dir != prev_dir) {
poly.add(Hyperplane2D((pts[i] + pts[j]) / 2, Vec2f(-dir(1), dir(0))));
prev_dir = dir;
}
}
return poly;
}
/// Minkowski sum, add B to A with center Bc
inline Polyhedron2D minkowski_sum(const Polyhedron2D &A, const Polyhedron2D &B,
const Vec2f &Bc) {
const auto A_vertices = cal_vertices(A);
const auto B_vertices = cal_vertices(B);
vec_Vec2f C_vertices;
for (const auto &it : A_vertices) {
for (const auto &itt : B_vertices)
C_vertices.push_back(it + itt - Bc);
}
return get_convex_hull(cal_convex_hull(C_vertices));
}
#endif

154
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_geometry/polyhedron.h Normal file
View File

@@ -0,0 +1,154 @@
/**
* @file polygon.h
* @brief Polygon class
*/
#ifndef DECOMP_POLYGON_H
#define DECOMP_POLYGON_H
#include <decomp_basis/data_type.h>
///Hyperplane class
template <int Dim>
struct Hyperplane {
Hyperplane() {}
Hyperplane(const Vecf<Dim>& p, const Vecf<Dim>& n) : p_(p), n_(n) {}
/// Calculate the signed distance from point
decimal_t signed_dist(const Vecf<Dim>& pt) const {
return n_.dot(pt - p_);
}
/// Calculate the distance from point
decimal_t dist(const Vecf<Dim>& pt) const {
return std::abs(signed_dist(pt));
}
/// Point on the plane
Vecf<Dim> p_;
/// Normal of the plane, directional
Vecf<Dim> n_;
};
///Hyperplane2D: first is the point on the hyperplane, second is the normal
typedef Hyperplane<2> Hyperplane2D;
///Hyperplane3D: first is the point on the hyperplane, second is the normal
typedef Hyperplane<3> Hyperplane3D;
///Polyhedron class
template <int Dim>
struct Polyhedron {
///Null constructor
Polyhedron() {}
///Construct from Hyperplane array
Polyhedron(const vec_E<Hyperplane<Dim>>& vs) : vs_(vs) {}
///Append Hyperplane
void add(const Hyperplane<Dim>& v) {
vs_.push_back(v);
}
/// Check if the point is inside polyhedron, non-exclusive
bool inside(const Vecf<Dim>& pt) const {
for (const auto& v : vs_) {
if (v.signed_dist(pt) > epsilon_) {
//printf("rejected pt: (%f, %f), d: %f\n",pt(0), pt(1), v.signed_dist(pt));
return false;
}
}
return true;
}
/// Calculate points inside polyhedron, non-exclusive
vec_Vecf<Dim> points_inside(const vec_Vecf<Dim> &O) const {
vec_Vecf<Dim> new_O;
for (const auto &it : O) {
if (inside(it))
new_O.push_back(it);
}
return new_O;
}
/// Calculate normals, used for visualization
vec_E<std::pair<Vecf<Dim>, Vecf<Dim>>> cal_normals() const {
vec_E<std::pair<Vecf<Dim>, Vecf<Dim>>> ns(vs_.size());
for (size_t i = 0; i < vs_.size(); i++)
ns[i] = std::make_pair(vs_[i].p_, vs_[i].n_); // fist is point, second is normal
return ns;
}
/// Get the hyperplane array
vec_E<Hyperplane<Dim>> hyperplanes() const {
return vs_;
}
/// Hyperplane array
vec_E<Hyperplane<Dim>> vs_; // normal must go outside
};
///Polyhedron2D, consists of 2D hyperplane
typedef Polyhedron<2> Polyhedron2D;
///Polyhedron3D, consists of 3D hyperplane
typedef Polyhedron<3> Polyhedron3D;
///[A, b] for \f$Ax < b\f$
template <int Dim>
struct LinearConstraint {
///Null constructor
LinearConstraint() {}
/// Construct from \f$A, b\f$ directly, s.t \f$Ax < b\f$
LinearConstraint(const MatDNf<Dim>& A, const VecDf& b) : A_(A), b_(b) {}
/**
* @brief Construct from a inside point and hyperplane array
* @param p0 point that is inside
* @param vs hyperplane array, normal should go outside
*/
LinearConstraint(const Vecf<Dim> p0, const vec_E<Hyperplane<Dim>>& vs) {
const unsigned int size = vs.size();
MatDNf<Dim> A(size, Dim);
VecDf b(size);
for (unsigned int i = 0; i < size; i++) {
auto n = vs[i].n_;
decimal_t c = vs[i].p_.dot(n);
if (n.dot(p0) - c > 0) {
n = -n;
c = -c;
}
A.row(i) = n;
b(i) = c;
}
A_ = A;
b_ = b;
}
/// Check if the point is inside polyhedron using linear constraint
bool inside(const Vecf<Dim> &pt) {
VecDf d = A_ * pt - b_;
for (unsigned int i = 0; i < d.rows(); i++) {
if (d(i) > 0)
return false;
}
return true;
}
/// Get \f$A\f$ matrix
MatDNf<Dim> A() const { return A_; }
/// Get \f$b\f$ matrix
VecDf b() const { return b_; }
MatDNf<Dim> A_;
VecDf b_;
};
///LinearConstraint 2D
typedef LinearConstraint<2> LinearConstraint2D;
///LinearConstraint 3D
typedef LinearConstraint<3> LinearConstraint3D;
#endif

160
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_ros_utils/data_ros_utils.h Normal file
View File

@@ -0,0 +1,160 @@
#ifndef DECOMP_ROS_UTILS_H
#define DECOMP_ROS_UTILS_H
#include <decomp_geometry/ellipsoid.h>
#include <decomp_geometry/polyhedron.h>
#include <sensor_msgs/PointCloud.h>
#include <decomp_ros_msgs/PolyhedronArray.h>
#include <decomp_ros_msgs/EllipsoidArray.h>
#include <nav_msgs/Path.h>
namespace DecompROS {
template <int Dim> nav_msgs::Path vec_to_path(const vec_Vecf<Dim> &vs) {
nav_msgs::Path path;
for (const auto& it : vs) {
geometry_msgs::PoseStamped pose;
pose.pose.position.x = it(0);
pose.pose.position.y = it(1);
pose.pose.position.z = Dim == 2 ? 0 : it(2);
pose.pose.orientation.w = 1.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
path.poses.push_back(pose);
}
return path;
}
inline sensor_msgs::PointCloud vec_to_cloud(const vec_Vec3f &pts) {
sensor_msgs::PointCloud cloud;
cloud.points.resize(pts.size());
for (unsigned int i = 0; i < pts.size(); i++) {
cloud.points[i].x = pts[i](0);
cloud.points[i].y = pts[i](1);
cloud.points[i].z = pts[i](2);
}
return cloud;
}
inline vec_Vec3f cloud_to_vec(const sensor_msgs::PointCloud &cloud) {
vec_Vec3f pts;
pts.resize(cloud.points.size());
for (unsigned int i = 0; i < cloud.points.size(); i++) {
pts[i](0) = cloud.points[i].x;
pts[i](1) = cloud.points[i].y;
pts[i](2) = cloud.points[i].z;
}
return pts;
}
inline Polyhedron3D ros_to_polyhedron(const decomp_ros_msgs::Polyhedron& msg){
Polyhedron3D poly;
for(unsigned int i = 0; i < msg.points.size(); i++){
Vec3f pt(msg.points[i].x,
msg.points[i].y,
msg.points[i].z);
Vec3f n(msg.normals[i].x,
msg.normals[i].y,
msg.normals[i].z);
poly.add(Hyperplane3D(pt, n));
}
return poly;
}
inline vec_E<Polyhedron3D> ros_to_polyhedron_array(const decomp_ros_msgs::PolyhedronArray& msg) {
vec_E<Polyhedron3D> polys(msg.polyhedrons.size());
for(size_t i = 0; i < msg.polyhedrons.size(); i++)
polys[i] = ros_to_polyhedron(msg.polyhedrons[i]);
return polys;
}
inline decomp_ros_msgs::Polyhedron polyhedron_to_ros(const Polyhedron2D& poly){
decomp_ros_msgs::Polyhedron msg;
for (const auto &p : poly.hyperplanes()) {
geometry_msgs::Point pt, n;
pt.x = p.p_(0);
pt.y = p.p_(1);
pt.z = 0;
n.x = p.n_(0);
n.y = p.n_(1);
n.z = 0;
msg.points.push_back(pt);
msg.normals.push_back(n);
}
geometry_msgs::Point pt1, n1;
pt1.x = 0, pt1.y = 0, pt1.z = 0.01;
n1.x = 0, n1.y = 0, n1.z = 1;
msg.points.push_back(pt1);
msg.normals.push_back(n1);
geometry_msgs::Point pt2, n2;
pt2.x = 0, pt2.y = 0, pt2.z = -0.01;
n2.x = 0, n2.y = 0, n2.z = -1;
msg.points.push_back(pt2);
msg.normals.push_back(n2);
return msg;
}
inline decomp_ros_msgs::Polyhedron polyhedron_to_ros(const Polyhedron3D& poly){
decomp_ros_msgs::Polyhedron msg;
for (const auto &p : poly.hyperplanes()) {
geometry_msgs::Point pt, n;
pt.x = p.p_(0);
pt.y = p.p_(1);
pt.z = p.p_(2);
n.x = p.n_(0);
n.y = p.n_(1);
n.z = p.n_(2);
msg.points.push_back(pt);
msg.normals.push_back(n);
}
return msg;
}
template <int Dim>
decomp_ros_msgs::PolyhedronArray polyhedron_array_to_ros(const vec_E<Polyhedron<Dim>>& vs){
decomp_ros_msgs::PolyhedronArray msg;
for (const auto &v : vs)
msg.polyhedrons.push_back(polyhedron_to_ros(v));
return msg;
}
template <int Dim>
decomp_ros_msgs::EllipsoidArray ellipsoid_array_to_ros(const vec_E<Ellipsoid<Dim>>& Es) {
decomp_ros_msgs::EllipsoidArray ellipsoids;
for (unsigned int i = 0; i < Es.size(); i++) {
decomp_ros_msgs::Ellipsoid ellipsoid;
auto d = Es[i].d();
ellipsoid.d[0] = d(0);
ellipsoid.d[1] = d(1);
ellipsoid.d[2] = Dim == 2 ? 0:d(2);
auto C = Es[i].C();
for (int x = 0; x < 3; x++) {
for (int y = 0; y < 3; y++) {
if(x < Dim && y < Dim)
ellipsoid.E[3 * x + y] = C(x, y);
else
ellipsoid.E[3 * x + y] = 0;
}
}
ellipsoids.ellipsoids.push_back(ellipsoid);
}
return ellipsoids;
}
}
#endif

96
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/decomp_base.h Normal file
View File

@@ -0,0 +1,96 @@
/**
* @file decomp_base.h
* @brief Decomp Base Class
*/
#ifndef DECOMP_BASE_H
#define DECOMP_BASE_H
#include <decomp_geometry/ellipsoid.h>
#include <decomp_geometry/polyhedron.h>
//#include <decomp_geometry/geometry_utils.h>
/**
* @brief Line Segment Class
*
* The basic element in EllipsoidDecomp
*/
template <int Dim>
class DecompBase {
public:
///Null constructor
DecompBase() {}
/**
* @brief Adding local bounding box around line seg
* @param Dim Distance in corresponding axis
*
* This virtual bounding box is parallel to the line segment, the x,y,z axes are not w.r.t the world coordinate system, but instead, x-axis is parallel to the line, y-axis is perpendicular to the line and world z-axis, z-axis is perpendiculat to the line and y-axis
*/
void set_local_bbox(const Vecf<Dim>& bbox) {
local_bbox_ = bbox;
}
///Import obstacle points
void set_obs(const vec_Vecf<Dim> &obs) {
// only consider points inside local bbox
Polyhedron<Dim> vs;
add_local_bbox(vs);
obs_ = vs.points_inside(obs);
}
///Get obstacel points
vec_Vecf<Dim> get_obs() const { return obs_; }
///Get ellipsoid
Ellipsoid<Dim> get_ellipsoid() const { return ellipsoid_; }
///Get polyhedron
Polyhedron<Dim> get_polyhedron() const { return polyhedron_; }
/**
* @brief Inflate the line segment
* @param radius the offset added to the long semi-axis
*/
virtual void dilate(decimal_t radius = 0) = 0;
/**
* @brief Shrink the polyhedron
* @param shrink_distance Shrink distance
*/
virtual void shrink(double shrink_distance) {}
protected:
virtual void add_local_bbox(Polyhedron<Dim> &Vs) = 0;
void find_polyhedron() {
//**** find half-space
Polyhedron<Dim> Vs;
vec_Vecf<Dim> obs_remain = obs_;
while (!obs_remain.empty()) {
const auto v = ellipsoid_.closest_hyperplane(obs_remain);
Vs.add(v);
vec_Vecf<Dim> obs_tmp;
for (const auto &it : obs_remain) {
if (v.signed_dist(it) < 0)
obs_tmp.push_back(it);
}
obs_remain = obs_tmp;
/*
std::cout << "a: " << a.transpose() << std::endl;
std::cout << "b: " << b << std::endl;
*/
}
polyhedron_ = Vs;
}
/// Obstacles, input
vec_Vecf<Dim> obs_;
/// Output ellipsoid
Ellipsoid<Dim> ellipsoid_;
/// Output polyhedron
Polyhedron<Dim> polyhedron_;
/// Local bounding box along the line segment
Vecf<Dim> local_bbox_{Vecf<Dim>::Zero()};
};
#endif

134
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/ellipsoid_decomp.h Normal file
View File

@@ -0,0 +1,134 @@
/**
* @file ellipsoid_decomp.h
* @brief EllipsoidDecomp Class
*/
#ifndef ELLIPSOID_DECOMP_H
#define ELLIPSOID_DECOMP_H
#include <memory>
#include <decomp_util/line_segment.h>
/**
* @brief EllipsoidDecomp Class
*
* EllipsoidDecomp takes input as a given path and find the Safe Flight Corridor around it using Ellipsoids
*/
template <int Dim>
class EllipsoidDecomp {
public:
///Simple constructor
EllipsoidDecomp() {}
/**
* @brief Basic constructor
* @param origin The origin of the global bounding box
* @param dim The dimension of the global bounding box
*/
EllipsoidDecomp(const Vecf<Dim> &origin, const Vecf<Dim> &dim) {
global_bbox_min_ = origin;
global_bbox_max_ = origin + dim;
}
///Set obstacle points
void set_obs(const vec_Vecf<Dim> &obs) { obs_ = obs; }
///Set dimension of bounding box
void set_local_bbox(const Vecf<Dim>& bbox) { local_bbox_ = bbox; }
///Get the path that is used for dilation
vec_Vecf<Dim> get_path() const { return path_; }
///Get the Safe Flight Corridor
vec_E<Polyhedron<Dim>> get_polyhedrons() const { return polyhedrons_; }
///Get the ellipsoids
vec_E<Ellipsoid<Dim>> get_ellipsoids() const { return ellipsoids_; }
///Get the constraints of SFC as \f$Ax\leq b \f$
vec_E<LinearConstraint<Dim>> get_constraints() const {
vec_E<LinearConstraint<Dim>> constraints;
constraints.resize(polyhedrons_.size());
for (unsigned int i = 0; i < polyhedrons_.size(); i++){
const Vecf<Dim> pt = (path_[i] + path_[i+1])/2;
constraints[i] = LinearConstraint<Dim>(pt, polyhedrons_[i].hyperplanes());
}
return constraints;
}
/**
* @brief Decomposition thread
* @param path The path to dilate
* @param offset_x offset added to the long semi-axis, default is 0
*/
void dilate(const vec_Vecf<Dim> &path, double offset_x = 0) {
const unsigned int N = path.size() - 1;
lines_.resize(N);
ellipsoids_.resize(N);
polyhedrons_.resize(N);
for (unsigned int i = 0; i < N; i++) {
lines_[i] = std::make_shared<LineSegment<Dim>>(path[i], path[i+1]);
lines_[i]->set_local_bbox(local_bbox_);
lines_[i]->set_obs(obs_);
lines_[i]->dilate(offset_x);
ellipsoids_[i] = lines_[i]->get_ellipsoid();
polyhedrons_[i] = lines_[i]->get_polyhedron();
}
path_ = path;
if(global_bbox_min_.norm() != 0 || global_bbox_max_.norm() != 0) {
for(auto& it: polyhedrons_)
add_global_bbox(it);
}
}
protected:
template<int U = Dim>
typename std::enable_if<U == 2>::type
add_global_bbox(Polyhedron<Dim> &Vs) {
//**** add bound along X, Y axis
//*** X
Vs.add(Hyperplane2D(Vec2f(global_bbox_max_(0), 0), Vec2f(1, 0)));
Vs.add(Hyperplane2D(Vec2f(global_bbox_min_(0), 0), Vec2f(-1, 0)));
//*** Y
Vs.add(Hyperplane2D(Vec2f(0, global_bbox_max_(1)), Vec2f(0, 1)));
Vs.add(Hyperplane2D(Vec2f(0, global_bbox_min_(1)), Vec2f(0, -1)));
}
template<int U = Dim>
typename std::enable_if<U == 3>::type
add_global_bbox(Polyhedron<Dim> &Vs) {
//**** add bound along X, Y, Z axis
//*** Z
Vs.add(Hyperplane3D(Vec3f(0, 0, global_bbox_max_(2)), Vec3f(0, 0, 1)));
Vs.add(Hyperplane3D(Vec3f(0, 0, global_bbox_min_(2)), Vec3f(0, 0, -1)));
//*** X
Vs.add(Hyperplane3D(Vec3f(global_bbox_max_(0), 0, 0), Vec3f(1, 0, 0)));
Vs.add(Hyperplane3D(Vec3f(global_bbox_min_(0), 0, 0), Vec3f(-1, 0, 0)));
//*** Y
Vs.add(Hyperplane3D(Vec3f(0, global_bbox_max_(1), 0), Vec3f(0, 1, 0)));
Vs.add(Hyperplane3D(Vec3f(0, global_bbox_max_(1), 0), Vec3f(0, -1, 0)));
}
vec_Vecf<Dim> path_;
vec_Vecf<Dim> obs_;
vec_E<Ellipsoid<Dim>> ellipsoids_;
vec_E<Polyhedron<Dim>> polyhedrons_;
std::vector<std::shared_ptr<LineSegment<Dim>>> lines_;
Vecf<Dim> local_bbox_{Vecf<Dim>::Zero()};
Vecf<Dim> global_bbox_min_{Vecf<Dim>::Zero()}; // bounding box params
Vecf<Dim> global_bbox_max_{Vecf<Dim>::Zero()};
};
typedef EllipsoidDecomp<2> EllipsoidDecomp2D;
typedef EllipsoidDecomp<3> EllipsoidDecomp3D;
#endif

105
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/iterative_decomp.h Normal file
View File

@@ -0,0 +1,105 @@
/**
* @file iterative_decomp.h
* @brief IterativeDecomp Class
*/
#ifndef ITERATIVE_DECOMP_H
#define ITERATIVE_DECOMP_H
#include <decomp_util/ellipsoid_decomp.h>
/**
* @brief IterativeDecomp Class
*
* Iteratively calls ElliseDecomp to form a safer Safe Flight Corridor that is away from obstacles
*/
template <int Dim>
class IterativeDecomp : public EllipsoidDecomp<Dim>
{
public:
///Simple constructor
IterativeDecomp() {}
/**
* @brief Basic constructor
* @param origin The origin of the global bounding box
* @param dim The dimension of the global bounding box
*/
IterativeDecomp(const Vecf<Dim> &origin, const Vecf<Dim> &dim) :
EllipsoidDecomp<Dim>(origin, dim) {}
/**
* @brief Decomposition thread
* @param path_raw The path to dilate
* @param iter_num Max iteration number
* @param offset_x offset added to the long semi-axis, default is 0
* @param res Resolution to downsample the path
*/
void dilate_iter(const vec_Vecf<Dim> &path_raw, int iter_num = 5,
decimal_t res = 0, decimal_t offset_x = 0) {
vec_Vecf<Dim> path = res > 0 ? downsample(path_raw, res) : path_raw;
this->dilate(path, offset_x);
vec_Vecf<Dim> new_path = simplify(path);
for (int i = 0; i < iter_num; i++) {
if (new_path.size() == path.size())
break;
else {
path = new_path;
this->dilate(path, offset_x);
new_path = simplify(path);
}
}
}
protected:
/// Uniformly sample path into many segments
vec_Vecf<Dim> downsample(const vec_Vecf<Dim> &ps, decimal_t d) {
// subdivide according to length
if (ps.size() < 2)
return ps;
vec_Vecf<Dim> path;
for (unsigned int i = 1; i < ps.size(); i++) {
decimal_t dist = (ps[i] - ps[i - 1]).norm();
int cnt = std::ceil(dist / d);
for (int j = 0; j < cnt; j++)
path.push_back(ps[i - 1] + j * (ps[i] - ps[i - 1]) / cnt);
}
path.push_back(ps.back());
return path;
}
/// Get closest distance
decimal_t cal_closest_dist(const Vecf<Dim>& pt, const Polyhedron<Dim>& vs){
decimal_t dist = std::numeric_limits<decimal_t>::infinity();
for(const auto& it: vs.hyperplanes()){
decimal_t d = std::abs(it.n_.dot(pt - it.p_));
if(d < dist)
dist = d;
}
return dist;
}
/// Remove redundant waypoints
vec_Vecf<Dim> simplify(const vec_Vecf<Dim>& path) {
if(path.size() <= 2)
return path;
Vecf<Dim> ref_pt = path.front();
vec_Vecf<Dim> new_path;
new_path.push_back(ref_pt);
for(size_t i = 2; i < path.size(); i ++){
if(this->polyhedrons_[i-1].inside(ref_pt) &&
cal_closest_dist(ref_pt, this->polyhedrons_[i-1]) > 0.1) {
}
else{
ref_pt = path[i-1];
new_path.push_back(ref_pt);
}
}
new_path.push_back(path.back());
return new_path;
}
};
typedef IterativeDecomp<2> IterativeDecomp2D;
typedef IterativeDecomp<3> IterativeDecomp3D;
#endif

222
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/line_segment.h Normal file
View File

@@ -0,0 +1,222 @@
/**
* @file line_segment.h
* @brief LineSegment Class
*/
#ifndef LINE_SEGMENT_H
#define LINE_SEGMENT_H
#include <decomp_util/decomp_base.h>
#include <decomp_geometry/geometric_utils.h>
/**
* @brief Line Segment Class
*
* The basic element in EllipsoidDecomp
*/
template <int Dim>
class LineSegment : public DecompBase<Dim> {
public:
///Simple constructor
LineSegment() {};
/**
* @brief Basic constructor
* @param p1 One end of the line seg
* @param p2 The other end of the line seg
*/
LineSegment(const Vecf<Dim> &p1, const Vecf<Dim> &p2) : p1_(p1), p2_(p2) {}
/**
* @brief Infalte the line segment
* @param radius the offset added to the long semi-axis
*/
void dilate(decimal_t radius) {
find_ellipsoid(radius);
this->find_polyhedron();
add_local_bbox(this->polyhedron_);
}
/// Get the line
vec_Vecf<Dim> get_line_segment() const {
vec_Vecf<Dim> line;
line.push_back(p1_);
line.push_back(p2_);
return line;
}
protected:
///Add the bounding box
void add_local_bbox(Polyhedron<Dim> &Vs) {
if(this->local_bbox_.norm() == 0)
return;
//**** virtual walls parallel to path p1->p2
Vecf<Dim> dir = (p2_ - p1_).normalized();
Vecf<Dim> dir_h = Vecf<Dim>::Zero();
dir_h(0) = dir(1), dir_h(1) = -dir(0);
if (dir_h.norm() == 0) {
if(Dim == 2)
dir_h << -1, 0;
else
dir_h << -1, 0, 0;
}
dir_h = dir_h.normalized();
// along x
Vecf<Dim> pp1 = p1_ + dir_h * this->local_bbox_(1);
Vecf<Dim> pp2 = p1_ - dir_h * this->local_bbox_(1);
Vs.add(Hyperplane<Dim>(pp1, dir_h));
Vs.add(Hyperplane<Dim>(pp2, -dir_h));
// along y
Vecf<Dim> pp3 = p2_ + dir * this->local_bbox_(0);
Vecf<Dim> pp4 = p1_ - dir * this->local_bbox_(0);
Vs.add(Hyperplane<Dim>(pp3, dir));
Vs.add(Hyperplane<Dim>(pp4, -dir));
// along z
if(Dim > 2) {
Vecf<Dim> dir_v;
dir_v(0) = dir(1) * dir_h(2) - dir(2) * dir_h(1);
dir_v(1) = dir(2) * dir_h(0) - dir(0) * dir_h(2);
dir_v(2) = dir(0) * dir_h(1) - dir(1) * dir_h(0);
Vecf<Dim> pp5 = p1_ + dir_v * this->local_bbox_(2);
Vecf<Dim> pp6 = p1_ - dir_v * this->local_bbox_(2);
Vs.add(Hyperplane<Dim>(pp5, dir_v));
Vs.add(Hyperplane<Dim>(pp6, -dir_v));
}
}
/// Find ellipsoid in 2D
template<int U = Dim>
typename std::enable_if<U == 2>::type
find_ellipsoid(double offset_x) {
const decimal_t f = (p1_ - p2_).norm() / 2;
Matf<Dim, Dim> C = f * Matf<Dim, Dim>::Identity();
Vecf<Dim> axes = Vecf<Dim>::Constant(f);
C(0, 0) += offset_x;
axes(0) += offset_x;
if(axes(0) > 0) {
double ratio = axes(1) / axes(0);
axes *= ratio;
C *= ratio;
}
const auto Ri = vec2_to_rotation(p2_ - p1_);
C = Ri * C * Ri.transpose();
Ellipsoid<Dim> E(C, (p1_ + p2_) / 2);
auto obs = E.points_inside(this->obs_);
auto obs_inside = obs;
//**** decide short axes
while (!obs_inside.empty()) {
const auto pw = E.closest_point(obs_inside);
Vecf<Dim> p = Ri.transpose() * (pw - E.d()); // to ellipsoid frame
if(p(0) < axes(0))
axes(1) = std::abs(p(1)) / std::sqrt(1 - std::pow(p(0) / axes(0), 2));
Matf<Dim, Dim> new_C = Matf<Dim, Dim>::Identity();
new_C(0, 0) = axes(0);
new_C(1, 1) = axes(1);
E.C_ = Ri * new_C * Ri.transpose();
vec_Vecf<Dim> obs_new;
for(const auto &it: obs_inside) {
if(1 - E.dist(it) > epsilon_)
obs_new.push_back(it);
}
obs_inside = obs_new;
}
this->ellipsoid_ = E;
}
/// Find ellipsoid in 3D
template<int U = Dim>
typename std::enable_if<U == 3>::type
find_ellipsoid(double offset_x) {
const decimal_t f = (p1_ - p2_).norm() / 2;
Matf<Dim, Dim> C = f * Matf<Dim, Dim>::Identity();
Vecf<Dim> axes = Vecf<Dim>::Constant(f);
C(0, 0) += offset_x;
axes(0) += offset_x;
if(axes(0) > 0) {
double ratio = axes(1) / axes(0);
axes *= ratio;
C *= ratio;
}
const auto Ri = vec3_to_rotation(p2_ - p1_);
C = Ri * C * Ri.transpose();
Ellipsoid<Dim> E(C, (p1_ + p2_) / 2);
auto Rf = Ri;
auto obs = E.points_inside(this->obs_);
auto obs_inside = obs;
//**** decide short axes
while (!obs_inside.empty()) {
const auto pw = E.closest_point(obs_inside);
Vecf<Dim> p = Ri.transpose() * (pw - E.d()); // to ellipsoid frame
const decimal_t roll = atan2(p(2), p(1));
Rf = Ri * Quatf(cos(roll / 2), sin(roll / 2), 0, 0);
p = Rf.transpose() * (pw - E.d());
if(p(0) < axes(0))
axes(1) = std::abs(p(1)) / std::sqrt(1 - std::pow(p(0) / axes(0), 2));
Matf<Dim, Dim> new_C = Matf<Dim, Dim>::Identity();
new_C(0, 0) = axes(0);
new_C(1, 1) = axes(1);
new_C(2, 2) = axes(1);
E.C_ = Rf * new_C * Rf.transpose();
vec_Vecf<Dim> obs_new;
for(const auto &it: obs_inside) {
if(1 - E.dist(it) > epsilon_)
obs_new.push_back(it);
}
obs_inside = obs_new;
}
//**** reset ellipsoid with old axes(2)
C = f * Matf<Dim, Dim>::Identity();
C(0, 0) = axes(0);
C(1, 1) = axes(1);
C(2, 2) = axes(2);
E.C_ = Rf * C * Rf.transpose();
obs_inside = E.points_inside(obs);
while (!obs_inside.empty()) {
const auto pw = E.closest_point(obs_inside);
Vec3f p = Rf.transpose() * (pw - E.d());
decimal_t dd = 1 - std::pow(p(0) / axes(0), 2) -
std::pow(p(1) / axes(1), 2);
if(dd > epsilon_)
axes(2) = std::abs(p(2)) / std::sqrt(dd);
Matf<Dim, Dim> new_C = Matf<Dim, Dim>::Identity();
new_C(0, 0) = axes(0);
new_C(1, 1) = axes(1);
new_C(2, 2) = axes(2);
E.C_ = Rf * new_C * Rf.transpose();
vec_Vecf<Dim> obs_new;
for(const auto &it: obs_inside) {
if(1 - E.dist(it) > epsilon_)
obs_new.push_back(it);
}
obs_inside = obs_new;
}
this->ellipsoid_ = E;
}
/// One end of line segment, input
Vecf<Dim> p1_;
/// The other end of line segment, input
Vecf<Dim> p2_;
};
typedef LineSegment<2> LineSegment2D;
typedef LineSegment<3> LineSegment3D;
#endif

80
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/seed_decomp.h Normal file
View File

@@ -0,0 +1,80 @@
/**
* @file seed_decomp.h
* @brief SeedDecomp Class
*/
#ifndef SEED_DECOMP_H
#define SEED_DECOMP_H
#include <decomp_util/decomp_base.h>
/**
* @brief Seed Decomp Class
*
* Dilate around the given point
*/
template <int Dim>
class SeedDecomp : public DecompBase<Dim> {
public:
///Simple constructor
SeedDecomp() {};
/**
* @brief Basic constructor
* @param p1 One end of the line seg
* @param p2 The other end of the line seg
*/
SeedDecomp(const Vecf<Dim> &p) : p_(p) {}
/**
* @brief Inflate the seed with a sphere
* @param radius Robot radius
*/
void dilate(decimal_t radius) {
this->ellipsoid_ = Ellipsoid<Dim>(radius * Matf<Dim, Dim>::Identity(), p_);
this->find_polyhedron();
add_local_bbox(this->polyhedron_);
}
/// Get the center
Vecf<Dim> get_seed() const {
return p_;
}
protected:
///Add the bounding box
void add_local_bbox(Polyhedron<Dim> &Vs) {
if(this->local_bbox_.norm() == 0)
return;
//**** virtual walls x-y-z
Vecf<Dim> dir = Vecf<Dim>::UnitX();
Vecf<Dim> dir_h = Vecf<Dim>::UnitY();
Vecf<Dim> pp1 = p_ + dir_h * this->local_bbox_(1);
Vecf<Dim> pp2 = p_ - dir_h * this->local_bbox_(1);
Vs.add(Hyperplane<Dim>(pp1, dir_h));
Vs.add(Hyperplane<Dim>(pp2, -dir_h));
// along y
Vecf<Dim> pp3 = p_ + dir * this->local_bbox_(0);
Vecf<Dim> pp4 = p_ - dir * this->local_bbox_(0);
Vs.add(Hyperplane<Dim>(pp3, dir));
Vs.add(Hyperplane<Dim>(pp4, -dir));
// along z
if(Dim > 2) {
Vecf<Dim> dir_v = Vecf<Dim>::UnitZ();
Vecf<Dim> pp5 = p_ + dir_v * this->local_bbox_(2);
Vecf<Dim> pp6 = p_ - dir_v * this->local_bbox_(2);
Vs.add(Hyperplane<Dim>(pp5, dir_v));
Vs.add(Hyperplane<Dim>(pp6, -dir_v));
}
}
///Seed location
Vecf<Dim> p_;
};
typedef SeedDecomp<2> SeedDecomp2D;
typedef SeedDecomp<3> SeedDecomp3D;
#endif

20
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/package.xml Normal file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<package format="2">
<name>decomp_ros_utils</name>
<version>0.0.0</version>
<description>The decomp_ros_utils package</description>
<maintainer email="sikang@todo.todo">sikang</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>catkin_simple</build_depend>
<depend>roscpp</depend>
<depend>rviz</depend>
<depend>decomp_ros_msgs</depend>
<export>
<rviz plugin="${prefix}/plugin_description.xml"/>
</export>
</package>

18
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/plugin_description.xml Normal file
View File

@@ -0,0 +1,18 @@
<library path="lib/libdecomp_rviz_plugins">
<class name="decomp_rviz_plugins/EllipsoidArray"
type="decomp_rviz_plugins::EllipsoidArrayDisplay"
base_class_type="rviz::Display">
<description>
visualize decomp_ros_msgs/EllipsoidArray msg.
</description>
</class>
<class name="decomp_rviz_plugins/PolyhedronArray"
type="decomp_rviz_plugins::PolyhedronArrayDisplay"
base_class_type="rviz::Display">
<description>
visualize decomp_ros_msgs/PolyhedronArray msg.
</description>
</class>
</library>

56
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/bound_visual.cpp Normal file
View File

@@ -0,0 +1,56 @@
#include "bound_visual.h"
namespace decomp_rviz_plugins {
BoundVisual::BoundVisual(Ogre::SceneManager *scene_manager,
Ogre::SceneNode *parent_node) {
scene_manager_ = scene_manager;
frame_node_ = parent_node->createChildSceneNode();
}
BoundVisual::~BoundVisual() { scene_manager_->destroySceneNode(frame_node_); }
void BoundVisual::setMessage(const vec_E<vec_Vec3f>& bds) {
objs_.clear();
if (bds.empty())
return;
size_t num_faces = bds.size();
objs_.resize(num_faces);
for (auto &it : objs_)
it.reset(new rviz::BillboardLine(scene_manager_, frame_node_));
int cnt = 0;
for (const auto &vs : bds) {
for (unsigned int i = 0; i <= vs.size(); i++) {
if (i < vs.size()) {
if(!std::isnan(vs[i](0)))
objs_[cnt]->addPoint(Ogre::Vector3(vs[i](0), vs[i](1), vs[i](2)));
}
else {
if(!std::isnan(vs[0](0)))
objs_[cnt]->addPoint(Ogre::Vector3(vs[0](0), vs[0](1), vs[0](2)));
}
}
cnt++;
}
}
void BoundVisual::setFramePosition(const Ogre::Vector3 &position) {
frame_node_->setPosition(position);
}
void BoundVisual::setFrameOrientation(const Ogre::Quaternion &orientation) {
frame_node_->setOrientation(orientation);
}
void BoundVisual::setColor(float r, float g, float b, float a) {
for (auto &it : objs_)
it->setColor(r, g, b, a);
}
void BoundVisual::setScale(float s) {
for (auto &it : objs_)
it->setLineWidth(s);
}
}

35
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/bound_visual.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef BOUND_VISUAL_H
#define BOUND_VISUAL_H
#include <decomp_basis/data_type.h>
#include <OGRE/OgreVector3.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <rviz/ogre_helpers/billboard_line.h>
namespace decomp_rviz_plugins {
class BoundVisual {
public:
BoundVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node);
~BoundVisual();
void setMessage(const vec_E<vec_Vec3f> &bds);
void setFramePosition(const Ogre::Vector3 &position);
void setFrameOrientation(const Ogre::Quaternion &orientation);
void setColor(float r, float g, float b, float a);
void setScale(float s);
private:
std::vector<std::unique_ptr<rviz::BillboardLine>> objs_;
Ogre::SceneNode *frame_node_;
Ogre::SceneManager *scene_manager_;
};
}
#endif

62
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/ellipsoid_array_display.cpp Normal file
View File

@@ -0,0 +1,62 @@
#include <tf/transform_listener.h>
#include "ellipsoid_array_display.h"
namespace decomp_rviz_plugins {
EllipsoidArrayDisplay::EllipsoidArrayDisplay() {
color_property_ = new rviz::ColorProperty("Color", QColor(204, 51, 204),
"Color of ellipsoids.",
this, SLOT(updateColorAndAlpha()));
alpha_property_ = new rviz::FloatProperty(
"Alpha", 0.5, "0 is fully transparent, 1.0 is fully opaque.", this,
SLOT(updateColorAndAlpha()));
}
void EllipsoidArrayDisplay::onInitialize() {
MFDClass::onInitialize();
}
EllipsoidArrayDisplay::~EllipsoidArrayDisplay() {}
void EllipsoidArrayDisplay::reset() {
MFDClass::reset();
visual_ = nullptr;
}
void EllipsoidArrayDisplay::updateColorAndAlpha() {
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
if (visual_)
visual_->setColor(color.r, color.g, color.b, alpha);
}
void EllipsoidArrayDisplay::processMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg) {
Ogre::Quaternion orientation;
Ogre::Vector3 position;
if (!context_->getFrameManager()->getTransform(
msg->header.frame_id, msg->header.stamp, position, orientation)) {
ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
return;
}
std::shared_ptr<EllipsoidArrayVisual> visual;
visual.reset(new EllipsoidArrayVisual(context_->getSceneManager(), scene_node_));
visual->setMessage(msg);
visual->setFramePosition(position);
visual->setFrameOrientation(orientation);
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
visual->setColor(color.r, color.g, color.b, alpha);
visual_ = visual;
}
}
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(decomp_rviz_plugins::EllipsoidArrayDisplay, rviz::Display)

41
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/ellipsoid_array_display.h Normal file
View File

@@ -0,0 +1,41 @@
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <rviz/visualization_manager.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/frame_manager.h>
#include <rviz/load_resource.h>
#include <decomp_ros_msgs/EllipsoidArray.h>
#include <rviz/message_filter_display.h>
#include "ellipsoid_array_visual.h"
namespace decomp_rviz_plugins {
class EllipsoidArrayVisual;
class EllipsoidArrayDisplay
: public rviz::MessageFilterDisplay<decomp_ros_msgs::EllipsoidArray> {
Q_OBJECT
public:
EllipsoidArrayDisplay();
~EllipsoidArrayDisplay();
protected:
void onInitialize();
void reset();
private Q_SLOTS:
void updateColorAndAlpha();
private:
void processMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg);
std::shared_ptr<EllipsoidArrayVisual> visual_;
rviz::ColorProperty *color_property_;
rviz::FloatProperty *alpha_property_;
};
}

72
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/ellipsoid_array_visual.cpp Normal file
View File

@@ -0,0 +1,72 @@
#include "ellipsoid_array_visual.h"
namespace decomp_rviz_plugins {
EllipsoidArrayVisual::EllipsoidArrayVisual(Ogre::SceneManager *scene_manager,
Ogre::SceneNode *parent_node) {
scene_manager_ = scene_manager;
frame_node_ = parent_node->createChildSceneNode();
}
EllipsoidArrayVisual::~EllipsoidArrayVisual() {
scene_manager_->destroySceneNode(frame_node_);
}
void EllipsoidArrayVisual::setMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg) {
objs_.clear();
if (msg->ellipsoids.empty())
return;
for (const auto& it: msg->ellipsoids) {
if(std::isnan(it.d[0]) ||
std::isnan(it.d[1]) ||
std::isnan(it.d[2]))
return;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
if(std::isnan(it.E[3 * i + j]))
return;
}
objs_.resize(msg->ellipsoids.size());
for (auto &it : objs_)
it.reset(new rviz::Shape(rviz::Shape::Type::Sphere, scene_manager_,
frame_node_));
int cnt = 0;
for (const auto &it : msg->ellipsoids) {
Mat3f E;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
E(i, j) = it.E[3 * i + j];
Eigen::SelfAdjointEigenSolver<Mat3f> es(E);
Ogre::Vector3 scale(2 * es.eigenvalues()[0], 2 * es.eigenvalues()[1],
2 * es.eigenvalues()[2]);
objs_[cnt]->setScale(scale);
Ogre::Vector3 d(it.d[0], it.d[1], it.d[2]);
objs_[cnt]->setPosition(d);
Quatf q(es.eigenvectors().determinant() * es.eigenvectors());
Ogre::Quaternion o(q.w(), q.x(), q.y(), q.z());
objs_[cnt]->setOrientation(o);
cnt++;
}
}
void EllipsoidArrayVisual::setFramePosition(const Ogre::Vector3 &position) {
frame_node_->setPosition(position);
}
void EllipsoidArrayVisual::setFrameOrientation(
const Ogre::Quaternion &orientation) {
frame_node_->setOrientation(orientation);
}
void EllipsoidArrayVisual::setColor(float r, float g, float b, float a) {
for (auto &it : objs_)
it->setColor(r, g, b, a);
}
}

37
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/ellipsoid_array_visual.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef ELLIPSOIDS_VISUAL_H
#define ELLIPSOIDS_VISUAL_H
#include <decomp_ros_msgs/EllipsoidArray.h>
#include <decomp_geometry/ellipsoid.h>
#include <Eigen/Eigenvalues>
#include <OGRE/OgreVector3.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <rviz/ogre_helpers/shape.h>
namespace decomp_rviz_plugins {
class EllipsoidArrayVisual {
public:
EllipsoidArrayVisual(Ogre::SceneManager *scene_manager,
Ogre::SceneNode *parent_node);
virtual ~EllipsoidArrayVisual();
void setMessage(const decomp_ros_msgs::EllipsoidArray::ConstPtr &msg);
void setFramePosition(const Ogre::Vector3 &position);
void setFrameOrientation(const Ogre::Quaternion &orientation);
void setColor(float r, float g, float b, float a);
private:
std::vector<std::unique_ptr<rviz::Shape>> objs_;
Ogre::SceneNode *frame_node_;
Ogre::SceneManager *scene_manager_;
};
}
#endif

56
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/mesh_visual.cpp Normal file
View File

@@ -0,0 +1,56 @@
#include "mesh_visual.h"
namespace decomp_rviz_plugins {
MeshVisual::MeshVisual(Ogre::SceneManager *scene_manager,
Ogre::SceneNode *parent_node) {
scene_manager_ = scene_manager;
frame_node_ = parent_node->createChildSceneNode();
obj_.reset(new rviz::MeshShape(scene_manager_, frame_node_));
}
MeshVisual::~MeshVisual() { scene_manager_->destroySceneNode(frame_node_); }
void MeshVisual::setMessage(const vec_E<vec_Vec3f> &bds) {
obj_->clear();
if (bds.empty())
return;
obj_->beginTriangles();
int free_cnt = 0;
for (const auto &vs: bds) {
if (vs.size() > 2) {
Vec3f p0 = vs[0];
Vec3f p1 = vs[1];
Vec3f p2 = vs[2];
Vec3f n = (p2-p0).cross(p1-p0);
n = n.normalized();
if(std::isnan(n(0)))
n = Vec3f(0, 0, -1);
int ref_cnt = free_cnt;
Ogre::Vector3 normal(n(0), n(1), n(2));
for (unsigned int i = 0; i < vs.size(); i++) {
obj_->addVertex(Ogre::Vector3(vs[i](0), vs[i](1), vs[i](2)), normal);
if (i > 1 && i < vs.size())
obj_->addTriangle(ref_cnt, free_cnt - 1, free_cnt);
free_cnt++;
}
}
}
obj_->endTriangles();
}
// Position and orientation are passed through to the SceneNode.
void MeshVisual::setFramePosition(const Ogre::Vector3 &position) {
frame_node_->setPosition(position);
}
void MeshVisual::setFrameOrientation(const Ogre::Quaternion &orientation) {
frame_node_->setOrientation(orientation);
}
void MeshVisual::setColor(float r, float g, float b, float a) {
obj_->setColor(r, g, b, a);
}
}

35
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/mesh_visual.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef MESH_VISUAL_H
#define MESH_VISUAL_H
#include <decomp_geometry/polyhedron.h>
#include <Eigen/Eigenvalues>
#include <OGRE/OgreVector3.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <rviz/ogre_helpers/mesh_shape.h>
namespace decomp_rviz_plugins {
class MeshVisual {
public:
MeshVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node);
virtual ~MeshVisual();
void setMessage(const vec_E<vec_Vec3f> &bds);
void setFramePosition(const Ogre::Vector3 &position);
void setFrameOrientation(const Ogre::Quaternion &orientation);
void setColor(float r, float g, float b, float a);
private:
std::unique_ptr<rviz::MeshShape> obj_;
Ogre::SceneNode *frame_node_;
Ogre::SceneManager *scene_manager_;
};
}
#endif

186
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/polyhedron_array_display.cpp Normal file
View File

@@ -0,0 +1,186 @@
#include "polyhedron_array_display.h"
namespace decomp_rviz_plugins {
PolyhedronArrayDisplay::PolyhedronArrayDisplay() {
mesh_color_property_ =
new rviz::ColorProperty("MeshColor", QColor(0, 170, 255), "Mesh color.",
this, SLOT(updateMeshColorAndAlpha()));
bound_color_property_ =
new rviz::ColorProperty("BoundColor", QColor(255, 0, 0), "Bound color.",
this, SLOT(updateBoundColorAndAlpha()));
alpha_property_ = new rviz::FloatProperty(
"Alpha", 0.2,
"0 is fully transparent, 1.0 is fully opaque, only affect mesh", this,
SLOT(updateMeshColorAndAlpha()));
scale_property_ = new rviz::FloatProperty("Scale", 0.1, "bound scale.", this,
SLOT(updateScale()));
vs_scale_property_ = new rviz::FloatProperty("VsScale", 1.0, "Vs scale.", this,
SLOT(updateVsScale()));
vs_color_property_ =
new rviz::ColorProperty("VsColor", QColor(0, 255, 0), "Vs color.",
this, SLOT(updateVsColorAndAlpha()));
state_property_ = new rviz::EnumProperty(
"State", "Mesh", "A Polygon can be represented as two states: Mesh and "
"Bound, this option allows selecting visualizing Polygon"
"in corresponding state",
this, SLOT(updateState()));
state_property_->addOption("Mesh", 0);
state_property_->addOption("Bound", 1);
state_property_->addOption("Both", 2);
state_property_->addOption("Vs", 3);
}
void PolyhedronArrayDisplay::onInitialize() { MFDClass::onInitialize(); }
PolyhedronArrayDisplay::~PolyhedronArrayDisplay() {}
void PolyhedronArrayDisplay::reset() {
MFDClass::reset();
visual_mesh_ = nullptr;
visual_bound_ = nullptr;
visual_vector_ = nullptr;
}
void PolyhedronArrayDisplay::processMessage(const decomp_ros_msgs::PolyhedronArray::ConstPtr &msg) {
if (!context_->getFrameManager()->getTransform(
msg->header.frame_id, msg->header.stamp, position_, orientation_)) {
ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
return;
}
vertices_.clear();
vs_.clear();
const auto polys = DecompROS::ros_to_polyhedron_array(*msg);
for(const auto& polyhedron: polys){
vec_E<vec_Vec3f> bds = cal_vertices(polyhedron);
vertices_.insert(vertices_.end(), bds.begin(), bds.end());
const auto vs = polyhedron.cal_normals();
vs_.insert(vs_.end(), vs.begin(), vs.end());
}
int state = state_property_->getOptionInt();
visualizeMessage(state);
}
void PolyhedronArrayDisplay::visualizeMesh() {
std::shared_ptr<MeshVisual> visual_mesh;
visual_mesh.reset(new MeshVisual(context_->getSceneManager(), scene_node_));
visual_mesh->setMessage(vertices_);
visual_mesh->setFramePosition(position_);
visual_mesh->setFrameOrientation(orientation_);
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = mesh_color_property_->getOgreColor();
visual_mesh->setColor(color.r, color.g, color.b, alpha);
visual_mesh_ = visual_mesh;
}
void PolyhedronArrayDisplay::visualizeBound() {
std::shared_ptr<BoundVisual> visual_bound;
visual_bound.reset(new BoundVisual(context_->getSceneManager(), scene_node_));
visual_bound->setMessage(vertices_);
visual_bound->setFramePosition(position_);
visual_bound->setFrameOrientation(orientation_);
Ogre::ColourValue color = bound_color_property_->getOgreColor();
visual_bound->setColor(color.r, color.g, color.b, 1.0);
float scale = scale_property_->getFloat();
visual_bound->setScale(scale);
visual_bound_ = visual_bound;
}
void PolyhedronArrayDisplay::visualizeVs() {
std::shared_ptr<VectorVisual> visual_vector;
visual_vector.reset(new VectorVisual(context_->getSceneManager(), scene_node_));
visual_vector->setMessage(vs_);
visual_vector->setFramePosition(position_);
visual_vector->setFrameOrientation(orientation_);
Ogre::ColourValue color = vs_color_property_->getOgreColor();
visual_vector->setColor(color.r, color.g, color.b, 1.0);
visual_vector_ = visual_vector;
}
void PolyhedronArrayDisplay::visualizeMessage(int state) {
switch (state) {
case 0:
visual_bound_ = nullptr;
visual_vector_ = nullptr;
visualizeMesh();
break;
case 1:
visual_mesh_ = nullptr;
visual_vector_ = nullptr;
visualizeBound();
break;
case 2:
visual_vector_ = nullptr;
visualizeMesh();
visualizeBound();
break;
case 3:
visualizeVs();
break;
default:
std::cout << "Invalid State: " << state << std::endl;
}
}
void PolyhedronArrayDisplay::updateMeshColorAndAlpha() {
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = mesh_color_property_->getOgreColor();
if(visual_mesh_)
visual_mesh_->setColor(color.r, color.g, color.b, alpha);
}
void PolyhedronArrayDisplay::updateBoundColorAndAlpha() {
Ogre::ColourValue color = bound_color_property_->getOgreColor();
if(visual_bound_)
visual_bound_->setColor(color.r, color.g, color.b, 1.0);
}
void PolyhedronArrayDisplay::updateState() {
int state = state_property_->getOptionInt();
visualizeMessage(state);
}
void PolyhedronArrayDisplay::updateScale() {
float s = scale_property_->getFloat();
if(visual_bound_)
visual_bound_->setScale(s);
}
void PolyhedronArrayDisplay::updateVsScale() {
float s = vs_scale_property_->getFloat();
if(visual_vector_)
visual_vector_->setScale(s);
}
void PolyhedronArrayDisplay::updateVsColorAndAlpha() {
Ogre::ColourValue color = vs_color_property_->getOgreColor();
if(visual_vector_)
visual_vector_->setColor(color.r, color.g, color.b, 1);
}
}
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(decomp_rviz_plugins::PolyhedronArrayDisplay, rviz::Display)

68
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/polyhedron_array_display.h Normal file
View File

@@ -0,0 +1,68 @@
#include <decomp_ros_msgs/PolyhedronArray.h>
#include <rviz/message_filter_display.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/properties/enum_property.h>
#include <rviz/visualization_manager.h>
#include <rviz/frame_manager.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <rviz/load_resource.h>
#include "mesh_visual.h"
#include "bound_visual.h"
#include "vector_visual.h"
#include <decomp_ros_utils/data_ros_utils.h>
#include <decomp_geometry/geometric_utils.h>
namespace decomp_rviz_plugins {
class PolyhedronArrayDisplay
: public rviz::MessageFilterDisplay<decomp_ros_msgs::PolyhedronArray> {
Q_OBJECT
public:
PolyhedronArrayDisplay();
virtual ~PolyhedronArrayDisplay();
protected:
virtual void onInitialize();
virtual void reset();
private Q_SLOTS:
void updateMeshColorAndAlpha();
void updateBoundColorAndAlpha();
void updateVsColorAndAlpha();
void updateState();
void updateScale();
void updateVsScale();
private:
void processMessage(const decomp_ros_msgs::PolyhedronArray::ConstPtr &msg);
void visualizeMessage(int state);
void visualizeMesh();
void visualizeBound();
void visualizeVs();
std::shared_ptr<MeshVisual> visual_mesh_;
std::shared_ptr<BoundVisual> visual_bound_;
std::shared_ptr<VectorVisual> visual_vector_;
rviz::ColorProperty *mesh_color_property_;
rviz::ColorProperty *bound_color_property_;
rviz::ColorProperty *vs_color_property_;
rviz::FloatProperty *alpha_property_;
rviz::FloatProperty *scale_property_;
rviz::FloatProperty *vs_scale_property_;
rviz::EnumProperty *state_property_;
Ogre::Vector3 position_;
Ogre::Quaternion orientation_;
vec_E<vec_Vec3f> vertices_;
vec_E<std::pair<Vec3f, Vec3f>> vs_;
};
}

58
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/vector_visual.cpp Normal file
View File

@@ -0,0 +1,58 @@
#include "vector_visual.h"
namespace decomp_rviz_plugins {
VectorVisual::VectorVisual(Ogre::SceneManager *scene_manager,
Ogre::SceneNode *parent_node) {
scene_manager_ = scene_manager;
frame_node_ = parent_node->createChildSceneNode();
}
VectorVisual::~VectorVisual() { scene_manager_->destroySceneNode(frame_node_); }
void VectorVisual::setMessage(const vec_E<std::pair<Vec3f, Vec3f>> &vs) {
objs_.clear();
vs_ = vs;
if (vs.empty())
return;
objs_.resize(vs.size());
for (auto &it : objs_)
it.reset(new rviz::Arrow(scene_manager_, frame_node_));
int cnt = 0;
for (const auto &v : vs) {
Vec3f n = v.second.normalized();
objs_[cnt]->setDirection(Ogre::Vector3(n(0), n(1), n(2)));
objs_[cnt]->setPosition(Ogre::Vector3(v.first(0), v.first(1), v.first(2)));
objs_[cnt]->setScale(s_ * Ogre::Vector3(1.0, 1.0, 1.0));
float d = s_ * v.second.norm();
float shaft_length = 0.7 * d;
float head_length = 0.3 * d;
objs_[cnt]->set(shaft_length, d / 8, head_length, d / 5);
cnt ++;
}
}
void VectorVisual::setFramePosition(const Ogre::Vector3 &position) {
frame_node_->setPosition(position);
}
void VectorVisual::setFrameOrientation(const Ogre::Quaternion &orientation) {
frame_node_->setOrientation(orientation);
}
void VectorVisual::setColor(float r, float g, float b, float a) {
for (auto &it : objs_)
it->setColor(r, g, b, a);
}
void VectorVisual::setScale(float s) {
s_ = s;
for (unsigned int i = 0; i < objs_.size(); i++){
float d = s_ * vs_[i].second.norm();
float shaft_length = 0.7 * d;
float head_length = 0.3 * d;
objs_[i]->set(shaft_length, d / 8, head_length, d / 5);
}
}
}

35
trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/vector_visual.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef VECTOR_VISUAL_H
#define VECTOR_VISUAL_H
#include <decomp_basis/data_type.h>
#include <OGRE/OgreVector3.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <rviz/ogre_helpers/arrow.h>
namespace decomp_rviz_plugins {
class VectorVisual {
public:
VectorVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node);
~VectorVisual();
void setMessage(const vec_E<std::pair<Vec3f, Vec3f>> &vs);
void setFramePosition(const Ogre::Vector3 &position);
void setFrameOrientation(const Ogre::Quaternion &orientation);
void setColor(float r, float g, float b, float a);
void setScale(float s);
private:
std::vector<std::unique_ptr<rviz::Arrow>> objs_;
Ogre::SceneNode *frame_node_;
Ogre::SceneManager *scene_manager_;
float s_ = 1.0;
vec_E<std::pair<Vec3f, Vec3f>> vs_;
};
}
#endif

9
trajectoryOpt/src/utils/catkin_simple/CMakeLists.txt Normal file
View File

@@ -0,0 +1,9 @@
cmake_minimum_required(VERSION 2.8.3)
project(catkin_simple)
find_package(catkin REQUIRED)
catkin_package(
CATKIN_DEPENDS catkin
CFG_EXTRAS catkin_simple-extras.cmake
)

226
trajectoryOpt/src/utils/catkin_simple/cmake/catkin_simple-extras.cmake.em Normal file
View File

@@ -0,0 +1,226 @@
# Generated from: catkin_simple/cmake/catkin_simple-extras.cmake.em
if(_CATKIN_SIMPLE_EXTRAS_INCLUDED_)
return()
endif()
set(_CATKIN_SIMPLE_EXTRAS_INCLUDED_ TRUE)
include(CMakeParseArguments)
@[if DEVELSPACE]@
# cmake dir in develspace
set(catkin_simple_CMAKE_DIR "@(CMAKE_CURRENT_SOURCE_DIR)/cmake")
@[else]@
# cmake dir in installspace
set(catkin_simple_CMAKE_DIR "@(PKG_CMAKE_DIR)")
@[end if]@
macro(catkin_simple)
# Arguments
# ALL_DEPS_REQUIRED -- Add the "REQUIRED" flag when calling
# FIND_PACKAGE() for each dependency
cmake_parse_arguments(cs_args "ALL_DEPS_REQUIRED" "" "" ${ARGN})
if(TARGET ${PROJECT_NAME}_package)
message(WARNING "Could not create target '${${PROJECT_NAME}_package}' for project ${PROJECT_NAME}, as it already exists.")
endif()
add_custom_target(${PROJECT_NAME}_package)
set(${PROJECT_NAME}_TARGETS)
set(${PROJECT_NAME}_LIBRARIES)
find_package(catkin REQUIRED)
# call catkin_package_xml() if it has not been called before
if(NOT _CATKIN_CURRENT_PACKAGE)
catkin_package_xml()
endif()
set(${PROJECT_NAME}_CATKIN_BUILD_DEPENDS)
set(${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS)
foreach(dep ${${PROJECT_NAME}_BUILD_DEPENDS})
# If this flag is defined, add the "REQUIRED" flag
# to all FIND_PACKAGE calls
if(cs_args_ALL_DEPS_REQUIRED)
find_package(${dep} REQUIRED)
else()
find_package(${dep} QUIET)
endif()
if(${dep}_FOUND_CATKIN_PROJECT)
list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS ${dep})
list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS ${${dep}_EXPORTED_TARGETS})
endif()
endforeach()
# Let find_package(catkin ...) do the heavy lifting
find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS})
# add include directory if available
set(${PROJECT_NAME}_LOCAL_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include)
if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR})
set(${PROJECT_NAME}_LOCAL_INCLUDE_DIR)
endif()
include_directories(${${PROJECT_NAME}_LOCAL_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
# perform action/msg/srv generation if necessary
if(message_generation_FOUND_CATKIN_PROJECT)
set(${PROJECT_NAME}_DO_MESSAGE_GENERATION FALSE)
# add action files if available
set(${PROJECT_NAME}_LOCAL_ACTION_DIR ${CMAKE_CURRENT_SOURCE_DIR}/action)
if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_ACTION_DIR})
set(${PROJECT_NAME}_LOCAL_ACTION_DIR)
endif()
if(${PROJECT_NAME}_LOCAL_ACTION_DIR)
add_action_files(DIRECTORY action)
set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE)
endif()
# add message files if available
set(${PROJECT_NAME}_LOCAL_MSG_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg)
if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_MSG_DIR})
set(${PROJECT_NAME}_LOCAL_MSG_DIR)
endif()
if(${PROJECT_NAME}_LOCAL_MSG_DIR)
add_message_files(DIRECTORY msg)
set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE)
endif()
# add service files if available
set(${PROJECT_NAME}_LOCAL_SRV_DIR ${CMAKE_CURRENT_SOURCE_DIR}/srv)
if(NOT IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_SRV_DIR})
set(${PROJECT_NAME}_LOCAL_SRV_DIR)
endif()
if(${PROJECT_NAME}_LOCAL_SRV_DIR)
add_service_files(DIRECTORY srv)
set(${PROJECT_NAME}_DO_MESSAGE_GENERATION TRUE)
endif()
# generate messages if necessary
if(${PROJECT_NAME}_DO_MESSAGE_GENERATION)
# identify all build dependencies which contain messages
set(${PROJECT_NAME}_MSG_PACKAGES)
foreach(dep ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS})
set(${PROJECT_NAME}_MSG_PACKAGE_FILE ${${dep}_DIR}/${dep}-msg-paths.cmake)
if(EXISTS ${${PROJECT_NAME}_MSG_PACKAGE_FILE})
list(APPEND ${PROJECT_NAME}_MSG_PACKAGES ${dep})
endif()
endforeach()
generate_messages(DEPENDENCIES ${${PROJECT_NAME}_MSG_PACKAGES})
# add additional exported targets coming from generate_messages()
list(INSERT ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS 0 ${${PROJECT_NAME}_EXPORTED_TARGETS})
endif()
endif()
# generate dynamic reconfigure files
if(dynamic_reconfigure_FOUND_CATKIN_PROJECT)
set(${PROJECT_NAME}_LOCAL_CFG_DIR ${CMAKE_CURRENT_SOURCE_DIR}/cfg)
if(IS_DIRECTORY ${${PROJECT_NAME}_LOCAL_CFG_DIR})
# create a list containing all the cfg files
file(GLOB ${PROJECT_NAME}_LOCAL_CFG_FILES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${${PROJECT_NAME}_LOCAL_CFG_DIR}/*.cfg")
if(${PROJECT_NAME}_LOCAL_CFG_FILES)
generate_dynamic_reconfigure_options(${${PROJECT_NAME}_LOCAL_CFG_FILES})
# add build dep on gencfg
list(APPEND ${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS ${PROJECT_NAME}_gencfg)
endif()
endif()
endif()
endmacro()
macro(cs_add_targets_to_package)
add_dependencies(${PROJECT_NAME}_package ${ARGN})
list(APPEND ${PROJECT_NAME}_TARGETS ${ARGN})
endmacro()
macro(cs_add_executable _target)
if(${_target} STREQUAL ${PROJECT_NAME}_package)
message(WARNING "Could not create executable with name '${_target}' as '${PROJECT_NAME}_package' is reserved for the top level target name for this project.")
endif()
cmake_parse_arguments(cs_add_executable_args "NO_AUTO_LINK;NO_AUTO_DEP" "" "" ${ARGN})
add_executable(${_target} ${cs_add_executable_args_UNPARSED_ARGUMENTS})
if(NOT cs_add_executable_args_NO_AUTO_LINK)
target_link_libraries(${_target} ${catkin_LIBRARIES})
endif()
if(NOT cs_add_executable_args_NO_AUTO_DEP)
if(NOT "${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}" STREQUAL "")
add_dependencies(${_target} ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS})
endif()
endif()
cs_add_targets_to_package(${_target})
endmacro()
macro(cs_add_library _target)
if(${_target} STREQUAL ${PROJECT_NAME}_package)
message(WARNING "Could not create library with name '${_target}' as '${PROJECT_NAME}_package' is reserved for the top level target name for this project.")
endif()
cmake_parse_arguments(cs_add_library "NO_AUTO_LINK;NO_AUTO_DEP;NO_AUTO_EXPORT" "" "" ${ARGN})
add_library(${_target} ${cs_add_library_UNPARSED_ARGUMENTS})
if(NOT cs_add_library_NO_AUTO_LINK)
target_link_libraries(${_target} ${catkin_LIBRARIES})
endif()
if(NOT cs_add_library_NO_AUTO_DEP)
if(NOT "${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}" STREQUAL "")
add_dependencies(${_target} ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS})
endif()
endif()
if(NOT cs_add_library_NO_AUTO_EXPORT)
list(APPEND ${PROJECT_NAME}_LIBRARIES ${_target})
endif()
cs_add_targets_to_package(${_target})
endmacro()
macro(cs_install)
# Install targets (exec's and lib's)
foreach(_target ${${PROJECT_NAME}_TARGETS})
get_target_property(${_target}_type ${_target} TYPE)
message(STATUS "Marking ${${_target}_type} \"${_target}\" of package \"${PROJECT_NAME}\" for installation")
endforeach()
install(TARGETS ${${PROJECT_NAME}_TARGETS} ${ARGN}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
if(EXISTS ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR})
# Install include directory
message(STATUS "Marking HEADER FILES in \"include\" folder of package \"${PROJECT_NAME}\" for installation")
install(DIRECTORY ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR}/
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
PATTERN ".svn" EXCLUDE
)
endif()
# Install shared content located in commonly used folders
set(_shared_content_folders launch rviz urdf meshes maps worlds Media param)
foreach(_folder ${_shared_content_folders})
if(IS_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${_folder})
message(STATUS "Marking SHARED CONTENT FOLDER \"${_folder}\" of package \"${PROJECT_NAME}\" for installation")
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${_folder}/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${_folder}
)
endif()
endforeach()
endmacro()
macro(cs_install_scripts)
install(PROGRAMS ${ARGN} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
endmacro()
macro(cs_export)
cmake_parse_arguments(CS_PROJECT
"" "" "INCLUDE_DIRS;LIBRARIES;CATKIN_DEPENDS;DEPENDS;CFG_EXTRAS"
${ARGN})
set(${PROJECT_NAME}_CATKIN_RUN_DEPENDS)
foreach(dep ${${PROJECT_NAME}_RUN_DEPENDS})
find_package(${dep} QUIET)
if(${dep}_FOUND_CATKIN_PROJECT)
list(APPEND ${PROJECT_NAME}_CATKIN_RUN_DEPENDS ${dep})
endif()
endforeach()
catkin_package(
INCLUDE_DIRS ${${PROJECT_NAME}_LOCAL_INCLUDE_DIR} ${CS_PROJECT_INCLUDE_DIRS}
LIBRARIES ${${PROJECT_NAME}_LIBRARIES} ${CS_PROJECT_LIBRARIES}
CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_RUN_DEPENDS} ${CS_PROJECT_CATKIN_DEPENDS}
DEPENDS ${CS_PROJECT_DEPENDS}
CFG_EXTRAS ${CS_PROJECT_CFG_EXTRAS}
)
endmacro()

17
trajectoryOpt/src/utils/catkin_simple/package.xml Normal file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<package>
<name>catkin_simple</name>
<version>0.1.1</version>
<description>catkin, simpler</description>
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<author email="william@osrfoundation.org">William Woodall</author>
<author email="dthomas@osrfoundation.org">Dirk Thomas</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>catkin</run_depend>
</package>

1
trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/bar/include/bar/hello.h Normal file
View File

@@ -0,0 +1 @@
void hello();

2
trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/bar/msg/HeaderString.msg Normal file
View File

@@ -0,0 +1,2 @@
Header header
string data

18
trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/bar/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<package>
<name>bar</name>
<version>0.1.0</version>
<description>The bar package</description>
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin_simple</buildtool_depend>
<run_depend>boost</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>std_msgs</run_depend>
<build_depend>boost-dev</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>std_msgs</build_depend>
</package>

16
trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/bar/src/hello.cpp Normal file
View File

@@ -0,0 +1,16 @@
#include <bar/hello.h>
#include <bar/HeaderString.h>
#include <iostream>
#include <boost/thread.hpp>
inline void print_hello() {
std::cout << "Hello ";
}
void hello() {
boost::thread t(print_hello);
t.join();
}

12
trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/baz/include/baz/world.h Normal file
View File

@@ -0,0 +1,12 @@
#include <iostream>
#include <boost/thread.hpp>
void print_world() {
std::cout << "world!" << std::endl;
}
void world() {
boost::thread t(print_world);
t.join();
}

13
trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/baz/package.xml Normal file
View File

@@ -0,0 +1,13 @@
<?xml version="1.0"?>
<package>
<name>baz</name>
<version>0.1.0</version>
<description>The baz package</description>
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>boost</run_depend>
<build_depend>boost-dev</build_depend>
</package>

18
trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/foo/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<package>
<name>foo</name>
<version>0.1.0</version>
<description>The foo package</description>
<maintainer email="william@osrfoundation.org">William Woodall</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin_simple</buildtool_depend>
<run_depend>bar</run_depend>
<build_depend>bar</build_depend>
<run_depend>baz</run_depend>
<build_depend>baz</build_depend>
<run_depend>boost</run_depend>
<build_depend>boost-dev</build_depend>
</package>

Some files were not shown because too many files have changed in this diff Show More