add backend

trajectoryOpt/src/.vscode/settings.json

@@ -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"
+    }
+}

trajectoryOpt/src/CMakeLists.txt

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

trajectoryOpt/src/path_search/CMakeLists.txt

@@ -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}
+
+
+)

trajectoryOpt/src/path_search/include/path_searching/kino_astar.h

@@ -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

trajectoryOpt/src/path_search/include/path_searching/kino_model.hpp

@@ -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

trajectoryOpt/src/path_search/package.xml

@@ -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>
+

trajectoryOpt/src/plan_manage/CMakeLists.txt

@@ -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}
+)

trajectoryOpt/src/plan_manage/config/planning copy.yaml

@@ -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

trajectoryOpt/src/plan_manage/config/planning.yaml

@@ -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

trajectoryOpt/src/plan_manage/config/planning_v1.yaml

@@ -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

trajectoryOpt/src/plan_manage/config/planning_v2.yaml

@@ -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

trajectoryOpt/src/plan_manage/config/planning_v3.yaml

@@ -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

trajectoryOpt/src/plan_manage/include/arcPlan/geoutils2d.hpp

@@ -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

trajectoryOpt/src/plan_manage/include/arcPlan/lbfgs.hpp

@@ -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

trajectoryOpt/src/plan_manage/include/arcPlan/sdlp.hpp

@@ -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

trajectoryOpt/src/plan_manage/include/difPlan/geoutils2d.hpp

@@ -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

trajectoryOpt/src/plan_manage/include/difPlan/lbfgs.hpp

@@ -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

trajectoryOpt/src/plan_manage/include/difPlan/poly_traj_utils.hpp

@@ -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

trajectoryOpt/src/plan_manage/include/difPlan/sdlp.hpp

@@ -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

trajectoryOpt/src/plan_manage/include/plan_manage/plan_manage.h

@@ -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

trajectoryOpt/src/plan_manage/launch/plan_node.launch

@@ -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>

trajectoryOpt/src/plan_manage/package.xml

@@ -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>
+

trajectoryOpt/src/plan_manage/rviz/default.rviz

@@ -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

trajectoryOpt/src/plan_manage/src/plan_manage.cpp

@@ -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);
+        // }
+    }
+    }
+}
+
+  

trajectoryOpt/src/plan_manage/src/plan_node.cpp

@@ -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;
+}

trajectoryOpt/src/simulator/kinematics_simulator/CMakeLists.txt

@@ -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})

trajectoryOpt/src/simulator/kinematics_simulator/launch/run_simulator.launch

@@ -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>

trajectoryOpt/src/simulator/kinematics_simulator/msg/ControlCmd.msg

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

trajectoryOpt/src/simulator/kinematics_simulator/package.xml

@@ -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>

trajectoryOpt/src/simulator/kinematics_simulator/params/simulator.yaml

@@ -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

trajectoryOpt/src/simulator/kinematics_simulator/src/kinematics_sim.cpp

@@ -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;
+}

trajectoryOpt/src/simulator/random_map_generator/CMakeLists.txt

@@ -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}
+)

trajectoryOpt/src/simulator/random_map_generator/launch/test.launch

@@ -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>

trajectoryOpt/src/simulator/random_map_generator/launch/vis.launch

@@ -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>

trajectoryOpt/src/simulator/random_map_generator/package.xml

@@ -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>

trajectoryOpt/src/simulator/random_map_generator/params/map.yaml

@@ -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

trajectoryOpt/src/simulator/random_map_generator/rviz/vis.rviz

@@ -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

trajectoryOpt/src/utils/DecompROS/LICENSE

@@ -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.

trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/CMakeLists.txt

@@ -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()

trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/msg/Ellipsoid.msg

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

trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/msg/EllipsoidArray.msg

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

trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/msg/Polyhedron.msg

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

trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/msg/PolyhedronArray.msg

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

trajectoryOpt/src/utils/DecompROS/decomp_ros_msgs/package.xml

@@ -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>

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/CMakeLists.txt

@@ -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()
+

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_basis/data_type.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_basis/data_utils.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_geometry/ellipsoid.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_geometry/geometric_utils.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_geometry/polyhedron.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_ros_utils/data_ros_utils.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/decomp_base.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/ellipsoid_decomp.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/iterative_decomp.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/line_segment.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/include/decomp_util/seed_decomp.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/package.xml

@@ -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>

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/plugin_description.xml

@@ -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>

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/bound_visual.cpp

@@ -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);
+  }
+}

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/bound_visual.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/ellipsoid_array_display.cpp

@@ -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)

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/ellipsoid_array_display.h

@@ -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_;
+};
+}

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/ellipsoid_array_visual.cpp

@@ -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);
+  }
+}

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/ellipsoid_array_visual.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/mesh_visual.cpp

@@ -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);
+  }
+}

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/mesh_visual.h

@@ -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

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/polyhedron_array_display.cpp

@@ -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)

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/polyhedron_array_display.h

@@ -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_;
+};
+
+}

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/vector_visual.cpp

@@ -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);
+    }
+  }
+}

trajectoryOpt/src/utils/DecompROS/decomp_ros_utils/src/vector_visual.h

@@ -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

trajectoryOpt/src/utils/catkin_simple/CMakeLists.txt

@@ -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
+)

trajectoryOpt/src/utils/catkin_simple/cmake/catkin_simple-extras.cmake.em

@@ -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()

trajectoryOpt/src/utils/catkin_simple/package.xml

@@ -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>

trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/bar/include/bar/hello.h

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

trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/bar/msg/HeaderString.msg

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

trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/bar/package.xml

@@ -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>

trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/bar/src/hello.cpp

@@ -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();
+}

trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/baz/include/baz/world.h

@@ -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();
+}

trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/baz/package.xml

@@ -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>

trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/foo/package.xml

@@ -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>

trajectoryOpt/src/utils/catkin_simple/test/scenarios/hello_world/foo/src/main.cpp

@@ -0,0 +1,9 @@
+#include <bar/hello.h>
+#include <baz/world.h>
+
+int main(void) {
+    hello();
+    world();
+
+    return 0;
+}

trajectoryOpt/src/utils/tools/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(tools)
+
+set(CMAKE_VERBOSE_MAKEFILE "true")
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++14 -g")
+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
+    sensor_msgs
+    geometry_msgs
+    cv_bridge
+    decomp_ros_utils
+)
+
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    ${DECOMP_UTIL_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 
+    CATKIN_DEPENDS decomp_ros_utils
+)

trajectoryOpt/src/utils/tools/include/tools/CorridorBuilder2d.hpp

@@ -0,0 +1,330 @@
+#include <math.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/core.hpp>
+#include <random>
+#include <decomp_basis/data_type.h>
+#include <Eigen/Eigen>
+#include "gridmap.hpp"
+
+namespace plan_utils
+{
+
+  void corridorBuilder2d(Eigen::Vector2d origin, float radius, float max_x, float max_y,
+                        vec_Vec2f &data, std::vector<Eigen::Vector2d> &add_data, 
+                        Eigen::MatrixXd &hPoly) {
+
+    //max_x max_y may be the local map?
+    //why add_data?
+    //radius = inf?
+    //origin is the root point of the poly
+    float interior_x = 0.0;
+    float interior_y = 0.0;
+    float safe_radius = radius;
+    std::vector<cv::Point2f> flipData;
+
+    cv::Point2f point;
+    vec_Vec2f new_data;
+
+
+    for (size_t i=0; i<data.size(); i++) {
+        float dx = data[i](0) - origin(0);
+        float dy = data[i](1) - origin(1);
+        float norm2 = std::sqrt(dx*dx + dy*dy);
+        if ( abs(dx) > max_x || abs(dy) > max_y) {
+        continue;
+        }
+        if (norm2 < safe_radius) safe_radius = norm2; //safe radius is the nearest distance between the root and obs
+        if (norm2 == 0) continue;
+        
+        point.x = dx + 2*(radius-norm2)*dx/norm2;
+        point.y = dy + 2*(radius-norm2)*dy/norm2; //radius is an  enough large value
+        new_data.push_back(data[i]);
+        flipData.push_back(point);
+    }
+
+    for (size_t i=0; i<add_data.size(); i++) {
+        float dx = add_data[i](0) - origin(0);
+        float dy = add_data[i](1) - origin(1);
+        float norm2 = std::sqrt(dx*dx + dy*dy);
+        if (norm2 < safe_radius) safe_radius = norm2;
+        if (norm2 == 0) continue;
+        point.x = dx + 2*(radius-norm2)*dx/norm2;
+        point.y = dy + 2*(radius-norm2)*dy/norm2;
+        new_data.push_back(add_data[i]);
+        flipData.push_back(point);
+    }
+
+    std::vector<int> vertexIndice;
+    cv::convexHull(flipData,vertexIndice,false,false);
+    //obtain the poly containing flipData
+    
+    bool isOriginAVertex = false;
+    int OriginIndex = -1;
+    std::vector<cv::Point2f> vertexData;
+    for (size_t i=0; i<vertexIndice.size(); i++) {
+        unsigned int v = vertexIndice[i];
+        if (v == new_data.size()) {
+            isOriginAVertex = true;
+            OriginIndex = i;
+            vertexData.push_back(cv::Point2f(origin(0), origin(1)));
+        }else {
+            vertexData.push_back(cv::Point2f(new_data[v](0), new_data[v](1)));
+        }
+    }
+
+    if (isOriginAVertex) {
+        int last_index = (OriginIndex - 1)%vertexIndice.size();
+        int next_index = (OriginIndex + 1)%vertexIndice.size();
+        float dx = (new_data[vertexIndice[last_index]](0) + origin(0) + new_data[vertexIndice[next_index]](0))/3 - origin(0);
+        float dy = (new_data[vertexIndice[last_index]](1) + origin(1) + new_data[vertexIndice[next_index]](1))/3 - origin(1);
+        float d = std::sqrt(dx*dx + dy*dy);
+        interior_x = 0.99*safe_radius*dx/d + origin(0);
+        interior_y = 0.99*safe_radius*dy/d + origin(1);
+    }else {
+        interior_x = origin(0);
+        interior_y = origin(1);
+    }
+
+    std::vector<int> vIndex2;
+    cv::convexHull(vertexData,vIndex2,false,false); // counterclockwise right-hand
+
+    
+
+    std::vector<Eigen::Vector3f> constraints; // (a,b,c) a x + b y <= c
+    for (size_t j=0; j<vIndex2.size(); j++) {
+        int jplus1 = (j+1)%vIndex2.size();
+        cv::Point2f rayV = vertexData[vIndex2[jplus1]] - vertexData[vIndex2[j]];
+        Eigen::Vector2f normalJ(rayV.y, -rayV.x);  // point to outside
+        normalJ.normalize();
+        int indexJ = vIndex2[j];
+        while (indexJ != vIndex2[jplus1]) {
+            float c = (vertexData[indexJ].x-interior_x) * normalJ(0) + (vertexData[indexJ].y-interior_y) * normalJ(1);
+            constraints.push_back(Eigen::Vector3f(normalJ(0), normalJ(1), c));
+            indexJ = (indexJ+1)%vertexData.size();
+        }
+    }    
+
+    std::vector<cv::Point2f> dualPoints(constraints.size(), cv::Point2f(0,0));
+    for (size_t i=0; i<constraints.size(); i++) {
+        dualPoints[i].x = constraints[i](0)/constraints[i](2);
+        dualPoints[i].y = constraints[i](1)/constraints[i](2);
+    }
+    
+    std::vector<cv::Point2f> dualVertex, finalVertex;
+    cv::convexHull(dualPoints,dualVertex,true,false);
+
+    for (size_t i=0; i<dualVertex.size(); i++) {
+        int iplus1 = (i+1)%dualVertex.size();
+        cv::Point2f rayi = dualVertex[iplus1] - dualVertex[i];
+        float c = rayi.y*dualVertex[i].x - rayi.x*dualVertex[i].y;
+        finalVertex.push_back(cv::Point2f(interior_x+rayi.y/c, interior_y-rayi.x/c));
+    }
+
+    unsigned int size = finalVertex.size();
+    hPoly.resize(4, size);
+    for (unsigned int i = 0; i < size; i++){
+    int iplus1 = (i+1)%size;
+    cv::Point2f rayi = finalVertex[iplus1] - finalVertex[i];           
+    hPoly.col(i).tail<2>()  = Eigen::Vector2d(finalVertex[i].x, finalVertex[i].y);  // the points on the plane
+    hPoly.col(i).head<2>()  = Eigen::Vector2d(-rayi.y, rayi.x); // outside
+    }
+      
+  }
+  Eigen::MatrixXd getDilateRec(Eigen::Vector3d state, map_util::OccMapUtil gridmap, int maxIndexL = 5, int maxIndexW = 8){
+
+    double resolution = gridmap.getRes();
+    double step = resolution * 1.0;
+    double limitBoundHalfL = maxIndexL * resolution;
+    double limitBoundHalfW = maxIndexW * resolution;
+    //generate a rectangle for this state px py yaw
+    //generate a hPoly
+    Eigen::Matrix<int,4,1> NotFinishTable = Eigen::Matrix<int,4,1>(1,1,1,1);      
+    Eigen::Vector2d sourcePt = state.head(2);
+    double yaw = state[2];
+    Eigen::Vector4d expandLength;
+    expandLength << 0.0, 0.0, 0.0, 0.0;
+    Eigen::Matrix2d egoR;
+      egoR << cos(yaw), -sin(yaw),
+              sin(yaw), cos(yaw);
+    //dcr width length
+    while(NotFinishTable.norm()>0){ 
+    //+dx -dy -dx  +dy  
+        for(int i = 0; i<4; i++){
+            if(!NotFinishTable[i]) continue;
+            //get the new source and vp
+            Eigen::Vector4d  tmp_expandLength = expandLength;
+            Eigen::Vector2d cor1, cor2, cor3, cor4, cor5;
+            bool isocc = false;
+            switch (i)
+            {
+            //+dx
+            case 0:
+                tmp_expandLength[0] += step;
+                cor1 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],tmp_expandLength[3]);
+                cor2 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],-tmp_expandLength[1]);
+                cor3 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],-tmp_expandLength[1]);
+                cor4 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],tmp_expandLength[3]);
+                cor5 = cor1;
+                //1 new1 new1 new2 new2 2
+                isocc = gridmap.isBlocked(cor1, cor2);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor2, cor3);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor3, cor4);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor4, cor5);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                if(tmp_expandLength[0] > limitBoundHalfL){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                expandLength = tmp_expandLength;
+                break;
+            //-dy
+            case 1:
+                tmp_expandLength[1] += step;
+                cor1 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],tmp_expandLength[3]);
+                cor2 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],-tmp_expandLength[1]);
+                cor3 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],-tmp_expandLength[1]);
+                cor4 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],tmp_expandLength[3]);
+                cor5 = cor1;
+                //1 new1 new1 new2 new2 2
+                isocc = gridmap.isBlocked(cor1, cor2);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor2, cor3);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor3, cor4);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor4, cor5);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                if(tmp_expandLength[1] > limitBoundHalfW){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                expandLength = tmp_expandLength;
+                break;
+            //-dx
+            case 2:
+                tmp_expandLength[2] += step;
+                cor1 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],tmp_expandLength[3]);
+                cor2 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],-tmp_expandLength[1]);
+                cor3 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],-tmp_expandLength[1]);
+                cor4 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],tmp_expandLength[3]);
+                cor5 = cor1;
+                //1 new1 new1 new2 new2 2
+                isocc = gridmap.isBlocked(cor1, cor2);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor2, cor3);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor3, cor4);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor4, cor5);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                if(tmp_expandLength[2] > limitBoundHalfL){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                expandLength = tmp_expandLength;
+                break;
+            //dy
+            case 3:
+                tmp_expandLength[3] += step;
+                cor1 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],tmp_expandLength[3]);
+                cor2 = sourcePt + egoR * Eigen::Vector2d(tmp_expandLength[0],-tmp_expandLength[1]);
+                cor3 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],-tmp_expandLength[1]);
+                cor4 = sourcePt + egoR * Eigen::Vector2d(-tmp_expandLength[2],tmp_expandLength[3]);
+                cor5 = cor1;
+                //1 new1 new1 new2 new2 2
+                isocc = gridmap.isBlocked(cor1, cor2);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor2, cor3);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor3, cor4);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                isocc = gridmap.isBlocked(cor4, cor5);
+                if(isocc){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                if(tmp_expandLength[3] > limitBoundHalfW){
+                    NotFinishTable[i] = 0.0;
+                    break;
+                }
+                expandLength = tmp_expandLength;
+                break;
+            }   
+        }
+    }
+    Eigen::MatrixXd poly;
+    poly.resize(4,4);
+    std::vector<Eigen::Vector2d> cors;
+    cors.resize(4);
+    cors[0] = sourcePt + egoR * Eigen::Vector2d(expandLength[0],expandLength[3]);
+    cors[1] = sourcePt + egoR * Eigen::Vector2d(expandLength[0],-expandLength[1]);
+    cors[2] = sourcePt + egoR * Eigen::Vector2d(-expandLength[2],-expandLength[1]);
+    cors[3] = sourcePt + egoR * Eigen::Vector2d(-expandLength[2],expandLength[3]);
+    poly.col(0) << cors[0], cos(yaw), sin(yaw);
+    poly.col(1) << cors[1], sin(yaw), -cos(yaw);
+    poly.col(2) << cors[2], -cos(yaw), -sin(yaw);
+    poly.col(3) << cors[3],-sin(yaw), cos(yaw);
+
+    return poly;
+
+}
+bool ifInside(Eigen::Vector2d pt, Eigen::MatrixXd poly){
+    for(int i = 0; i < poly.cols(); i++){
+        Eigen::Vector2d p = poly.col(i).head(2);
+        Eigen::Vector2d n = poly.col(i).tail(2);
+        if((pt-p).dot(n)>0){
+            return false;
+        }
+    }
+    return true;
+
+}
+} // namespace plan_utils

trajectoryOpt/src/utils/tools/include/tools/config.hpp

@@ -0,0 +1,144 @@
+#ifndef CONFIG.HPP
+#define CONFIG.HPP
+#include <ros/ros.h>
+#include <iostream>
+#include <cmath>
+#include <cfloat>
+#include <vector>
+#include <Eigen/Eigen>
+#include <XmlRpcValue.h>
+struct Config
+{
+    double mini_T = 0.0;
+    double wei_time_ = 500.0;
+    double kmax = 0.45;
+    double vmax = 3.0;
+    double latAccmax = 3.0;
+    double lonAccmax = 3.0;
+    double accRatemax = 8.0;
+    double kdotmax = 5.0;
+    double yawdotmax = 3.14;
+    int traj_res = 16;
+    double scaling_wf_min_ = 0.01, scaling_wc_min_ = 0.01;
+    double rho = 1.0, rho_max =1000.0;
+    double gamma = 1;
+    double cons_eps_ = 0.20;
+    int mem_size = 256;
+    int past = 3; //3 
+    double g_epsilon = 1.0e-3;
+    double min_step = 1.0e-32;
+    double delta = 1.0e-4;
+    int max_iterations = 12000;
+    int amlMaxIter = 15;
+    double pieceTime = 0.7;
+    XmlRpc::XmlRpcValue xmlconpts;
+    std::vector<Eigen::Vector2d> conpts; 
+    /*map parameters*/
+    double mapRes = 0.1;
+    double mapX = 50.0, mapY = 50.0, mapZ = 10.0;
+    int expandSize = 1;
+    /*front end*/
+    double wheel_base = 0.6;
+    double horizon_ = 50.0;
+    double yaw_resolution_ = 0.3;
+    double lambda_heu_ = 5.0;
+    int allocate_num_ = 100000;
+    int check_num_ = 5;
+    double max_seach_time = 1.0;
+    double step_arc = 0.9;
+    double checkl = 0.2;
+    double non_siguav = 0.0;
+    double backW = 2.0;
+    double penaWei = 500.0;
+    double esdfWei = 1000.0;
+    double miniS =0.9;
+    bool isdebug = true;
+    bool isfixGear = false;
+    bool isVis = false;
+    bool enable_shot = false;
+    double safeMargin = 0.1;
+    bool useDP = true;
+    double phimax = 0.785;
+    double omegamax = 0.3;
+    double steer_res = 0.5;
+    std::string model_path = "/home/han/2023codes/NeuralTraj/model/big_cock.pt";
+
+
+
+    Config(const ros::NodeHandle &nh_priv)
+    {
+        nh_priv.param("mini_T", mini_T, 0.0);
+        nh_priv.param("wei_time_", wei_time_, 500.0);
+        nh_priv.param("kmax", kmax, 0.45);
+        nh_priv.param("vmax", vmax, 3.0);
+        nh_priv.param("latAccmax", latAccmax, 3.0);
+        nh_priv.param("lonAccmax", lonAccmax, 3.0);
+        nh_priv.param("accRatemax", accRatemax, 8.0);
+        nh_priv.param("kdotmax", kdotmax, 1.0);
+        nh_priv.param("yawdotmax", yawdotmax, 3.14);
+        nh_priv.param("traj_res", traj_res, 16);
+        nh_priv.param("rho", rho, 1.0);
+        nh_priv.param("rho_max", rho_max, 1000.0);
+        nh_priv.param("gamma", gamma, 1.0);
+        nh_priv.param("cons_eps_", cons_eps_, 0.2);
+        nh_priv.param("mem_size", mem_size, 256);
+        nh_priv.param("past", past, 3);
+        nh_priv.param("g_epsilon", g_epsilon, 1.0e-3);
+        nh_priv.param("min_step", min_step, 1.0e-32);
+        nh_priv.param("delta", delta, 1.0e-4);
+        nh_priv.param("max_iterations", max_iterations, 10000);
+        nh_priv.param("amlMaxIter", amlMaxIter, 15);
+        nh_priv.param("pieceTime", pieceTime, 0.7);
+        nh_priv.getParam("conpts", xmlconpts);
+        nh_priv.getParam("penaWei", penaWei);
+        nh_priv.getParam("esdfWei", esdfWei);
+        nh_priv.getParam("miniS", miniS);
+        nh_priv.getParam("safeMargin", safeMargin);
+        nh_priv.getParam("phimax", phimax);
+        nh_priv.getParam("omegamax", omegamax);
+        nh_priv.getParam("useDP", useDP);
+        nh_priv.getParam("steer_res", steer_res);
+        for(int i = 0; i < xmlconpts.size(); ++i)
+        {
+            Eigen::Vector2d pt;
+        for(int j = 0; j < 2; ++j)
+        {
+            pt[j] = xmlconpts[i][j];
+        }
+        conpts.push_back(pt);
+        }
+        nh_priv.param("mapRes", mapRes, 0.1);
+        nh_priv.param("mapX", mapX, 50.0);
+        nh_priv.param("mapY", mapY, 50.0);
+        nh_priv.param("expandSize", expandSize, 2);
+        nh_priv.param("wheel_base", wheel_base, 0.6);
+        nh_priv.param("horizon_", horizon_, 50.0);
+        nh_priv.param("yaw_resolution_", yaw_resolution_, 0.3);
+        nh_priv.param("lambda_heu_", lambda_heu_, 5.0);
+        nh_priv.param("allocate_num_", allocate_num_, 100000);
+        nh_priv.param("check_num_", check_num_, 5);
+        nh_priv.param("max_seach_time", max_seach_time, 1.0);
+        nh_priv.param("step_arc", step_arc, 0.9);
+        nh_priv.param("checkl", checkl, 0.2);
+        nh_priv.param("non_siguav", non_siguav, 0.0);
+        nh_priv.param("isdebug", isdebug, false);
+        nh_priv.param("isfixGear", isfixGear, false);
+        nh_priv.param("isVis", isVis, false);
+        nh_priv.param("enable_shot", enable_shot, false);
+        nh_priv.param("backW", backW, 2.0);
+
+    }
+};
+double normalize_angle(const double& theta) {
+    double theta_tmp = theta;    
+    while(theta_tmp >= acos(-1.0)){
+        theta_tmp -= 2 * acos(-1.0);
+    }
+    while(theta_tmp < -acos(-1.0)){
+        theta_tmp += 2 * acos(-1.0);
+    } 
+  return theta_tmp;
+}
+typedef std::shared_ptr<Config> ConfigPtr;
+
+#endif 

trajectoryOpt/src/utils/tools/include/tools/gridmap.hpp

@@ -0,0 +1,792 @@
+#ifndef GRIDMAP_H
+#define GRIDMAP_H
+
+
+#include <iostream>
+#include <ros/ros.h>
+#include <ros/console.h>
+#include <map>
+#include <tools/config.hpp>
+#include <decomp_basis/data_type.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+namespace map_util{
+    using Tmap = std::vector<signed char>;
+    template <int Dim> class MapUtil {
+    public:
+      ///Simple constructor
+      MapUtil() {}
+      ///Get map data
+      Tmap getMap() { return map_; }
+      //
+      bool has_map_(){return has_map;}
+      ///Get resolution
+      double getRes() { return res_; }
+      ///Get dimensions
+      Veci<Dim> getDim() { return dim_; }
+      ///Get origin
+      Vecf<Dim> getOrigin() { return origin_d_; }
+      Vecf<Dim> getMapSize() { return map_size; }
+      void setParam(ConfigPtr config_, ros::NodeHandle& nh){
+        res_ = config_->mapRes;
+        int buffer_size;
+        expand_size = config_->expandSize;
+        priv_config_ = config_;
+        if(Dim == 3){
+            map_size(0) = config_->mapX;
+            map_size(1) = config_->mapY;
+            map_size(2) = config_->mapZ;
+            origin_d_[0] = -map_size(0)/2;
+            origin_d_[1] = -map_size(1)/2;
+            origin_d_[2] = 0;
+            dim_(0) = map_size(0)/res_;
+            dim_(1) = map_size(1)/res_;
+            dim_(2) = map_size(2)/res_;
+            buffer_size = dim_(0)*dim_(1)*dim_(2);
+        }
+        else if(Dim==2){
+            map_size(0) = config_->mapX + 1.0e-4;
+            map_size(1) = config_->mapY + 1.0e-4;
+            origin_d_[0] = -map_size(0)/2;
+            origin_d_[1] = -map_size(1)/2;
+            dim_(0) = map_size(0)/res_;
+            dim_(1) = map_size(1)/res_;
+            buffer_size = dim_(0)*dim_(1);
+
+            /*ESDF*/
+            distance_buffer_ = std::vector<double>(buffer_size, 10000);
+            distance_buffer_neg_ = std::vector<double>(buffer_size, 10000);
+            distance_buffer_all_ = std::vector<double>(buffer_size, 10000);
+            tmp_buffer1_ = std::vector<double>(buffer_size, 0);
+            occupancy_buffer_neg = std::vector<char>(buffer_size, 0);
+            esdf_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/global_esdf", 10);
+            pcl_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/global_pcl", 10);
+
+        }
+        else{
+            ROS_ERROR("grid map dimensions must be 2 or 3!");
+        }
+        map_.resize(buffer_size,val_free);
+        point_cloud_sub_ = nh.subscribe("/global_map", 10, &MapUtil::MapBuild, this);
+      }
+      void setEnv(const sensor_msgs::PointCloud2 & pointcloud_map){
+        //only used in 2D environment
+        int buffer_size;
+        buffer_size = dim_(0)*dim_(1);
+        distance_buffer_ = std::vector<double>(buffer_size, 10000);
+        distance_buffer_neg_ = std::vector<double>(buffer_size, 10000);
+        distance_buffer_all_ = std::vector<double>(buffer_size, 10000);
+        tmp_buffer1_ = std::vector<double>(buffer_size, 0);
+        occupancy_buffer_neg = std::vector<char>(buffer_size, 0);
+        std::fill(map_.begin(), map_.end(), val_free);
+    
+
+
+        pcl::PointCloud<pcl::PointXYZ> cloud;
+        pcl::fromROSMsg(pointcloud_map, cloud);
+        if( (int)cloud.points.size() == 0 ) return;
+        pcl::PointXYZ pt;
+        for (int idx = 0; idx < (int)cloud.points.size(); idx++)
+        {
+          pt = cloud.points[idx];
+          setObs(pt);
+        }
+        has_map = true;
+        // std::cout << "!map build\n";
+        updateESDF2d();
+        publishESDF();
+        publishPCL();
+      }
+      void MapBuild(const sensor_msgs::PointCloud2 & pointcloud_map){
+        if(has_map) {
+          publishESDF();
+          return;}
+        pcl::PointCloud<pcl::PointXYZ> cloud;
+        pcl::fromROSMsg(pointcloud_map, cloud);
+        if( (int)cloud.points.size() == 0 ) return;
+        pcl::PointXYZ pt;
+        // cloud.points.clear();//data generaion
+
+        for (int idx = 0; idx < (int)cloud.points.size(); idx++)
+        {
+          pt = cloud.points[idx];
+          setObs(pt);
+        }
+        has_map = true;
+        updateESDF2d();
+        publishESDF();
+      }
+      void setObs(pcl::PointXYZ pt){
+        if(Dim==3){
+            double coord_x = pt.x;
+            double coord_y = pt.y;
+            double coord_z = pt.z;
+            Vec3f pt = Vec3f(coord_x,coord_y,coord_z);
+            Veci<Dim> index3i;
+            for(int i = 0; i < Dim; i++)
+              index3i(i) = std::round((pt(i) - origin_d_(i)) / res_ - 0.5);
+            if(isOutside(index3i))
+                return;
+            for (int i=-expand_size;i<=expand_size;i++)
+            for (int j=-expand_size;j<=expand_size;j++)
+            for (int k=-expand_size;k<=expand_size;k++)
+                {
+                    Veci<Dim> temp_index;
+                    temp_index(0) = index3i(0)+i;
+                    temp_index(1) = index3i(1)+j;
+                    temp_index(2) = index3i(2)+k;
+                    if(isOutside(temp_index)) continue;
+                    map_[getIndex(temp_index)] = val_occ;
+                }
+        }
+        else{
+            double coord_x = pt.x;
+            double coord_y = pt.y;
+            Vec2f pt = Vec2f(coord_x,coord_y);
+            Veci<Dim> index2i;
+            for(int i = 0; i < Dim; i++)
+              index2i(i) = std::round((pt(i) - origin_d_(i)) / res_ - 0.5);
+            if(isOutside(index2i))
+                return;
+            for (int i=-expand_size;i<=expand_size;i++)
+            for (int j=-expand_size;j<=expand_size;j++)
+                {
+                    Veci<Dim> temp_index;
+                    temp_index(0) = index2i(0)+i;
+                    temp_index(1) = index2i(1)+j;
+                    if(isOutside(temp_index)) continue;
+                    map_[getIndex(temp_index)] = val_occ;
+                }
+
+        }
+        return;
+
+      }
+      ///Get index of a cell
+      int getIndex(const Veci<Dim>& pn) {
+          return Dim == 2 ? pn(0) + dim_(0) * pn(1) :
+                            pn(0) + dim_(0) * pn(1) + dim_(0) * dim_(1) * pn(2);
+      }
+      ///Check if the given cell is outside of the map in i-the dimension
+      bool isOutsideXYZ(const Veci<Dim> &n, int i) { return n(i) < 0 || n(i) >= dim_(i); }
+      ///Check if the cell is free by index
+      bool isFree(int idx) { return map_[idx] == val_free; }
+      ///Check if the cell is unknown by index
+      bool isUnknown(int idx) { return map_[idx] == val_unknown; }
+      ///Check if the cell is occupied by index
+      bool isOccupied(int idx) { return map_[idx] > val_free; }
+      //free 0 occ 100 unknow -1
+      ///Check if the cell is outside by coordinate
+      bool isOutside(const Veci<Dim> &pn) {
+        for(int i = 0; i < Dim; i++)
+          if (pn(i) < 0 || pn(i) >= dim_(i))
+            return true;
+        return false;
+      }
+      ///Check if the given cell is free by coordinate
+      bool isFree(const Veci<Dim> &pn) {
+        if (isOutside(pn))
+          return false;
+        else
+          return isFree(getIndex(pn));
+      }
+      ///Check if the given cell is occupied by coordinate
+      bool isOccupied(const Veci<Dim> &pn) {
+        if (isOutside(pn))
+          return true;
+        else
+          return isOccupied(getIndex(pn));
+      }
+      bool isOccupied(const Vecf<Dim> &pt) {
+        Veci<Dim> pn = floatToInt(pt);
+        if (isOutside(pn))
+          return true;
+        else
+          return isOccupied(getIndex(pn));
+      }
+      
+      
+      ///Check if the given cell is unknown by coordinate
+      bool isUnknown(const Veci<Dim> &pn) {
+        if (isOutside(pn))
+          return false;
+        return map_[getIndex(pn)] == val_unknown;
+      }
+
+      /**
+       * @brief Set map
+       *
+       * @param ori origin position
+       * @param dim number of cells in each dimension
+       * @param map array of cell values
+       * @param res map resolution
+       */
+      void setMap(const Vecf<Dim>& ori, const Veci<Dim>& dim, const Tmap &map, double res) {
+        map_ = map;
+        dim_ = dim;
+        origin_d_ = ori;
+        res_ = res;
+      }
+
+      ///Print basic information about the util
+      void info() {
+        Vecf<Dim> range = dim_.template cast<double>() * res_;
+        std::cout << "MapUtil Info ========================== " << std::endl;
+        std::cout << "   res: [" << res_ << "]" << std::endl;
+        std::cout << "   origin: [" << origin_d_.transpose() << "]" << std::endl;
+        std::cout << "   range: [" << range.transpose() << "]" << std::endl;
+        std::cout << "   dim: [" << dim_.transpose() << "]" << std::endl;
+      };
+
+      ///Float position to discrete cell coordinate
+      Veci<Dim> floatToInt(const Vecf<Dim> &pt) {
+        Veci<Dim> pn;
+        for(int i = 0; i < Dim; i++)
+          pn(i) = floor((pt(i) - origin_d_(i)) / res_);
+        return pn;
+      }
+      ///Discrete cell coordinate to float position
+      Vecf<Dim> intToFloat(const Veci<Dim> &pn) {
+        //return pn.template cast<double>() * res_ + origin_d_;
+        return (pn.template cast<double>() + Vecf<Dim>::Constant(0.5)) * res_ + origin_d_;
+      }
+
+      ///Raytrace from float point pt1 to pt2
+      vec_Veci<Dim> rayTrace(const Vecf<Dim> &pt1, const Vecf<Dim> &pt2) {
+        Vecf<Dim> diff = pt2 - pt1;
+        double k = 0.8;
+        int max_diff = (diff / res_).template lpNorm<Eigen::Infinity>() / k;
+        double s = 1.0 / max_diff;
+        Vecf<Dim> step = diff * s;
+
+        vec_Veci<Dim> pns;
+        Veci<Dim> prev_pn = Veci<Dim>::Constant(-1);
+        for (int n = 1; n < max_diff; n++) {
+          Vecf<Dim> pt = pt1 + step * n;
+          Veci<Dim> new_pn = floatToInt(pt);
+          if (isOutside(new_pn))
+            break;
+          if (new_pn != prev_pn)
+            pns.push_back(new_pn);
+          prev_pn = new_pn;
+        }
+        return pns;
+      }
+
+      ///Check if the ray from p1 to p2 is occluded
+      bool isBlocked(const Vecf<Dim>& p1, const Vecf<Dim>& p2, int8_t val = 100) {
+        vec_Veci<Dim> pns = rayTrace(p1, p2);
+        for (const auto &pn : pns) {
+          if (map_[getIndex(pn)] >= val)
+            return true;
+        }
+        return false;
+      }
+
+      ///Get occupied voxels
+      vec_Vecf<Dim> getCloud() {
+        vec_Vecf<Dim> cloud;
+        Veci<Dim> n;
+        if(Dim == 3) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
+                if (isOccupied(getIndex(n)))
+                  cloud.push_back(intToFloat(n));
+              }
+            }
+          }
+        }
+        else if (Dim == 2) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              if (isOccupied(getIndex(n)))
+                cloud.push_back(intToFloat(n));
+            }
+          }
+        }
+
+        return cloud;
+      }
+      void updateESDF2d() {
+        if(Dim != 2){
+          ROS_ERROR("Dimension mismatch!");
+        }
+        Eigen::Vector2i min_esdf = Eigen::Vector2i(0,0);
+        Eigen::Vector2i max_esdf = Eigen::Vector2i(dim_(0) - 1, dim_(1) - 1);
+
+        /* ========== compute positive DT ========== */
+
+        for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
+          fillESDF(
+              [&](int y) {
+                return isOccupied(Vec2i(x, y))?
+                    0 :
+                    std::numeric_limits<double>::max();
+              },
+              [&](int y, double val) {tmp_buffer1_[getIndex(Vec2i(x, y))] = val; }, min_esdf[1],
+              max_esdf[1], 1);
+        }
+
+        for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
+          fillESDF([&](int x) { return tmp_buffer1_[getIndex(Vec2i(x, y))]; },
+                    [&](int x, double val) {
+                      distance_buffer_[getIndex(Vec2i(x, y))] = res_ * std::sqrt(val);
+                      //  min(mp_.resolution_ * std::sqrt(val),
+                      //      md_.distance_buffer_[toAddress(x, y, z)]);
+                    },
+                    min_esdf[0], max_esdf[0], 0);
+        }
+
+        /* ========== compute negative distance ========== */
+        for (int x = min_esdf(0); x <= max_esdf(0); ++x)
+          for (int y = min_esdf(1); y <= max_esdf(1); ++y)
+          {
+            int idx = getIndex(Vec2i(x, y));
+            if (isFree(idx)) {
+              occupancy_buffer_neg[idx] = 1;
+
+            } else if (isOccupied(idx)) {
+              occupancy_buffer_neg[idx] = 0;
+            } else {
+              ROS_ERROR("what?");
+            }
+          }
+
+        ros::Time t1, t2;
+
+        for (int x = min_esdf[0]; x <= max_esdf[0]; x++) {
+          fillESDF(
+              [&](int y) {
+                return occupancy_buffer_neg[getIndex(Vec2i(x, y))] == 1 ?
+                    0 :
+                    std::numeric_limits<double>::max();
+              },
+              [&](int y, double val) { tmp_buffer1_[getIndex(Vec2i(x, y))] = val; }, min_esdf[1],
+              max_esdf[1], 1);
+        }
+
+        for (int y = min_esdf[1]; y <= max_esdf[1]; y++) {
+          fillESDF([&](int x) { return tmp_buffer1_[getIndex(Vec2i(x, y))]; },
+                    [&](int x, double val) {
+                      distance_buffer_neg_[getIndex(Vec2i(x, y))] = res_ * std::sqrt(val);
+                    },
+                    min_esdf[0], max_esdf[0], 0);
+        }
+
+        /* ========== combine pos and neg DT ========== */
+        for (int x = min_esdf(0); x <= max_esdf(0); ++x)
+          for (int y = min_esdf(1); y <= max_esdf(1); ++y)
+          {
+            int idx = getIndex(Vec2i(x, y));
+            distance_buffer_all_[idx] = distance_buffer_[idx];
+
+            if (distance_buffer_neg_[idx] > 0.0){
+              distance_buffer_all_[idx] += (-distance_buffer_neg_[idx] + res_);
+              }
+          }
+
+      }
+      template <typename F_get_val, typename F_set_val>
+      void fillESDF(F_get_val f_get_val, F_set_val f_set_val, int start, int end, int d) {
+        int v[dim_(d)];
+        double z[dim_(d) + 1];
+
+        int k = start;
+        v[start] = start;
+        z[start] = -std::numeric_limits<double>::max();
+        z[start + 1] = std::numeric_limits<double>::max();
+
+        for (int q = start + 1; q <= end; q++) {
+          k++;
+          double s;
+
+          do {
+            k--;
+            s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) / (2 * q - 2 * v[k]);
+          } while (s <= z[k]);
+
+          k++;
+
+          v[k] = q;
+          z[k] = s;
+          z[k + 1] = std::numeric_limits<double>::max();
+        }
+
+        k = start;
+
+        for (int q = start; q <= end; q++) {
+          while (z[k + 1] < q) k++;
+          double val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
+          f_set_val(q, val);
+        }
+      }
+      inline double getDistance(const Eigen::Vector2d& pos) {
+        Eigen::Vector2i id;
+        id = floatToInt(pos);
+        boundIndex(id);
+        return distance_buffer_all_[getIndex(id)];
+      }
+      inline double getDistance(const Eigen::Vector2i& id) {
+      Eigen::Vector2i id1 = id;
+      boundIndex(id1);
+      return distance_buffer_all_[getIndex(id1)];
+      }
+      inline void boundIndex(Eigen::Vector2i& id) {
+        Eigen::Vector2i id1;
+        id1(0) = std::max(std::min(id(0), dim_(0) - 1), 0);
+        id1(1) = std::max(std::min(id(1), dim_(1) - 1), 0);
+        id = id1;
+    }
+    void publishPCL(){
+      vec_Vecf<Dim> pcs = getCloud();
+      pcl::PointCloud<pcl::PointXYZI> cloud;
+      pcl::PointXYZI pt;
+      for(const auto& pc : pcs){
+        pt.x = pc[0];
+        pt.y = pc[1];
+        pt.z = 0.0;
+        cloud.push_back(pt);
+      }
+      cloud.width = cloud.points.size();
+      cloud.height = 1;
+      cloud.is_dense = true;
+      cloud.header.frame_id = "world";
+      sensor_msgs::PointCloud2 cloud_msg;
+      pcl::toROSMsg(cloud, cloud_msg);
+      pcl_pub_.publish(cloud_msg);
+
+    }
+    void publishESDF() {
+      double dist;
+      pcl::PointCloud<pcl::PointXYZI> cloud;
+      pcl::PointXYZI pt;
+      const double min_dist = -4.0;
+      const double max_dist = 4.0;
+      Eigen::Vector2i min_cut = Eigen::Vector2i(0,0);
+      Eigen::Vector2i max_cut = Eigen::Vector2i(dim_(0)-1,dim_(1)-1);
+
+      for (int x = min_cut(0); x <= max_cut(0); ++x)
+        for (int y = min_cut(1); y <= max_cut(1); ++y) {
+          Vec2f pos;
+          pos = intToFloat(Vec2i(x, y));
+          dist = getDistance(pos);
+          // if(dist<min_dist) dist = min_dist;
+          // if(dist>max_dist) dist = max_dist;
+          pt.x = pos(0);
+          pt.y = pos(1);
+          pt.z = dist;
+          pt.intensity = (dist - min_dist) / (max_dist - min_dist);
+          cloud.push_back(pt);
+          // }
+        }
+      cloud.width = cloud.points.size();
+      cloud.height = 1;
+      cloud.is_dense = true;
+      cloud.header.frame_id = "world";
+      sensor_msgs::PointCloud2 cloud_msg;
+      pcl::toROSMsg(cloud, cloud_msg);
+      esdf_pub_.publish(cloud_msg);
+    }
+      inline double getDistGrad(Eigen::Vector2d po, Eigen::Vector2d& grad) {
+        Eigen::Vector2d pos(po(0),po(1));
+        if (isOutside(floatToInt(pos))) {
+          grad.setZero();
+          return 0;
+        }
+
+        /* use bilinear interpolation */
+        Eigen::Vector2d pos_m = pos - 0.5 * res_ * Eigen::Vector2d::Ones();
+
+        Eigen::Vector2i idx;
+        idx = floatToInt(pos_m);
+        Eigen::Vector2d idx_pos, diff;
+        idx_pos = intToFloat(idx);
+
+        diff = (pos - idx_pos) / res_;
+
+        double values[2][2];
+        for (int x = 0; x < 2; x++) {
+          for (int y = 0; y < 2; y++) {
+              Eigen::Vector2i current_idx = idx + Eigen::Vector2i(x, y);
+              values[x][y] = getDistance(current_idx);
+          }   
+        }
+        double v00 = (1-diff(0)) * values[0][0] + diff(0) * values[1][0];
+        double v10 = (1-diff(0)) * values[0][1] + diff(0) * values[1][1];
+        double v0  = (1-diff(1)) * v00 + diff(1) * v10;
+        // double dist = v0;
+        grad[1] = (v10 - v00) / res_;
+        grad[0] = ((1-diff(1)) * (values[1][0]-values[0][0]) + diff(1) * (values[1][1]-values[0][1])) / res_;
+
+        return v0;
+      }
+      
+      ///Get free voxels
+      vec_Vecf<Dim> getFreeCloud() {
+        vec_Vecf<Dim> cloud;
+        Veci<Dim> n;
+        if(Dim == 3) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
+                if (isFree(getIndex(n)))
+                  cloud.push_back(intToFloat(n));
+              }
+            }
+          }
+        }
+        else if (Dim == 2) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              if (isFree(getIndex(n)))
+                cloud.push_back(intToFloat(n));
+            }
+          }
+        }
+
+        return cloud;
+      }
+
+      ///Get unknown voxels
+      vec_Vecf<Dim> getUnknownCloud() {
+        vec_Vecf<Dim> cloud;
+        Veci<Dim> n;
+        if(Dim == 3) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
+                if (isUnknown(getIndex(n)))
+                  cloud.push_back(intToFloat(n));
+              }
+            }
+          }
+        }
+        else if (Dim == 2) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              if (isUnknown(getIndex(n)))
+                cloud.push_back(intToFloat(n));
+            }
+          }
+        }
+
+        return cloud;
+      }
+
+      ///Dilate occupied cells
+      void dilate(const vec_Veci<Dim>& dilate_neighbor) {
+        Tmap map = map_;
+        Veci<Dim> n = Veci<Dim>::Zero();
+        if(Dim == 3) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
+                if (isOccupied(getIndex(n))) {
+                  for (const auto &it : dilate_neighbor) {
+                    if (!isOutside(n + it))
+                      map[getIndex(n + it)] = val_occ;
+                  }
+                }
+              }
+            }
+          }
+        }
+        else if(Dim == 2) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              if (isOccupied(getIndex(n))) {
+                for (const auto &it : dilate_neighbor) {
+                  if (!isOutside(n + it))
+                    map[getIndex(n + it)] = val_occ;
+                }
+              }
+            }
+          }
+        }
+
+        map_ = map;
+      }
+
+      ///Free unknown voxels
+      void freeUnknown() {
+        Veci<Dim> n;
+        if(Dim == 3) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              for (n(2) = 0; n(2) < dim_(2); n(2)++) {
+                if (isUnknown(getIndex(n)))
+                  map_[getIndex(n)] = val_free;
+              }
+            }
+          }
+        }
+        else if (Dim == 2) {
+          for (n(0) = 0; n(0) < dim_(0); n(0)++) {
+            for (n(1) = 0; n(1) < dim_(1); n(1)++) {
+              if (isUnknown(getIndex(n)))
+                map_[getIndex(n)] = val_free;
+            }
+          }
+        }
+      }
+      void CheckIfCollisionUsingPosAndYaw(const Eigen::Vector3d &state, bool *res, double flat = 0.0){
+        if(Dim==3){
+          ROS_ERROR("CheckIfCollisionUsingPosAndYaw Only works with Dimension = 2!");
+          return;
+        }
+        else{
+          Eigen::Matrix2d rotM;
+          double yaw = state[2];
+          rotM << cos(yaw), -sin(yaw),
+                  sin(yaw),  cos(yaw);
+          Eigen::Vector2d p = state.head(2);
+          for(int i = 0; i < priv_config_->conpts.size(); i++){
+            // Eigen::Vector2d rep1 = priv_config_->conpts[i];
+            // Eigen::Vector2d rep2 = priv_config_->conpts[(i+1)%priv_config_->conpts.size()];
+            // if(rep1[0] > 0){
+            //   rep1[0] += res_;
+            // }
+            // else{
+            //   rep1[0] -= res_;
+            // }
+            // if(rep1[1] > 0){
+            //   rep1[1] += res_;
+            // }
+            // else{
+            //   rep1[1] -= res_;
+            // }
+            // if(rep2[0] > 0){
+            //   rep2[0] += res_;
+            // }
+            // else{
+            //   rep2[0] -= res_;
+            // }
+            // if(rep2[1] > 0){
+            //   rep2[1] += res_;
+            // }
+            // else{
+            //   rep2[1] -= res_;
+            // }
+            // Eigen::Vector2d pt1 = p + rotM * rep1;
+            // Eigen::Vector2d pt2 = p + rotM * rep2;
+            // if(isBlocked(pt1, pt2)){
+            //   *res = true;
+            //   return;
+            // }
+            
+            Eigen::Vector2d pt = p + rotM * priv_config_->conpts[i];
+            Eigen::Vector2d grad;
+            // if(flat > 0){
+            // std::cout << "dis: "<<getDistGrad(pt, grad)<<"\n";
+            // }
+            if(getDistGrad(pt, grad)<priv_config_->safeMargin + flat){
+              *res = true;
+              return;
+            }
+
+          }
+        }
+        *res = false;
+        return;
+      }
+
+
+      void CheckIfCollisionForBackEnd(const Eigen::Vector3d &state, bool *res){
+        if(Dim==3){
+          ROS_ERROR("CheckIfCollisionUsingPosAndYaw Only works with Dimension = 2!");
+          return;
+        }
+        else{
+          Eigen::Matrix2d rotM;
+          double yaw = state[2];
+          rotM << cos(yaw), -sin(yaw),
+                  sin(yaw),  cos(yaw);
+          Eigen::Vector2d p = state.head(2);
+          for(int i = 0; i < priv_config_->conpts.size(); i++){
+            // Eigen::Vector2d rep1 = priv_config_->conpts[i];
+            // Eigen::Vector2d rep2 = priv_config_->conpts[(i+1)%priv_config_->conpts.size()];
+            // if(rep1[0] > 0){
+            //   rep1[0] += res_;
+            // }
+            // else{
+            //   rep1[0] -= res_;
+            // }
+            // if(rep1[1] > 0){
+            //   rep1[1] += res_;
+            // }
+            // else{
+            //   rep1[1] -= res_;
+            // }
+            // if(rep2[0] > 0){
+            //   rep2[0] += res_;
+            // }
+            // else{
+            //   rep2[0] -= res_;
+            // }
+            // if(rep2[1] > 0){
+            //   rep2[1] += res_;
+            // }
+            // else{
+            //   rep2[1] -= res_;
+            // }
+            // Eigen::Vector2d pt1 = p + rotM * rep1;
+            // Eigen::Vector2d pt2 = p + rotM * rep2;
+            // if(isBlocked(pt1, pt2)){
+            //   *res = true;
+            //   return;
+            // }
+            
+            Eigen::Vector2d pt = p + rotM * priv_config_->conpts[i];
+            Eigen::Vector2d grad;
+            if(getDistGrad(pt, grad)<priv_config_->safeMargin){
+              *res = true;
+              return;
+            }
+
+          }
+        }
+        *res = false;
+        return;
+      }
+      ///Map entity
+      Tmap map_;
+      std::vector<double> distance_buffer_;
+      std::vector<double> distance_buffer_neg_;
+      std::vector<char> occupancy_buffer_neg;
+      std::vector<double> distance_buffer_all_;
+      std::vector<double> tmp_buffer1_;
+      std::string world_frame_id;
+
+    protected:
+      ///Resolution
+      double res_;
+      ///Origin, float type
+      Vecf<Dim> origin_d_;
+      ///Dimension, int type
+      Veci<Dim> dim_;
+      Vecf<Dim> map_size;
+      ///Assume occupied cell has value 100
+      int8_t val_occ = 100;
+      ///Assume free cell has value 0
+      int8_t val_free = 0;
+      ///Assume unknown cell has value -1
+      int8_t val_unknown = -1;
+      ///inflate size
+      int  expand_size = 1;
+      bool has_map = false;
+      ros::Subscriber point_cloud_sub_;
+      ConfigPtr priv_config_;
+      ros::Publisher esdf_pub_;
+      ros::Publisher pcl_pub_;
+  };
+  typedef MapUtil<2> OccMapUtil;
+  typedef MapUtil<3> VoxelMapUtil;
+
+
+}
+
+
+#endif

trajectoryOpt/src/utils/tools/include/tools/tic_toc.hpp

@@ -0,0 +1,53 @@
+/**
+ * @file tic_toc.h
+ * @author HKUST Aerial Robotics Group
+ * @brief
+ * @version 0.1
+ * @date 2019-03-17
+ *
+ * @copyright Copyright (c) 2019
+ */
+#ifndef _COMMON_INC_COMMON_BASICS_TIC_TOC_H_
+#define _COMMON_INC_COMMON_BASICS_TIC_TOC_H_
+
+#include <chrono>
+#include <cstdlib>
+#include <ctime>
+
+class TicToc {
+ public:
+  TicToc() { tic(); }
+
+  /**
+   * @brief start timing
+   * @note the unit of time is in millisecond (ms)
+   */
+  void tic() { start = std::chrono::system_clock::now(); }
+
+  /**
+   * @brief get time elapsed
+   * @note the unit of time is in millisecond (ms)
+   */
+  double toc() {
+    end = std::chrono::system_clock::now();
+    std::chrono::duration<double> elapsed_seconds = end - start;
+    return elapsed_seconds.count() * 1000;
+  }
+
+  /**
+   * @brief Convert chrono::system_clock::time_point to double
+   *
+   * @param t
+   * @return double
+   */
+  static double TimePointToDouble(
+      const std::chrono::system_clock::time_point& t) {
+    auto tt = std::chrono::duration<double>(t.time_since_epoch());
+    return tt.count();
+  }
+
+ private:
+  std::chrono::time_point<std::chrono::system_clock> start, end;
+};
+
+#endif