From 4b7427e3d18c040dc3b0861a9b3229f0972cd415 Mon Sep 17 00:00:00 2001 From: Kaneko Date: Tue, 6 Aug 2024 19:22:16 +0900 Subject: [PATCH] [hydrus_controller] [hydrus_xi_navigator] modify to use the value of FTsensor instead of est_ex_wrench --- .../hydrus/hydrus_tilted_lqi_controller.h | 9 +- .../hydrus/hydrus_tilted_robot_model.h | 4 + .../src/hydrus_tilted_lqi_controller.cpp | 130 +++++++++++------- .../hydrus/src/hydrus_tilted_robot_model.cpp | 8 +- .../hydrus_xi/config/quad/FlightControl.yaml | 1 + .../config/quad/NavigationConfig.yaml | 3 +- .../hydrus_xi_under_actuated_navigation.h | 4 + .../hydrus_xi_under_actuated_navigation.cpp | 115 +++++++++------- 8 files changed, 168 insertions(+), 106 deletions(-) diff --git a/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h b/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h index 734dd2b8b..8bffc5b33 100644 --- a/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h +++ b/robots/hydrus/include/hydrus/hydrus_tilted_lqi_controller.h @@ -67,8 +67,9 @@ namespace aerial_robot_control ros::Publisher filtered_est_external_wrench_pub_; ros::Subscriber desire_wrench_sub_; ros::Subscriber desire_pos_sub_; - ros::Subscriber acc_root_sub_; ros::Subscriber hand_force_switch_sub_; + ros::Subscriber filterd_ftsensor_sub_; + Eigen::VectorXd estimated_external_wrench_in_cog_; Eigen::VectorXd desire_wrench_; Eigen::VectorXd desire_wrench_from_pos_; @@ -76,12 +77,15 @@ namespace aerial_robot_control Eigen::VectorXd p_wrench_stamp_; Eigen::VectorXd feedforward_sum_; Eigen::VectorXd desire_pos_; + Eigen::VectorXd filtered_ftsensor_wrench_; + 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 wrench_diff_gain_; double acc_shock_thres_; - double y_p_gain_; + double x_p_gain_, y_p_gain_; IirFilter lpf_est_external_wrench_; bool update() override; void controlCore() override; @@ -89,6 +93,7 @@ namespace aerial_robot_control void DesireWrenchCallback(geometry_msgs::WrenchStamped msg); void DesirePosCallback(aerial_robot_msgs::FlightNav msg); void HandForceSwitchCallBack(std_msgs::Int8 msg); + void FilterdFtsensorCallBack(geometry_msgs::WrenchStamped msg); }; }; diff --git a/robots/hydrus/include/hydrus/hydrus_tilted_robot_model.h b/robots/hydrus/include/hydrus/hydrus_tilted_robot_model.h index ef0d39b2e..cf1df92f6 100644 --- a/robots/hydrus/include/hydrus/hydrus_tilted_robot_model.h +++ b/robots/hydrus/include/hydrus/hydrus_tilted_robot_model.h @@ -51,6 +51,8 @@ class HydrusTiltedRobotModel : public HydrusRobotModel { KDL::Frame getLinkEnd() {return link_end_;} void setTargetForceInRootEnd(Eigen::Vector3d target_force) {target_force_in_root_end_ = target_force;} void setTargetForceInLinkEnd(Eigen::Vector3d target_force) {target_force_in_link_end_ = target_force;} + Eigen::Vector3d getCompensateForceForRootEndInCog() {return compensate_force_for_root_end_in_cog_;} + Eigen::Vector3d getCompensateForceForLinkEndInCog() {return compensate_force_for_link_end_in_cog_;} Eigen::Vector3d getCompensateTorqueForRootEndInCog() {return compensate_torque_for_root_end_in_cog_;} Eigen::Vector3d getCompensateTorqueForLinkEndInCog() {return compensate_torque_for_link_end_in_cog_;} @@ -63,6 +65,8 @@ class HydrusTiltedRobotModel : public HydrusRobotModel { std::string link_end_name_; Eigen::Vector3d target_force_in_root_end_; Eigen::Vector3d target_force_in_link_end_; + Eigen::Vector3d compensate_force_for_root_end_in_cog_; + Eigen::Vector3d compensate_force_for_link_end_in_cog_; Eigen::Vector3d compensate_torque_for_root_end_in_cog_; Eigen::Vector3d compensate_torque_for_link_end_in_cog_; diff --git a/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp b/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp index b75bec77d..15760211a 100644 --- a/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp +++ b/robots/hydrus/src/hydrus_tilted_lqi_controller.cpp @@ -29,10 +29,12 @@ void HydrusTiltedLQIController::initialize(ros::NodeHandle nh, attaching_flag_pub_ = nh_.advertise("attaching_flag",1); filtered_est_external_wrench_pub_ = nh_.advertise("filtered_est_external_wrench",1); desire_wrench_sub_ = nh_.subscribe("desire_wrench", 1, &HydrusTiltedLQIController::DesireWrenchCallback, this); + filterd_ftsensor_sub_ = nh_.subscribe("/filterd_ftsensor", 1, &HydrusTiltedLQIController::FilterdFtsensorCallBack, this); desire_pos_sub_ = nh_.subscribe("uav/nav", 1, &HydrusTiltedLQIController::DesirePosCallback, this); hand_force_switch_sub_ = nh_.subscribe("hand_force_switch", 1, &HydrusTiltedLQIController::HandForceSwitchCallBack, this); estimated_external_wrench_in_cog_ = Eigen::VectorXd::Zero(6); desire_wrench_ = Eigen::VectorXd::Zero(6); + filtered_ftsensor_wrench_ = Eigen::VectorXd::Zero(6); desire_wrench_from_pos_ = Eigen::VectorXd::Zero(6); target_wrench_cog_ = Eigen::VectorXd::Zero(6); p_wrench_stamp_ = Eigen::VectorXd::Zero(6); @@ -43,15 +45,16 @@ void HydrusTiltedLQIController::initialize(ros::NodeHandle nh, first_flag_ = true; ros::NodeHandle control_nh(nh_, "controller"); - getParam(control_nh, "wrench_diff_gain", wrench_diff_gain_, 1.0); getParam(control_nh, "send_feedforward_switch_flag", send_feedforward_switch_flag_, false); + getParam(control_nh, "using_FTsensor", using_FTsensor_, false); getParam(control_nh, "acc_shock_thres", acc_shock_thres_, 20.0); double cutoff_freq, sample_freq; getParam(control_nh, "cutoff_freq", cutoff_freq, 25.0); getParam(control_nh, "sample_freq", sample_freq, 100.0); lpf_est_external_wrench_ = IirFilter(sample_freq, cutoff_freq, 6); + x_p_gain_ = pid_controllers_.at(X).getPGain(); y_p_gain_ = pid_controllers_.at(Y).getPGain(); } @@ -64,10 +67,11 @@ void HydrusTiltedLQIController::DesireWrenchCallback(geometry_msgs::WrenchStampe desire_force_at_end[1] = msg.wrench.force.y; desire_force_at_end[2] = msg.wrench.force.z; hydrus_robot_model_->setTargetForceInRootEnd(desire_force_at_end); + Eigen::Vector3d des_force_for_root_end_in_cog = hydrus_robot_model_->getCompensateForceForRootEndInCog(); Eigen::Vector3d des_torque_for_root_end_in_cog = hydrus_robot_model_->getCompensateTorqueForRootEndInCog(); - KDL::Frame root_end = hydrus_robot_model_->getRootEnd(); - KDL::Frame cog = hydrus_robot_model_->getCog(); - Eigen::Vector3d des_force_for_root_end_in_cog = aerial_robot_model::kdlToEigen(cog.M.Inverse() * root_end.M) * desire_force_at_end; + // KDL::Frame root_end = hydrus_robot_model_->getRootEnd(); + // KDL::Frame cog = hydrus_robot_model_->getCog(); + // Eigen::Vector3d des_force_for_root_end_in_cog = aerial_robot_model::kdlToEigen(cog.M.Inverse() * root_end.M) * desire_force_at_end; desire_wrench_[0] = des_force_for_root_end_in_cog[0]; desire_wrench_[1] = des_force_for_root_end_in_cog[1]; @@ -77,6 +81,34 @@ void HydrusTiltedLQIController::DesireWrenchCallback(geometry_msgs::WrenchStampe desire_wrench_[5] = des_torque_for_root_end_in_cog[2]; } +void HydrusTiltedLQIController::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(); + + 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; + 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]; + filtered_ftsensor_wrench_[1] = force_for_root_end_in_cog[1]; + filtered_ftsensor_wrench_[2] = force_for_root_end_in_cog[2]; + filtered_ftsensor_wrench_[3] = torque_at_end[0]; + filtered_ftsensor_wrench_[4] = torque_at_end[1]; + filtered_ftsensor_wrench_[5] = torque_at_end[2]; + + // for(int i; i<6; i++){ + // std::cout << filtered_ftsensor_wrench_[i] << ", "; + // } + // std::cout << std::endl; +} + + void HydrusTiltedLQIController::DesirePosCallback(aerial_robot_msgs::FlightNav msg) { desire_pos_[0] = msg.target_pos_x; @@ -145,12 +177,6 @@ void HydrusTiltedLQIController::controlCore() Eigen::VectorXd filtered_est_external_wrench; filtered_est_external_wrench = lpf_est_external_wrench_.filterFunction(est_external_wrench_); - int navi_state = navigator_->getNaviState(); - tf::Vector3 pos = estimator_->getPos(Frame::COG, estimate_mode_); - tf::Vector3 euler = estimator_->getEuler(Frame::COG, estimate_mode_); - double yaw_diff = desire_pos_[5] - euler.z(); - double pos_x_diff = desire_pos_[0] - pos.x(); - double pos_y_diff = desire_pos_[1] - pos.y(); /* if(abs(pos_x_diff)<=0.1 && yaw_diff<=0.1) { @@ -165,62 +191,61 @@ void HydrusTiltedLQIController::controlCore() attaching_flag_msg.data = attaching_flag_; attaching_flag_pub_.publish(attaching_flag_msg); - double du = ros::Time::now().toSec() - control_timestamp_; + Eigen::VectorXd target_wrench_acc_cog = Eigen::VectorXd::Zero(6); tf::Matrix3x3 uav_rot = estimator_->getOrientation(Frame::COG, estimate_mode_); tf::Vector3 target_acc_w(pid_controllers_.at(X).result(), pid_controllers_.at(Y).result(), pid_controllers_.at(Z).result()); - tf::Vector3 target_acc_cog = uav_rot.inverse() * target_acc_w; - Eigen::VectorXd target_wrench_acc_cog = Eigen::VectorXd::Zero(6); - target_wrench_acc_cog.head(3) = Eigen::Vector3d(target_acc_cog.x(), target_acc_cog.y(), target_acc_cog.z()); - double target_ang_acc_x = pid_controllers_.at(ROLL).result(); double target_ang_acc_y = pid_controllers_.at(PITCH).result(); //double target_ang_acc_z = pid_controllers_.at(YAW).result(); double target_ang_acc_z = candidate_yaw_term_; + tf::Vector3 target_acc_cog = uav_rot.inverse() * target_acc_w; + target_wrench_acc_cog.head(3) = Eigen::Vector3d(target_acc_cog.x(), target_acc_cog.y(), target_acc_cog.z()); target_wrench_acc_cog.tail(3) = Eigen::Vector3d(target_ang_acc_x, target_ang_acc_y, target_ang_acc_z); /* feedforward */ double mass_inv = 1/ hydrus_robot_model_->getMass(); Eigen::Matrix3d inertia_inv = hydrus_robot_model_->getInertia().inverse(); - Eigen::Vector3d des_force,des_torque; Eigen::Matrix3d cog_rot; - Eigen::Vector3d est_external_force_cog; - Eigen::Vector3d est_external_force; - est_external_force = filtered_est_external_wrench.head(3); tf::matrixTFToEigen(estimator_->getOrientation(Frame::COG, estimate_mode_), cog_rot); - est_external_force_cog = cog_rot.inverse() * est_external_force; - for(int i;i<3;i++) - { - des_force[i] = desire_wrench_[i] + est_external_force_cog[i]; - des_torque[i] = desire_wrench_[i+3] + filtered_est_external_wrench[i+3]; - } - Eigen::Vector3d des_acc = des_force * mass_inv; - 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); + 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); + } + else + { + target_force = desire_wrench_.head(3) + cog_rot.inverse() * filtered_est_external_wrench.head(3); + target_torque = desire_wrench_.tail(3) + cog_rot.inverse() * filtered_est_external_wrench.tail(3); + } + + Eigen::Vector3d target_acc = mass_inv * target_force; + Eigen::Vector3d target_ang_acc = inertia_inv * target_torque; + Eigen::Vector3d feedforward_acc = cog_rot * (target_acc + feedforward_sum_.head(3)); + Eigen::Vector3d feedforward_ang_acc = cog_rot * (target_ang_acc + feedforward_sum_.head(3)); if(send_feedforward_switch_flag_ && attaching_flag_) { - // 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_; + // target_pitch_ += target_acc[0]; + // target_roll_ += target_acc[1]; + navigator_->setTargetAccX(feedforward_acc[0]); + navigator_->setTargetAccY(feedforward_acc[1]); + // navigator_->setTargetAngAccZ(feedforward_ang_acc[2]); + target_wrench_acc_cog[0] += feedforward_acc[0]; + target_wrench_acc_cog[1] += feedforward_acc[1]; + // target_wrench_acc_cog[5] += feedforward_ang_acc[2]; + + feedforward_sum_.head(3) += target_acc * wrench_diff_gain_; + feedforward_sum_.tail(3) += target_ang_acc * wrench_diff_gain_; std::cout << "send_feedforward" << std::endl; } if(!attaching_flag_) { - // navigator_->setTargetAccX(0); + navigator_->setTargetAccX(0); navigator_->setTargetAccY(0); // navigator_->setTargetAngAccZ(0); feedforward_sum_ = Eigen::VectorXd::Zero(6); @@ -238,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(); + x_p_gain_ = pid_controllers_.at(X).getPGain(); y_p_gain_ = pid_controllers_.at(Y).getPGain(); //err_p_y_ = pid_controllers_.at(Y).getErrP(); const_err_i_flag_ = true; @@ -246,10 +272,12 @@ void HydrusTiltedLQIController::controlCore() pid_controllers_.at(Y).setErrI(err_i_y_); pid_controllers_.at(Z).setErrI(err_i_z_); //pid_controllers_.at(Y).setErrP(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(Y).setPGain(y_p_gain_); } @@ -260,18 +288,18 @@ void HydrusTiltedLQIController::controlCore() geometry_msgs::Vector3Stamped feedforward_ang_acc_cog_msg; geometry_msgs::WrenchStamped des_wrench_cog_msg; geometry_msgs::WrenchStamped filtered_est_external_wrench_msg; - 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_acc_cog_msg.vector.x = feedforward_acc[0]; + feedforward_acc_cog_msg.vector.y = feedforward_acc[1]; + feedforward_acc_cog_msg.vector.z = feedforward_acc[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]; - des_wrench_cog_msg.wrench.torque.x = des_torque[0]; - des_wrench_cog_msg.wrench.torque.y = des_torque[1]; - des_wrench_cog_msg.wrench.torque.z = des_torque[2]; + des_wrench_cog_msg.wrench.force.x = target_force[0]; + des_wrench_cog_msg.wrench.force.y = target_force[1]; + des_wrench_cog_msg.wrench.force.z = target_force[2]; + des_wrench_cog_msg.wrench.torque.x = target_torque[0]; + des_wrench_cog_msg.wrench.torque.y = target_torque[1]; + des_wrench_cog_msg.wrench.torque.z = target_torque[2]; filtered_est_external_wrench_msg.wrench.force.x = filtered_est_external_wrench[0]; filtered_est_external_wrench_msg.wrench.force.y = filtered_est_external_wrench[1]; filtered_est_external_wrench_msg.wrench.force.z = filtered_est_external_wrench[2]; diff --git a/robots/hydrus/src/hydrus_tilted_robot_model.cpp b/robots/hydrus/src/hydrus_tilted_robot_model.cpp index 42e0be1f9..21b3236c4 100644 --- a/robots/hydrus/src/hydrus_tilted_robot_model.cpp +++ b/robots/hydrus/src/hydrus_tilted_robot_model.cpp @@ -56,10 +56,10 @@ void HydrusTiltedRobotModel::updateRobotModelImpl(const KDL::JntArray& joint_pos { ROS_WARN_STREAM_ONCE("[model] there is a end-effector named " << root_end_name_); root_end_ = seg_tf_map.at(root_end_name_); - Eigen::Vector3d compensate_force_for_root_end_in_cog = aerial_robot_model::kdlToEigen(cog.M.Inverse() * root_end_.M) * target_force_in_root_end_; + compensate_force_for_root_end_in_cog_ = aerial_robot_model::kdlToEigen(cog.M.Inverse() * root_end_.M) * target_force_in_root_end_; KDL::Frame cog_to_root_end = cog.Inverse() * root_end_; Eigen::MatrixXd cog_to_root_end_skew = aerial_robot_model::skew(Eigen::Vector3d(cog_to_root_end.p.x(), cog_to_root_end.p.y(), cog_to_root_end.p.z())); - compensate_torque_for_root_end_in_cog_ = cog_to_root_end_skew * compensate_force_for_root_end_in_cog; + compensate_torque_for_root_end_in_cog_ = cog_to_root_end_skew * compensate_force_for_root_end_in_cog_; } else { @@ -70,10 +70,10 @@ void HydrusTiltedRobotModel::updateRobotModelImpl(const KDL::JntArray& joint_pos { ROS_WARN_STREAM_ONCE("[model] there is a end-effector named " << link_end_name_); link_end_ = seg_tf_map.at(link_end_name_); - Eigen::Vector3d compensate_force_for_link_end_in_cog = aerial_robot_model::kdlToEigen(cog.M.Inverse() * link_end_.M) * target_force_in_link_end_; + compensate_force_for_link_end_in_cog_ = aerial_robot_model::kdlToEigen(cog.M.Inverse() * link_end_.M) * target_force_in_link_end_; KDL::Frame cog_to_link_end = cog.Inverse() * link_end_; Eigen::MatrixXd cog_to_link_end_skew = aerial_robot_model::skew(Eigen::Vector3d(cog_to_link_end.p.x(), cog_to_link_end.p.y(), cog_to_link_end.p.z())); - compensate_torque_for_link_end_in_cog_ = cog_to_link_end_skew * compensate_force_for_link_end_in_cog; + compensate_torque_for_link_end_in_cog_ = cog_to_link_end_skew * compensate_force_for_link_end_in_cog_; } else { diff --git a/robots/hydrus_xi/config/quad/FlightControl.yaml b/robots/hydrus_xi/config/quad/FlightControl.yaml index 5a9559bbb..664220a84 100644 --- a/robots/hydrus_xi/config/quad/FlightControl.yaml +++ b/robots/hydrus_xi/config/quad/FlightControl.yaml @@ -8,6 +8,7 @@ controller: wrench_diff_gain: 0.01 wrench_estimate_flag: true send_feedforward_switch_flag: true + using_FTsensor: true acc_shock_thres: 18.0 cutoff_freq: 25.0 diff --git a/robots/hydrus_xi/config/quad/NavigationConfig.yaml b/robots/hydrus_xi/config/quad/NavigationConfig.yaml index 0f41dd408..fe28ea83f 100644 --- a/robots/hydrus_xi/config/quad/NavigationConfig.yaml +++ b/robots/hydrus_xi/config/quad/NavigationConfig.yaml @@ -33,7 +33,8 @@ navigation: # gimbal vectoring planner plan_verbose: false maximize_yaw: false # true: maximize min yaw torque, false: maximize feasible control torque convex - optimize_wide_x: true + optimize_wide_x: false + using_FTsensor: true plan_freq: 20.0 baselink_rot_thresh: 0.01 gimbal_delta_angle: 0.2 diff --git a/robots/hydrus_xi/include/hydrus_xi/hydrus_xi_under_actuated_navigation.h b/robots/hydrus_xi/include/hydrus_xi/hydrus_xi_under_actuated_navigation.h index 1d7d7c79e..70ad6107a 100644 --- a/robots/hydrus_xi/include/hydrus_xi/hydrus_xi_under_actuated_navigation.h +++ b/robots/hydrus_xi/include/hydrus_xi/hydrus_xi_under_actuated_navigation.h @@ -73,8 +73,10 @@ namespace aerial_robot_navigation const std::vector& getControlIndices() const { return control_gimbal_indices_; } const Eigen::VectorXd getDesireWrench() const { return desire_wrench_; } const Eigen::VectorXd getEstExternalWrench() const { return est_external_wrench_; } + const Eigen::VectorXd getFilterdFtsensorWrench() const { return filtered_ftsensor_wrench_; } const bool getPlanVerbose() const { return plan_verbose_; } + const bool getUsingFTsensor() const { return using_FTsensor_; } void setMaxMinYaw(const double max_min_yaw) { max_min_yaw_ = max_min_yaw;} @@ -99,6 +101,7 @@ namespace aerial_robot_navigation bool plan_verbose_; bool maximize_yaw_; bool optimize_wide_x_; // optimize 8d x include thrusts + bool using_FTsensor_; double force_norm_weight_; // cost func double force_variant_weight_; // cost func double yaw_torque_weight_; // cost func @@ -116,6 +119,7 @@ namespace aerial_robot_navigation Eigen::VectorXd filtered_ftsensor_wrench_; bool attaching_flag_; + void threadFunc(); bool plan(); void DesireWrenchCallback(geometry_msgs::WrenchStamped msg); 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 a0a6eb85b..3f985281d 100644 --- a/robots/hydrus_xi/src/hydrus_xi_under_actuated_navigation.cpp +++ b/robots/hydrus_xi/src/hydrus_xi_under_actuated_navigation.cpp @@ -187,20 +187,6 @@ namespace return planner->getFCTMinThresh() - planner->getRobotModelForPlan()->getFeasibleControlTMin(); } - // void thrustLimitConstraint(unsigned m, double* result, unsigned n, const double* x, double *gradient, void *planner_ptr) - // { - // HydrusXiUnderActuatedNavigator *planner = reinterpret_cast(planner_ptr); - // auto robot_model = planner->getRobotModelForPlan(); - // robot_model->calc3DoFThrust(planner->ff_f_xy_[0], planner->ff_f_xy_[1]); - // auto thrust = robot_model->get3DoFThrust(); - // Eigen::VectorXd ret_e(8); - // ret_e << thrust-Eigen::VectorXd::Constant(4, robot_model->getThrustUpperLimit()), Eigen::VectorXd::Constant(4, 8.9)-thrust; - - // for (int i=0; i(planner_ptr); @@ -217,35 +203,42 @@ namespace auto Q = robot_model->calcWrenchMatrixOnCoG(); Eigen::VectorXd desire_wrench = planner->getDesireWrench(); Eigen::VectorXd est_external_wrench = planner->getEstExternalWrench(); - - //double mass_inv = 1/ robot_model_for_plan_->getMass(); - //Eigen::Matrix3d inertia_inv = robot_model_for_plan_->getInertia().inverse(); - Eigen::Vector3d des_force,des_torque; - Eigen::Matrix3d cog_rot; - Eigen::Vector3d est_external_force_cog, des_force_cog; - Eigen::Vector3d est_external_force = est_external_wrench.head(3); - - tf::matrixTFToEigen(planner->getEstimator()->getOrientation(Frame::COG, planner->getEstimateMode()), cog_rot); - est_external_force_cog = cog_rot.inverse() * est_external_force; - des_force_cog = cog_rot.inverse() * desire_wrench.head(3); - des_force = des_force_cog + est_external_force_cog; - des_torque = desire_wrench.tail(3) + est_external_wrench.tail(3); + Eigen::VectorXd filtered_ftsensor_wrench = planner->getFilterdFtsensorWrench(); + bool using_FTsensor = planner->getUsingFTsensor(); + + // Eigen::Vector3d des_force,des_torque; + // Eigen::Matrix3d cog_rot; + // tf::matrixTFToEigen(planner->getEstimator()->getOrientation(Frame::COG, planner->getEstimateMode()), cog_rot); + // Eigen::Vector3d est_external_force_cog, des_force_cog; + // Eigen::Vector3d est_external_force = est_external_wrench.head(3); + + // est_external_force_cog = cog_rot.inverse() * est_external_force; + // des_force_cog = cog_rot.inverse() * desire_wrench.head(3); + // des_force = des_force_cog + est_external_force_cog; + // des_torque = desire_wrench.tail(3) + est_external_wrench.tail(3); - Eigen::VectorXd thrusts(4), wrench_des(6), yaw_comp(6); + // Eigen::VectorXd thrusts(4), wrench_des(6), yaw_comp(6); // 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); + // // thrusts << robot_model->getStaticThrust(); + // 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]; - //test 4 July - des_wrench_cog << est_external_force_cog[0], est_external_force_cog[1], robot_model->getMass()*robot_model->getGravity()[2], 0, 0, est_external_wrench[5]; + Eigen::VectorXd thrusts(4), target_wrench(6); + thrusts << x[4], x[5], x[6], x[7]; + if(using_FTsensor) + { + target_wrench << desire_wrench - filtered_ftsensor_wrench; + } + else + { + target_wrench << desire_wrench - est_external_wrench; + } + target_wrench[3] = 0.0; + target_wrench[4] = 0.0; //std::vector res(m, 0); // auto res = Q*thrusts - robot_model->getMass()*wrench_des - yaw_comp; - auto res = Q*thrusts - des_wrench_cog; + auto res = Q*thrusts - target_wrench; for (int i = 0; i < m; i++) { result[i] = res[i]; } @@ -584,12 +577,25 @@ void HydrusXiUnderActuatedNavigator::DesireWrenchCallback(geometry_msgs::WrenchS void HydrusXiUnderActuatedNavigator::EstExternalWrenchCallBack(geometry_msgs::WrenchStamped msg) { - est_external_wrench_[0] = msg.wrench.force.x; - est_external_wrench_[1] = msg.wrench.force.y; - est_external_wrench_[2] = msg.wrench.force.z; - est_external_wrench_[3] = msg.wrench.torque.x; - est_external_wrench_[4] = msg.wrench.torque.y; - est_external_wrench_[5] = msg.wrench.torque.z; + Eigen::Matrix3d cog_rot; + tf::matrixTFToEigen(estimator_->getOrientation(Frame::COG, estimator_->getEstimateMode()), cog_rot); + Eigen::Vector3d est_external_force, est_external_force_cog; + est_external_force[0] = msg.wrench.force.x; + est_external_force[1] = msg.wrench.force.y; + est_external_force[2] = msg.wrench.force.z; + est_external_force_cog = cog_rot.inverse() * est_external_force; + Eigen::Vector3d est_external_torque, est_external_torque_cog; + est_external_torque[0] = msg.wrench.torque.x; + est_external_torque[1] = msg.wrench.torque.y; + est_external_torque[2] = msg.wrench.torque.z; + est_external_torque_cog = cog_rot.inverse() * est_external_torque; + + for(int i;i<3;i++) + { + est_external_wrench_[i] = est_external_force_cog[i]; + est_external_wrench_[i+3] = est_external_torque_cog[i]; + } + for(int i;i=15){est_external_wrench_[i]=15;} @@ -604,12 +610,24 @@ void HydrusXiUnderActuatedNavigator::AttachingFlagCallBack(std_msgs::Bool msg) void HydrusXiUnderActuatedNavigator::FilterdFtsensorCallBack(geometry_msgs::WrenchStamped msg) { - filtered_ftsensor_wrench_[0] = msg.wrench.force.x; - filtered_ftsensor_wrench_[1] = msg.wrench.force.y; - filtered_ftsensor_wrench_[2] = msg.wrench.force.z; - filtered_ftsensor_wrench_[3] = msg.wrench.torque.x; - filtered_ftsensor_wrench_[4] = msg.wrench.torque.y; - filtered_ftsensor_wrench_[5] = msg.wrench.torque.z; + Eigen::Vector3d force_at_end, torque_at_end; + KDL::Frame root_end = hydrus_robot_model_->getRootEnd(); + KDL::Frame cog = hydrus_robot_model_->getCog(); + + 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; + 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]; + filtered_ftsensor_wrench_[1] = force_for_root_end_in_cog[1]; + filtered_ftsensor_wrench_[2] = force_for_root_end_in_cog[2]; + filtered_ftsensor_wrench_[3] = torque_at_end[0]; + filtered_ftsensor_wrench_[4] = torque_at_end[1]; + filtered_ftsensor_wrench_[5] = torque_at_end[2]; } @@ -621,6 +639,7 @@ void HydrusXiUnderActuatedNavigator::rosParamInit() getParam(navi_nh, "plan_verbose", plan_verbose_, false); getParam(navi_nh, "maximize_yaw", maximize_yaw_, false); getParam(navi_nh, "optimize_wide_x", optimize_wide_x_, false); + getParam(navi_nh, "using_FTsensor", using_FTsensor_, false); getParam(navi_nh, "gimbal_delta_angle", gimbal_delta_angle_, 0.2); getParam(navi_nh, "force_norm_rate", force_norm_weight_, 2.0); getParam(navi_nh, "force_variant_rate", force_variant_weight_, 0.01);