Skip to content

Commit

Permalink
Merge pull request #942 from snozawa/update_jointpathexik
Browse files Browse the repository at this point in the history 
Update JointPathEx IK
  • Loading branch information
@fkanehiro
fkanehiro committed Jan 10, 2016
2 parents 16fd654 + 26077ae commit 25e71b1
Show file tree
Hide file tree
Showing 8 changed files with 125 additions and 60 deletions.
6 changes: 2 additions & 4 deletions idl/AutoBalancerService.idl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,10 +137,6 @@ module OpenHRP
*/
struct FootstepParam
{
/// Current right foot coords
Footstep rleg_coords;
/// Current left foot coords
Footstep lleg_coords;
/// Support foot coords
Footstep support_leg_coords;
/// Swing foot coords, which is interpolation betwee swing_leg_src_coords and swing_leg_dst_coords
Expand Down Expand Up @@ -261,6 +257,8 @@ module OpenHRP
sequence<Footstep> end_effector_list;
/// Default GaitType
GaitType default_gait_type;
/// Sequence of joint weight for Inverse Kinematics calculation. Sequence for all end-effectors.
sequence<sequence<double> > ik_optional_weight_vectors;
};

/**
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 @@ -180,6 +180,8 @@ module OpenHRP
sequence<AutoBalancerService::Footstep> end_effector_list;
/// whether an emergency stop is used while walking
boolean is_estop_while_walking;
/// Sequence of joint weight for Inverse Kinematics calculation. Sequence for all end-effectors.
sequence<sequence<double> > ik_optional_weight_vectors;
};

/**
Expand Down
59 changes: 36 additions & 23 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,6 +190,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
tp.pos_ik_error_count = tp.rot_ik_error_count = 0;
ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
ikp[ee_name].target_link = m_robot->link(ee_target);
ee_vec.push_back(ee_name);
std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
std::cerr << "[" << m_profile.instance_name << "] offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
Expand Down Expand Up @@ -457,12 +458,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
solveLimbIK();
rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
} else {
for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
it->second.current_p0 = it->second.target_link->p;
it->second.current_r0 = it->second.target_link->R;
}
}
rel_ref_zmp = input_zmp;
}
// transition
Expand Down Expand Up @@ -918,16 +913,9 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri

bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param)
{
param.current_p0 = param.target_link->p;
param.current_r0 = param.target_link->R;

hrp::Vector3 vel_p, vel_r;
vel_p = param.target_p0 - param.current_p0;
rats::difference_rotation(vel_r, param.current_r0, param.target_r0);
vel_p *= transition_interpolator_ratio * leg_names_interpolator_ratio;
vel_r *= transition_interpolator_ratio * leg_names_interpolator_ratio;
param.manip->calcInverseKinematics2Loop(vel_p, vel_r, 1.0, 0.001, 0.01, &qrefv);
param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, 0.001, 0.01, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio);
// IK check
hrp::Vector3 vel_p, vel_r;
vel_p = param.target_p0 - param.target_link->p;
rats::difference_rotation(vel_r, param.target_link->R, param.target_r0);
if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) {
Expand Down Expand Up @@ -1585,6 +1573,15 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto
} else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
gait_type = GALLOP;
}
for (size_t i = 0; i < ee_vec.size(); i++) {
ABCIKparam& param = ikp[ee_vec[i]];
std::vector<double> ov;
ov.resize(param.manip->numJoints());
for (size_t j = 0; j < param.manip->numJoints(); j++) {
ov[j] = i_param.ik_optional_weight_vectors[i][j];
}
param.manip->setOptionalWeightVector(ov);
}
for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
Expand All @@ -1608,6 +1605,19 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto
for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "] leg_names [" << *it << "]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] default_gait_type = " << gait_type << std::endl;
std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vectors = ";
for (size_t i = 0; i < ee_vec.size(); i++) {
ABCIKparam& param = ikp[ee_vec[i]];
std::vector<double> ov;
ov.resize(param.manip->numJoints());
param.manip->getOptionalWeightVector(ov);
std::cerr << "[";
for (size_t j = 0; j < param.manip->numJoints(); j++) {
std::cerr << ov[j] << " ";
}
std::cerr << "]";
}
std::cerr << std::endl;
return true;
};

Expand Down Expand Up @@ -1670,6 +1680,17 @@ bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalanc
case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
default: break;
}
i_param.ik_optional_weight_vectors.length(ee_vec.size());
for (size_t i = 0; i < ee_vec.size(); i++) {
ABCIKparam& param = ikp[ee_vec[i]];
i_param.ik_optional_weight_vectors[i].length(param.manip->numJoints());
std::vector<double> ov;
ov.resize(param.manip->numJoints());
param.manip->getOptionalWeightVector(ov);
for (size_t j = 0; j < param.manip->numJoints(); j++) {
i_param.ik_optional_weight_vectors[i][j] = ov[j];
}
}
return true;
};

Expand All @@ -1685,14 +1706,6 @@ void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footste

bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
{
std::vector<rats::coordinates> leg_coords;
for (size_t i = 0; i < leg_names.size(); i++) {
ABCIKparam& tmpikp = ikp[leg_names[i]];
leg_coords.push_back(coordinates(tmpikp.current_p0 + tmpikp.current_r0 * tmpikp.localPos,
tmpikp.current_r0 * tmpikp.localR));
}
copyRatscoords2Footstep(i_param.rleg_coords, leg_coords[0]);
copyRatscoords2Footstep(i_param.lleg_coords, leg_coords[1]);
copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
Expand Down
6 changes: 3 additions & 3 deletions rtc/AutoBalancer/AutoBalancer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,8 +186,8 @@ class AutoBalancer

private:
struct ABCIKparam {
hrp::Vector3 target_p0, current_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
hrp::Matrix33 target_r0, current_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
rats::coordinates target_end_coords;
hrp::Link* target_link;
hrp::JointPathExPtr manip;
Expand Down Expand Up @@ -227,7 +227,7 @@ class AutoBalancer
std::map<std::string, ABCIKparam> ikp;
std::map<std::string, size_t> contact_states_index_map;
std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
std::vector<std::string> sensor_names, leg_names;
std::vector<std::string> sensor_names, leg_names, ee_vec;
hrp::dvector qorg, qrefv;
hrp::Vector3 current_root_p, target_root_p;
hrp::Matrix33 current_root_R, target_root_R;
Expand Down
10 changes: 2 additions & 8 deletions rtc/ImpedanceController/ImpedanceController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -482,17 +482,11 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
DEBUGP, std::string(m_profile.instance_name), it->first);

// Solve ik
// Fix ee frame objective vel => link frame objective vel
hrp::Vector3 link_frame_pos;
hrp::Matrix33 link_frame_rot;
link_frame_rot = param.getOutputRot() * ee_map[it->first].localR.transpose();
link_frame_pos = param.getOutputPos() - link_frame_rot * ee_map[it->first].localPos;
vel_p = link_frame_pos - target->p;
rats::difference_rotation(vel_r, target->R, link_frame_rot);
hrp::JointPathExPtr manip = param.manip;
assert(manip);
//const int n = manip->numJoints();
manip->calcInverseKinematics2Loop(vel_p, vel_r, 1.0, param.avoid_gain, param.reference_gain, &qrefv);
manip->calcInverseKinematics2Loop(param.getOutputPos(), param.getOutputRot(), 1.0, param.avoid_gain, param.reference_gain, &qrefv, 1.0,
ee_map[it->first].localPos, ee_map[it->first].localR);

if ( param.transition_count < 0 ) {
param.transition_count++;
Expand Down
41 changes: 41 additions & 0 deletions rtc/ImpedanceController/JointPathEx.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -425,6 +425,47 @@ bool JointPathEx::calcInverseKinematics2Loop(const Vector3& dp, const Vector3& o
return true;
}

// TODO : matrix_logEx should be omegaFromRotEx after checking on real robot testing.
hrp::Vector3 matrix_logEx(const hrp::Matrix33& m) {
hrp::Vector3 mlog;
double q0, th;
hrp::Vector3 q;
double norm;

Eigen::Quaternion<double> eiq(m);
q0 = eiq.w();
q = eiq.vec();
norm = q.norm();
if (norm > 0) {
if ((q0 > 1.0e-10) || (q0 < -1.0e-10)) {
th = 2 * std::atan(norm / q0);
} else if (q0 > 0) {
th = M_PI / 2;
} else {
th = -M_PI / 2;
}
mlog = (th / norm) * q ;
} else {
mlog = hrp::Vector3::Zero();
}
return mlog;
}

bool JointPathEx::calcInverseKinematics2Loop(const Vector3& end_effector_p, const Matrix33& end_effector_R,
const double LAMBDA, const double avoid_gain, const double reference_gain, const hrp::dvector* reference_q,
const double vel_gain,
const hrp::Vector3& localPos, const hrp::Matrix33& localR)
{
hrp::Matrix33 target_link_R(end_effector_R * localR.transpose());
hrp::Vector3 target_link_p(end_effector_p - target_link_R * localPos);
hrp::Vector3 vel_p(target_link_p - endLink()->p);
// TODO : matrix_logEx should be omegaFromRotEx after checking on real robot testing.
//hrp::Vector3 vel_r(endLink()->R * omegaFromRotEx(endLink()->R.transpose() * target_link_R));
hrp::Vector3 vel_r(endLink()->R * matrix_logEx(endLink()->R.transpose() * target_link_R));
vel_p *= vel_gain;
vel_r *= vel_gain;
return calcInverseKinematics2Loop(vel_p, vel_r, LAMBDA, avoid_gain, reference_gain, reference_q);
}

bool JointPathEx::calcInverseKinematics2(const Vector3& end_p, const Matrix33& end_R,
const double avoid_gain, const double reference_gain, const hrp::dvector* reference_q)
Expand Down
4 changes: 4 additions & 0 deletions rtc/ImpedanceController/JointPathEx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,10 @@ namespace hrp {
JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = "");
bool calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull);
bool calcInverseKinematics2Loop(const Vector3& dp, const Vector3& omega, const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL);
bool calcInverseKinematics2Loop(const Vector3& end_effector_p, const Matrix33& end_effector_R,
const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const hrp::dvector* reference_q = NULL,
const double vel_gain = 1.0,
const hrp::Vector3& localPos = hrp::Vector3::Zero(), const hrp::Matrix33& localR = hrp::Matrix33::Identity());
bool calcInverseKinematics2(const Vector3& end_p, const Matrix33& end_R, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL);
double getSRGain() { return sr_gain; }
bool setSRGain(double g) { sr_gain = g; }
Expand Down
57 changes: 35 additions & 22 deletions rtc/Stabilizer/Stabilizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1259,11 +1259,7 @@ void Stabilizer::calcTPCC() {
m_robot->calcForwardKinematics();
for (size_t i = 0; i < stikp.size(); i++) {
if (is_ik_enable[i]) {
hrp::Link* target = m_robot->link(stikp[i].target_name);
hrp::Vector3 vel_p, vel_r;
vel_p = target_link_p[i] - target->p;
rats::difference_rotation(vel_r, target->R, target_link_R[i]);
jpe_v[i]->calcInverseKinematics2Loop(vel_p, vel_r, 1.0, 0.001, 0.01, &qrefv);
jpe_v[i]->calcInverseKinematics2Loop(target_link_p[i], target_link_R[i], 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain);
}
}
}
Expand Down Expand Up @@ -1308,8 +1304,6 @@ void Stabilizer::calcEEForceMomentControl() {
current_d_foot_pos.push_back(foot_origin_rot * stikp[i].d_foot_pos);

// 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)
Expand All @@ -1326,8 +1320,6 @@ void Stabilizer::calcEEForceMomentControl() {
// foot force independent damping control
tmpp = target_ee_p[i] - current_d_foot_pos[i];
} else {
// target_link_p[i] = target_ee_p[i];
// target_link_R[i] = target_ee_R[i];
target_ee_diff_p[i] *= transition_smooth_gain;
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];
Expand Down Expand Up @@ -1401,24 +1393,15 @@ void Stabilizer::calcEEForceMomentControl() {
}
}
}
for (size_t i = 0; i < stikp.size(); i++){
// target at ee => target at link-origin
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.
for (size_t jj = 0; jj < 3; jj++) {
for (size_t i = 0; i < stikp.size(); i++) {
if (is_ik_enable[i]) {
hrp::Link* target = m_robot->link(stikp[i].target_name);
hrp::Vector3 vel_p, vel_r;
vel_p = target_link_p[i] - target->p;
rats::difference_rotation(vel_r, target->R, target_link_R[i]);
vel_p *= transition_smooth_gain;
vel_r *= transition_smooth_gain;
jpe_v[i]->calcInverseKinematics2Loop(vel_p, vel_r, 1.0, 0.001, 0.01, &qrefv);
jpe_v[i]->calcInverseKinematics2Loop(tmpp_list.at(i), tmpR_list.at(i), 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain,
//stikp[i].localCOPPos;
stikp[i].localp,
stikp[i].localR);
}
}
}
Expand Down Expand Up @@ -1669,6 +1652,16 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
// set
i_stp.end_effector_list[i] = ret_ee;
}
i_stp.ik_optional_weight_vectors.length(jpe_v.size());
for (size_t i = 0; i < jpe_v.size(); i++) {
i_stp.ik_optional_weight_vectors[i].length(jpe_v[i]->numJoints());
std::vector<double> ov;
ov.resize(jpe_v[i]->numJoints());
jpe_v[i]->getOptionalWeightVector(ov);
for (size_t j = 0; j < jpe_v[i]->numJoints(); j++) {
i_stp.ik_optional_weight_vectors[i][j] = ov[j];
}
}
};

void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
Expand Down Expand Up @@ -1789,6 +1782,14 @@ 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;
for (size_t i = 0; i < jpe_v.size(); i++) {
std::vector<double> ov;
ov.resize(jpe_v[i]->numJoints());
for (size_t j = 0; j < jpe_v[i]->numJoints(); j++) {
ov[j] = i_stp.ik_optional_weight_vectors[i][j];
}
jpe_v[i]->setOptionalWeightVector(ov);
}
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 Expand Up @@ -1867,6 +1868,18 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
std::cerr << "[" << m_profile.instance_name << "] cop_check_margin = " << cop_check_margin << "[m]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] cp_check_margin = [" << cp_check_margin[0] << ", " << cp_check_margin[1] << ", " << cp_check_margin[2] << ", " << cp_check_margin[3] << "] [m]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] contact_decision_threshold = " << contact_decision_threshold << "[N]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vectors = ";
for (size_t i = 0; i < jpe_v.size(); i++) {
std::vector<double> ov;
ov.resize(jpe_v[i]->numJoints());
jpe_v[i]->getOptionalWeightVector(ov);
std::cerr << "[";
for (size_t j = 0; j < jpe_v[i]->numJoints(); j++) {
std::cerr << ov[j] << " ";
}
std::cerr << "]";
}
std::cerr << std::endl;
}

std::string Stabilizer::getStabilizerAlgorithmString (OpenHRP::StabilizerService::STAlgorithm _st_algorithm)
Expand Down

0 comments on commit 25e71b1

Please sign in to comment.