[rolling][control] add robot model instance for optimization

sugikazu75 · Dec 23, 2023 · 98a6df6 · 98a6df6
1 parent a06d36a
commit 98a6df6
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 0 deletions.
diff --git 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
@@ -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);