From 7598a896235198faeba360da6aaeea9b9f9086b8 Mon Sep 17 00:00:00 2001
From: Junichiro Sugihara <j-sugihara@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 15 Sep 2024 17:58:51 +0900
Subject: [PATCH] [Ninja][Controller] workaround to control joint position with
 thrust during free joint mode

---
 .../control/gimbalrotor_controller.h          |  2 +-
 .../include/ninja/control/ninja_controller.h  | 16 +++++
 robots/ninja/src/control/ninja_controller.cpp | 64 ++++++++++++++++++-
 3 files changed, 80 insertions(+), 2 deletions(-)

diff --git a/robots/gimbalrotor/include/gimbalrotor/control/gimbalrotor_controller.h b/robots/gimbalrotor/include/gimbalrotor/control/gimbalrotor_controller.h
index 4b778c0cf..90af92fdf 100644
--- a/robots/gimbalrotor/include/gimbalrotor/control/gimbalrotor_controller.h
+++ b/robots/gimbalrotor/include/gimbalrotor/control/gimbalrotor_controller.h
@@ -58,7 +58,6 @@ namespace aerial_robot_control
     int gimbal_dof_;
     int rotor_coef_;
 
-    bool update() override;
     void sendCmd() override;
     void sendFourAxisCommand();
     void sendGimbalCommand();
@@ -71,6 +70,7 @@ namespace aerial_robot_control
     virtual void setAttitudeGains();
     virtual void rosParamInit();
     virtual void controlCore() override;
+    bool update() override;
     Eigen::VectorXd additional_wrench_acc_cog_term_;
   };
 };
diff --git a/robots/ninja/include/ninja/control/ninja_controller.h b/robots/ninja/include/ninja/control/ninja_controller.h
index 0eadc6409..9e3d6b58b 100644
--- a/robots/ninja/include/ninja/control/ninja_controller.h
+++ b/robots/ninja/include/ninja/control/ninja_controller.h
@@ -7,6 +7,12 @@
 
 namespace aerial_robot_control
 {
+  enum
+    {
+     JOINT_PITCH = TZ +1,
+     JOINT_YAW,
+    };
+  
   class NinjaController: public BeetleController
   {
   public:
@@ -21,7 +27,17 @@ namespace aerial_robot_control
   private:
     boost::shared_ptr<aerial_robot_navigation::NinjaNavigator> ninja_navigator_;
     boost::shared_ptr<NinjaRobotModel> ninja_robot_model_;
+
+    double joint_p_gain_;
+    double joint_i_gain_;
+    double joint_d_gain_;
+
+    double joint_control_timestamp_;
   protected:
     void externalWrenchEstimate() override;
+    void rosParamInit() override;
+    void controlCore() override;
+    bool update() override;
+    void reset() override;
   };
 };
diff --git a/robots/ninja/src/control/ninja_controller.cpp b/robots/ninja/src/control/ninja_controller.cpp
index 6bbbe292d..4d9ef5171 100644
--- a/robots/ninja/src/control/ninja_controller.cpp
+++ b/robots/ninja/src/control/ninja_controller.cpp
@@ -5,7 +5,8 @@ using namespace std;
 namespace aerial_robot_control
 {
   NinjaController::NinjaController():
-    BeetleController()
+    BeetleController(),
+    joint_control_timestamp_(-1)
   {}
   void NinjaController::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
                                     boost::shared_ptr<aerial_robot_model::RobotModel> robot_model,
@@ -17,6 +18,45 @@ namespace aerial_robot_control
     BeetleController::initialize(nh, nhp, robot_model, estimator, navigator, ctrl_loop_rate);
     ninja_navigator_ = boost::dynamic_pointer_cast<aerial_robot_navigation::NinjaNavigator>(navigator);
     ninja_robot_model_ = boost::dynamic_pointer_cast<NinjaRobotModel>(robot_model);
+    pid_controllers_.push_back(PID("joint_pitch", joint_p_gain_, joint_i_gain_, joint_d_gain_));
+    pid_controllers_.push_back(PID("joint_yaw", joint_p_gain_, joint_i_gain_, joint_d_gain_));
+  }
+  bool NinjaController::update()
+  {
+    if(!ninja_navigator_->getControlFlag())
+      joint_control_timestamp_ = -1;
+    else if(ninja_navigator_->getControlFlag() && joint_control_timestamp_ < 0)
+      joint_control_timestamp_ = ros::Time::now().toSec();
+    return GimbalrotorController::update();
+  }
+
+  void NinjaController::controlCore()
+  {
+    if(ninja_navigator_->getCurrentAssembled() && joint_control_timestamp_ > 0 && ninja_navigator_->getFreeJointFlag())
+      {
+        int my_id = ninja_navigator_->getMyID();
+        int leader_id = ninja_navigator_->getLeaderID();
+        std::vector<int> assembled_modules_ids = ninja_navigator_->getAssemblyIds();
+        double du = ros::Time::now().toSec() - joint_control_timestamp_;
+        std::vector<double> joint_errs =  ninja_navigator_->getJointPosErr();
+        pid_controllers_.at(JOINT_PITCH).updateWoVel(joint_errs.at(0),du);
+        pid_controllers_.at(JOINT_YAW).updateWoVel(joint_errs.at(1),du);
+        Eigen::VectorXd joint_ff_wrench = Eigen::VectorXd::Zero(6);
+        joint_ff_wrench.topRows(4) = Eigen::VectorXd::Zero(4);
+        joint_ff_wrench(4) = pid_controllers_.at(JOINT_PITCH).result();
+        joint_ff_wrench(5) = pid_controllers_.at(JOINT_YAW).result();
+        if(my_id < leader_id)
+          {
+            setFfInterWrench(my_id,-joint_ff_wrench);
+          }
+        else if(my_id > leader_id)
+          {
+            int left_module_id = assembled_modules_ids[ninja_navigator_->getMyIndex() -1];
+            setFfInterWrench(left_module_id, joint_ff_wrench);
+          }
+      }
+    joint_control_timestamp_ = ros::Time::now().toSec();
+    BeetleController::controlCore();
   }
 
   void NinjaController::externalWrenchEstimate()
@@ -108,6 +148,28 @@ namespace aerial_robot_control
     prev_est_wrench_timestamp_ = ros::Time::now().toSec();
   }
 
+  void NinjaController::rosParamInit()
+  {
+    BeetleController::rosParamInit();
+    ros::NodeHandle control_nh(nh_, "controller");
+    ros::NodeHandle joint_nh(control_nh, "joint_comp");
+    getParam<double>(joint_nh, "p_gain", joint_p_gain_, 0.1);
+    getParam<double>(joint_nh, "i_gain", joint_i_gain_, 0.005);
+    getParam<double>(joint_nh, "d_gain", joint_d_gain_, 0.07);  
+  }
+
+  void NinjaController::reset()
+  {
+    BeetleController::reset();
+    pid_controllers_.at(JOINT_PITCH).reset();
+    pid_controllers_.at(JOINT_YAW).reset();
+    for(const auto & id : ninja_navigator_->getAssemblyIds())
+      {
+        Eigen::VectorXd reset_ff_wrench = Eigen::VectorXd::Zero(6);
+        setFfInterWrench(id,reset_ff_wrench); 
+      }
+    joint_control_timestamp_ = -1;
+  }
 } //namespace aerial_robot_controller
 
 /* plugin registration */