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

change root link height depending on limb length #1069

Merged
merged 4 commits into from
Nov 9, 2016
Merged

change root link height depending on limb length #1069

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ac17aed
[Stabilizer.*] change root link height depending on limb length
YutaKojio Nov 6, 2016
f65f125
[StabilizerService.idl, Stabilizer.*] add idl for changing root link …
YutaKojio Nov 6, 2016
67f951c
[Stabilizer.cpp] change root link height on world frame not root link…
YutaKojio Nov 6, 2016
c9f312e
[StabilizerService.idl, Stabilizer.*] rename variable and add functio…
YutaKojio Nov 8, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions idl/StabilizerService.idl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -219,6 +219,14 @@ module OpenHRP
boolean is_estop_while_walking;
/// Sequence for all end-effectors' ik limb parameters
sequence<IKLimbParameters> ik_limb_parameters;
/// Whether change root link height for avoiding limb stretch
boolean use_limb_stretch_avoidance;
/// Limb stretch avoidance time constant [s]
double limb_stretch_avoidance_time_const;
/// Root link height change limitation for avoiding limb stretch [m/s] (lower, upper)
DblArray2 limb_stretch_avoidance_vlimit;
/// Sequence of limb length margin from max limb length [m]
sequence<double> limb_length_margin;
};

/**
Expand Down
109 changes: 85 additions & 24 deletions rtc/Stabilizer/Stabilizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -328,6 +328,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
#define deg2rad(x) ((x) * M_PI / 180.0)
for (size_t i = 0; i < stikp.size(); i++) {
STIKParam& ikp = stikp[i];
hrp::Link* root = m_robot->link(ikp.target_name);
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);
Expand All @@ -347,6 +348,13 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
ikp.eefm_ee_forcemoment_distribution_weight[j+3] = 1e-2; // Moment
}
}
ikp.max_limb_length = 0.0;
while (!root->isRoot()) {
ikp.max_limb_length += root->b.norm();
ikp.parent_name = root->name;
root = root->parent;
}
ikp.limb_length_margin = 0.13;
}
eefm_swing_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5);
eefm_swing_pos_damping_gain = hrp::Vector3(33600, 33600, 7000);
Expand All @@ -373,6 +381,10 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
is_walking = false;
is_estop_while_walking = false;
sbp_cog_offset = hrp::Vector3(0.0, 0.0, 0.0);
use_limb_stretch_avoidance = false;
limb_stretch_avoidance_time_const = 1.5;
limb_stretch_avoidance_vlimit[0] = -100 * 1e-3 * dt; // lower limit
limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * dt; // upper limit

// parameters for RUNST
double ke = 0, tc = 0;
Expand Down Expand Up @@ -1490,30 +1502,38 @@ void Stabilizer::calcEEForceMomentControl() {

// solveIK
// IK target is link origin pos and rot, not ee pos and rot.
for (size_t jj = 0; jj < 3; jj++) {
for (size_t i = 0; i < stikp.size(); i++) {
if (is_ik_enable[i]) {
hrp::Vector3 tmpp;
hrp::Matrix33 tmpR;
// Add damping_control compensation to target value
if (is_feedback_control_enable[i]) {
rats::rotm3times(tmpR, target_ee_R[i], hrp::rotFromRpy(-stikp[i].ee_d_foot_rpy(0), -stikp[i].ee_d_foot_rpy(1), 0));
// foot force difference control version
// total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl;
// foot force independent damping control
tmpp = target_ee_p[i] - current_d_foot_pos[i];
} else {
tmpp = target_ee_p[i];
tmpR = target_ee_R[i];
}
// Add swing ee compensation
rats::rotm3times(tmpR, tmpR, hrp::rotFromRpy(stikp[i].d_rpy_swing));
tmpp = tmpp + foot_origin_rot * stikp[i].d_pos_swing;
// IK
jpe_v[i]->calcInverseKinematics2Loop(tmpp, tmpR, 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain,
//stikp[i].localCOPPos;
stikp[i].localp,
stikp[i].localR);
std::vector<hrp::Vector3> tmpp(stikp.size());
std::vector<hrp::Matrix33> tmpR(stikp.size());
double tmp_d_pos_z_root = 0.0;
for (size_t i = 0; i < stikp.size(); i++) {
if (is_ik_enable[i]) {
// Add damping_control compensation to target value
if (is_feedback_control_enable[i]) {
rats::rotm3times(tmpR[i], target_ee_R[i], hrp::rotFromRpy(-stikp[i].ee_d_foot_rpy(0), -stikp[i].ee_d_foot_rpy(1), 0));
// foot force difference control version
// total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl;
// foot force independent damping control
tmpp[i] = target_ee_p[i] - current_d_foot_pos[i];
} else {
tmpp[i] = target_ee_p[i];
tmpR[i] = target_ee_R[i];
}
// Add swing ee compensation
rats::rotm3times(tmpR[i], tmpR[i], hrp::rotFromRpy(stikp[i].d_rpy_swing));
tmpp[i] = tmpp[i] + foot_origin_rot * stikp[i].d_pos_swing;
}
}

if (use_limb_stretch_avoidance) limbStretchAvoidanceControl(tmpp ,tmpR);

// IK
for (size_t i = 0; i < stikp.size(); i++) {
if (is_ik_enable[i]) {
for (size_t jj = 0; jj < 3; jj++) {
jpe_v[i]->calcInverseKinematics2Loop(tmpp[i], tmpR[i], 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain,
//stikp[i].localCOPPos;
stikp[i].localp,
stikp[i].localR);
}
}
}
Expand Down Expand Up @@ -1566,6 +1586,28 @@ void Stabilizer::calcSwingEEModification ()
}
};

void Stabilizer::limbStretchAvoidanceControl (const std::vector<hrp::Vector3>& ee_p, const std::vector<hrp::Matrix33>& ee_R)
{
double tmp_d_pos_z_root = 0.0;
for (size_t i = 0; i < stikp.size(); i++) {
if (is_ik_enable[i]) {
// Check whether inside limb length limitation
hrp::Link* parent_link = m_robot->link(stikp[i].parent_name);
hrp::Vector3 targetp = (ee_p[i] - ee_R[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame)
double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin;
double tmp = limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) - targetp(1) * targetp(1);
if (targetp.norm() > limb_length_limitation && tmp >= 0) {
tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, targetp(2) + std::sqrt(tmp));
}
}
}
// Change root link height depending on limb length
double prev_d_pos_z_root = d_pos_z_root;
d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const) : tmp_d_pos_z_root;
d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + limb_stretch_avoidance_vlimit[0], prev_d_pos_z_root + limb_stretch_avoidance_vlimit[1]);
m_robot->rootLink()->p(2) += d_pos_z_root;
}

// Damping control functions
// Basically Equation (14) in the paper [1]
double Stabilizer::calcDampingControl (const double tau_d, const double tau, const double prev_d,
Expand All @@ -1580,6 +1622,12 @@ hrp::Vector3 Stabilizer::calcDampingControl (const hrp::Vector3& prev_d, const h
return (- prev_d.cwiseQuotient(TT)) * dt + prev_d;
};

// Retrieving only
double Stabilizer::calcDampingControl (const double prev_d, const double TT)
{
return - 1/TT * prev_d * dt + prev_d;
};

hrp::Vector3 Stabilizer::calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d,
const hrp::Vector3& DD, const hrp::Vector3& TT)
{
Expand Down Expand Up @@ -1822,6 +1870,12 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
}
i_stp.emergency_check_mode = emergency_check_mode;
i_stp.end_effector_list.length(stikp.size());
i_stp.use_limb_stretch_avoidance = use_limb_stretch_avoidance;
i_stp.limb_stretch_avoidance_time_const = limb_stretch_avoidance_time_const;
i_stp.limb_length_margin.length(stikp.size());
for (size_t i = 0; i < 2; i++) {
i_stp.limb_stretch_avoidance_vlimit[i] = limb_stretch_avoidance_vlimit[i];
}
for (size_t i = 0; i < stikp.size(); i++) {
const rats::coordinates cur_ee = rats::coordinates(stikp.at(i).localp, stikp.at(i).localR);
OpenHRP::AutoBalancerService::Footstep ret_ee;
Expand All @@ -1837,6 +1891,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
ret_ee.leg = stikp.at(i).ee_name.c_str();
// set
i_stp.end_effector_list[i] = ret_ee;
i_stp.limb_length_margin[i] = stikp[i].limb_length_margin;
}
i_stp.ik_limb_parameters.length(jpe_v.size());
for (size_t i = 0; i < jpe_v.size(); i++) {
Expand Down Expand Up @@ -1975,6 +2030,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
for (size_t i = 0; i < stikp.size(); i++) {
stikp[i].target_ee_diff_p_filter->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq);
stikp[i].target_ee_diff_r_filter->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq);
stikp[i].limb_length_margin = i_stp.limb_length_margin[i];
}
setBoolSequenceParam(is_ik_enable, i_stp.is_ik_enable, std::string("is_ik_enable"));
setBoolSequenceParam(is_feedback_control_enable, i_stp.is_feedback_control_enable, std::string("is_feedback_control_enable"));
Expand All @@ -1991,6 +2047,11 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
}
contact_decision_threshold = i_stp.contact_decision_threshold;
is_estop_while_walking = i_stp.is_estop_while_walking;
use_limb_stretch_avoidance = i_stp.use_limb_stretch_avoidance;
limb_stretch_avoidance_time_const = i_stp.limb_stretch_avoidance_time_const;
for (size_t i = 0; i < 2; i++) {
limb_stretch_avoidance_vlimit[i] = i_stp.limb_stretch_avoidance_vlimit[i];
}
if (control_mode == MODE_IDLE) {
for (size_t i = 0; i < i_stp.end_effector_list.length(); i++) {
std::vector<STIKParam>::iterator it = std::find_if(stikp.begin(), stikp.end(), (&boost::lambda::_1->* &std::vector<STIKParam>::value_type::ee_name == std::string(i_stp.end_effector_list[i].leg)));
Expand Down
9 changes: 6 additions & 3 deletions rtc/Stabilizer/Stabilizer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,7 @@ class Stabilizer
void calcTPCC();
void calcEEForceMomentControl();
void calcSwingEEModification ();
void limbStretchAvoidanceControl (const std::vector<hrp::Vector3>& ee_p, const std::vector<hrp::Matrix33>& ee_R);
void getParameter(OpenHRP::StabilizerService::stParam& i_stp);
void setParameter(const OpenHRP::StabilizerService::stParam& i_stp);
void setBoolSequenceParam (std::vector<bool>& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name);
Expand All @@ -132,6 +133,7 @@ class Stabilizer
double calcDampingControl (const double tau_d, const double tau, const double prev_d,
const double DD, const double TT);
hrp::Vector3 calcDampingControl (const hrp::Vector3& prev_d, const hrp::Vector3& TT);
double calcDampingControl (const double prev_d, const double TT);
hrp::Vector3 calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d,
const hrp::Vector3& DD, const hrp::Vector3& TT);
double vlimit(double value, double llimit_value, double ulimit_value);
Expand Down Expand Up @@ -247,6 +249,7 @@ class Stabilizer
std::string target_name; // Name of end link
std::string ee_name; // Name of ee (e.g., rleg, lleg, ...)
std::string sensor_name; // Name of force sensor in the limb
std::string parent_name; // Name of parent ling in the limb
hrp::Vector3 localp; // Position of ee in end link frame (^{l}p_e = R_l^T (p_e - p_l))
hrp::Vector3 localCOPPos; // Position offset of reference COP in end link frame (^{l}p_{cop} = R_l^T (p_{cop} - p_l) - ^{l}p_e)
hrp::Matrix33 localR; // Rotation of ee in end link frame (^{l}R_e = R_l^T R_e)
Expand All @@ -262,7 +265,7 @@ class Stabilizer
hrp::Vector3 target_ee_diff_p, d_pos_swing, d_rpy_swing, prev_d_pos_swing, prev_d_rpy_swing;
hrp::Matrix33 target_ee_diff_r;
// IK parameter
double avoid_gain, reference_gain;
double avoid_gain, reference_gain, max_limb_length, limb_length_margin;
};
enum cmode {MODE_IDLE, MODE_AIR, MODE_ST, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_AIR} control_mode;
// members
Expand All @@ -278,7 +281,7 @@ class Stabilizer
std::vector<double> toeheel_ratio;
double dt;
int transition_count, loop;
bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error;
bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error, use_limb_stretch_avoidance;
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;
Expand All @@ -290,7 +293,7 @@ class Stabilizer
hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset;
hrp::Vector3 foot_origin_offset[2];
std::vector<double> prev_act_force_z;
double zmp_origin_off, transition_smooth_gain;
double zmp_origin_off, transition_smooth_gain, d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2];
boost::shared_ptr<FirstOrderLowPassFilter<hrp::Vector3> > act_cogvel_filter;
OpenHRP::StabilizerService::STAlgorithm st_algorithm;
SimpleZMPDistributor* szd;
Expand Down