diff --git a/idl/StabilizerService.idl b/idl/StabilizerService.idl index ffe10b28e3e..3e97d83f5ef 100644 --- a/idl/StabilizerService.idl +++ b/idl/StabilizerService.idl @@ -104,6 +104,10 @@ module OpenHRP sequence > eefm_pos_damping_gain; /// Sequence of all end-effector position damping time constant for double support phase [s] (x,y,z). sequence > eefm_pos_time_const_support; + /// Sequence of all swing leg end-effector rotation spring gain (r,p,y). + sequence > eefm_swing_rot_spring_gain; + /// Sequence of all swing leg end-effector position spring gain (x,y,z). + sequence > eefm_swing_pos_spring_gain; /// Sequence of all end-effector position compensation limit [m] sequence eefm_pos_compensation_limit; /// Sequence of all end-effector rot compensation limit [rad] diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index c13a372e29f..06be50a9aba 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -247,9 +247,12 @@ RTC::ReturnCode_t Stabilizer::onInitialize() } target_ee_p.push_back(hrp::Vector3::Zero()); target_ee_R.push_back(hrp::Matrix33::Identity()); + act_ee_R.push_back(hrp::Matrix33::Identity()); target_ee_diff_p.push_back(hrp::Vector3::Zero()); target_ee_diff_r.push_back(hrp::Vector3::Zero()); prev_target_ee_diff_r.push_back(hrp::Vector3::Zero()); + d_rpy_swing.push_back(hrp::Vector3::Zero()); + d_pos_swing.push_back(hrp::Vector3::Zero()); target_ee_diff_p_filter.push_back(boost::shared_ptr >(new FirstOrderLowPassFilter(50.0, dt, hrp::Vector3::Zero()))); // [Hz] contact_states_index_map.insert(std::pair(ee_name, i)); is_ik_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // Hands ik => disabled, feet ik => enabled, by default @@ -296,9 +299,11 @@ RTC::ReturnCode_t Stabilizer::onInitialize() 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); + ikp.eefm_swing_rot_spring_gain = hrp::Vector3(0.0, 0.0, 0.0); ikp.eefm_pos_damping_gain = hrp::Vector3(3500*10, 3500*10, 3500); ikp.eefm_pos_time_const_support = hrp::Vector3(1.5, 1.5, 1.5); ikp.eefm_pos_compensation_limit = 0.025; + ikp.eefm_swing_pos_spring_gain = hrp::Vector3(0.0, 0.0, 0.0); } eefm_pos_time_const_swing = 0.08; eefm_pos_transition_time = 0.01; @@ -738,6 +743,7 @@ void Stabilizer::getActualParameters () hrp::Vector3 act_ee_p = target->p + target->R * stikp[i].localp; //target_ee_R[i] = target->R * stikp[i].localR; target_ee_diff_p[i] -= foot_origin_rot.transpose() * (act_ee_p - foot_origin_pos); + act_ee_R[i] = foot_origin_rot.transpose() * (target->R * stikp[i].localR); } // capture point act_cp = act_cog + act_cogvel / std::sqrt(eefm_gravitational_acceleration / (act_cog - act_zmp)(2)); @@ -1302,6 +1308,8 @@ void Stabilizer::calcEEForceMomentControl() { // Feet and hands modification hrp::Vector3 target_link_p[stikp.size()]; hrp::Matrix33 target_link_R[stikp.size()]; + std::vector tmpp_list; // modified ee Pos + std::vector tmpR_list; // modified ee Rot #define deg2rad(x) ((x) * M_PI / 180.0) for (size_t i = 0; i < stikp.size(); i++) { hrp::Vector3 tmpp; // modified ee Pos @@ -1322,10 +1330,80 @@ void Stabilizer::calcEEForceMomentControl() { tmpp = target_ee_p[i] + eefm_ee_pos_error_p_gain * target_foot_origin_rot * target_ee_diff_p_filter[i]->passFilter(target_ee_diff_p[i]);// tempolarily disabled tmpR = target_ee_R[i]; } + tmpR_list.push_back(tmpR); + tmpp_list.push_back(tmpp); + } + // follow swing foot rotation to target one in single support phase on the assumption that the robot rotates around support foot. + { + hrp::Matrix33 cur_sup_R, act_sup_R; + hrp::Vector3 cur_sup_p; + for (size_t i = 0; i < stikp.size(); i++) { + if (isContact(i)) { + cur_sup_R = tmpR_list.at(i); + cur_sup_p = tmpp_list.at(i); + act_sup_R = act_ee_R.at(i); + break; + } + } + for (size_t i = 0; i < stikp.size(); i++) { + if (isContact(i) or contact_states[contact_states_index_map[stikp[i].ee_name]]) { + /* method A */ + // for (size_t j = 0; j < 3; j++) { + // d_rpy_swing.at(i)[j] = (-1 / 0.5 * d_rpy_swing.at(i)[j]) * dt + d_rpy_swing.at(i)[j]; + // d_pos_swing.at(i)[j] = (-1 / 0.5 * d_pos_swing.at(i)[j]) * dt + d_pos_swing.at(i)[j]; + // } + /* method B */ + d_rpy_swing.at(i) = hrp::Vector3::Zero(); + d_pos_swing.at(i) = hrp::Vector3::Zero(); + } else { + /* rotation */ + { + hrp::Matrix33 cur_swg_R = tmpR_list.at(i); + hrp::Matrix33 swg_R_relative_to_sup_R = cur_sup_R.transpose() * cur_swg_R; + hrp::Matrix33 new_swg_R; + rats::rotm3times(new_swg_R, act_sup_R, swg_R_relative_to_sup_R); + rats::rotm3times(new_swg_R, foot_origin_rot, new_swg_R); + hrp::Vector3 tmp_diff_rpy = hrp::rpyFromRot(new_swg_R.transpose() * tmpR_list.at(i)); + for (size_t j = 0; j < 3; j++) { + /* method A */ + // d_rpy_swing.at(i)[j] = (stikp[i].eefm_swing_rot_spring_gain[j] * tmp_diff_rpy[j] - 1 / 1.5 * d_rpy_swing.at(i)[j]) * dt + d_rpy_swing.at(i)[j]; + /* method B */ + d_rpy_swing.at(i)[j] = tmp_diff_rpy[j] * stikp[i].eefm_swing_rot_spring_gain[j]; + } + rats::rotm3times(tmpR_list.at(i), tmpR_list.at(i), hrp::rotFromRpy(d_rpy_swing.at(i))); + } + /* position */ + { + hrp::Vector3 cur_swg_p = tmpp_list.at(i); + hrp::Vector3 swg_p_relative_to_sup_R = cur_sup_R.transpose() * (cur_swg_p - cur_sup_p); + hrp::Vector3 new_swg_p = (foot_origin_rot * act_sup_R) * swg_p_relative_to_sup_R + cur_sup_p; + hrp::Vector3 tmp_diff_pos = tmpp_list.at(i) - new_swg_p; + for (size_t j = 0; j < 3; j++) { + /* method A */ + // d_pos_swing.at(i)[j] = (stikp[i].eefm_swing_pos_spring_gain[j] * tmp_diff_pos[j] - 1 / 1.5 * d_pos_swing.at(i)[j]) * dt + d_pos_swing.at(i)[j]; + /* method B */ + d_pos_swing.at(i)[j] = tmp_diff_pos[j] * stikp[i].eefm_swing_pos_spring_gain[j]; + } + tmpp_list.at(i) = tmpp_list.at(i) + d_pos_swing.at(i); + } + } + } + if (DEBUGP) { + for (size_t i = 0; i < stikp.size(); i++) { + if (is_feedback_control_enable[i]) { + std::cerr << "[" << m_profile.instance_name << "] " + << "d_rpy_swing (" << stikp[i].ee_name << ") = " << (d_rpy_swing.at(i) / M_PI * 180.0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[deg]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] " + << "d_pos_swing (" << stikp[i].ee_name << ") = " << (d_pos_swing.at(i) * 1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm]" << std::endl; + } + } + } + } + for (size_t i = 0; i < stikp.size(); i++){ // target at ee => target at link-origin - rats::rotm3times(target_link_R[i], tmpR, stikp[i].localR.transpose()); - //target_link_p[i] = tmpp - target_link_R[i] * stikp[i].localCOPPos; - target_link_p[i] = tmpp - target_link_R[i] * stikp[i].localp; + rats::rotm3times(target_link_R[i], tmpR_list.at(i), stikp[i].localR.transpose()); + //target_link_p[i] = tmpp_list.at(i) - target_link_R[i] * stikp[i].localCOPPos; + target_link_p[i] = tmpp_list.at(i) - target_link_R[i] * stikp[i].localp; } // solveIK // IK target is link origin pos and rot, not ee pos and rot. @@ -1405,6 +1483,8 @@ void Stabilizer::sync_2_st () target_ee_diff_p[i] = hrp::Vector3::Zero(); target_ee_diff_r[i] = hrp::Vector3::Zero(); prev_target_ee_diff_r[i] = hrp::Vector3::Zero(); + d_rpy_swing[i] = hrp::Vector3::Zero(); + d_pos_swing[i] = hrp::Vector3::Zero(); STIKParam& ikp = stikp[i]; ikp.d_foot_pos = ikp.d_foot_rpy = ikp.ee_d_foot_rpy = hrp::Vector3::Zero(); } @@ -1479,19 +1559,25 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_pos_time_const_support.length(stikp.size()); i_stp.eefm_pos_damping_gain.length(stikp.size()); i_stp.eefm_pos_compensation_limit.length(stikp.size()); + i_stp.eefm_swing_pos_spring_gain.length(stikp.size()); i_stp.eefm_rot_time_const.length(stikp.size()); i_stp.eefm_rot_damping_gain.length(stikp.size()); i_stp.eefm_rot_compensation_limit.length(stikp.size()); + i_stp.eefm_swing_rot_spring_gain.length(stikp.size()); for (size_t j = 0; j < stikp.size(); j++) { i_stp.eefm_pos_damping_gain[j].length(3); i_stp.eefm_pos_time_const_support[j].length(3); + i_stp.eefm_swing_pos_spring_gain[j].length(3); i_stp.eefm_rot_damping_gain[j].length(3); i_stp.eefm_rot_time_const[j].length(3); + i_stp.eefm_swing_rot_spring_gain[j].length(3); for (size_t i = 0; i < 3; i++) { i_stp.eefm_pos_damping_gain[j][i] = stikp[j].eefm_pos_damping_gain(i); i_stp.eefm_pos_time_const_support[j][i] = stikp[j].eefm_pos_time_const_support(i); + i_stp.eefm_swing_pos_spring_gain[j][i] = stikp[j].eefm_swing_pos_spring_gain(i); i_stp.eefm_rot_damping_gain[j][i] = stikp[j].eefm_rot_damping_gain(i); i_stp.eefm_rot_time_const[j][i] = stikp[j].eefm_rot_time_const(i); + i_stp.eefm_swing_rot_spring_gain[j][i] = stikp[j].eefm_swing_rot_spring_gain(i); } i_stp.eefm_pos_compensation_limit[j] = stikp[j].eefm_pos_compensation_limit; i_stp.eefm_rot_compensation_limit[j] = stikp[j].eefm_rot_compensation_limit; @@ -1627,16 +1713,20 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) if ( i_stp.eefm_pos_damping_gain.length () == stikp.size() && i_stp.eefm_pos_time_const_support.length () == stikp.size() && i_stp.eefm_pos_compensation_limit.length () == stikp.size() && + i_stp.eefm_swing_pos_spring_gain.length () == stikp.size() && i_stp.eefm_rot_damping_gain.length () == stikp.size() && i_stp.eefm_rot_time_const.length () == stikp.size() && - i_stp.eefm_rot_compensation_limit.length () == stikp.size() ) { + i_stp.eefm_rot_compensation_limit.length () == stikp.size() && + i_stp.eefm_swing_rot_spring_gain.length () == stikp.size() ) { is_damping_parameter_ok = true; for (size_t j = 0; j < stikp.size(); j++) { for (size_t i = 0; i < 3; i++) { stikp[j].eefm_pos_damping_gain(i) = i_stp.eefm_pos_damping_gain[j][i]; stikp[j].eefm_pos_time_const_support(i) = i_stp.eefm_pos_time_const_support[j][i]; + stikp[j].eefm_swing_pos_spring_gain(i) = i_stp.eefm_swing_pos_spring_gain[j][i]; stikp[j].eefm_rot_damping_gain(i) = i_stp.eefm_rot_damping_gain[j][i]; stikp[j].eefm_rot_time_const(i) = i_stp.eefm_rot_time_const[j][i]; + stikp[j].eefm_swing_rot_spring_gain(i) = i_stp.eefm_swing_rot_spring_gain[j][i]; } stikp[j].eefm_pos_compensation_limit = i_stp.eefm_pos_compensation_limit[j]; stikp[j].eefm_rot_compensation_limit = i_stp.eefm_rot_compensation_limit[j]; @@ -1744,6 +1834,9 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] " << "eefm_pos_compensation_limit = " << stikp[j].eefm_pos_compensation_limit << "[m], " << " " << "eefm_rot_compensation_limit = " << stikp[j].eefm_rot_compensation_limit << "[rad]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] " + << "eefm_swing_pos_spring_gain = " << stikp[j].eefm_swing_pos_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", " + << "eefm_swing_rot_spring_gain = " << stikp[j].eefm_swing_rot_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; } } else { std::cerr << "[" << m_profile.instance_name << "] eefm damping parameters cannot be set because of invalid param." << std::endl; diff --git a/rtc/Stabilizer/Stabilizer.h b/rtc/Stabilizer/Stabilizer.h index 9bc9298e4b1..4b15cf128da 100644 --- a/rtc/Stabilizer/Stabilizer.h +++ b/rtc/Stabilizer/Stabilizer.h @@ -244,7 +244,7 @@ class Stabilizer hrp::Matrix33 localR; // Rotation of ee in end link frame (^{l}R_e = R_l^T R_e) // For eefm hrp::Vector3 d_foot_pos, d_foot_rpy, ee_d_foot_rpy; - hrp::Vector3 eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_rot_damping_gain, eefm_rot_time_const; + hrp::Vector3 eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_rot_damping_gain, eefm_rot_time_const, eefm_swing_rot_spring_gain, eefm_swing_pos_spring_gain; double eefm_pos_compensation_limit, eefm_rot_compensation_limit; hrp::Vector3 ref_force, ref_moment; double swing_support_gain, support_time; @@ -265,8 +265,8 @@ class Stabilizer 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; - std::vector target_ee_p, target_ee_diff_p, target_ee_diff_r, prev_target_ee_diff_r, rel_ee_pos; - std::vector target_ee_R, rel_ee_rot; + std::vector target_ee_p, target_ee_diff_p, target_ee_diff_r, prev_target_ee_diff_r, rel_ee_pos, d_pos_swing, d_rpy_swing; + std::vector target_ee_R, rel_ee_rot, act_ee_R; std::vector rel_ee_name; rats::coordinates target_foot_midcoords; hrp::Vector3 ref_zmp, ref_cog, ref_cp, ref_cogvel, rel_ref_cp, prev_ref_cog, prev_ref_zmp;