From 98a6df69606f67eede940e1f110a95099205e776 Mon Sep 17 00:00:00 2001
From: sugikazu75 <sugihara@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 24 Dec 2023 05:08:16 +0900
Subject: [PATCH] [rolling][control] add robot model instance for optimization

---
 robots/rolling/include/rolling/control/rolling_controller.h | 5 +++++
 robots/rolling/src/control/rolling_controller.cpp           | 2 ++
 2 files changed, 7 insertions(+)

diff --git a/robots/rolling/include/rolling/control/rolling_controller.h b/robots/rolling/include/rolling/control/rolling_controller.h
index 48c5202de..5b0e12d75 100644
--- a/robots/rolling/include/rolling/control/rolling_controller.h
+++ b/robots/rolling/include/rolling/control/rolling_controller.h
@@ -35,6 +35,10 @@ namespace aerial_robot_control
                     boost::shared_ptr<aerial_robot_navigation::BaseNavigator> navigator,
                     double ctrl_loop_rate) override;
 
+    boost::shared_ptr<aerial_robot_model::RobotModel> getRobotModel() {return robot_model_;}
+    boost::shared_ptr<RollingRobotModel> getRollingRobotModel() {return rolling_robot_model_;}
+    boost::shared_ptr<RollingRobotModel> getRollingRobotModelForOpt() {return rolling_robot_model_for_opt_;}
+
     Eigen::VectorXd getTargetWrenchAccCog() {return target_wrench_acc_cog_;}
     Eigen::MatrixXd getFullQ() {return full_q_mat_;}
     Eigen::MatrixXd getFullQTrans() {return full_q_trans_;}
@@ -68,6 +72,7 @@ namespace aerial_robot_control
 
     boost::shared_ptr<aerial_robot_navigation::RollingNavigator> rolling_navigator_;
     boost::shared_ptr<RollingRobotModel> rolling_robot_model_;
+    boost::shared_ptr<RollingRobotModel> rolling_robot_model_for_opt_;
     boost::shared_ptr<aerial_robot_model::RobotModel> robot_model_for_control_;
 
     std::vector<double> rotor_tilt_;
diff --git a/robots/rolling/src/control/rolling_controller.cpp b/robots/rolling/src/control/rolling_controller.cpp
index 816dc41c6..f454635db 100644
--- a/robots/rolling/src/control/rolling_controller.cpp
+++ b/robots/rolling/src/control/rolling_controller.cpp
@@ -19,6 +19,7 @@ void RollingController::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
 
   rolling_navigator_ = boost::dynamic_pointer_cast<aerial_robot_navigation::RollingNavigator>(navigator_);
   rolling_robot_model_ = boost::dynamic_pointer_cast<RollingRobotModel>(robot_model_);
+  rolling_robot_model_for_opt_ = boost::make_shared<RollingRobotModel>();
   robot_model_for_control_ = boost::make_shared<aerial_robot_model::RobotModel>();
 
   rotor_tilt_.resize(motor_num_);
@@ -105,6 +106,7 @@ void RollingController::rosParamInit()
 
   getParam<double>(nh_, "circle_radius", circle_radius_, 0.5);
   rolling_robot_model_->setCircleRadius(circle_radius_);
+  rolling_robot_model_for_opt_->setCircleRadius(circle_radius_);
 
   getParam<double>(z_nh, "i_gain", default_z_i_gain_, 0.0);