Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Calculate LPF parameter based on controller time step dt and enable to set cut-off frequency from IDL. #389

Merged
merged 1 commit into from Nov 1, 2014
Merged

Calculate LPF parameter based on controller time step dt and enable to set cut-off frequency from IDL. #389

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions idl/StabilizerService.idl
View file
Open in desktop
Expand Up @@ -83,6 +83,8 @@ module OpenHRP
DblArray2 eefm_body_attitude_control_gain;
/// Time constant for body attitude control [s] (roll, pitch) for EEFM.
DblArray2 eefm_body_attitude_control_time_const;
/// Cutoff frequency of LPF in calculation of COG velocity [Hz]
double eefm_cogvel_cutoff_freq;
// common
STAlgorithm st_algorithm;
ControllerMode controller_mode;
Expand Down
8 changes: 6 additions & 2 deletions rtc/Stabilizer/Stabilizer.cpp
View file
Open in desktop
Expand Up @@ -214,6 +214,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
eefm_pos_margin_time = 0.02;
eefm_zmp_delay_time_const[0] = eefm_zmp_delay_time_const[1] = 0.04;
eefm_leg_inside_margin = 0.065; // [m]
eefm_cogvel_cutoff_freq = 35.3678; //[Hz]

// parameters for RUNST
double ke = 0, tc = 0;
Expand Down Expand Up @@ -673,8 +674,8 @@ void Stabilizer::getActualParameters ()
act_cogvel = (act_cog - prev_act_cog)/dt;
}
prev_act_foot_origin_rot = foot_origin_rot;
//act_cogvel = 0.8 * prev_act_cogvel + 0.2 * act_cogvel;
act_cogvel = 0.9 * prev_act_cogvel + 0.1 * act_cogvel;
double const_param = 2 * M_PI * eefm_cogvel_cutoff_freq * dt;
act_cogvel = 1.0/(1+const_param) * prev_act_cogvel + const_param/(1+const_param) * act_cogvel;
prev_act_cog = act_cog;
prev_act_cogvel = act_cogvel;
//act_root_rot = m_robot->rootLink()->R;
Expand Down Expand Up @@ -1374,6 +1375,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
i_stp.eefm_pos_transition_time = eefm_pos_transition_time;
i_stp.eefm_pos_margin_time = eefm_pos_margin_time;
i_stp.eefm_leg_inside_margin = eefm_leg_inside_margin;
i_stp.eefm_cogvel_cutoff_freq = eefm_cogvel_cutoff_freq;
i_stp.st_algorithm = st_algorithm;
switch(control_mode) {
case MODE_IDLE: i_stp.controller_mode = OpenHRP::StabilizerService::MODE_IDLE; break;
Expand Down Expand Up @@ -1433,6 +1435,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
eefm_pos_transition_time = i_stp.eefm_pos_transition_time;
eefm_pos_margin_time = i_stp.eefm_pos_margin_time;
eefm_leg_inside_margin = i_stp.eefm_leg_inside_margin;
eefm_cogvel_cutoff_freq = i_stp.eefm_cogvel_cutoff_freq;
std::cerr << "[" << m_profile.instance_name << "] eefm_k1 = [" << eefm_k1[0] << ", " << eefm_k1[1] << "]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] eefm_k2 = [" << eefm_k2[0] << ", " << eefm_k2[1] << "]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] eefm_k3 = [" << eefm_k3[0] << ", " << eefm_k3[1] << "]" << std::endl;
Expand All @@ -1445,6 +1448,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
<< "eefm_pos_time_const_swing = " << eefm_pos_time_const_swing << "[s]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] eefm_pos_transition_time = " << eefm_pos_transition_time << "[s], eefm_pos_margin_time = " << eefm_pos_margin_time << "[s]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] eefm_leg_inside_margin = " << eefm_leg_inside_margin << "[m]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] eefm_cogvel_cutoff_freq = " << eefm_cogvel_cutoff_freq << "[Hz]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] COMMON" << std::endl;
if (control_mode == MODE_IDLE) {
st_algorithm = i_stp.st_algorithm;
Expand Down
2 changes: 1 addition & 1 deletion rtc/Stabilizer/Stabilizer.h
View file
Open in desktop
Expand Up @@ -250,7 +250,7 @@ class Stabilizer
double rdx, rdy, rx, ry;
// EEFM ST
double eefm_k1[2], eefm_k2[2], eefm_k3[2], eefm_zmp_delay_time_const[2], eefm_body_attitude_control_gain[2], eefm_body_attitude_control_time_const[2];
double eefm_rot_damping_gain, eefm_rot_time_const, eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_pos_time_const_swing, eefm_pos_transition_time, eefm_pos_margin_time, eefm_leg_inside_margin;
double eefm_rot_damping_gain, eefm_rot_time_const, eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_pos_time_const_swing, eefm_pos_transition_time, eefm_pos_margin_time, eefm_leg_inside_margin, eefm_cogvel_cutoff_freq;
hrp::Vector3 d_foot_rpy[2], new_refzmp, rel_cog, ref_zmp_aux;
hrp::Vector3 ref_foot_force[2];
hrp::Vector3 ref_foot_moment[2];
Expand Down