Merge pull request jsk-ros-pkg#556 from tongtybj/develop/navigation/t…

…rajectory_generation

[Navigation][merge after jsk-ros-pkg#554 jsk-ros-pkg#555] trajectory generation based on optimization methods (e.g. minimum snap trajectory)

aerial_robot_control/CMakeLists.txt

@@ -38,7 +38,7 @@ set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DNDEBUG")
 
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES control_utils flight_control_pluginlib flight_navigation
+  LIBRARIES control_utils flight_control_pluginlib flight_navigation trajectory_generation
   CATKIN_DEPENDS aerial_robot_estimation aerial_robot_model aerial_robot_msgs dynamic_reconfigure pluginlib roscpp spinal tf
 )
 
@@ -69,13 +69,38 @@ add_dependencies(flight_control_pluginlib  ${PROJECT_NAME}_gencfg)
 add_library (flight_navigation src/flight_navigation.cpp)
 target_link_libraries (flight_navigation ${catkin_LIBRARIES})
 
+### trajectory generate based on dodgelib
+add_library (trajectory_generation
+  src/trajectory/reference_base.cpp
+  src/trajectory/trajectory_reference/polynomial.cpp
+  src/trajectory/trajectory_reference/polynomial_trajectory.cpp
+  src/trajectory/trajectory_reference/sampled_trajectory.cpp
+  src/trajectory/base/parameter_base.cpp
+  src/trajectory/math/math.cpp
+  src/trajectory/types/command.cpp
+  src/trajectory/types/quad_state.cpp
+  src/trajectory/types/quadrotor.cpp
+  src/trajectory/utils/logger.cpp
+  src/trajectory/utils/timer.cpp
+  )
+target_link_libraries (trajectory_generation ${catkin_LIBRARIES})
+
+add_executable(trajectory_generation_node src/trajectory/test.cpp)
+target_link_libraries (trajectory_generation_node ${catkin_LIBRARIES} trajectory_generation)
+
+
+
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
 
-install(TARGETS control_utils flight_control_pluginlib flight_navigation
+install(TARGETS control_utils flight_control_pluginlib flight_navigation trajectory_generation
   DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
 
+install(TARGETS trajectory_generation_node
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 install(DIRECTORY scripts plugins
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS

aerial_robot_control/include/aerial_robot_control/control/base/pose_linear_controller.h

@@ -84,7 +84,7 @@ namespace aerial_robot_control
 
     tf::Vector3 pos_, target_pos_;
     tf::Vector3 vel_, target_vel_;
-    tf::Vector3 target_acc_;
+    tf::Vector3 target_acc_, target_ang_acc_;
     tf::Vector3 rpy_, target_rpy_;
     tf::Vector3 omega_, target_omega_;
 

aerial_robot_control/include/aerial_robot_control/control/under_actuated_lqi_controller.h

@@ -105,6 +105,7 @@ namespace aerial_robot_control
     virtual void sendCmd() override;
     virtual void sendFourAxisCommand();
 
+    Eigen::MatrixXd getQInv();
     virtual void allocateYawTerm();
     void cfgLQICallback(aerial_robot_control::LQIConfig &config, uint32_t level); //dynamic reconfigure
 

aerial_robot_control/include/aerial_robot_control/flight_navigation.h

@@ -6,13 +6,16 @@
 #include <aerial_robot_msgs/FlightNav.h>
 #include <angles/angles.h>
 #include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <ros/ros.h>
 #include <sensor_msgs/Joy.h>
 #include <spinal/FlightConfigCmd.h>
 #include <std_msgs/Empty.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/Int8.h>
 #include <std_msgs/UInt8.h>
+#include <nav_msgs/Path.h>
+#include <aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp>
 
 namespace aerial_robot_navigation
 {
@@ -75,6 +78,7 @@ namespace aerial_robot_navigation
     inline tf::Vector3 getTargetAcc() {return target_acc_;}
     inline tf::Vector3 getTargetRPY() {return target_rpy_;}
     inline tf::Vector3 getTargetOmega() {return target_omega_;}
+    inline tf::Vector3 getTargetAngAcc() {return target_ang_acc_;}
 
     inline void setTargetPos(tf::Vector3 pos) { target_pos_ = pos; }
     inline void setTargetPos(double x, double y, double z) { setTargetPos(tf::Vector3(x, y, z)); }
@@ -88,12 +92,22 @@ namespace aerial_robot_navigation
     inline void setTargetZeroAcc() { setTargetAcc(tf::Vector3(0,0,0)); }
 
     inline void setTargetRoll(float value) { target_rpy_.setX(value); }
+    inline void setTargetOmega(tf::Vector3 omega) { target_omega_ = omega; }
+    inline void setTargetOmega(double x, double y, double z) { setTargetOmega(tf::Vector3(x, y, z)); }
+    inline void setTargetZeroOmega() { setTargetOmega(0,0,0); }
     inline void setTargetOmegaX(float value) { target_omega_.setX(value); }
     inline void setTargetPitch(float value) { target_rpy_.setY(value); }
     inline void setTargetOmegaY(float value) { target_omega_.setY(value); }
     inline void setTargetYaw(float value) { target_rpy_.setZ(value); }
     inline void addTargetYaw(float value) { setTargetYaw(angles::normalize_angle(target_rpy_.z() + value)); }
     inline void setTargetOmegaZ(float value) { target_omega_.setZ(value); }
+    inline void setTargetAngAcc(tf::Vector3 acc) { target_ang_acc_ = acc; }
+    inline void setTargetAngAcc(double x, double y, double z) { setTargetAngAcc(tf::Vector3(x, y, z)); }
+    inline void setTargetZeroAngAcc() { setTargetAngAcc(tf::Vector3(0,0,0)); }
+    inline void setTargetAngAccX(double value) { target_ang_acc_.setX(value); }
+    inline void setTargetAngAccY(double value) { target_ang_acc_.setY(value); }
+    inline void setTargetAngAccZ(double value) { target_ang_acc_.setZ(value); }
+
     inline void setTargetPosX( float value){  target_pos_.setX(value);}
     inline void setTargetVelX( float value){  target_vel_.setX(value);}
     inline void setTargetAccX( float value){  target_acc_.setX(value);}
@@ -116,6 +130,8 @@ namespace aerial_robot_navigation
     uint8_t getEstimateMode(){ return estimate_mode_;}
     void setEstimateMode(uint8_t estimate_mode){ estimate_mode_ = estimate_mode;}
 
+    void generateNewTrajectory(geometry_msgs::PoseStamped pose);
+
     static constexpr uint8_t POS_CONTROL_COMMAND = 0;
     static constexpr uint8_t VEL_CONTROL_COMMAND = 1;
 
@@ -219,7 +235,10 @@ namespace aerial_robot_navigation
     ros::Publisher  flight_config_pub_;
     ros::Publisher  power_info_pub_;
     ros::Publisher  flight_state_pub_;
+    ros::Publisher  path_pub_;
     ros::Subscriber navi_sub_;
+    ros::Subscriber pose_sub_;
+    ros::Subscriber simple_move_base_goal_sub_;
     ros::Subscriber battery_sub_;
     ros::Subscriber flight_status_ack_sub_;
     ros::Subscriber takeoff_sub_;
@@ -266,7 +285,7 @@ namespace aerial_robot_navigation
 
     /* target value */
     tf::Vector3 target_pos_, target_vel_, target_acc_;
-    tf::Vector3 target_rpy_, target_omega_;
+    tf::Vector3 target_rpy_, target_omega_, target_ang_acc_;
 
     double takeoff_height_;
     double init_height_;
@@ -307,6 +326,13 @@ namespace aerial_robot_navigation
 
     std::string teleop_local_frame_;
 
+    /* trajectory */
+    double trajectory_mean_vel_;
+    double trajectory_mean_yaw_rate_;
+    double trajectory_min_du_;
+    bool enable_latch_yaw_trajectory_;
+    std::shared_ptr<agi::MinJerkTrajectory> traj_generator_ptr_;
+
     /* battery info */
     double low_voltage_thre_;
     bool low_voltage_flag_;
@@ -316,6 +342,8 @@ namespace aerial_robot_navigation
     double hovering_current_;
 
     virtual void rosParamInit();
+    void poseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
+    void simpleMoveBaseGoalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
     void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg);
     void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg);
     void batteryCheckCallback(const std_msgs::Float32ConstPtr &msg);

aerial_robot_control/include/aerial_robot_control/trajectory/base/module.hpp

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <memory>
+
+#include "aerial_robot_control/trajectory/utils/logger.hpp"
+
+namespace agi {
+
+template<typename Derived>
+class Module {
+ public:
+  Module(const std::string& name) : logger_(name) {}
+  virtual ~Module() {}
+
+  inline const std::string& name() const { return logger_.name(); }
+  virtual void logTiming() const {};
+
+ protected:
+  Logger logger_;
+};
+
+}  // namespace agi

aerial_robot_control/include/aerial_robot_control/trajectory/base/parameter_base.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <aerial_robot_control/trajectory/utils/filesystem.hpp>
+#include <exception>
+#include <sstream>
+
+#include "aerial_robot_control/trajectory/math/types.hpp"
+#include "aerial_robot_control/trajectory/utils/filesystem.hpp"
+#include "aerial_robot_control/trajectory/utils/yaml.hpp"
+
+namespace agi {
+
+
+struct ParameterException : public std::exception {
+  ParameterException() = default;
+  ParameterException(const std::string& msg)
+    : msg(std::string("Dodgelib Parameter Exception: ") + msg) {}
+  const char* what() const throw() { return msg.c_str(); }
+
+  const std::string msg{"Dodgelib Parameter Exception"};
+};
+
+struct ParameterBase {
+  virtual ~ParameterBase() = default;
+  virtual bool load(const fs::path& filename);
+  virtual bool load(const Yaml& yaml);
+
+  virtual bool valid() const;
+};
+
+}  // namespace agi

aerial_robot_control/include/aerial_robot_control/trajectory/math/gravity.hpp

@@ -0,0 +1,22 @@
+#pragma once
+#include "aerial_robot_control/trajectory/math/types.hpp"
+
+namespace agi {
+
+/**
+ * Gravity Value [m/s^2]
+ *
+ * This is the gravity value used in the whole project.
+ */
+static constexpr Scalar G = 9.8066;
+
+
+/**
+ * Gravity Vector [m/s^2]
+ *
+ * This is the gravity vector pointing in negative z-direction.
+ * It uses the value of #G.
+ */
+const Vector<3> GVEC{0, 0, -G};
+
+}  // namespace agi

aerial_robot_control/include/aerial_robot_control/trajectory/math/math.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <aerial_robot_control/trajectory/math/types.hpp>
+
+namespace agi {
+
+Matrix<3, 3> skew(const Vector<3>& v);
+
+Matrix<4, 4> Q_left(const Quaternion& q);
+
+Matrix<4, 4> Q_right(const Quaternion& q);
+
+Matrix<4, 3> qFromQeJacobian(const Quaternion& q);
+
+Matrix<4, 4> qConjugateJacobian();
+
+Matrix<3, 3> qeRotJacobian(const Quaternion& q, const Matrix<3, 1>& t);
+
+Matrix<3, 3> qeInvRotJacobian(const Quaternion& q, const Matrix<3, 1>& t);
+
+void matrixToTripletList(const SparseMatrix& matrix,
+                         std::vector<SparseTriplet>* const list,
+                         const int row_offset = 0, const int col_offset = 0);
+
+void matrixToTripletList(const Matrix<>& matrix,
+                         std::vector<SparseTriplet>* const list,
+                         const int row_offset = 0, const int col_offset = 0);
+
+void insert(const SparseMatrix& from, SparseMatrix* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+void insert(const Matrix<>& from, SparseMatrix* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+void insert(const Matrix<>& from, Matrix<>* const into,
+            const int row_offset = 0, const int col_offset = 0);
+
+Vector<> clip(const Vector<>& v, const Vector<>& bound);
+
+inline constexpr Scalar toRad(const Scalar angle_deg) {
+  return angle_deg * M_PI / 180.0;
+}
+inline constexpr Scalar toDeg(const Scalar angle_deg) {
+  return angle_deg / M_PI * 180.0;
+}
+
+}  // namespace agi

aerial_robot_control/include/aerial_robot_control/trajectory/math/types.hpp

@@ -0,0 +1,53 @@
+#pragma once
+
+#include <Eigen/Eigen>
+#include <limits>
+
+namespace agi {
+
+// Define the scalar type used.
+using Scalar = double;
+static constexpr Scalar INF = std::numeric_limits<Scalar>::infinity();
+
+// Define `Dynamic` matrix size.
+static constexpr int Dynamic = Eigen::Dynamic;
+
+// Using shorthand for `Matrix<rows, cols>` with scalar type.
+template<int rows = Dynamic, int cols = rows>
+using Matrix = Eigen::Matrix<Scalar, rows, cols>;
+
+// Using shorthand for `Vector<rows>` with scalar type.
+template<int rows = Dynamic>
+using Vector = Matrix<rows, 1>;
+
+// Using shorthand for `Array<rows, cols>` with scalar type.
+template<int rows = Dynamic, int cols = rows>
+using Array = Eigen::Array<Scalar, rows, cols>;
+
+template<int rows = Dynamic>
+using ArrayVector = Array<rows, 1>;
+
+// Using `SparseMatrix` with type.
+using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+// Using SparseTriplet with type.
+using SparseTriplet = Eigen::Triplet<Scalar>;
+
+// Using `Quaternion` with type.
+using Quaternion = Eigen::Quaternion<Scalar>;
+
+// Using `Ref` for modifier references.
+template<class Derived>
+using Ref = Eigen::Ref<Derived>;
+
+// Using `ConstRef` for constant references.
+template<class Derived>
+using ConstRef = const Eigen::Ref<const Derived>;
+
+// Using `Map`.
+template<class Derived>
+using Map = Eigen::Map<Derived>;
+
+using DynamicsFunction =
+  std::function<bool(const Ref<const Vector<>>, Ref<Vector<>>)>;
+}  // namespace agi

aerial_robot_control/include/aerial_robot_control/trajectory/reference_base.hpp

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "aerial_robot_control/trajectory/base/module.hpp"
+#include "aerial_robot_control/trajectory/math/types.hpp"
+#include "aerial_robot_control/trajectory/types/setpoint.hpp"
+#include "aerial_robot_control/trajectory/utils/logger.hpp"
+
+namespace agi {
+
+class ReferenceBase {
+ public:
+  ReferenceBase(const std::string& name = "ReferenceBase");
+  ReferenceBase(const QuadState& state, const Scalar duration,
+                const std::string& name = "ReferenceBase");
+  virtual ~ReferenceBase();
+
+  virtual Setpoint getSetpoint(const QuadState& state, const Scalar t) = 0;
+  virtual Setpoint getSetpoint(const QuadState& state) final;
+  virtual Setpoint getSetpoint(const Scalar t,
+                               const Scalar heading = NAN) final;
+  virtual Setpoint getStartSetpoint();
+  virtual Setpoint getEndSetpoint();
+
+  inline Scalar getStartTime() const { return start_state_.t; }
+  inline Scalar getEndTime() const { return start_state_.t + duration_; }
+  inline Scalar getDuration() const { return duration_; }
+  bool isTimeInRange(const Scalar time) const;
+  std::string time_in_HH_MM_SS_MMM(const Scalar time) const;
+
+  virtual bool valid() const { return start_state_.valid(); }
+  const std::string& name() const { return name_; }
+  virtual bool isHover() const { return false; }
+  virtual bool isVelocityRefernce() const { return false; }
+  virtual bool isAbsolute() const { return true; }
+
+  bool truncate(const Scalar& t);
+  friend std::ostream& operator<<(std::ostream& os, const ReferenceBase& ref);
+
+ protected:
+  QuadState start_state_;
+  Scalar duration_;
+  const std::string name_;
+};
+
+using ReferenceVector = std::vector<std::shared_ptr<ReferenceBase>>;
+
+}  // namespace agi