[force_sensor] [controller] [navigator]

some debug
ready for experiment on real machineTue Aug  6 20:33:02 2024

avatar/scripts/lpf_for_FTsensor.py

@@ -39,7 +39,7 @@ def call_calib(self):
       service = rospy.ServiceProxy('/cfs_sensor_calib', Empty)
       response = service()
     except rospy.ServiceException:
-      print('cfs_calib service call failed')
+      print('cfs_calib service calling failed')
 
 
   def main(self):
@@ -57,12 +57,12 @@ def main(self):
       for i in range (len(self.raw_val)):
         self.filterd_val[i] = (1-self.delay_param) * self.filterd_val[i] + self.delay_param * self.raw_val[i]
 
-      self.filterd_val_msg.wrench.force.x = self.filterd_val[0]
-      self.filterd_val_msg.wrench.force.y = self.filterd_val[1]
-      self.filterd_val_msg.wrench.force.z = self.filterd_val[2]
-      self.filterd_val_msg.wrench.torque.x = self.filterd_val[3]
-      self.filterd_val_msg.wrench.torque.y = self.filterd_val[4]
-      self.filterd_val_msg.wrench.torque.z = self.filterd_val[5]
+      self.filterd_val_msg.wrench.force.x = self.filterd_val[2]
+      self.filterd_val_msg.wrench.force.y = -self.filterd_val[0]
+      self.filterd_val_msg.wrench.force.z = -self.filterd_val[1]
+      self.filterd_val_msg.wrench.torque.x = self.filterd_val[5]
+      self.filterd_val_msg.wrench.torque.y = -self.filterd_val[3]
+      self.filterd_val_msg.wrench.torque.z = -self.filterd_val[4]
 
       self.filterd_val_pub.publish(self.filterd_val_msg)
 

robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h

@@ -82,7 +82,7 @@ namespace aerial_robot_control
     bool send_feedforward_switch_flag_;
     bool using_FTsensor_;
     bool attaching_flag_, const_err_i_flag_, first_flag_;
-    double err_i_x_, err_i_y_, err_i_z_, err_p_y_;
+    double err_i_x_, err_i_y_, err_i_z_, err_i_yaw_, err_p_y_;
     double wrench_diff_gain_;
     double acc_shock_thres_;
     double x_p_gain_, y_p_gain_;

robots/hydrus/src/hydrus_tilted_lqi_controller.cpp

@@ -87,12 +87,12 @@ void HydrusTiltedLQIController::FilterdFtsensorCallBack(geometry_msgs::WrenchSta
   KDL::Frame root_end = hydrus_robot_model_->getRootEnd();
   KDL::Frame cog = hydrus_robot_model_->getCog<KDL::Frame>();
 
-  force_at_end[0] = msg.wrench.force.z;
-  force_at_end[1] = msg.wrench.force.x;
-  force_at_end[2] = -msg.wrench.force.y;
-  torque_at_end[0] = msg.wrench.torque.z;
-  torque_at_end[1] = msg.wrench.torque.x;
-  torque_at_end[2] = -msg.wrench.torque.y;
+  force_at_end[0] = msg.wrench.force.x;
+  force_at_end[1] = msg.wrench.force.y;
+  force_at_end[2] = msg.wrench.force.z;
+  torque_at_end[0] = msg.wrench.torque.x;
+  torque_at_end[1] = msg.wrench.torque.y;
+  torque_at_end[2] = msg.wrench.torque.z;
   Eigen::Vector3d force_for_root_end_in_cog = aerial_robot_model::kdlToEigen(cog.M.Inverse() * root_end.M) * force_at_end;
 
   filtered_ftsensor_wrench_[0] = force_for_root_end_in_cog[0];
@@ -213,8 +213,8 @@ void HydrusTiltedLQIController::controlCore()
   Eigen::Vector3d target_force, target_torque;
   if(using_FTsensor_)
   {
-  target_force = desire_wrench_.head(3) + cog_rot.inverse() * filtered_ftsensor_wrench_.head(3);
-  target_torque = desire_wrench_.tail(3) + cog_rot.inverse() * filtered_ftsensor_wrench_.tail(3);
+  target_force = desire_wrench_.head(3) + filtered_ftsensor_wrench_.head(3);
+  target_torque = desire_wrench_.tail(3) + filtered_ftsensor_wrench_.tail(3);
   }
   else
   {
@@ -263,6 +263,7 @@ void HydrusTiltedLQIController::controlCore()
           err_i_x_ = pid_controllers_.at(X).getErrI();
           err_i_y_ = pid_controllers_.at(Y).getErrI();
           err_i_z_ = pid_controllers_.at(Z).getErrI();
+          err_i_yaw_ = pid_controllers_.at(YAW).getErrI();
           x_p_gain_ = pid_controllers_.at(X).getPGain();
           y_p_gain_ = pid_controllers_.at(Y).getPGain();
           //err_p_y_ = pid_controllers_.at(Y).getErrP();
@@ -271,14 +272,16 @@ void HydrusTiltedLQIController::controlCore()
       pid_controllers_.at(X).setErrI(err_i_x_);
       pid_controllers_.at(Y).setErrI(err_i_y_);
       pid_controllers_.at(Z).setErrI(err_i_z_);
+      pid_controllers_.at(YAW).setErrI(err_i_yaw_);
       //pid_controllers_.at(Y).setErrP(0);
-      pid_controllers_.at(X).setPGain(0.0);
+      // pid_controllers_.at(X).setPGain(0.0);
       pid_controllers_.at(Y).setPGain(0.0);
     }
   if(!attaching_flag_)
   {
-    pid_controllers_.at(X).setPGain(x_p_gain_);
+    // pid_controllers_.at(X).setPGain(x_p_gain_);
     pid_controllers_.at(Y).setPGain(y_p_gain_);
+    const_err_i_flag_ = false;
   }
 
   UnderActuatedTiltedLQIController::controlCore();

robots/hydrus_xi/config/quad/NavigationConfig.yaml

@@ -33,7 +33,7 @@ navigation:
   # gimbal vectoring planner
   plan_verbose: false
   maximize_yaw: false # true: maximize min yaw torque, false: maximize feasible control torque convex
-  optimize_wide_x: false
+  optimize_wide_x: true
   using_FTsensor: true
   plan_freq: 20.0
   baselink_rot_thresh: 0.01

robots/hydrus_xi/src/hydrus_xi_under_actuated_navigation.cpp

@@ -611,15 +611,15 @@ void HydrusXiUnderActuatedNavigator::AttachingFlagCallBack(std_msgs::Bool msg)
 void HydrusXiUnderActuatedNavigator::FilterdFtsensorCallBack(geometry_msgs::WrenchStamped msg)
 {
   Eigen::Vector3d force_at_end, torque_at_end;
-  KDL::Frame root_end = hydrus_robot_model_->getRootEnd();
-  KDL::Frame cog = hydrus_robot_model_->getCog<KDL::Frame>();
-
-  force_at_end[0] = msg.wrench.force.z;
-  force_at_end[1] = msg.wrench.force.x;
-  force_at_end[2] = -msg.wrench.force.y;
-  torque_at_end[0] = msg.wrench.torque.z;
-  torque_at_end[1] = msg.wrench.torque.x;
-  torque_at_end[2] = -msg.wrench.torque.y;
+  KDL::Frame root_end = robot_model_for_plan_->getRootEnd();
+  KDL::Frame cog = robot_model_for_plan_->getCog<KDL::Frame>();
+
+  force_at_end[0] = msg.wrench.force.x;
+  force_at_end[1] = msg.wrench.force.y;
+  force_at_end[2] = msg.wrench.force.z;
+  torque_at_end[0] = msg.wrench.torque.x;
+  torque_at_end[1] = msg.wrench.torque.y;
+  torque_at_end[2] = msg.wrench.torque.z;
   Eigen::Vector3d force_for_root_end_in_cog = aerial_robot_model::kdlToEigen(cog.M.Inverse() * root_end.M) * force_at_end;
 
   filtered_ftsensor_wrench_[0] = force_for_root_end_in_cog[0];