[hydrus] [feedforward]

modefied feedforward term

avatar/scripts/real_avatar_mocap_control.py

@@ -22,7 +22,7 @@ def __init__(self):
     if self.real_machine == True:
       topic_name = '/avatar_mocap_node/avatar/pose'
 
-    self.pos_scaling = rospy.get_param("~pos_scaling", 1.1)
+    self.pos_scaling = rospy.get_param("~pos_scaling", 1.5)
     self.period = rospy.get_param("~period", 40.0)
     self.radius = rospy.get_param("~radius", 1.0)
     self.init_theta = rospy.get_param("~init_theta", 0.0)

avatar/scripts/real_avatar_position_control.py

@@ -233,7 +233,7 @@ def main(self):
             desire_wrench_msg.wrench.force.x = 3.0
             desire_wrench_msg.wrench.force.y = 0.0
             desire_wrench_msg.wrench.force.z = 0.0
-            self.des_wrench_pub.publish(desire_wrench_msg)
+            #self.des_wrench_pub.publish(desire_wrench_msg)
 
             # send joint angles
             self.joint_control_pub.publish(self.desire_joint)

robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h

@@ -78,7 +78,7 @@ namespace aerial_robot_control
     Eigen::VectorXd desire_wrench_from_pos_;
     Eigen::VectorXd target_wrench_cog_;
     Eigen::VectorXd p_wrench_stamp_;
-    Eigen::VectorXd feedforward_term_;
+    Eigen::VectorXd feedforward_sum_;
     Eigen::VectorXd desire_pos_;
     bool send_feedforward_switch_flag_;
     bool attaching_flag_, const_err_i_flag_;

robots/hydrus/src/hydrus_tilted_lqi_controller.cpp

@@ -41,7 +41,7 @@ void HydrusTiltedLQIController::initialize(ros::NodeHandle nh,
   desire_wrench_from_pos_ = Eigen::VectorXd::Zero(6);
   target_wrench_cog_ = Eigen::VectorXd::Zero(6);
   p_wrench_stamp_ = Eigen::VectorXd::Zero(6);
-  feedforward_term_ = Eigen::VectorXd::Zero(6);
+  feedforward_sum_ = Eigen::VectorXd::Zero(6);
   desire_pos_ = Eigen::Vector3d::Zero(6);
   attaching_flag_ = false;
   const_err_i_flag_ = false;
@@ -381,31 +381,48 @@ void HydrusTiltedLQIController::controlCore()
     }
 
   Eigen::Vector3d des_acc = des_force * mass_inv;
-  Eigen::Vector3d des_acc_ang = inertia_inv * des_torque * wrench_diff_gain_;
+  Eigen::Vector3d des_ang_acc = inertia_inv * des_torque;
   //ROS_INFO("send_feedforward_switch_flag: %d", send_feedforward_switch_flag_);
+  Eigen::Vector3d feedforward_sum_3 = feedforward_sum_.head(3);
+  Eigen::Vector3d feedforward_world = cog_rot * (des_acc + feedforward_sum_3);
+
   if(send_feedforward_switch_flag_ && attaching_flag_)
   {
-    target_acc_.setX(des_acc[0]+feedforward_term_[0]);
-    target_acc_.setY(des_acc[1]+feedforward_term_[1]);
-    //target_acc_.setZ(des_acc[2]+feedforward_term_[2]);
-    target_ang_acc_.setZ(des_acc_ang[2]+feedforward_term_[5]);
-    target_wrench_acc_cog[0] += des_acc[0]+feedforward_term_[0];
-    target_wrench_acc_cog[1] += des_acc[1]+feedforward_term_[1];
-    target_wrench_acc_cog[5] += des_acc_ang[2]+feedforward_term_[5];
+    // target_pitch_ += des_acc[0];
+    // target_roll_ += des_acc[1];
+    navigator_->setTargetAccX(feedforward_world[0]);
+    navigator_->setTargetAccY(feedforward_world[1]);
+    navigator_->setTargetAngAccZ(des_ang_acc[2] + feedforward_sum_[5]);
+    target_wrench_acc_cog[0] += feedforward_world[0];
+    target_wrench_acc_cog[1] += feedforward_world[1];
+    target_wrench_acc_cog[5] += des_ang_acc[2] + feedforward_sum_[5];
+
+    feedforward_sum_.head(3) += des_acc * wrench_diff_gain_;
+    feedforward_sum_.tail(3) += des_ang_acc * wrench_diff_gain_;
+
     std::cout << "send_feedforward" << std::endl;
   }
-  feedforward_term_.head(3) += des_acc;
-  feedforward_term_.tail(3) += des_acc_ang;
+  if(!attaching_flag_)
+  {
+    navigator_->setTargetAccX(0);
+    navigator_->setTargetAccY(0);
+    navigator_->setTargetAngAccZ(0);
+    feedforward_sum_ = Eigen::VectorXd::Zero(6);
+  }
+  if(pid_controllers_.at(X).result()<0.0)
+  {
+    //attaching_flag_ = false;
+  }
 
   geometry_msgs::Vector3Stamped feedforward_acc_cog_msg;
   geometry_msgs::Vector3Stamped feedforward_ang_acc_cog_msg;
   geometry_msgs::WrenchStamped des_wrench_cog_msg;
-  feedforward_acc_cog_msg.vector.x = des_acc[0];
-  feedforward_acc_cog_msg.vector.y = des_acc[1];
-  feedforward_acc_cog_msg.vector.z = des_acc[2];
-  feedforward_ang_acc_cog_msg.vector.x = des_acc_ang[0];
-  feedforward_ang_acc_cog_msg.vector.y = des_acc_ang[1];
-  feedforward_ang_acc_cog_msg.vector.z = des_acc_ang[2];
+  feedforward_acc_cog_msg.vector.x = feedforward_world[0];
+  feedforward_acc_cog_msg.vector.y = feedforward_world[1];
+  feedforward_acc_cog_msg.vector.z = feedforward_world[2];
+  feedforward_ang_acc_cog_msg.vector.x = feedforward_sum_[3];
+  feedforward_ang_acc_cog_msg.vector.y = feedforward_sum_[4];
+  feedforward_ang_acc_cog_msg.vector.z = feedforward_sum_[5];
   des_wrench_cog_msg.wrench.force.x = des_force[0];
   des_wrench_cog_msg.wrench.force.y = des_force[1];
   des_wrench_cog_msg.wrench.force.z = des_force[2];

robots/hydrus_xi/config/quad/FlightControl.yaml

@@ -5,7 +5,7 @@ controller:
   momentum_observer_force_weight: 10
   momentum_observer_torque_weight: 10
 
-  wrench_diff_gain: 1.0
+  wrench_diff_gain: 0.03
   wrench_estimate_flag: true
   send_feedforward_switch_flag: true
   acc_shock_thres: 18.0 

robots/hydrus_xi/src/hydrus_xi_under_actuated_navigation.cpp

@@ -72,8 +72,8 @@ namespace
 
     // Thrust norm/var
     Eigen::VectorXd force_v(4);
-    force_v << x_wide[4], x_wide[5], x_wide[6], x_wide[7];
-    //force_v = robot_model->getStaticThrust();
+    //force_v << x_wide[4], x_wide[5], x_wide[6], x_wide[7];
+    force_v = robot_model->getStaticThrust();
     double average_force = force_v.sum() / force_v.size();
     double variant = 0;
 
@@ -207,26 +207,24 @@ namespace
     Eigen::Vector3d des_force,des_torque;
     Eigen::Matrix3d cog_rot;
     Eigen::Vector3d est_external_wrench_cog;
-    Eigen::Vector3d est_external_force;
-    for(int i;i<3;i++)
-    {
-      est_external_force[i] = est_external_wrench[i];
-    }
+    Eigen::Vector3d est_external_force = est_external_wrench.head(3);
+
     tf::matrixTFToEigen(planner->getEstimator()->getOrientation(Frame::COG, planner->getEstimateMode()), cog_rot);
     est_external_wrench_cog = cog_rot.inverse() * est_external_force;
-    for(int i;i<3;i++)
-      {
-        des_force[i] = desire_wrench[i] + est_external_wrench_cog[i];
-        des_torque[i] = desire_wrench[i+3] + est_external_wrench[i+3];
-      }
-
+    des_force = desire_wrench.head(3) + est_external_wrench_cog;
+    des_torque = desire_wrench.tail(3) + est_external_wrench.tail(3);
+
     Eigen::VectorXd thrusts(4), wrench_des(6), yaw_comp(6);
-    thrusts << x[4], x[5], x[6], x[7];
+    // thrusts << x[4], x[5], x[6], x[7];
+    thrusts << robot_model->getStaticThrust();
     //wrench_des << desire_wrench[0], desire_wrench[1], robot_model->getGravity()[2], 0, 0, desire_wrench[5];
     wrench_des << des_force[0], des_force[1], robot_model->getGravity()[2], 0, 0, des_torque[2];
     yaw_comp << 0, 0, 0, 0, 0, 0;
+    Eigen::VectorXd des_wrench_cog(6);
+    des_wrench_cog << des_force[0], des_force[1], robot_model->getMass()*robot_model->getGravity()[2], 0, 0, des_torque[2]; 
     //std::vector<double> res(m, 0);
-    auto res = Q*thrusts - robot_model->getMass()*wrench_des - yaw_comp;
+    // auto res = Q*thrusts - robot_model->getMass()*wrench_des - yaw_comp;
+    auto res = Q*thrusts - des_wrench_cog;
     for (int i = 0; i < m; i++) {
       result[i] = res[i];
     }
@@ -415,7 +413,7 @@ bool HydrusXiUnderActuatedNavigator::plan()
             ub.at(i) = opt_gimbal_angles_.at(i) + delta_angle;
             lb_wide.at(i) = opt_gimbal_angles_.at(i) - delta_angle;
             ub_wide.at(i) = opt_gimbal_angles_.at(i) + delta_angle;
-            lb_wide.at(4+i) = 8.9;
+            lb_wide.at(4+i) = 8.6;
             ub_wide.at(4+i) = robot_model_->getThrustUpperLimit();
          }
       /*avoid over angle (hard cording)*/
@@ -468,6 +466,12 @@ bool HydrusXiUnderActuatedNavigator::plan()
             }
         }
     }
+
+  Eigen::VectorXd static_thrust = robot_model_->getStaticThrust();
+  for(int i;i<4;i++)
+  {
+    opt_x_.at(i+4) = static_thrust[i];
+  }
 
   vectoring_nl_solver_->set_lower_bounds(lb);
   vectoring_nl_solver_->set_upper_bounds(ub);