From 05154542c3f8da330a226254e5c5361a6ef66397 Mon Sep 17 00:00:00 2001
From: Junichiro Sugihara <j-sugihara@jsk.imi.i.u-tokyo.ac.jp>
Date: Thu, 16 May 2024 21:24:03 +0900
Subject: [PATCH] [Gimbalrotor][dragonfly] workaround to enable aerial
 transformation of dragonfly

---
 robots/gimbalrotor/config/dragonfly/Servo.yaml                | 4 ++--
 .../include/gimbalrotor/model/gimbalrotor_robot_model.h       | 2 +-
 robots/gimbalrotor/src/control/gimbalrotor_controller.cpp     | 2 +-
 robots/gimbalrotor/src/model/gimbalrotor_robot_model.cpp      | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/robots/gimbalrotor/config/dragonfly/Servo.yaml b/robots/gimbalrotor/config/dragonfly/Servo.yaml
index c17d68ad2..3f8592bc6 100644
--- a/robots/gimbalrotor/config/dragonfly/Servo.yaml
+++ b/robots/gimbalrotor/config/dragonfly/Servo.yaml
@@ -18,7 +18,7 @@ servo_controller:
       name: head_joint
       angle_sgn: -1
       simulation:
-        pid: {p: 50.0, i: 0.01, d: 1.0}
+        pid: {p: 20.0, i: 0.1, d: 1.0}
         init_value: 0
         type: effort_controllers/JointPositionController
 
@@ -27,7 +27,7 @@ servo_controller:
       name: tail_joint
       angle_sgn: -1
       simulation:
-        pid: {p: 50.0, i: 0.01, d: 1.0}
+        pid: {p: 20.0, i: 0.1, d: 1.0}
         init_value: 0
         type: effort_controllers/JointPositionController
 
diff --git a/robots/gimbalrotor/include/gimbalrotor/model/gimbalrotor_robot_model.h b/robots/gimbalrotor/include/gimbalrotor/model/gimbalrotor_robot_model.h
index ad61806c9..0facc7559 100644
--- a/robots/gimbalrotor/include/gimbalrotor/model/gimbalrotor_robot_model.h
+++ b/robots/gimbalrotor/include/gimbalrotor/model/gimbalrotor_robot_model.h
@@ -6,7 +6,7 @@
 
 using namespace aerial_robot_model;
 
-class GimbalrotorRobotModel : public aerial_robot_model::RobotModel{
+class GimbalrotorRobotModel : public transformable::RobotModel{
 public:
   GimbalrotorRobotModel(bool init_with_rosparam = true,
                     bool verbose = false,
diff --git a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
index 79e9fccdb..7529a146a 100644
--- a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
+++ b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp
@@ -273,7 +273,7 @@ namespace aerial_robot_control
         gimbal_state_msg.name.push_back(gimbal_pitch_name);
       }
     }
-    gimbal_state_pub_.publish(gimbal_state_msg);
+    // gimbal_state_pub_.publish(gimbal_state_msg);
 
   }
 
diff --git a/robots/gimbalrotor/src/model/gimbalrotor_robot_model.cpp b/robots/gimbalrotor/src/model/gimbalrotor_robot_model.cpp
index cbf32e178..4b8c3b35c 100644
--- a/robots/gimbalrotor/src/model/gimbalrotor_robot_model.cpp
+++ b/robots/gimbalrotor/src/model/gimbalrotor_robot_model.cpp
@@ -17,7 +17,7 @@ void GimbalrotorRobotModel::updateRobotModelImpl(const KDL::JntArray& joint_posi
   KDL::Frame f_baselink;
   fk_solver.JntToCart(joint_positions, f_baselink, getBaselinkName());
   const KDL::Rotation cog_frame = f_baselink.M * getCogDesireOrientation<KDL::Rotation>().Inverse();
-  RobotModel::updateRobotModelImpl(joint_positions);
+  transformable::RobotModel::updateRobotModelImpl(joint_positions);
   const auto seg_tf_map = getSegmentsTf();
 
   /* get local coords of thrust links */