Skip to content

Commit

Permalink
[AutoBalancerService.idl, StabilizerService.idl, hrpsys_config.py, Au…
Browse files Browse the repository at this point in the history 
…toBalancer.*, AutoBalancer.*, Stabilizer.*] modify footsteps based on CP
  • Loading branch information
@YutaKojio
YutaKojio committed May 2, 2017
1 parent 1c2dc2e commit ed02df8
Show file tree
Hide file tree
Showing 9 changed files with 145 additions and 8 deletions.
9 changes: 9 additions & 0 deletions idl/AutoBalancerService.idl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ module OpenHRP
{
typedef sequence<double, 3> DblSequence3;
//typedef sequence<double, 4> DblSequence4;
typedef double DblArray2[2];
typedef double DblArray3[3];
typedef double DblArray4[4];
typedef double DblArray5[5];
Expand Down Expand Up @@ -249,6 +250,14 @@ module OpenHRP
StrideLimitationType stride_limitation_type;
/// Leg margin between foot end effector position and foot edge (front, rear, outside, inside) [m]
DblArray4 leg_margin;
/// Feedback gain when modifying footsteps based on Capture Point
double footstep_modification_gain;
/// Whether modify footsteps based on Capture Point
boolean modify_footsteps;
/// CP check margin when modifying footsteps (x, y) [m]
DblArray2 cp_check_margin;
/// Margin time ratio for footstep modification before landing [s]
double margin_time_ratio;
};

/**
Expand Down
2 changes: 2 additions & 0 deletions idl/StabilizerService.idl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,6 +208,8 @@ module OpenHRP
DblArray2 ref_capture_point;
/// act_CP [m] (x,y) (foot_origin relative coordinate)
DblArray2 act_capture_point;
/// CP_offset [m] (x,y) (foot_origin relative coordinate)
DblArray2 cp_offset;
/// contact decision threshold [N]
double contact_decision_threshold;
/// Foot origin position offset
Expand Down
2 changes: 2 additions & 0 deletions python/hrpsys_config.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -401,6 +401,8 @@ def connectComps(self):
if self.es:
connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal"))
connectPorts(self.st.port("emergencySignal"), self.abc.port("emergencySignal"))
connectPorts(self.st.port("diffCapturePoint"), self.abc.port("diffCapturePoint"))
connectPorts(self.st.port("actContactStates"), self.abc.port("actContactStates"))

# ref force moment connection
for sen in self.getForceSensorNames():
Expand Down
26 changes: 26 additions & 0 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager)
m_zmpIn("zmpIn", m_zmp),
m_optionalDataIn("optionalData", m_optionalData),
m_emergencySignalIn("emergencySignal", m_emergencySignal),
m_diffCPIn("diffCapturePoint", m_diffCP),
m_actContactStatesIn("actContactStates", m_actContactStates),
m_qOut("q", m_qRef),
m_zmpOut("zmpOut", m_zmp),
m_basePosOut("basePosOut", m_basePos),
Expand Down Expand Up @@ -90,6 +92,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
addInPort("zmpIn", m_zmpIn);
addInPort("optionalData", m_optionalDataIn);
addInPort("emergencySignal", m_emergencySignalIn);
addInPort("diffCapturePoint", m_diffCPIn);
addInPort("actContactStates", m_actContactStatesIn);

// Set OutPort buffer
addOutPort("q", m_qOut);
Expand Down Expand Up @@ -439,6 +443,18 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
// gg->emergency_stop();
// }
}
if (m_diffCPIn.isNew()) {
m_diffCPIn.read();
gg->set_diff_cp(hrp::Vector3(m_diffCP.data.x, m_diffCP.data.y, m_diffCP.data.z));
}
if (m_actContactStatesIn.isNew()) {
m_actContactStatesIn.read();
std::vector<bool> tmp_contacts(m_actContactStates.data.length());
for (size_t i = 0; i < m_actContactStates.data.length(); i++) {
tmp_contacts[i] = m_actContactStates.data[i];
}
gg->set_act_contact_states(tmp_contacts);
}

// Calculation
Guard guard(m_mutex);
Expand Down Expand Up @@ -1471,6 +1487,10 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai
gg->set_stride_limitation_for_circle_type(i_param.stride_limitation_for_circle_type);
gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
gg->set_use_stride_limitation(i_param.use_stride_limitation);
gg->set_footstep_modification_gain(i_param.footstep_modification_gain);
gg->set_modify_footsteps(i_param.modify_footsteps);
gg->set_cp_check_margin(i_param.cp_check_margin);
gg->set_margin_time_ratio(i_param.margin_time_ratio);
if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
gg->set_stride_limitation_type(SQUARE);
} else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
Expand Down Expand Up @@ -1559,6 +1579,12 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener
i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
}
i_param.use_stride_limitation = gg->get_use_stride_limitation();
i_param.footstep_modification_gain = gg->get_footstep_modification_gain();
i_param.modify_footsteps = gg->get_modify_footsteps();
for (size_t i=0; i<2; i++) {
i_param.cp_check_margin[i] = gg->get_cp_check_margin(i);
}
i_param.margin_time_ratio = gg->get_margin_time_ratio();
if (gg->get_stride_limitation_type() == SQUARE) {
i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
} else if (gg->get_stride_limitation_type() == CIRCLE) {
Expand Down
4 changes: 4 additions & 0 deletions rtc/AutoBalancer/AutoBalancer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -417,6 +417,10 @@ class AutoBalancer
std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
TimedLong m_emergencySignal;
InPort<TimedLong> m_emergencySignalIn;
TimedPoint3D m_diffCP;
InPort<TimedPoint3D> m_diffCPIn;
TimedBooleanSeq m_actContactStates;
InPort<TimedBooleanSeq> m_actContactStatesIn;
// for debug
TimedPoint3D m_cog;

Expand Down
55 changes: 55 additions & 0 deletions rtc/AutoBalancer/GaitGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -611,6 +611,8 @@ namespace rats
overwrite_refzmp_queue(overwrite_footstep_nodes_list);
overwrite_footstep_nodes_list.clear();
}
// modify footsteps based on diff_cp
if(modify_footsteps) modify_footsteps_for_recovery();

if ( !solved ) {
hrp::Vector3 rzmp;
Expand Down Expand Up @@ -680,6 +682,59 @@ namespace rats
cur_fs.worldcoords.pos = prev_fs.worldcoords.pos + prev_fs.worldcoords.rot * cur_fs.worldcoords.pos;
};

void gait_generator::modify_footsteps_for_recovery ()
{
if (isfinite(diff_cp(0)) && isfinite(diff_cp(1))) {
// calculate diff_cp
hrp::Vector3 tmp_diff_cp;
for (size_t i = 0; i < 2; i++) {
if (std::fabs(diff_cp(i)) > cp_check_margin[i]) {
is_emergency_walking[i] = true;
tmp_diff_cp(i) = diff_cp(i) - cp_check_margin[i] * diff_cp(i)/std::fabs(diff_cp(i));
} else {
is_emergency_walking[i] = false;
}
}
if (lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-2) {
static int not_emergency_cnt;
// calculate sum of preview_f
static double preview_f_sum;
if (lcg.get_lcg_count() == static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1) {
preview_f_sum = preview_controller_ptr->get_preview_f(preview_controller_ptr->get_delay());
for (size_t i = preview_controller_ptr->get_delay()-1; i >= lcg.get_lcg_count()+1; i--) {
preview_f_sum += preview_controller_ptr->get_preview_f(i);
}
}
if (lcg.get_lcg_count() <= preview_controller_ptr->get_delay()) {
preview_f_sum += preview_controller_ptr->get_preview_f(lcg.get_lcg_count());
}
// calculate modified footstep position
double preview_db = 1/6.0 * dt * dt * dt + 1/2.0 * dt * dt * 1/std::sqrt(gravitational_acceleration / (cog(2) - refzmp(2)));
hrp::Vector3 d_footstep = -1/preview_f_sum * 1/preview_db * footstep_modification_gain * tmp_diff_cp;
d_footstep(2) = 0.0;
// overwrite footsteps
if (lcg.get_lcg_count() <= static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1 &&
lcg.get_lcg_count() >= static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (default_double_support_ratio_after + margin_time_ratio)) - 1 &&
!(lcg.get_lcg_count() <= static_cast<size_t>(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1 && act_contact_states[0] && act_contact_states[1])) {
// stride limitation check
hrp::Vector3 orig_footstep_pos = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos;
for (size_t i = 0; i < 2; i++) {
if (is_emergency_walking[i]) footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos(i) += d_footstep(i);
}
limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front(), overwritable_stride_limitation);
d_footstep = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos - orig_footstep_pos;
for (size_t i = lcg.get_footstep_index()+1; i < footstep_nodes_list.size(); i++) {
footstep_nodes_list[i].front().worldcoords.pos += d_footstep;
}
}
overwrite_footstep_nodes_list.insert(overwrite_footstep_nodes_list.end(), footstep_nodes_list.begin()+lcg.get_footstep_index(), footstep_nodes_list.end());
// overwrite zmp
overwrite_refzmp_queue(overwrite_footstep_nodes_list);
overwrite_footstep_nodes_list.clear();
}
}
}

/* generate vector of step_node from :go-pos params
* x, y and theta are simply divided by using stride params
* unit system -> x [mm], y [mm], theta [deg]
Expand Down
34 changes: 29 additions & 5 deletions rtc/AutoBalancer/GaitGenerator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1051,8 +1051,10 @@ namespace rats
std::map<leg_type, std::string> leg_type_map;
coordinates initial_foot_mid_coords;
bool solved;
double leg_margin[4], stride_limitation_for_circle_type[5], overwritable_stride_limitation[5];
bool use_stride_limitation;
double leg_margin[4], stride_limitation_for_circle_type[5], overwritable_stride_limitation[5], footstep_modification_gain, cp_check_margin[2], margin_time_ratio;
bool use_stride_limitation, is_emergency_walking[2], modify_footsteps;
hrp::Vector3 diff_cp;
std::vector<bool> act_contact_states;
stride_limitation_type default_stride_limitation_type;

/* preview controller parameters */
Expand Down Expand Up @@ -1100,18 +1102,20 @@ namespace rats
const double _stride_fwd_x, const double _stride_y, const double _stride_theta, const double _stride_bwd_x)
: footstep_nodes_list(), overwrite_footstep_nodes_list(), rg(_dt), lcg(_dt),
footstep_param(_leg_pos, _stride_fwd_x, _stride_y, _stride_theta, _stride_bwd_x),
vel_param(), offset_vel_param(), thtc(), cog(hrp::Vector3::Zero()), refzmp(hrp::Vector3::Zero()), prev_que_rzmp(hrp::Vector3::Zero()),
vel_param(), offset_vel_param(), thtc(), cog(hrp::Vector3::Zero()), refzmp(hrp::Vector3::Zero()), prev_que_rzmp(hrp::Vector3::Zero()), diff_cp(hrp::Vector3::Zero()),
dt(_dt), all_limbs(_all_limbs), default_step_time(1.0), default_double_support_ratio_before(0.1), default_double_support_ratio_after(0.1), default_double_support_static_ratio_before(0.0), default_double_support_static_ratio_after(0.0), default_double_support_ratio_swing_before(0.1), default_double_support_ratio_swing_after(0.1), gravitational_acceleration(DEFAULT_GRAVITATIONAL_ACCELERATION),
finalize_count(0), optional_go_pos_finalize_footstep_num(0), overwrite_footstep_index(0), overwritable_footstep_index_offset(1),
velocity_mode_flg(VEL_IDLING), emergency_flg(IDLING),
use_inside_step_limitation(true), use_stride_limitation(false), default_stride_limitation_type(SQUARE),
velocity_mode_flg(VEL_IDLING), emergency_flg(IDLING), margin_time_ratio(0.01), footstep_modification_gain(5e-6),
use_inside_step_limitation(true), use_stride_limitation(false), modify_footsteps(false), default_stride_limitation_type(SQUARE),
preview_controller_ptr(NULL) {
swing_foot_zmp_offsets = boost::assign::list_of<hrp::Vector3>(hrp::Vector3::Zero());
prev_que_sfzos = boost::assign::list_of<hrp::Vector3>(hrp::Vector3::Zero());
leg_type_map = boost::assign::map_list_of<leg_type, std::string>(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm");
for (size_t i = 0; i < 4; i++) leg_margin[i] = 0.1;
for (size_t i = 0; i < 5; i++) stride_limitation_for_circle_type[i] = 0.2;
for (size_t i = 0; i < 5; i++) overwritable_stride_limitation[i] = 0.2;
for (size_t i = 0; i < 2; i++) is_emergency_walking[i] = false;
for (size_t i = 0; i < 2; i++) cp_check_margin[i] = 0.025;
};
~gait_generator () {
if ( preview_controller_ptr != NULL ) {
Expand All @@ -1125,6 +1129,7 @@ namespace rats
const double delay = 1.6);
bool proc_one_tick ();
void limit_stride (step_node& cur_fs, const step_node& prev_fs, const double (&limit)[5]) const;
void modify_footsteps_for_recovery ();
void append_footstep_nodes (const std::vector<std::string>& _legs, const std::vector<coordinates>& _fss)
{
std::vector<step_node> tmp_sns;
Expand Down Expand Up @@ -1276,7 +1281,22 @@ namespace rats
overwritable_stride_limitation[i] = _overwritable_stride_limitation[i];
}
};
void set_footstep_modification_gain (const double _footstep_modification_gain) { footstep_modification_gain = _footstep_modification_gain; };
void set_cp_check_margin (const double (&_cp_check_margin)[2]) {
for (size_t i=0; i < 2; i++) {
cp_check_margin[i] = _cp_check_margin[i];
}
};
void set_act_contact_states (const std::vector<bool>& _act_contact_states) {
if (act_contact_states.empty()) act_contact_states.resize(_act_contact_states.size());
for (size_t i = 0; i < act_contact_states.size(); i++) {
act_contact_states[i] = _act_contact_states[i];
}
};
void set_use_stride_limitation (const bool _use_stride_limitation) { use_stride_limitation = _use_stride_limitation; };
void set_modify_footsteps (const bool _modify_footsteps) { modify_footsteps = _modify_footsteps; };
void set_margin_time_ratio (const double _margin_time_ratio) { margin_time_ratio = _margin_time_ratio; };
void set_diff_cp (const hrp::Vector3 _cp) { diff_cp = _cp; };
void set_stride_limitation_type (const stride_limitation_type _tmp) { default_stride_limitation_type = _tmp; };
void set_toe_check_thre (const double _a) { thtc.set_toe_check_thre(_a); };
void set_heel_check_thre (const double _a) { thtc.set_heel_check_thre(_a); };
Expand Down Expand Up @@ -1472,6 +1492,10 @@ namespace rats
double get_leg_margin (const size_t idx) const { return leg_margin[idx]; };
double get_stride_limitation_for_circle_type (const size_t idx) const { return stride_limitation_for_circle_type[idx]; };
double get_overwritable_stride_limitation (const size_t idx) const { return overwritable_stride_limitation[idx]; };
double get_footstep_modification_gain () const { return footstep_modification_gain; };
double get_cp_check_margin (const size_t idx) const { return cp_check_margin[idx]; };
bool get_modify_footsteps () const { return modify_footsteps; };
double get_margin_time_ratio () const { return margin_time_ratio; };
bool get_use_stride_limitation () const { return use_stride_limitation; };
stride_limitation_type get_stride_limitation_type () const { return default_stride_limitation_type; };
double get_toe_check_thre () const { return thtc.get_toe_check_thre(); };
Expand Down
15 changes: 14 additions & 1 deletion rtc/Stabilizer/Stabilizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@ Stabilizer::Stabilizer(RTC::Manager* manager)
m_zmpOut("zmp", m_zmp),
m_refCPOut("refCapturePoint", m_refCP),
m_actCPOut("actCapturePoint", m_actCP),
m_diffCPOut("diffCapturePoint", m_diffCP),
m_actContactStatesOut("actContactStates", m_actContactStates),
m_COPInfoOut("COPInfo", m_COPInfo),
m_emergencySignalOut("emergencySignal", m_emergencySignal),
Expand Down Expand Up @@ -133,6 +134,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
addOutPort("zmp", m_zmpOut);
addOutPort("refCapturePoint", m_refCPOut);
addOutPort("actCapturePoint", m_actCPOut);
addOutPort("diffCapturePoint", m_diffCPOut);
addOutPort("actContactStates", m_actContactStatesOut);
addOutPort("COPInfo", m_COPInfoOut);
addOutPort("emergencySignal", m_emergencySignalOut);
Expand Down Expand Up @@ -381,6 +383,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
eefm_gravitational_acceleration = 9.80665; // [m/s^2]
cop_check_margin = 20.0*1e-3; // [m]
cp_check_margin.resize(4, 30*1e-3); // [m]
cp_offset = hrp::Vector3(0.0, 0.0, 0.0); // [m]
tilt_margin.resize(2, 30 * M_PI / 180); // [rad]
contact_decision_threshold = 50; // [N]
eefm_use_force_difference_control = true;
Expand Down Expand Up @@ -653,6 +656,14 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id)
m_actCP.data.z = rel_act_cp(2);
m_actCP.tm = m_qRef.tm;
m_actCPOut.write();
{
hrp::Vector3 tmp_diff_cp = ref_foot_origin_rot * (ref_cp - act_cp - cp_offset);
m_diffCP.data.x = tmp_diff_cp(0);
m_diffCP.data.y = tmp_diff_cp(1);
m_diffCP.data.z = tmp_diff_cp(2);
m_diffCP.tm = m_qRef.tm;
m_diffCPOut.write();
}
m_actContactStates.tm = m_qRef.tm;
m_actContactStatesOut.write();
m_COPInfo.tm = m_qRef.tm;
Expand Down Expand Up @@ -1183,7 +1194,7 @@ void Stabilizer::getTargetParameters ()
} else {
ref_cogvel = (ref_cog - prev_ref_cog)/dt;
}
prev_ref_foot_origin_rot = foot_origin_rot;
prev_ref_foot_origin_rot = ref_foot_origin_rot = foot_origin_rot;
for (size_t i = 0; i < stikp.size(); i++) {
stikp[i].target_ee_diff_p = foot_origin_rot.transpose() * (target_ee_p[i] - foot_origin_pos);
stikp[i].target_ee_diff_r = foot_origin_rot.transpose() * target_ee_R[i];
Expand Down Expand Up @@ -1797,6 +1808,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
i_stp.eefm_body_attitude_control_gain[i] = eefm_body_attitude_control_gain[i];
i_stp.ref_capture_point[i] = ref_cp(i);
i_stp.act_capture_point[i] = act_cp(i);
i_stp.cp_offset[i] = cp_offset(i);
}
i_stp.eefm_pos_time_const_support.length(stikp.size());
i_stp.eefm_pos_damping_gain.length(stikp.size());
Expand Down Expand Up @@ -1991,6 +2003,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
eefm_body_attitude_control_time_const[i] = i_stp.eefm_body_attitude_control_time_const[i];
ref_cp(i) = i_stp.ref_capture_point[i];
act_cp(i) = i_stp.act_capture_point[i];
cp_offset(i) = i_stp.cp_offset[i];
}
bool is_damping_parameter_ok = true;
if ( i_stp.eefm_pos_damping_gain.length () == stikp.size() &&
Expand Down
Loading

0 comments on commit ed02df8

Please sign in to comment.