From f2f5cc77d28459d8a327463b8dfb897e8513dcf0 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 9 Jan 2016 23:32:11 +0900 Subject: [PATCH 1/4] [idl/AutoBalancerService.idl,rtc/AutoBalancer/AutoBalancer.*] Remove deprecated footstep information lleg_coords and rleg_coords. Remove unused current_* parameter from ABCIKparam. --- idl/AutoBalancerService.idl | 4 ---- rtc/AutoBalancer/AutoBalancer.cpp | 21 ++------------------- rtc/AutoBalancer/AutoBalancer.h | 4 ++-- 3 files changed, 4 insertions(+), 25 deletions(-) diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index b15fe149642..056b8c08266 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -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 diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 8300163197f..ae62f9377ee 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -457,12 +457,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::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 @@ -918,12 +912,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 = param.target_p0 - param.target_link->p; + rats::difference_rotation(vel_r, param.target_link->R, 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); @@ -1685,14 +1676,6 @@ void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footste bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param) { - std::vector 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); diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index 7e5223abfdb..bf71c11b982 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -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; From ec20170d8bf75fbfd49d88ecd214bba775e11705 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sun, 10 Jan 2016 11:05:31 +0900 Subject: [PATCH 2/4] [rtc/ImpedanceController/ImpedanceController.cpp,JointPathEx.*,rtc/Stabilizer/Stabilizer.cpp,rtc/AutoBalancer/AutoBalancer.cpp] Add calcInverseKinematics2Loop function to take target pos and Rot and use it in ic, abc, and st. Currently omegaFromRot is under checking and tempolarily use old matrix_log function, so program behaviour does not change. --- rtc/AutoBalancer/AutoBalancer.cpp | 8 +--- .../ImpedanceController.cpp | 4 +- rtc/ImpedanceController/JointPathEx.cpp | 38 +++++++++++++++++++ rtc/ImpedanceController/JointPathEx.h | 3 ++ rtc/Stabilizer/Stabilizer.cpp | 14 +------ 5 files changed, 46 insertions(+), 21 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index ae62f9377ee..795723f777d 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -912,13 +912,9 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param) { - 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); - 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()) { diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp index 82ed5007080..8e0623a9380 100644 --- a/rtc/ImpedanceController/ImpedanceController.cpp +++ b/rtc/ImpedanceController/ImpedanceController.cpp @@ -487,12 +487,10 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id) 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(link_frame_pos, link_frame_rot, 1.0, param.avoid_gain, param.reference_gain, &qrefv); if ( param.transition_count < 0 ) { param.transition_count++; diff --git a/rtc/ImpedanceController/JointPathEx.cpp b/rtc/ImpedanceController/JointPathEx.cpp index 1cdcf875c59..e501d640058 100644 --- a/rtc/ImpedanceController/JointPathEx.cpp +++ b/rtc/ImpedanceController/JointPathEx.cpp @@ -425,6 +425,44 @@ 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 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& target_link_p, const Matrix33& target_link_R, + const double LAMBDA, const double avoid_gain, const double reference_gain, const hrp::dvector* reference_q, + const double vel_gain) +{ + 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) diff --git a/rtc/ImpedanceController/JointPathEx.h b/rtc/ImpedanceController/JointPathEx.h index 36ca2401cae..d2b84e3fd3d 100644 --- a/rtc/ImpedanceController/JointPathEx.h +++ b/rtc/ImpedanceController/JointPathEx.h @@ -18,6 +18,9 @@ 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& target_link_p, const Matrix33& target_link_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); 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; } diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index 7b07a90529a..e3470fe8e5e 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -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); } } } @@ -1412,13 +1408,7 @@ void Stabilizer::calcEEForceMomentControl() { 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(target_link_p[i], target_link_R[i], 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain); } } } From 9ae2e57e6cb7e61b9c207f58d836aa47a81d915d Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sun, 10 Jan 2016 14:31:58 +0900 Subject: [PATCH 3/4] [rtc/ImpedanceController/ImpedanceController.cpp,rtc/ImpedanceController/JointPathEx.*,rtc/Stabilizer/Stabilizer.cpp] Move end-effector version inverse kinematics to JointPathEx and use it in IC and ST. --- rtc/ImpedanceController/ImpedanceController.cpp | 8 ++------ rtc/ImpedanceController/JointPathEx.cpp | 7 +++++-- rtc/ImpedanceController/JointPathEx.h | 5 +++-- rtc/Stabilizer/Stabilizer.cpp | 15 ++++----------- 4 files changed, 14 insertions(+), 21 deletions(-) diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp index 8e0623a9380..8f428a71b72 100644 --- a/rtc/ImpedanceController/ImpedanceController.cpp +++ b/rtc/ImpedanceController/ImpedanceController.cpp @@ -482,15 +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; hrp::JointPathExPtr manip = param.manip; assert(manip); //const int n = manip->numJoints(); - manip->calcInverseKinematics2Loop(link_frame_pos, link_frame_rot, 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++; diff --git a/rtc/ImpedanceController/JointPathEx.cpp b/rtc/ImpedanceController/JointPathEx.cpp index e501d640058..c0dfa3d2f5f 100644 --- a/rtc/ImpedanceController/JointPathEx.cpp +++ b/rtc/ImpedanceController/JointPathEx.cpp @@ -451,10 +451,13 @@ hrp::Vector3 matrix_logEx(const hrp::Matrix33& m) { return mlog; } -bool JointPathEx::calcInverseKinematics2Loop(const Vector3& target_link_p, const Matrix33& target_link_R, +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 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)); diff --git a/rtc/ImpedanceController/JointPathEx.h b/rtc/ImpedanceController/JointPathEx.h index d2b84e3fd3d..2889cc508ab 100644 --- a/rtc/ImpedanceController/JointPathEx.h +++ b/rtc/ImpedanceController/JointPathEx.h @@ -18,9 +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& target_link_p, const Matrix33& target_link_R, + 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 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; } diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index e3470fe8e5e..47e8bb4cf21 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -1304,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 tmpp_list; // modified ee Pos std::vector tmpR_list; // modified ee Rot #define deg2rad(x) ((x) * M_PI / 180.0) @@ -1322,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]; @@ -1397,18 +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]) { - jpe_v[i]->calcInverseKinematics2Loop(target_link_p[i], target_link_R[i], 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain); + 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); } } } From 26077aebc148445688767f0ae2e30253fda0e2f8 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sun, 10 Jan 2016 16:19:07 +0900 Subject: [PATCH 4/4] [idl/AutoBalancerService.idl,idl/StabilizerService.idl,rtc/AutoBalancer/AutoBalancer.*,rtc/Stabilizer/Stabilizer.cpp] Enable to set IK weight vector for STtabilizer and Autobalancer like ImpedanceController. --- idl/AutoBalancerService.idl | 2 ++ idl/StabilizerService.idl | 2 ++ rtc/AutoBalancer/AutoBalancer.cpp | 34 +++++++++++++++++++++++++++++++ rtc/AutoBalancer/AutoBalancer.h | 2 +- rtc/Stabilizer/Stabilizer.cpp | 30 +++++++++++++++++++++++++++ 5 files changed, 69 insertions(+), 1 deletion(-) diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index 056b8c08266..da45409e101 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -257,6 +257,8 @@ module OpenHRP sequence end_effector_list; /// Default GaitType GaitType default_gait_type; + /// Sequence of joint weight for Inverse Kinematics calculation. Sequence for all end-effectors. + sequence > ik_optional_weight_vectors; }; /** diff --git a/idl/StabilizerService.idl b/idl/StabilizerService.idl index 57c44962043..9d7668ec5b8 100644 --- a/idl/StabilizerService.idl +++ b/idl/StabilizerService.idl @@ -180,6 +180,8 @@ module OpenHRP sequence 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 > ik_optional_weight_vectors; }; /** diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 795723f777d..55e63e85832 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -190,6 +190,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() tp.pos_ik_error_count = tp.rot_ik_error_count = 0; ikp.insert(std::pair(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; @@ -1572,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 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::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; @@ -1595,6 +1605,19 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto for (std::vector::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 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; }; @@ -1657,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 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; }; diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index bf71c11b982..8fbc045b33f 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -227,7 +227,7 @@ class AutoBalancer std::map ikp; std::map contact_states_index_map; std::map m_vfs; - std::vector sensor_names, leg_names; + std::vector 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; diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index 47e8bb4cf21..73f72155488 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -1652,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 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) @@ -1772,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 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::iterator it = std::find_if(stikp.begin(), stikp.end(), (&boost::lambda::_1->* &std::vector::value_type::ee_name == std::string(i_stp.end_effector_list[i].leg))); @@ -1850,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 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)