Skip to content

Commit

Permalink
Merge pull request fkanehiro#934 from eisoku9618/modify_swing_leg_end…
Browse files Browse the repository at this point in the history 
…-coords

Modify swing leg end coords
  • Loading branch information
@fkanehiro
fkanehiro committed Jan 1, 2016
2 parents 93be4ca + 7d11716 commit e6ae02d
Show file tree
Hide file tree
Showing 3 changed files with 104 additions and 7 deletions.
4 changes: 4 additions & 0 deletions idl/StabilizerService.idl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,6 +104,10 @@ module OpenHRP
sequence<sequence<double, 3> > eefm_pos_damping_gain;
/// Sequence of all end-effector position damping time constant for double support phase [s] (x,y,z).
sequence<sequence<double, 3> > eefm_pos_time_const_support;
/// Sequence of all swing leg end-effector rotation spring gain (r,p,y).
sequence<sequence<double, 3> > eefm_swing_rot_spring_gain;
/// Sequence of all swing leg end-effector position spring gain (x,y,z).
sequence<sequence<double, 3> > eefm_swing_pos_spring_gain;
/// Sequence of all end-effector position compensation limit [m]
sequence<double> eefm_pos_compensation_limit;
/// Sequence of all end-effector rot compensation limit [rad]
Expand Down
101 changes: 97 additions & 4 deletions rtc/Stabilizer/Stabilizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<FirstOrderLowPassFilter<hrp::Vector3> >(new FirstOrderLowPassFilter<hrp::Vector3>(50.0, dt, hrp::Vector3::Zero()))); // [Hz]
contact_states_index_map.insert(std::pair<std::string, size_t>(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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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<hrp::Vector3> tmpp_list; // modified ee Pos
std::vector<hrp::Matrix33> 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
Expand All @@ -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.
Expand Down Expand Up @@ -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();
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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];
Expand Down Expand Up @@ -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;
Expand Down
6 changes: 3 additions & 3 deletions rtc/Stabilizer/Stabilizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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 <hrp::Vector3> target_ee_p, target_ee_diff_p, target_ee_diff_r, prev_target_ee_diff_r, rel_ee_pos;
std::vector <hrp::Matrix33> target_ee_R, rel_ee_rot;
std::vector <hrp::Vector3> 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 <hrp::Matrix33> target_ee_R, rel_ee_rot, act_ee_R;
std::vector<std::string> 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;
Expand Down

0 comments on commit e6ae02d

Please sign in to comment.