From b2875e46aeb19385c106e8509c9b59dd2af57135 Mon Sep 17 00:00:00 2001 From: Kaneko Date: Sat, 23 Dec 2023 19:31:21 +0900 Subject: [PATCH] [hydrus] [feedforward] modefied feedforward term --- avatar/scripts/real_avatar_mocap_control.py | 2 +- .../scripts/real_avatar_position_control.py | 2 +- .../hydrus/hydrus_tilted_lqi_controller.h | 2 +- .../src/hydrus_tilted_lqi_controller.cpp | 51 ++++++++++++------- .../hydrus_xi/config/quad/FlightControl.yaml | 2 +- .../hydrus_xi_under_actuated_navigation.cpp | 36 +++++++------ 6 files changed, 58 insertions(+), 37 deletions(-) diff --git a/avatar/scripts/real_avatar_mocap_control.py b/avatar/scripts/real_avatar_mocap_control.py index e70239b65..c8b28c28b 100755 --- a/avatar/scripts/real_avatar_mocap_control.py +++ b/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) diff --git a/avatar/scripts/real_avatar_position_control.py b/avatar/scripts/real_avatar_position_control.py index 1bbedb180..885046abd 100755 --- a/avatar/scripts/real_avatar_position_control.py +++ b/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) diff --git a/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h b/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h index 90f623cd2..50d64c99e 100644 --- a/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h +++ b/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_; diff --git a/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp b/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp index 47c1822df..a9e219145 100644 --- a/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp +++ b/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]; diff --git a/robots/hydrus_xi/config/quad/FlightControl.yaml b/robots/hydrus_xi/config/quad/FlightControl.yaml index 70f1b4e80..18a960c41 100644 --- a/robots/hydrus_xi/config/quad/FlightControl.yaml +++ b/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 diff --git a/robots/hydrus_xi/src/hydrus_xi_under_actuated_navigation.cpp b/robots/hydrus_xi/src/hydrus_xi_under_actuated_navigation.cpp index b23b5020a..48c7b5c69 100644 --- a/robots/hydrus_xi/src/hydrus_xi_under_actuated_navigation.cpp +++ b/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 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);