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 c6fd1ba814a..9c8527f47a6 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -297,9 +297,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; @@ -1507,19 +1509,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; @@ -1655,16 +1663,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]; @@ -1772,6 +1784,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 1c89bcd467a..a5ebb2445d3 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;