Skip to content

Commit

Permalink
Merge pull request #1013 from snozawa/add_new_st_distribution
Browse files Browse the repository at this point in the history 
Add new st distribution and add interpolator printing.
  • Loading branch information
@fkanehiro
fkanehiro authored Jun 21, 2016
2 parents 4aff9e6 + a8648af commit e435265
Show file tree
Hide file tree
Showing 5 changed files with 201 additions and 9 deletions.
5 changes: 4 additions & 1 deletion idl/StabilizerService.idl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,8 @@ module OpenHRP
TPCC,
EEFM,
EEFMQP,
EEFMQPCOP
EEFMQPCOP,
EEFMQPCOP2
};

/**
Expand Down Expand Up @@ -172,6 +173,8 @@ module OpenHRP
sequence< SupportPolygonVertices > eefm_support_polygon_vertices_sequence;
/// Use force difference control or each limb force control. True by default.
boolean eefm_use_force_difference_control;
/// Sequence of all end-effector force/moment distribution weight
sequence<sequence<double, 6> > eefm_ee_forcemoment_distribution_weight;
// common
/// Current Stabilizer algorithm
STAlgorithm st_algorithm;
Expand Down
6 changes: 5 additions & 1 deletion rtc/SequencePlayer/interpolator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,11 @@ double interpolator::calc_interpolation_time(const double *newg)
}
remain_t_ = max_diff/default_avg_vel;
#define MIN_INTERPOLATION_TIME (1.0)
if (remain_t_ < MIN_INTERPOLATION_TIME) remain_t_ = MIN_INTERPOLATION_TIME;
if (remain_t_ < MIN_INTERPOLATION_TIME) {
std::cerr << "[interpolator][" << name << "] MIN_INTERPOLATION_TIME violated!! Limit remain_t (" << remain_t << ") by MIN_INTERPOLATION_TIME (" << MIN_INTERPOLATION_TIME << ")."
<< "(max_diff = " << max_diff << ", default_avg_vel = " << default_avg_vel << ")" << std::endl;;
remain_t_ = MIN_INTERPOLATION_TIME;
}
return remain_t_;
}

Expand Down
47 changes: 40 additions & 7 deletions rtc/Stabilizer/Stabilizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -555,7 +555,7 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id)
if ( transition_count == 0 && on_ground ) sync_2_st();
break;
case MODE_ST:
if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
calcEEForceMomentControl();
} else {
calcTPCC();
Expand Down Expand Up @@ -710,7 +710,7 @@ void Stabilizer::getActualParameters ()
// Actual world frame =>
hrp::Vector3 foot_origin_pos;
hrp::Matrix33 foot_origin_rot;
if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
// update by current joint angles
for ( int i = 0; i < m_robot->numJoints(); i++ ){
m_robot->joint(i)->q = m_qCurrent.data[i];
Expand Down Expand Up @@ -738,7 +738,7 @@ void Stabilizer::getActualParameters ()
act_cog = m_robot->calcCM();
// zmp
on_ground = false;
if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
on_ground = calcZMP(act_zmp, zmp_origin_off+foot_origin_pos(2));
} else {
on_ground = calcZMP(act_zmp, ref_zmp(2));
Expand All @@ -752,7 +752,7 @@ void Stabilizer::getActualParameters ()

// convert absolute (in st) -> root-link relative
rel_act_zmp = m_robot->rootLink()->R.transpose() * (act_zmp - m_robot->rootLink()->p);
if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
// Actual foot_origin frame =>
act_zmp = foot_origin_rot.transpose() * (act_zmp - foot_origin_pos);
act_cog = foot_origin_rot.transpose() * (act_cog - foot_origin_pos);
Expand Down Expand Up @@ -815,6 +815,7 @@ void Stabilizer::getActualParameters ()
// distribute new ZMP into foot force & moment
std::vector<hrp::Vector3> ref_force, ref_moment;
std::vector<double> limb_gains;
std::vector<hrp::dvector6> ee_forcemoment_distribution_weight;
if (control_mode == MODE_ST) {
std::vector<hrp::Vector3> ee_pos, cop_pos;
std::vector<hrp::Matrix33> ee_rot;
Expand All @@ -832,6 +833,11 @@ void Stabilizer::getActualParameters ()
rel_ee_pos.push_back(foot_origin_rot.transpose() * (ee_pos.back() - foot_origin_pos));
rel_ee_rot.push_back(foot_origin_rot.transpose() * ee_rot.back());
rel_ee_name.push_back(ee_name.back());
// std::cerr << ee_forcemoment_distribution_weight[i] << std::endl;
ee_forcemoment_distribution_weight.push_back(hrp::dvector6::Zero(6,1));
for (size_t j = 0; j < 6; j++) {
ee_forcemoment_distribution_weight[i][j] = ikp.eefm_ee_forcemoment_distribution_weight[j];
}
}
//for ABC ref force
if ( ref_force[0](2) + ref_force[1](2) == 0 ) ref_force[0](2) = ref_force[1](2) = eefm_gravitational_acceleration * total_mass / 2.0;
Expand Down Expand Up @@ -871,6 +877,14 @@ void Stabilizer::getActualParameters ()
eefm_gravitational_acceleration * total_mass, dt,
DEBUGP, std::string(m_profile.instance_name),
(st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP));
} else if (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP2) {
szd->distributeZMPToForceMomentsPseudoInverse2(ref_force, ref_moment,
ee_pos, cop_pos, ee_rot, ee_name, limb_gains,
new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos),
ref_total_force, ref_total_moment,
ee_forcemoment_distribution_weight,
eefm_gravitational_acceleration * total_mass, dt,
DEBUGP, std::string(m_profile.instance_name));
}
// for debug output
new_refzmp = foot_origin_rot.transpose() * (new_refzmp - foot_origin_pos);
Expand Down Expand Up @@ -1045,22 +1059,30 @@ void Stabilizer::getTargetParameters ()
m_robot->rootLink()->R = target_root_R;
m_robot->calcForwardKinematics();
ref_zmp = m_robot->rootLink()->R * hrp::Vector3(m_zmpRef.data.x, m_zmpRef.data.y, m_zmpRef.data.z) + m_robot->rootLink()->p; // base frame -> world frame
if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
// apply inverse system
hrp::Vector3 tmp_ref_zmp = ref_zmp + eefm_zmp_delay_time_const[0] * (ref_zmp - prev_ref_zmp) / dt;
prev_ref_zmp = ref_zmp;
ref_zmp = tmp_ref_zmp;
}
ref_cog = m_robot->calcCM();
ref_total_force = hrp::Vector3::Zero();
ref_total_moment = hrp::Vector3::Zero(); // Total moment around reference ZMP tmp
for (size_t i = 0; i < stikp.size(); i++) {
hrp::Link* target = m_robot->link(stikp[i].target_name);
//target_ee_p[i] = target->p + target->R * stikp[i].localCOPPos;
target_ee_p[i] = target->p + target->R * stikp[i].localp;
target_ee_R[i] = target->R * stikp[i].localR;
ref_total_force += hrp::Vector3(m_ref_wrenches[i].data[0], m_ref_wrenches[i].data[1], m_ref_wrenches[i].data[2]);
// Force/moment diff control
ref_total_moment += (target_ee_p[i]-ref_zmp).cross(hrp::Vector3(m_ref_wrenches[i].data[0], m_ref_wrenches[i].data[1], m_ref_wrenches[i].data[2]));
// Force/moment control
// ref_total_moment += (target_ee_p[i]-ref_zmp).cross(hrp::Vector3(m_ref_wrenches[i].data[0], m_ref_wrenches[i].data[1], m_ref_wrenches[i].data[2]))
// + hrp::Vector3(m_ref_wrenches[i].data[3], m_ref_wrenches[i].data[4], m_ref_wrenches[i].data[5]);
}
// <= Reference world frame

if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
// Reference foot_origin frame =>
hrp::Vector3 foot_origin_pos;
hrp::Matrix33 foot_origin_rot;
Expand Down Expand Up @@ -1637,6 +1659,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
i_stp.eefm_swing_rot_spring_gain.length(stikp.size());
i_stp.eefm_swing_rot_time_const.length(stikp.size());
i_stp.eefm_ee_moment_limit.length(stikp.size());
i_stp.eefm_ee_forcemoment_distribution_weight.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);
Expand All @@ -1647,6 +1670,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
i_stp.eefm_swing_rot_spring_gain[j].length(3);
i_stp.eefm_swing_rot_time_const[j].length(3);
i_stp.eefm_ee_moment_limit[j].length(3);
i_stp.eefm_ee_forcemoment_distribution_weight[j].length(6);
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);
Expand All @@ -1657,6 +1681,8 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
i_stp.eefm_swing_rot_spring_gain[j][i] = stikp[j].eefm_swing_rot_spring_gain(i);
i_stp.eefm_swing_rot_time_const[j][i] = stikp[j].eefm_swing_rot_time_const(i);
i_stp.eefm_ee_moment_limit[j][i] = stikp[j].eefm_ee_moment_limit(i);
i_stp.eefm_ee_forcemoment_distribution_weight[j][i] = stikp[j].eefm_ee_forcemoment_distribution_weight(i);
i_stp.eefm_ee_forcemoment_distribution_weight[j][i+3] = stikp[j].eefm_ee_forcemoment_distribution_weight(i+3);
}
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 @@ -1817,7 +1843,8 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
i_stp.eefm_rot_compensation_limit.length () == stikp.size() &&
i_stp.eefm_swing_rot_spring_gain.length () == stikp.size() &&
i_stp.eefm_swing_rot_time_const.length () == stikp.size() &&
i_stp.eefm_ee_moment_limit.length () == stikp.size() ) {
i_stp.eefm_ee_moment_limit.length () == stikp.size() &&
i_stp.eefm_ee_forcemoment_distribution_weight.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++) {
Expand All @@ -1830,6 +1857,8 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
stikp[j].eefm_swing_rot_spring_gain(i) = i_stp.eefm_swing_rot_spring_gain[j][i];
stikp[j].eefm_swing_rot_time_const(i) = i_stp.eefm_swing_rot_time_const[j][i];
stikp[j].eefm_ee_moment_limit(i) = i_stp.eefm_ee_moment_limit[j][i];
stikp[j].eefm_ee_forcemoment_distribution_weight(i) = i_stp.eefm_ee_forcemoment_distribution_weight[j][i];
stikp[j].eefm_ee_forcemoment_distribution_weight(i+3) = i_stp.eefm_ee_forcemoment_distribution_weight[j][i+3];
}
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 @@ -1948,6 +1977,8 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
<< std::endl;
std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] "
<< "eefm_ee_moment_limit = " << stikp[j].eefm_ee_moment_limit.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] "
<< "eefm_ee_forcemoment_distribution_weight = " << stikp[j].eefm_ee_forcemoment_distribution_weight.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 Expand Up @@ -2054,6 +2085,8 @@ std::string Stabilizer::getStabilizerAlgorithmString (OpenHRP::StabilizerService
return "EEFMQP";
case OpenHRP::StabilizerService::EEFMQPCOP:
return "EEFMQPCOP";
case OpenHRP::StabilizerService::EEFMQPCOP2:
return "EEFMQPCOP2";
default:
return "";
}
Expand Down
2 changes: 2 additions & 0 deletions rtc/Stabilizer/Stabilizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -248,6 +248,7 @@ class Stabilizer
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, eefm_swing_rot_time_const, eefm_swing_pos_time_const, eefm_ee_moment_limit;
double eefm_pos_compensation_limit, eefm_rot_compensation_limit;
hrp::Vector3 ref_force, ref_moment;
hrp::dvector6 eefm_ee_forcemoment_distribution_weight;
double swing_support_gain, support_time;
// IK parameter
double avoid_gain, reference_gain;
Expand Down Expand Up @@ -296,6 +297,7 @@ class Stabilizer
double eefm_pos_time_const_swing, eefm_pos_transition_time, eefm_pos_margin_time, eefm_gravitational_acceleration, eefm_ee_pos_error_p_gain, eefm_ee_rot_error_p_gain;
hrp::Vector3 new_refzmp, rel_cog, ref_zmp_aux;
hrp::Vector3 pos_ctrl;
hrp::Vector3 ref_total_force, ref_total_moment;
double total_mass, transition_time, cop_check_margin, contact_decision_threshold;
std::vector<double> cp_check_margin, tilt_margin;
OpenHRP::StabilizerService::EmergencyCheckMode emergency_check_mode;
Expand Down
Loading

0 comments on commit e435265

Please sign in to comment.