From 21d2d1cc5349e8077588d9234e7f000234620b86 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sat, 27 Apr 2024 17:47:15 +0900 Subject: [PATCH] [SPIDAR][Control] remove unnecessary part, especially about the joint control --- .../octo/control/WalkControlConfig.yaml | 4 - .../include/spidar/control/walk_control.h | 7 - robots/spidar/src/control/walk_control.cpp | 390 ++++-------------- 3 files changed, 83 insertions(+), 318 deletions(-) diff --git a/robots/spidar/config/octo/control/WalkControlConfig.yaml b/robots/spidar/config/octo/control/WalkControlConfig.yaml index fae5a371e1..d7827a3437 100644 --- a/robots/spidar/config/octo/control/WalkControlConfig.yaml +++ b/robots/spidar/config/octo/control/WalkControlConfig.yaml @@ -17,12 +17,8 @@ controller: joint_static_torque_limit: 3.0 raise_joint_static_torque_limit: 1.5 # < joint_static_torque_limit - all_joint_position_control: true - free_leg_torque_mode: false raise_separate_motion: true - opposite_free_leg_joint_torque_control_mode: true - raise_leg_large_torque_control: true lower_leg_speed: 0.5 # rad/s raise_leg_force_i_gain: 100.0 # /s imediately diff --git a/robots/spidar/include/spidar/control/walk_control.h b/robots/spidar/include/spidar/control/walk_control.h index c6ebeb9f73..49918990c9 100644 --- a/robots/spidar/include/spidar/control/walk_control.h +++ b/robots/spidar/include/spidar/control/walk_control.h @@ -108,11 +108,7 @@ namespace aerial_robot_control double joint_ctrl_rate_; double tor_kp_; - bool joint_soft_compliance_; - double joint_compliance_end_t_; - bool set_init_servo_torque_; - bool all_joint_position_control_; bool free_leg_torque_mode_; bool raise_separate_motion_; double joint_error_angle_thresh_; @@ -130,9 +126,6 @@ namespace aerial_robot_control double thrust_force_weight_; double joint_torque_weight_; - bool opposite_free_leg_joint_torque_control_mode_; - bool raise_leg_large_torque_control_; - double raise_leg_force_i_gain_; double modify_leg_force_i_gain_; double lower_leg_force_i_gain_; diff --git a/robots/spidar/src/control/walk_control.cpp b/robots/spidar/src/control/walk_control.cpp index 1851b451f1..ab02a7fe4b 100644 --- a/robots/spidar/src/control/walk_control.cpp +++ b/robots/spidar/src/control/walk_control.cpp @@ -43,8 +43,6 @@ WalkController::WalkController(): walk_pid_controllers_(0), joint_torque_controllers_(0), joint_index_map_(0), - joint_soft_compliance_(false), - joint_compliance_end_t_(0), prev_navi_target_joint_angles_(0), free_leg_force_ratio_(0), raise_transition_(false), @@ -86,7 +84,6 @@ void WalkController::initialize(ros::NodeHandle nh, ros::NodeHandle nhp, joint_servo_enable_pub_ = nh_.advertise("servo/torque_enable", 1); joint_yaw_torque_srv_ = nh_.advertiseService("joint_yaw/torque_enable", boost::bind(&WalkController::servoTorqueCtrlCallback, this, _1, _2, "yaw")); joint_pitch_torque_srv_ = nh_.advertiseService("joint_pitch/torque_enable", boost::bind(&WalkController::servoTorqueCtrlCallback, this, _1, _2, "pitch")); - joint_force_compliance_sub_ = nh_.subscribe("joint_force_compliance", 1, &WalkController::jointSoftComplianceCallback, this); target_joint_angles_.name = spidar_robot_model_->getLinkJointNames(); int joint_num = spidar_robot_model_->getLinkJointNames().size(); @@ -126,11 +123,9 @@ void WalkController::rosParamInit() getParam(walk_control_nh, "raise_leg_burst_thresh", raise_leg_burst_thresh_, 0.1); getParam(walk_control_nh, "raise_leg_burst_bias", raise_leg_burst_bias_, 0.1); - getParam(walk_control_nh, "all_joint_position_control", all_joint_position_control_, true); - getParam(walk_control_nh, "opposite_free_leg_joint_torque_control_mode", opposite_free_leg_joint_torque_control_mode_, true); + getParam(walk_control_nh, "free_leg_torque_mode", free_leg_torque_mode_, false); getParam(walk_control_nh, "raise_separate_motion", raise_separate_motion_, false); - getParam(walk_control_nh, "raise_leg_large_torque_control", raise_leg_large_torque_control_, true); getParam(walk_control_nh, "lower_leg_speed", lower_leg_speed_, 0.5); getParam(walk_control_nh, "thrust_force_weight", thrust_force_weight_, 1.0); @@ -561,12 +556,6 @@ void WalkController::jointControl() target_joint_torques_.name.resize(0); target_joint_torques_.effort.resize(0); - // do joint soft compliance control - if (joint_soft_compliance_) { - jointSoftComplianceControl(); - return; - } - // basically, use position control for all joints auto navi_target_joint_angles = spidar_walk_navigator_->getTargetJointState().position; target_joint_angles_.position = navi_target_joint_angles; @@ -597,309 +586,120 @@ void WalkController::jointControl() int leg_num = joint_num / 4; // use position control for all joints - if (all_joint_position_control_) { - // modification for target joint angle - - bool raise_flag = spidar_walk_navigator_->getRaiseLegFlag(); - bool raise_converge = spidar_walk_navigator_->isRaiseLegConverge(); - int free_leg_id = spidar_walk_navigator_->getFreeleg(); - - for(int i = 0; i < joint_num; i++) { - double tor = static_joint_torque_(i); - std::string name = names.at(i); - int j = atoi(name.substr(5,1).c_str()) - 1; // start from 0 - int leg_id = j / 2; - - // WIP: add extra delta angle to deal with pulley strench - // TODO: move this function to - // 1. servo_bridge - // 2. neuron - double bias = tor / servo_angle_bias_torque_ * servo_angle_bias_; - if (contact_transition_ && leg_id == contact_leg_id_ && j % 2 == 1) { - // no extra angle error for outer joint pitch in free leg in contact transition - // e.g., joint2_pitch - bias = 0.0; - } - target_angles.at(i) += bias; - - // set the servo limit torque - tor = servo_max_torque_; - if (navigator_->getNaviState() != aerial_robot_navigation::ARM_ON_STATE) { - continue; - } + // modification for target joint angle + bool raise_flag = spidar_walk_navigator_->getRaiseLegFlag(); + bool raise_converge = spidar_walk_navigator_->isRaiseLegConverge(); + int free_leg_id = spidar_walk_navigator_->getFreeleg(); - // only consider the yaw angle in the initialize phase - if (name.find("yaw") != std::string::npos && set_init_servo_torque_) { - continue; - } + for(int i = 0; i < joint_num; i++) { + double tor = static_joint_torque_(i); + std::string name = names.at(i); + int j = atoi(name.substr(5,1).c_str()) - 1; // start from 0 + int leg_id = j / 2; - target_joint_torques_.name.push_back(name); - target_joint_torques_.effort.push_back(tor); + // WIP: add extra delta angle to deal with pulley strench + // TODO: move this function to + // 1. servo_bridge + // 2. neuron + double bias = tor / servo_angle_bias_torque_ * servo_angle_bias_; + if (contact_transition_ && leg_id == contact_leg_id_ && j % 2 == 1) { + // no extra angle error for outer joint pitch in free leg in contact transition + // e.g., joint2_pitch + bias = 0.0; } + target_angles.at(i) += bias; - // special process raise leg - if (raise_flag && !raise_converge) { - // inside pitch joint of the free leg - int i = free_leg_id * 4 + 1; - if (free_leg_torque_mode_) { - - target_angles.at(i) = prior_raise_leg_target_joint_angles_.position.at(i); - if (!raise_transition_) { - int j = free_leg_id * 2; - target_joint_torques_.effort.at(j) = static_joint_torque_(i); - } - } - - if (raise_separate_motion_) { - if (prior_raise_leg_target_joint_angles_.position.at(i) - current_angles.at(i) < 0.05) { - // inside yaw joint of the free leg - i = free_leg_id * 4; - target_angles.at(i) = prior_raise_leg_target_joint_angles_.position.at(i); - // outside pitch joint of the free leg - i = free_leg_id * 4 + 3; - target_angles.at(i) = prior_raise_leg_target_joint_angles_.position.at(i); - } - } + // set the servo limit torque + tor = servo_max_torque_; + if (navigator_->getNaviState() != aerial_robot_navigation::ARM_ON_STATE) { + continue; } - if (navigator_->getNaviState() == aerial_robot_navigation::ARM_ON_STATE && - !set_init_servo_torque_) { - set_init_servo_torque_ = true; + // only consider the yaw angle in the initialize phase + if (name.find("yaw") != std::string::npos && set_init_servo_torque_) { + continue; } - // feedback control about joint torque - double du = ros::Time::now().toSec() - control_timestamp_; - for(int i = 0; i < joint_num; i++) { - std::string name = names.at(i); - int j = atoi(name.substr(5,1).c_str()) - 1; // start from 0 - int leg_id = j / 2; - - double current_angle = current_angles.at(i); - double target_angle = target_angles.at(i); - double err = target_angle - current_angle; - double tor = static_joint_torque_(i); - - // TODO: consider whether skip yaw joint - // if (name.find("yaw") != std::string::npos) { - // target_extra_joint_torque_(i) = 0; - // continue; - // } - - // skip the free leg - if (leg_id == free_leg_id) { - joint_torque_controllers_.at(i).reset(); - target_extra_joint_torque_(i) = 0; - continue; - } + target_joint_torques_.name.push_back(name); + target_joint_torques_.effort.push_back(tor); + } - // skip if angle error is too small - if (fabs(err) < joint_error_angle_thresh_) { - target_extra_joint_torque_(i) = 0; - continue; - } + // special process raise leg + if (raise_flag && !raise_converge) { + // inside pitch joint of the free leg + int i = free_leg_id * 4 + 1; + if (free_leg_torque_mode_) { - // only consider if the joint servo torque is potentially saturated - if (err * tor < joint_error_angle_thresh_) { - target_extra_joint_torque_(i) = 0; - continue; + target_angles.at(i) = prior_raise_leg_target_joint_angles_.position.at(i); + if (!raise_transition_) { + int j = free_leg_id * 2; + target_joint_torques_.effort.at(j) = static_joint_torque_(i); } - - joint_torque_controllers_.at(i).update(err, du, 0); - target_extra_joint_torque_(i) = joint_torque_controllers_.at(i).result(); - } - std_msgs::Float32MultiArray msg; - for(int i = 0; i < target_extra_joint_torque_.size(); i++) { - msg.data.push_back(target_extra_joint_torque_(i)); } - extra_joint_torque_pub_.publish(msg); - return; + if (raise_separate_motion_) { + if (prior_raise_leg_target_joint_angles_.position.at(i) - current_angles.at(i) < 0.05) { + // inside yaw joint of the free leg + i = free_leg_id * 4; + target_angles.at(i) = prior_raise_leg_target_joint_angles_.position.at(i); + // outside pitch joint of the free leg + i = free_leg_id * 4 + 3; + target_angles.at(i) = prior_raise_leg_target_joint_angles_.position.at(i); + } + } } + if (navigator_->getNaviState() == aerial_robot_navigation::ARM_ON_STATE && + !set_init_servo_torque_) { + set_init_servo_torque_ = true; + } - // use torque control for joints that needs large torque load + // feedback control about joint torque + double du = ros::Time::now().toSec() - control_timestamp_; for(int i = 0; i < joint_num; i++) { - - double current_angle = current_angles.at(i); - double navi_target_angle = navi_target_joint_angles.at(i); - double prev_navi_target_angle = prev_navi_target_joint_angles_.at(i); - double tor = static_joint_torque_(i); std::string name = names.at(i); int j = atoi(name.substr(5,1).c_str()) - 1; // start from 0 int leg_id = j / 2; - // no joint torque control if robot is not armed - if (navigator_->getNaviState() != aerial_robot_navigation::ARM_ON_STATE) { - return; - } - - // position control for yaw joints - if (name.find("yaw") != std::string::npos) { - // ROS_INFO_STREAM("position control for: " << name); - continue; - } - - // heuristic rule for joints in free leg - if (leg_id == spidar_walk_navigator_->getFreeleg()) { - - prev_navi_target_joint_angles_.at(i) = navi_target_joint_angles.at(i); - bool lower_flag = spidar_walk_navigator_->getLowerLegFlag(); - bool raise_flag = spidar_walk_navigator_->getRaiseLegFlag(); - std::string status = raise_flag?std::string("raise"):std::string("lower"); - - if (j % 2 == 0) { - // inner joint (e.g., joint1_pitch) - // torque rule: set the torque bound as the static torque (small value) for raise and max torque for lower. - // joint is expected to raise / lower quick, - // angle rule: basic position control - - // set joint torque - target_joint_torques_.name.push_back(name); - if (lower_flag && lower_leg_force_i_gain_ == 0) { - // use large torque to lower leg instead of decrease thrust force - tor = servo_max_torque_; - double extra_angle_err = servo_angle_bias_; // workaround: for enough margin to touch ground - target_angles.at(i) += (tor / fabs(tor) * extra_angle_err); - } - target_joint_torques_.effort.push_back(tor); - - ROS_INFO_STREAM(" " << name << ", " << status << ", torque:" << tor << "; target angle: " << target_angles.at(i) << "; current angle: " << current_angle); - } - - if (j % 2 == 1) { - // outer joint (e.g., joint2_pitch) - // torque rule: set the largest one - // angle rule: basic position control - - // set joint torque - target_joint_torques_.name.push_back(name); - target_joint_torques_.effort.push_back(servo_max_torque_); + double current_angle = current_angles.at(i); + double target_angle = target_angles.at(i); + double err = target_angle - current_angle; + double tor = static_joint_torque_(i); - ROS_INFO_STREAM(" " << name << ", " << status << ", torque:" << servo_max_torque_ << "; target angle: " << target_angles.at(i) << "; current angle: " << current_angle); - } + // TODO: consider whether skip yaw joint + // if (name.find("yaw") != std::string::npos) { + // target_extra_joint_torque_(i) = 0; + // continue; + // } + // skip the free leg + if (leg_id == free_leg_id) { + joint_torque_controllers_.at(i).reset(); + target_extra_joint_torque_(i) = 0; continue; } - // heuristic rule for joints in rest of the legs when there is a free leg - // if ((leg_id + leg_num / 2) % leg_num == spidar_walk_navigator_->getFreeleg()) { - if (spidar_walk_navigator_->getFreeleg() != -1 && - leg_id != spidar_walk_navigator_->getFreeleg()) { - - if (j % 2 == 0) { - // inner joint (e.g., joint5_pitch) - - prev_navi_target_joint_angles_.at(i) = navi_target_joint_angles.at(i); - if (raise_leg_large_torque_control_) { - // torque rule: set the torque bound as the max torque to resist the torque from opposite raised leg. - tor = servo_max_torque_; - } - - target_joint_torques_.name.push_back(name); - target_joint_torques_.effort.push_back(tor); - - // angle rule: - double extra_angle_err = 0; - if (opposite_free_leg_joint_torque_control_mode_) { - // rule1: large diff from real target angles to reach the target joint torque - extra_angle_err = 0.1; // for enough margin for large angle error - } - else { - // rule2: basic position control - extra_angle_err = servo_angle_bias_; // add bais due to the pulley - } - target_angles.at(i) += (tor / fabs(tor) * extra_angle_err); - - - // ROS_INFO_STREAM(" " << name << ", opposite free leg, torque:" << tor << "; target angle: " << target_angles.at(i) << "; current angle: " << current_angle << "; real target angle: " << navi_target_angle); - continue; - } - } - - // position control for small joint torque - if (fabs(tor) < joint_torque_control_thresh_) { + // skip if angle error is too small + if (fabs(err) < joint_error_angle_thresh_) { + target_extra_joint_torque_(i) = 0; continue; } - // set joint angles - // large diff from real target angles to reach the target joint torque: torque control - double extra_angle_err = 0.1; // for enough margin for large angle error - - // in contact transition - if (contact_transition_) { - // no extra angle error for outer joint pitch in free leg in contact transition (e.g., joint2_pitch) - if (leg_id == contact_leg_id_ && j % 2 == 1) { - extra_angle_err = 0.0; - } - } - target_angles.at(i) += (tor / fabs(tor) * extra_angle_err); - - // Note: tor > 0: inner pitch joint (e.g., joint1_pitch) - // tor < 0: outer pitch joint (e.g., joint2_pitch) - if (fabs(prev_navi_target_angle - navi_target_angle) > 1e-4) { - - // address the angle bias in pulley system - double modified_navi_target_angle = navi_target_angle + tor / fabs(tor) * servo_angle_bias_; - - if (navi_target_angle > prev_navi_target_angle) { - if (current_angle >= modified_navi_target_angle) { - // the joint converges - prev_navi_target_joint_angles_.at(i) = navi_target_joint_angles.at(i); - ROS_INFO_STREAM("[Spider][Control]" << name << " reaches the new target angle " << navi_target_angle); - } - else { - // increase servo torque to reach the target angle, feedforwardly - if (tor > 0) { - tor = clamp(tor * servo_torque_change_rate_, servo_max_torque_); - ROS_INFO_STREAM("[Spider][Control]" << name << " increase torque."); - } - else { - // increase servo torque to reach the target angle, feedforwardly - tor = clamp(tor / servo_torque_change_rate_, servo_max_torque_); - ROS_INFO_STREAM("[Spider][Control]" << name << " decrease torque."); - - // set joint angles - // set to the current angle for small joint torque - target_angles.at(i) = current_angle; - } - } - } - - if (navi_target_angle < prev_navi_target_angle) { - if (current_angle <= modified_navi_target_angle) { - // the joint converges - prev_navi_target_joint_angles_.at(i) = navi_target_joint_angles.at(i); - ROS_INFO_STREAM("[Spider][Control]" << name << " reaches the new target angle " << navi_target_angle); - } - else { - - if (tor > 0) { - // decrease servo torque to reach the target angle, feedforwardly - tor = clamp(tor / servo_torque_change_rate_, servo_max_torque_); - ROS_INFO_STREAM("[Spider][Control]" << name << " decrease torque."); - - // set joint angles - // set to the current angle for small joint torque - target_angles.at(i) = current_angle; - } - else { - // increase servo torque to reach the target angle, feedforwardly - tor = clamp(tor * servo_torque_change_rate_, servo_max_torque_); - ROS_INFO_STREAM("[Spider][Control]" << name << " increase torque."); - } - } - } - - ROS_WARN("[Spider][Control] %s not converge. prev target: %f, curr target: %f, modified target: %f, real target: %f, curr pos: %f, tor: %f", - name.c_str(), prev_navi_target_angle, navi_target_angle, - modified_navi_target_angle, target_angles.at(i), current_angle, tor); + // only consider if the joint servo torque is potentially saturated + if (err * tor < joint_error_angle_thresh_) { + target_extra_joint_torque_(i) = 0; + continue; } - // set joint torque - target_joint_torques_.name.push_back(names.at(i)); - target_joint_torques_.effort.push_back(tor); + joint_torque_controllers_.at(i).update(err, du, 0); + target_extra_joint_torque_(i) = joint_torque_controllers_.at(i).result(); } + std_msgs::Float32MultiArray msg; + for(int i = 0; i < target_extra_joint_torque_.size(); i++) { + msg.data.push_back(target_extra_joint_torque_(i)); + } + extra_joint_torque_pub_.publish(msg); } void WalkController::calcStaticBalance() @@ -1095,23 +895,6 @@ void WalkController::calcStaticBalance() static_joint_torque_ = tor; } -void WalkController::jointSoftComplianceControl() -{ - // use torque control for pitch joints that needs large torque load - const auto current_angles = getCurrentJointAngles(); - - if (current_angles.size() == 0) { - return; - } - - if (joint_compliance_end_t_ < ros::Time::now().toSec()) { - joint_soft_compliance_ = false; - } - - // compliance to fit the current joint angles - target_joint_angles_.position = current_angles; -} - void WalkController::sendCmd() { PoseLinearController::sendCmd(); @@ -1124,8 +907,7 @@ void WalkController::sendCmd() target_vectoring_force_pub_.publish(target_vectoring_force_msg); // send joint command - if (navigator_->getNaviState() == aerial_robot_navigation::ARM_ON_STATE || - joint_soft_compliance_) { + if (navigator_->getNaviState() == aerial_robot_navigation::ARM_ON_STATE) { // joint target angles double st = target_joint_angles_.header.stamp.toSec(); @@ -1217,12 +999,6 @@ bool WalkController::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, st return true; } -void WalkController::jointSoftComplianceCallback(const std_msgs::EmptyConstPtr& msg) -{ - joint_soft_compliance_ = true; - joint_compliance_end_t_ = ros::Time::now().toSec() + 5.0; // 10.0 is a paramter -} - void WalkController::cfgPidCallback(aerial_robot_control::PidControlConfig &config, uint32_t level, std::vector controller_indices) { using Levels = aerial_robot_msgs::DynamicReconfigureLevels;