From cc6f33b967b086eec82992e92bad95f2649dff43 Mon Sep 17 00:00:00 2001
From: Moju Zhao <chou@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 8 Oct 2023 19:27:28 +0900
Subject: [PATCH] [Aerial Robot Model][Underactuated] use transformable model
 for planning

---
 robots/dragon/include/dragon/control/full_vectoring_control.h | 2 +-
 .../dragon/include/dragon/model/full_vectoring_robot_model.h  | 4 ++--
 robots/dragon/src/control/full_vectoring_control.cpp          | 2 +-
 robots/dragon/src/model/full_vectoring_robot_model.cpp        | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/robots/dragon/include/dragon/control/full_vectoring_control.h b/robots/dragon/include/dragon/control/full_vectoring_control.h
index 2a956da14..380b3dd14 100644
--- a/robots/dragon/include/dragon/control/full_vectoring_control.h
+++ b/robots/dragon/include/dragon/control/full_vectoring_control.h
@@ -74,7 +74,7 @@ namespace aerial_robot_control
     ros::Publisher interfrence_marker_pub_;
 
     boost::shared_ptr<Dragon::FullVectoringRobotModel> dragon_robot_model_;
-    boost::shared_ptr<aerial_robot_model::RobotModel> robot_model_for_control_;
+    boost::shared_ptr<aerial_robot_model::transformable::RobotModel> robot_model_for_control_;
     std::vector<float> target_base_thrust_;
     std::vector<double> target_gimbal_angles_;
     Eigen::VectorXd target_vectoring_f_;
diff --git a/robots/dragon/include/dragon/model/full_vectoring_robot_model.h b/robots/dragon/include/dragon/model/full_vectoring_robot_model.h
index d930a38ef..22bc859a3 100644
--- a/robots/dragon/include/dragon/model/full_vectoring_robot_model.h
+++ b/robots/dragon/include/dragon/model/full_vectoring_robot_model.h
@@ -55,7 +55,7 @@ namespace Dragon
     virtual ~FullVectoringRobotModel() = default;
 
 
-    inline boost::shared_ptr<aerial_robot_model::RobotModel> getRobotModelForPlan() { return robot_model_for_plan_;}
+    inline boost::shared_ptr<aerial_robot_model::transformable::RobotModel> getRobotModelForPlan() { return robot_model_for_plan_;}
     inline const Eigen::VectorXd& getHoverVectoringF() const {return hover_vectoring_f_;}
 
     const std::vector<int> getRollLockedGimbal()
@@ -88,7 +88,7 @@ namespace Dragon
 
   private:
 
-    boost::shared_ptr<aerial_robot_model::RobotModel> robot_model_for_plan_;
+    boost::shared_ptr<aerial_robot_model::transformable::RobotModel> robot_model_for_plan_;
 
     Eigen::VectorXd hover_vectoring_f_;
 
diff --git a/robots/dragon/src/control/full_vectoring_control.cpp b/robots/dragon/src/control/full_vectoring_control.cpp
index dbfdfc80c..bbd11799b 100644
--- a/robots/dragon/src/control/full_vectoring_control.cpp
+++ b/robots/dragon/src/control/full_vectoring_control.cpp
@@ -18,7 +18,7 @@ void DragonFullVectoringController::initialize(ros::NodeHandle nh, ros::NodeHand
   rosParamInit();
 
   dragon_robot_model_ = boost::dynamic_pointer_cast<Dragon::FullVectoringRobotModel>(robot_model);
-  robot_model_for_control_ = boost::make_shared<aerial_robot_model::RobotModel>();
+  robot_model_for_control_ = boost::make_shared<aerial_robot_model::transformable::RobotModel>();
 
   /* initialize the gimbal target angles */
   target_base_thrust_.resize(motor_num_);
diff --git a/robots/dragon/src/model/full_vectoring_robot_model.cpp b/robots/dragon/src/model/full_vectoring_robot_model.cpp
index c937c561a..936707245 100644
--- a/robots/dragon/src/model/full_vectoring_robot_model.cpp
+++ b/robots/dragon/src/model/full_vectoring_robot_model.cpp
@@ -82,7 +82,7 @@ FullVectoringRobotModel::FullVectoringRobotModel(bool init_with_rosparam, bool v
   gimbal_roll_origin_from_cog_.resize(rotor_num);
   setGimbalNominalAngles(std::vector<double>(0)); // for online initialize
 
-  robot_model_for_plan_ = boost::make_shared<aerial_robot_model::RobotModel>();
+  robot_model_for_plan_ = boost::make_shared<aerial_robot_model::transformable::RobotModel>();
 
   if(debug_verbose_)
     {