From ac17aedebe8ef1b960b4d4be1f01f1c5bc16ccd4 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sun, 6 Nov 2016 14:59:01 +0900 Subject: [PATCH 1/4] [Stabilizer.*] change root link height depending on limb length --- rtc/Stabilizer/Stabilizer.cpp | 79 ++++++++++++++++++++++++----------- rtc/Stabilizer/Stabilizer.h | 6 ++- 2 files changed, 59 insertions(+), 26 deletions(-) diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index e21a95138d0..cbd4e37b7a5 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -328,6 +328,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize() #define deg2rad(x) ((x) * M_PI / 180.0) for (size_t i = 0; i < stikp.size(); i++) { STIKParam& ikp = stikp[i]; + hrp::Link* root = m_robot->link(ikp.target_name); ikp.eefm_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5); ikp.eefm_rot_time_const = hrp::Vector3(1.5, 1.5, 1.5); ikp.eefm_rot_compensation_limit = deg2rad(10.0); @@ -347,6 +348,13 @@ RTC::ReturnCode_t Stabilizer::onInitialize() ikp.eefm_ee_forcemoment_distribution_weight[j+3] = 1e-2; // Moment } } + ikp.max_limb_length = 0.0; + while (!root->isRoot()) { + ikp.max_limb_length += root->b.norm(); + ikp.parent_name = root->name; + root = root->parent; + } + ikp.limb_length_margin = 0.13; } eefm_swing_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5); eefm_swing_pos_damping_gain = hrp::Vector3(33600, 33600, 7000); @@ -1490,30 +1498,47 @@ void Stabilizer::calcEEForceMomentControl() { // solveIK // IK target is link origin pos and rot, not ee pos and rot. - for (size_t jj = 0; jj < 3; jj++) { - for (size_t i = 0; i < stikp.size(); i++) { - if (is_ik_enable[i]) { - hrp::Vector3 tmpp; - hrp::Matrix33 tmpR; - // Add damping_control compensation to target value - if (is_feedback_control_enable[i]) { - rats::rotm3times(tmpR, target_ee_R[i], hrp::rotFromRpy(-stikp[i].ee_d_foot_rpy(0), -stikp[i].ee_d_foot_rpy(1), 0)); - // foot force difference control version - // total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl; - // foot force independent damping control - tmpp = target_ee_p[i] - current_d_foot_pos[i]; - } else { - tmpp = target_ee_p[i]; - tmpR = target_ee_R[i]; - } - // Add swing ee compensation - rats::rotm3times(tmpR, tmpR, hrp::rotFromRpy(stikp[i].d_rpy_swing)); - tmpp = tmpp + foot_origin_rot * stikp[i].d_pos_swing; - // IK - jpe_v[i]->calcInverseKinematics2Loop(tmpp, tmpR, 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain, - //stikp[i].localCOPPos; - stikp[i].localp, - stikp[i].localR); + std::vector tmpp(stikp.size()); + std::vector tmpR(stikp.size()); + double tmp_d_pos_z_root = 0.0; + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + // Add damping_control compensation to target value + if (is_feedback_control_enable[i]) { + rats::rotm3times(tmpR[i], target_ee_R[i], hrp::rotFromRpy(-stikp[i].ee_d_foot_rpy(0), -stikp[i].ee_d_foot_rpy(1), 0)); + // foot force difference control version + // total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl; + // foot force independent damping control + tmpp[i] = target_ee_p[i] - current_d_foot_pos[i]; + } else { + tmpp[i] = target_ee_p[i]; + tmpR[i] = target_ee_R[i]; + } + // Add swing ee compensation + rats::rotm3times(tmpR[i], tmpR[i], hrp::rotFromRpy(stikp[i].d_rpy_swing)); + tmpp[i] = tmpp[i] + foot_origin_rot * stikp[i].d_pos_swing; + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(stikp[i].parent_name); + hrp::Vector3 rel_targetp = m_robot->rootLink()->R.transpose() * ((tmpp[i] - tmpR[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p); //parent link relative + double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin; + if (rel_targetp.norm() > limb_length_limitation && limb_length_limitation * limb_length_limitation > rel_targetp(0) * rel_targetp(0) + rel_targetp(1) * rel_targetp(1)) { + tmp_d_pos_z_root = std::max(tmp_d_pos_z_root, fabs(rel_targetp(2) + sqrt(limb_length_limitation * limb_length_limitation - rel_targetp(0) * rel_targetp(0) + rel_targetp(1) * rel_targetp(1)))); + } + } + } + // Change root link height depending on limb length + double lvlimit = -10 * 1e-3 * dt, uvlimit = 150 * 1e-3 * dt, prev_d_pos_z_root = d_pos_z_root; + d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, 1.5) : tmp_d_pos_z_root; + d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + lvlimit, prev_d_pos_z_root + uvlimit); + m_robot->rootLink()->p -= m_robot->rootLink()->R * hrp::Vector3(0.0, 0.0, d_pos_z_root); + // IK + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + for (size_t jj = 0; jj < 3; jj++) { + jpe_v[i]->calcInverseKinematics2Loop(tmpp[i], tmpR[i], 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain, + //stikp[i].localCOPPos; + stikp[i].localp, + stikp[i].localR); } } } @@ -1580,6 +1605,12 @@ hrp::Vector3 Stabilizer::calcDampingControl (const hrp::Vector3& prev_d, const h return (- prev_d.cwiseQuotient(TT)) * dt + prev_d; }; +// Retrieving only +double Stabilizer::calcDampingControl (const double prev_d, const double TT) +{ + return - 1/TT * prev_d * dt + prev_d; +}; + hrp::Vector3 Stabilizer::calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d, const hrp::Vector3& DD, const hrp::Vector3& TT) { diff --git a/rtc/Stabilizer/Stabilizer.h b/rtc/Stabilizer/Stabilizer.h index 1fe127a0cea..4389c766e5a 100644 --- a/rtc/Stabilizer/Stabilizer.h +++ b/rtc/Stabilizer/Stabilizer.h @@ -132,6 +132,7 @@ class Stabilizer double calcDampingControl (const double tau_d, const double tau, const double prev_d, const double DD, const double TT); hrp::Vector3 calcDampingControl (const hrp::Vector3& prev_d, const hrp::Vector3& TT); + double calcDampingControl (const double prev_d, const double TT); hrp::Vector3 calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d, const hrp::Vector3& DD, const hrp::Vector3& TT); double vlimit(double value, double llimit_value, double ulimit_value); @@ -247,6 +248,7 @@ class Stabilizer std::string target_name; // Name of end link std::string ee_name; // Name of ee (e.g., rleg, lleg, ...) std::string sensor_name; // Name of force sensor in the limb + std::string parent_name; // Name of parent ling in the limb hrp::Vector3 localp; // Position of ee in end link frame (^{l}p_e = R_l^T (p_e - p_l)) hrp::Vector3 localCOPPos; // Position offset of reference COP in end link frame (^{l}p_{cop} = R_l^T (p_{cop} - p_l) - ^{l}p_e) hrp::Matrix33 localR; // Rotation of ee in end link frame (^{l}R_e = R_l^T R_e) @@ -262,7 +264,7 @@ class Stabilizer hrp::Vector3 target_ee_diff_p, d_pos_swing, d_rpy_swing, prev_d_pos_swing, prev_d_rpy_swing; hrp::Matrix33 target_ee_diff_r; // IK parameter - double avoid_gain, reference_gain; + double avoid_gain, reference_gain, max_limb_length, limb_length_margin; }; enum cmode {MODE_IDLE, MODE_AIR, MODE_ST, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_AIR} control_mode; // members @@ -290,7 +292,7 @@ class Stabilizer hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset; hrp::Vector3 foot_origin_offset[2]; std::vector prev_act_force_z; - double zmp_origin_off, transition_smooth_gain; + double zmp_origin_off, transition_smooth_gain, d_pos_z_root; boost::shared_ptr > act_cogvel_filter; OpenHRP::StabilizerService::STAlgorithm st_algorithm; SimpleZMPDistributor* szd; From f65f125e8a2977c76da1006fd1068f50c5a810f0 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sun, 6 Nov 2016 16:07:11 +0900 Subject: [PATCH 2/4] [StabilizerService.idl, Stabilizer.*] add idl for changing root link height --- idl/StabilizerService.idl | 8 ++++++++ rtc/Stabilizer/Stabilizer.cpp | 26 +++++++++++++++++++++----- rtc/Stabilizer/Stabilizer.h | 4 ++-- 3 files changed, 31 insertions(+), 7 deletions(-) diff --git a/idl/StabilizerService.idl b/idl/StabilizerService.idl index 63494db1460..42f6b86aa28 100644 --- a/idl/StabilizerService.idl +++ b/idl/StabilizerService.idl @@ -219,6 +219,14 @@ module OpenHRP boolean is_estop_while_walking; /// Sequence for all end-effectors' ik limb parameters sequence ik_limb_parameters; + /// Whether change root link height depending on limb length + boolean use_root_height_control; + /// Root link height time constant [s] + double root_height_time_const; + /// Root link height change limitation [m/s] (lower, upper) + DblArray2 root_height_vlimit; + /// Sequence of limb length margin from max limb length [m] + sequence limb_length_margin; }; /** diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index cbd4e37b7a5..f6ad36251a4 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -381,6 +381,10 @@ RTC::ReturnCode_t Stabilizer::onInitialize() is_walking = false; is_estop_while_walking = false; sbp_cog_offset = hrp::Vector3(0.0, 0.0, 0.0); + use_root_height_control = false; + root_height_time_const = 1.5; + root_height_vlimit[0] = -10 * 1e-3 * dt; // lower limit + root_height_vlimit[1] = 150 * 1e-3 * dt; // upper limit // parameters for RUNST double ke = 0, tc = 0; @@ -1527,10 +1531,10 @@ void Stabilizer::calcEEForceMomentControl() { } } // Change root link height depending on limb length - double lvlimit = -10 * 1e-3 * dt, uvlimit = 150 * 1e-3 * dt, prev_d_pos_z_root = d_pos_z_root; - d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, 1.5) : tmp_d_pos_z_root; - d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + lvlimit, prev_d_pos_z_root + uvlimit); - m_robot->rootLink()->p -= m_robot->rootLink()->R * hrp::Vector3(0.0, 0.0, d_pos_z_root); + double prev_d_pos_z_root = d_pos_z_root; + d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, root_height_time_const) : tmp_d_pos_z_root; + d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + root_height_vlimit[0], prev_d_pos_z_root + root_height_vlimit[1]); + if (use_root_height_control) m_robot->rootLink()->p -= m_robot->rootLink()->R * hrp::Vector3(0.0, 0.0, d_pos_z_root); // IK for (size_t i = 0; i < stikp.size(); i++) { if (is_ik_enable[i]) { @@ -1811,7 +1815,6 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_use_swing_damping = eefm_use_swing_damping; i_stp.eefm_swing_damping_force_thre = eefm_swing_damping_force_thre; i_stp.eefm_swing_damping_moment_thre = eefm_swing_damping_moment_thre; - i_stp.is_ik_enable.length(is_ik_enable.size()); for (size_t i = 0; i < is_ik_enable.size(); i++) { i_stp.is_ik_enable[i] = is_ik_enable[i]; @@ -1853,6 +1856,12 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) } i_stp.emergency_check_mode = emergency_check_mode; i_stp.end_effector_list.length(stikp.size()); + i_stp.use_root_height_control = use_root_height_control; + i_stp.root_height_time_const = root_height_time_const; + i_stp.limb_length_margin.length(stikp.size()); + for (size_t i = 0; i < 2; i++) { + i_stp.root_height_vlimit[i] = root_height_vlimit[i]; + } for (size_t i = 0; i < stikp.size(); i++) { const rats::coordinates cur_ee = rats::coordinates(stikp.at(i).localp, stikp.at(i).localR); OpenHRP::AutoBalancerService::Footstep ret_ee; @@ -1868,6 +1877,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) ret_ee.leg = stikp.at(i).ee_name.c_str(); // set i_stp.end_effector_list[i] = ret_ee; + i_stp.limb_length_margin[i] = stikp[i].limb_length_margin; } i_stp.ik_limb_parameters.length(jpe_v.size()); for (size_t i = 0; i < jpe_v.size(); i++) { @@ -2006,6 +2016,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) for (size_t i = 0; i < stikp.size(); i++) { stikp[i].target_ee_diff_p_filter->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq); stikp[i].target_ee_diff_r_filter->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq); + stikp[i].limb_length_margin = i_stp.limb_length_margin[i]; } setBoolSequenceParam(is_ik_enable, i_stp.is_ik_enable, std::string("is_ik_enable")); setBoolSequenceParam(is_feedback_control_enable, i_stp.is_feedback_control_enable, std::string("is_feedback_control_enable")); @@ -2022,6 +2033,11 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) } contact_decision_threshold = i_stp.contact_decision_threshold; is_estop_while_walking = i_stp.is_estop_while_walking; + use_root_height_control = i_stp.use_root_height_control; + root_height_time_const = i_stp.root_height_time_const; + for (size_t i = 0; i < 2; i++) { + root_height_vlimit[i] = i_stp.root_height_vlimit[i]; + } if (control_mode == MODE_IDLE) { for (size_t i = 0; i < i_stp.end_effector_list.length(); i++) { std::vector::iterator it = std::find_if(stikp.begin(), stikp.end(), (&boost::lambda::_1->* &std::vector::value_type::ee_name == std::string(i_stp.end_effector_list[i].leg))); diff --git a/rtc/Stabilizer/Stabilizer.h b/rtc/Stabilizer/Stabilizer.h index 4389c766e5a..6227614005e 100644 --- a/rtc/Stabilizer/Stabilizer.h +++ b/rtc/Stabilizer/Stabilizer.h @@ -280,7 +280,7 @@ class Stabilizer std::vector toeheel_ratio; double dt; int transition_count, loop; - bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error; + bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error, use_root_height_control; bool is_walking, is_estop_while_walking; hrp::Vector3 current_root_p, target_root_p; hrp::Matrix33 current_root_R, target_root_R, prev_act_foot_origin_rot, prev_ref_foot_origin_rot, target_foot_origin_rot; @@ -292,7 +292,7 @@ class Stabilizer hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset; hrp::Vector3 foot_origin_offset[2]; std::vector prev_act_force_z; - double zmp_origin_off, transition_smooth_gain, d_pos_z_root; + double zmp_origin_off, transition_smooth_gain, d_pos_z_root, root_height_time_const, root_height_vlimit[2]; boost::shared_ptr > act_cogvel_filter; OpenHRP::StabilizerService::STAlgorithm st_algorithm; SimpleZMPDistributor* szd; From 67f951ce31825282c1d76ed3c1b43797fea9a761 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sun, 6 Nov 2016 17:01:09 +0900 Subject: [PATCH 3/4] [Stabilizer.cpp] change root link height on world frame not root link frame --- rtc/Stabilizer/Stabilizer.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index f6ad36251a4..2498a9ab87c 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -383,8 +383,8 @@ RTC::ReturnCode_t Stabilizer::onInitialize() sbp_cog_offset = hrp::Vector3(0.0, 0.0, 0.0); use_root_height_control = false; root_height_time_const = 1.5; - root_height_vlimit[0] = -10 * 1e-3 * dt; // lower limit - root_height_vlimit[1] = 150 * 1e-3 * dt; // upper limit + root_height_vlimit[0] = -5 * 1e-3 * dt; // lower limit + root_height_vlimit[1] = 100 * 1e-3 * dt; // upper limit // parameters for RUNST double ke = 0, tc = 0; @@ -1523,10 +1523,10 @@ void Stabilizer::calcEEForceMomentControl() { tmpp[i] = tmpp[i] + foot_origin_rot * stikp[i].d_pos_swing; // Check whether inside limb length limitation hrp::Link* parent_link = m_robot->link(stikp[i].parent_name); - hrp::Vector3 rel_targetp = m_robot->rootLink()->R.transpose() * ((tmpp[i] - tmpR[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p); //parent link relative + hrp::Vector3 targetp = (tmpp[i] - tmpR[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame) double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin; - if (rel_targetp.norm() > limb_length_limitation && limb_length_limitation * limb_length_limitation > rel_targetp(0) * rel_targetp(0) + rel_targetp(1) * rel_targetp(1)) { - tmp_d_pos_z_root = std::max(tmp_d_pos_z_root, fabs(rel_targetp(2) + sqrt(limb_length_limitation * limb_length_limitation - rel_targetp(0) * rel_targetp(0) + rel_targetp(1) * rel_targetp(1)))); + if (targetp.norm() > limb_length_limitation && limb_length_limitation * limb_length_limitation > targetp(0) * targetp(0) + targetp(1) * targetp(1)) { + tmp_d_pos_z_root = std::max(tmp_d_pos_z_root, fabs(targetp(2) + sqrt(limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) + targetp(1) * targetp(1)))); } } } @@ -1534,7 +1534,7 @@ void Stabilizer::calcEEForceMomentControl() { double prev_d_pos_z_root = d_pos_z_root; d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, root_height_time_const) : tmp_d_pos_z_root; d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + root_height_vlimit[0], prev_d_pos_z_root + root_height_vlimit[1]); - if (use_root_height_control) m_robot->rootLink()->p -= m_robot->rootLink()->R * hrp::Vector3(0.0, 0.0, d_pos_z_root); + if (use_root_height_control) m_robot->rootLink()->p(2) -= d_pos_z_root; // IK for (size_t i = 0; i < stikp.size(); i++) { if (is_ik_enable[i]) { @@ -1815,6 +1815,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_use_swing_damping = eefm_use_swing_damping; i_stp.eefm_swing_damping_force_thre = eefm_swing_damping_force_thre; i_stp.eefm_swing_damping_moment_thre = eefm_swing_damping_moment_thre; + i_stp.is_ik_enable.length(is_ik_enable.size()); for (size_t i = 0; i < is_ik_enable.size(); i++) { i_stp.is_ik_enable[i] = is_ik_enable[i]; From c9f312e343ad28cd164352883d8db6fc1efbe469 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Tue, 8 Nov 2016 21:06:33 +0900 Subject: [PATCH 4/4] [StabilizerService.idl, Stabilizer.*] rename variable and add function for avoiding limb stretch --- idl/StabilizerService.idl | 12 ++++---- rtc/Stabilizer/Stabilizer.cpp | 57 +++++++++++++++++++++-------------- rtc/Stabilizer/Stabilizer.h | 5 +-- 3 files changed, 44 insertions(+), 30 deletions(-) diff --git a/idl/StabilizerService.idl b/idl/StabilizerService.idl index 42f6b86aa28..4fa08d79446 100644 --- a/idl/StabilizerService.idl +++ b/idl/StabilizerService.idl @@ -219,12 +219,12 @@ module OpenHRP boolean is_estop_while_walking; /// Sequence for all end-effectors' ik limb parameters sequence ik_limb_parameters; - /// Whether change root link height depending on limb length - boolean use_root_height_control; - /// Root link height time constant [s] - double root_height_time_const; - /// Root link height change limitation [m/s] (lower, upper) - DblArray2 root_height_vlimit; + /// Whether change root link height for avoiding limb stretch + boolean use_limb_stretch_avoidance; + /// Limb stretch avoidance time constant [s] + double limb_stretch_avoidance_time_const; + /// Root link height change limitation for avoiding limb stretch [m/s] (lower, upper) + DblArray2 limb_stretch_avoidance_vlimit; /// Sequence of limb length margin from max limb length [m] sequence limb_length_margin; }; diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index 2498a9ab87c..3dd8c905661 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -381,10 +381,10 @@ RTC::ReturnCode_t Stabilizer::onInitialize() is_walking = false; is_estop_while_walking = false; sbp_cog_offset = hrp::Vector3(0.0, 0.0, 0.0); - use_root_height_control = false; - root_height_time_const = 1.5; - root_height_vlimit[0] = -5 * 1e-3 * dt; // lower limit - root_height_vlimit[1] = 100 * 1e-3 * dt; // upper limit + use_limb_stretch_avoidance = false; + limb_stretch_avoidance_time_const = 1.5; + limb_stretch_avoidance_vlimit[0] = -100 * 1e-3 * dt; // lower limit + limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * dt; // upper limit // parameters for RUNST double ke = 0, tc = 0; @@ -1521,20 +1521,11 @@ void Stabilizer::calcEEForceMomentControl() { // Add swing ee compensation rats::rotm3times(tmpR[i], tmpR[i], hrp::rotFromRpy(stikp[i].d_rpy_swing)); tmpp[i] = tmpp[i] + foot_origin_rot * stikp[i].d_pos_swing; - // Check whether inside limb length limitation - hrp::Link* parent_link = m_robot->link(stikp[i].parent_name); - hrp::Vector3 targetp = (tmpp[i] - tmpR[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame) - double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin; - if (targetp.norm() > limb_length_limitation && limb_length_limitation * limb_length_limitation > targetp(0) * targetp(0) + targetp(1) * targetp(1)) { - tmp_d_pos_z_root = std::max(tmp_d_pos_z_root, fabs(targetp(2) + sqrt(limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) + targetp(1) * targetp(1)))); - } } } - // Change root link height depending on limb length - double prev_d_pos_z_root = d_pos_z_root; - d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, root_height_time_const) : tmp_d_pos_z_root; - d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + root_height_vlimit[0], prev_d_pos_z_root + root_height_vlimit[1]); - if (use_root_height_control) m_robot->rootLink()->p(2) -= d_pos_z_root; + + if (use_limb_stretch_avoidance) limbStretchAvoidanceControl(tmpp ,tmpR); + // IK for (size_t i = 0; i < stikp.size(); i++) { if (is_ik_enable[i]) { @@ -1595,6 +1586,28 @@ void Stabilizer::calcSwingEEModification () } }; +void Stabilizer::limbStretchAvoidanceControl (const std::vector& ee_p, const std::vector& ee_R) +{ + double tmp_d_pos_z_root = 0.0; + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(stikp[i].parent_name); + hrp::Vector3 targetp = (ee_p[i] - ee_R[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame) + double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin; + double tmp = limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) - targetp(1) * targetp(1); + if (targetp.norm() > limb_length_limitation && tmp >= 0) { + tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, targetp(2) + std::sqrt(tmp)); + } + } + } + // Change root link height depending on limb length + double prev_d_pos_z_root = d_pos_z_root; + d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const) : tmp_d_pos_z_root; + d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + limb_stretch_avoidance_vlimit[0], prev_d_pos_z_root + limb_stretch_avoidance_vlimit[1]); + m_robot->rootLink()->p(2) += d_pos_z_root; +} + // Damping control functions // Basically Equation (14) in the paper [1] double Stabilizer::calcDampingControl (const double tau_d, const double tau, const double prev_d, @@ -1857,11 +1870,11 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) } i_stp.emergency_check_mode = emergency_check_mode; i_stp.end_effector_list.length(stikp.size()); - i_stp.use_root_height_control = use_root_height_control; - i_stp.root_height_time_const = root_height_time_const; + i_stp.use_limb_stretch_avoidance = use_limb_stretch_avoidance; + i_stp.limb_stretch_avoidance_time_const = limb_stretch_avoidance_time_const; i_stp.limb_length_margin.length(stikp.size()); for (size_t i = 0; i < 2; i++) { - i_stp.root_height_vlimit[i] = root_height_vlimit[i]; + i_stp.limb_stretch_avoidance_vlimit[i] = limb_stretch_avoidance_vlimit[i]; } for (size_t i = 0; i < stikp.size(); i++) { const rats::coordinates cur_ee = rats::coordinates(stikp.at(i).localp, stikp.at(i).localR); @@ -2034,10 +2047,10 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) } contact_decision_threshold = i_stp.contact_decision_threshold; is_estop_while_walking = i_stp.is_estop_while_walking; - use_root_height_control = i_stp.use_root_height_control; - root_height_time_const = i_stp.root_height_time_const; + use_limb_stretch_avoidance = i_stp.use_limb_stretch_avoidance; + limb_stretch_avoidance_time_const = i_stp.limb_stretch_avoidance_time_const; for (size_t i = 0; i < 2; i++) { - root_height_vlimit[i] = i_stp.root_height_vlimit[i]; + limb_stretch_avoidance_vlimit[i] = i_stp.limb_stretch_avoidance_vlimit[i]; } if (control_mode == MODE_IDLE) { for (size_t i = 0; i < i_stp.end_effector_list.length(); i++) { diff --git a/rtc/Stabilizer/Stabilizer.h b/rtc/Stabilizer/Stabilizer.h index 6227614005e..27fe0476fdf 100644 --- a/rtc/Stabilizer/Stabilizer.h +++ b/rtc/Stabilizer/Stabilizer.h @@ -119,6 +119,7 @@ class Stabilizer void calcTPCC(); void calcEEForceMomentControl(); void calcSwingEEModification (); + void limbStretchAvoidanceControl (const std::vector& ee_p, const std::vector& ee_R); void getParameter(OpenHRP::StabilizerService::stParam& i_stp); void setParameter(const OpenHRP::StabilizerService::stParam& i_stp); void setBoolSequenceParam (std::vector& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name); @@ -280,7 +281,7 @@ class Stabilizer std::vector toeheel_ratio; double dt; int transition_count, loop; - bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error, use_root_height_control; + bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error, use_limb_stretch_avoidance; bool is_walking, is_estop_while_walking; hrp::Vector3 current_root_p, target_root_p; hrp::Matrix33 current_root_R, target_root_R, prev_act_foot_origin_rot, prev_ref_foot_origin_rot, target_foot_origin_rot; @@ -292,7 +293,7 @@ class Stabilizer hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset; hrp::Vector3 foot_origin_offset[2]; std::vector prev_act_force_z; - double zmp_origin_off, transition_smooth_gain, d_pos_z_root, root_height_time_const, root_height_vlimit[2]; + double zmp_origin_off, transition_smooth_gain, d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2]; boost::shared_ptr > act_cogvel_filter; OpenHRP::StabilizerService::STAlgorithm st_algorithm; SimpleZMPDistributor* szd;