[MPC][ocs2] workaround to implement mpc for mini_quadrotor

.github/workflows/catkin_lint.yml

@@ -28,4 +28,4 @@ jobs:
           wstool init
           wstool merge aerial_robot_noetic.rosinstall
           wstool update
-          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty  --skip-path rosserial --skip-path livox_ros_driver2 --skip-path fast_lio
+          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty  --skip-path rosserial --skip-path livox_ros_driver2 --skip-path fast_lio --skip-path ocs2 --skip-path ocs2_robotic_assets

.travis.sh

@@ -71,6 +71,6 @@ fi
 
 # Build
 catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
-catkin build --no-status
+catkin build --no-status aerial_robot
 catkin build --catkin-make-args run_tests -- -i --no-deps --no-status -p 1 -j 1 aerial_robot
 catkin_test_results --verbose build || catkin_test_results --all build

aerial_robot_estimation/include/aerial_robot_estimation/state_estimation.h

@@ -453,7 +453,7 @@ namespace aerial_robot_estimation
     std::string tf_prefix_;
 
     /* 9: x_w, y_w, z_w, roll_w, pitch_w, yaw_cog_w, x_b, y_b, yaw_board_w */
-    array<AxisState, State::TOTAL_NUM> state_;
+    std::array<AxisState, State::TOTAL_NUM> state_;
 
     /* for calculate the sensor to baselink with the consideration of time delay */
     int qu_size_;
@@ -464,7 +464,7 @@ namespace aerial_robot_estimation
     /* sensor fusion */
     boost::shared_ptr< pluginlib::ClassLoader<kf_plugin::KalmanFilter> > sensor_fusion_loader_ptr_;
     bool sensor_fusion_flag_;
-    array<SensorFuser, 2> fuser_; //0: egomotion; 1: experiment
+    std::array<SensorFuser, 2> fuser_; //0: egomotion; 1: experiment
 
     /* sensor (un)health level */
     uint8_t unhealth_level_;

aerial_robot_kinetic.rosinstall

@@ -36,3 +36,13 @@
     local-name: realsense-ros
     uri: https://github.com/tongtybj/realsense-ros.git
     version: multi_realsense_t265
+
+- git:
+     local-name: ocs2
+     uri: https://github.com/leggedrobotics/ocs2.git
+     version: main
+
+- git:
+     local-name: ocs2_robotic_assets
+     uri:  https://github.com/leggedrobotics/ocs2_robotic_assets.git
+     version: main

aerial_robot_melodic.rosinstall

@@ -50,3 +50,13 @@
 #    local-name: realsense-ros
 #    uri: https://github.com/tongtybj/realsense-ros.git
 #    version: multi_realsense_t265
+
+- git:
+     local-name: ocs2
+     uri: https://github.com/leggedrobotics/ocs2.git
+     version: main
+
+- git:
+     local-name: ocs2_robotic_assets
+     uri:  https://github.com/leggedrobotics/ocs2_robotic_assets.git
+     version: main

aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model.h

@@ -141,6 +141,7 @@ namespace aerial_robot_model {
     const double getMFRate() const  {return m_f_rate_;}
     const Eigen::VectorXd& getStaticThrust() const {return static_thrust_;}
     const std::vector<Eigen::MatrixXd>& getThrustWrenchAllocations() const {return thrust_wrench_allocations_;}
+    const Eigen::MatrixXd& getQ() const {return Q_;}
     const Eigen::MatrixXd& getThrustWrenchMatrix() const {return q_mat_;}
     const std::vector<Eigen::VectorXd>& getThrustWrenchUnits() const {return thrust_wrench_units_;}
     const double getThrustUpperLimit() const {return thrust_max_;}
@@ -202,6 +203,7 @@ namespace aerial_robot_model {
     Eigen::VectorXd gravity_;
     Eigen::VectorXd gravity_3d_;
     double m_f_rate_; //moment / force rate
+    Eigen::MatrixXd Q_;
     Eigen::MatrixXd q_mat_;
     std::map<int, int> rotor_direction_;
     Eigen::VectorXd static_thrust_;

aerial_robot_model/src/model/base_model/robot_model.cpp

@@ -452,6 +452,7 @@ namespace aerial_robot_model {
       Q.block(0, i, 3, 1) = u.at(i);
       Q.block(3, i, 3, 1) = p.at(i).cross(u.at(i)) + m_f_rate * sigma.at(i + 1) * u.at(i);
     }
+    Q_ = Q;
     return Q;
   }
 

aerial_robot_noetic.rosinstall

@@ -50,3 +50,13 @@
 #    local-name: realsense-ros
 #    uri: https://github.com/tongtybj/realsense-ros.git
 #    version: multi_realsense_t265
+
+- git:
+     local-name: ocs2
+     uri: https://github.com/leggedrobotics/ocs2.git
+     version: main
+
+- git:
+     local-name: ocs2_robotic_assets
+     uri:  https://github.com/leggedrobotics/ocs2_robotic_assets.git
+     version: main

robots/dragon/CMakeLists.txt

@@ -9,22 +9,31 @@ find_package(catkin REQUIRED COMPONENTS
   aerial_robot_msgs
   hydrus
   mujoco_ros_control
+  ocs2_centroidal_model
+  ocs2_core
+  ocs2_ddp
+  ocs2_mpc
+  ocs2_oc
+  ocs2_pinocchio_interface
+  ocs2_robotic_tools
   pluginlib
   roscpp
 )
 
-find_package(Eigen3 REQUIRED)
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 if(NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE RelWithDebInfo)
 endif()
 set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DNDEBUG")
 
 find_package(NLopt REQUIRED)
 
+set(CMAKE_CXX_STANDARD 17)
+
 catkin_package(
   INCLUDE_DIRS include test
   LIBRARIES dragon_robot_model dragon_aerial_robot_controllib dragon_navigation dragon_numerical_jacobians
-  CATKIN_DEPENDS   aerial_robot_control aerial_robot_model aerial_robot_msgs hydrus pluginlib roscpp
+  CATKIN_DEPENDS   aerial_robot_control aerial_robot_model aerial_robot_msgs hydrus ocs2_centroidal_model ocs2_core ocs2_ddp ocs2_mpc ocs2_oc ocs2_pinocchio_interface ocs2_robotic_tools pluginlib roscpp
 )
 
 ###########
@@ -47,7 +56,7 @@ add_dependencies(dragon_sensor_pluginlib aerial_robot_msgs_generate_messages_cpp
 add_library(dragon_robot_model src/model/hydrus_like_robot_model.cpp src/model/full_vectoring_robot_model.cpp)
 target_link_libraries(dragon_robot_model ${catkin_LIBRARIES} ${NLOPT_LIBRARIES})
 
-add_library(dragon_aerial_robot_controllib src/control/lqi_gimbal_control.cpp src/control/full_vectoring_control.cpp)
+add_library(dragon_aerial_robot_controllib src/control/lqi_gimbal_control.cpp src/control/full_vectoring_control.cpp src/control/mpc.cpp)
 target_link_libraries (dragon_aerial_robot_controllib dragon_robot_model dragon_navigation dragon_sensor_pluginlib ${catkin_LIBRARIES} ${Eigen3_LIBRARIES})
 add_dependencies(dragon_aerial_robot_controllib aerial_robot_msgs_generate_messages_cpp)
 

robots/dragon/config/quad/control/FullVectoringControlConfig.yaml

@@ -1,4 +1,4 @@
-aerial_robot_control_name: aerial_robot_control/dragon_full_vectoring
+aerial_robot_control_name: aerial_robot_control/mpc
 
 controller:
 

robots/dragon/config/quad/control/Mpc.yaml

@@ -0,0 +1,5 @@
+aerial_robot_control_name: aerial_robot_control/dragon_mpc
+
+controller:
+
+  control_verbose: false

robots/dragon/include/dragon/control/mpc.h

@@ -0,0 +1,97 @@
+#pragma once
+
+#include <pinocchio/fwd.hpp>
+#include <array>
+#include <aerial_robot_control/control/base/base.h>
+#include <ocs2_centroidal_model/PinocchioCentroidalDynamicsAD.h>
+#include <ocs2_core/augmented_lagrangian/AugmentedLagrangian.h>
+#include <ocs2_core/constraint/LinearStateInputConstraint.h>
+#include <ocs2_core/cost/QuadraticStateCost.h>
+#include <ocs2_core/cost/QuadraticStateInputCost.h>
+#include <ocs2_core/dynamics/SystemDynamicsBase.h>
+#include <ocs2_core/initialization/OperatingPoints.h>
+#include <ocs2_core/misc/LoadData.h>
+#include <ocs2_core/penalties/Penalties.h>
+#include <ocs2_core/PreComputation.h>
+#include <ocs2_core/Types.h>
+#include <ocs2_ddp/DDP_Settings.h>
+#include <ocs2_ddp/GaussNewtonDDP_MPC.h>
+#include <ocs2_mpc/MPC_MRT_Interface.h>
+#include <ocs2_mpc/MPC_Settings.h>
+#include <ocs2_oc/oc_problem/OptimalControlProblem.h>
+#include <ocs2_oc/rollout/TimeTriggeredRollout.h>
+#include <ocs2_oc/synchronized_module/ReferenceManager.h>
+#include <ocs2_pinocchio_interface/PinocchioInterface.h>
+#include <ocs2_robotic_tools/common/AngularVelocityMapping.h>
+#include <ocs2_robotic_tools/common/RobotInterface.h>
+
+namespace ocs2
+{
+  class dragonSystemDyamicsAD final : public SystemDynamicsBase
+  {
+    dragonSystemDyamicsAD(const PinocchioInterface& pinocchioInterface,
+                          const CentroidalModelInfo& info,
+                          const std::string& modelName);
+
+    ~dragonSystemDyamicsAD() override = default;
+    dragonSystemDyamicsAD* clone() const override { return new dragonSystemDyamicsAD(*this); }
+
+    vector_t computeFlowMap(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) override;
+    VectorFunctionLinearApproximation linearApproximation(scalar_t time, const vector_t& state, const vector_t& input,
+                                                          const PreComputation& preComp) override;
+  private:
+    dragonSystemDyamicsAD(const dragonSystemDyamicsAD& rhs) = default;
+
+    PinocchioCentroidalDynamicsAD pinocchioCentroidalDynamicsAd_;
+  };
+
+  class dragonInterface final : public RobotInterface
+  {
+  public:
+    dragonInterface(const std::string& taskFile, const std::string& urdfFile);
+    ~dragonInterface() override = default;
+
+    ddp::Settings& ddpSettings() { return ddpSettings_; }
+    mpc::Settings& mpcSettings() { return mpcSettings_; }
+    const OptimalControlProblem& getOptimalControlProblem() const override { return *problemPtr_; }
+    std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override { return referenceManagerPtr_; }
+    const RolloutBase& getRollout() const { return *rolloutPtr_; }
+    const Initializer& getInitializer() const override { return *operatingPointPtr_; }
+
+    PinocchioInterface& getPinocchioInterface() { return *pinocchioInterfacePtr_; }
+
+  private:
+    ddp::Settings ddpSettings_;
+    mpc::Settings mpcSettings_;
+    std::unique_ptr<OptimalControlProblem> problemPtr_;
+    std::shared_ptr<ReferenceManager> referenceManagerPtr_;
+    std::unique_ptr<RolloutBase> rolloutPtr_;
+    std::unique_ptr<Initializer> operatingPointPtr_;
+
+    std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_;
+  };
+};
+
+namespace aerial_robot_control
+{
+  class mpc : public ControlBase
+  {
+  public:
+    mpc();
+    virtual ~mpc() = default;
+
+    void initialize(ros::NodeHandle nh,
+                    ros::NodeHandle nhp,
+                    boost::shared_ptr<aerial_robot_model::RobotModel> robot_model_for_control,
+                    boost::shared_ptr<aerial_robot_estimation::StateEstimator> estimator,
+                    boost::shared_ptr<aerial_robot_navigation::BaseNavigator> navigator,
+                    double ctrl_loop_du) override;
+
+    virtual bool update() override;
+    virtual void reset() override;
+
+  private:
+    virtual void controlCore();
+    virtual void sendCmd();
+  };
+};

robots/dragon/package.xml

@@ -18,6 +18,14 @@
   <build_depend>hydrus</build_depend>
   <build_depend>mujoco_ros_control</build_depend>
   <build_depend>nlopt</build_depend>
+  <build_depend>ocs2_centroidal_model</build_depend>
+  <build_depend>ocs2_core</build_depend>
+  <build_depend>ocs2_ddp</build_depend>
+  <build_depend>ocs2_mpc</build_depend>
+  <build_depend>ocs2_oc</build_depend>
+  <build_depend>ocs2_pinocchio_interface</build_depend>
+  <build_depend>ocs2_robotic_tools</build_depend>
+  <build_depend>pinocchio</build_depend>
   <build_depend>pluginlib</build_depend>
 
   <run_depend>aerial_robot_control</run_depend>
@@ -29,6 +37,14 @@
   <run_depend>roscpp</run_depend>
   <run_depend>hydrus</run_depend>
   <run_depend>nlopt</run_depend>
+  <run_depend>ocs2_centroidal_model</run_depend>
+  <run_depend>ocs2_core</run_depend>
+  <run_depend>ocs2_ddp</run_depend>
+  <run_depend>ocs2_mpc</run_depend>
+  <run_depend>ocs2_oc</run_depend>
+  <run_depend>ocs2_pinocchio_interface</run_depend>
+  <run_depend>ocs2_robotic_tools</run_depend>
+  <run_depend>pinocchio</run_depend>
   <run_depend>pluginlib</run_depend>
 
   <test_depend>rostest</test_depend>

robots/dragon/plugins/flight_control_plugins.xml

@@ -7,4 +7,9 @@
   <class name="aerial_robot_control/dragon_full_vectoring" type="aerial_robot_control::DragonFullVectoringController" base_class_type="aerial_robot_control::ControlBase">
     <description> dragon control plugin: full vectoring control for x,y,z,roll,pitch,yaw </description>
   </class>
+
+  <class name="aerial_robot_control/mpc" type="aerial_robot_control::mpc" base_class_type="aerial_robot_control::ControlBase">
+    <description> mpc </description>
+  </class>
+
 </library>

robots/dragon/src/control/mpc.cpp

@@ -0,0 +1,66 @@
+#include <dragon/control/mpc.h>
+
+using namespace aerial_robot_control;
+
+namespace ocs2
+{
+  dragonSystemDyamicsAD::dragonSystemDyamicsAD(const PinocchioInterface& pinocchioInterface,
+                                               const CentroidalModelInfo& info,
+                                               const std::string& modelName):
+    pinocchioCentroidalDynamicsAd_(pinocchioInterface, info, modelName) {}
+
+  vector_t dragonSystemDyamicsAD::computeFlowMap(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) {
+    return pinocchioCentroidalDynamicsAd_.getValue(time, state, input);
+  }
+
+  VectorFunctionLinearApproximation dragonSystemDyamicsAD::linearApproximation(scalar_t time, const vector_t& state, const vector_t& input,
+                                                                               const PreComputation& preComp) {
+    return pinocchioCentroidalDynamicsAd_.getLinearApproximation(time, state, input);
+  }
+};
+
+
+mpc::mpc():
+  ControlBase()
+{
+}
+
+void mpc::initialize(ros::NodeHandle nh,
+                     ros::NodeHandle nhp,
+                     boost::shared_ptr<aerial_robot_model::RobotModel> robot_model,
+                     boost::shared_ptr<aerial_robot_estimation::StateEstimator> estimator,
+                     boost::shared_ptr<aerial_robot_navigation::BaseNavigator> navigator,
+                     double ctrl_loop_rate)
+{
+  ControlBase::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate);
+}
+
+void mpc::reset()
+{
+  ControlBase::reset();
+}
+
+bool mpc::update()
+{
+  if(!ControlBase::update()) return false;
+
+  controlCore();
+  sendCmd();
+
+  return true;
+}
+
+void mpc::controlCore()
+{
+  ROS_INFO_STREAM("mpc controlcore");
+}
+
+void::mpc::sendCmd()
+{
+  ROS_INFO_STREAM("mpc send command");
+}
+
+
+/* plugin registration */
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(aerial_robot_control::mpc, aerial_robot_control::ControlBase);