diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index f2773aa8553..e9144094fbb 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -190,14 +190,13 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str()); } tp.localR = Eigen::AngleAxis(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle - tp.base_name = ee_base; - tp.target_name = ee_target; - tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(tp.base_name), - m_robot->link(tp.target_name))); + tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), + m_robot->link(ee_target))); ikp.insert(std::pair(ee_name , tp)); - std::cerr << m_profile.instance_name << " End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl; - std::cerr << m_profile.instance_name << " target = " << ee_target << ", base = " << ee_base << std::endl; - std::cerr << m_profile.instance_name << " offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; + ikp[ee_name].target_link = m_robot->link(ee_target); + 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; contact_states_index_map.insert(std::pair(ee_name, i)); } m_contactStates.data.length(num); @@ -252,6 +251,9 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0; prev_imu_sensor_vel = hrp::Vector3::Zero(); + leg_names.push_back("rleg"); + leg_names.push_back("lleg"); + return RTC::RTC_OK; } @@ -358,8 +360,8 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) } else { for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { if (it->first == "rleg" || it->first == "lleg") { - it->second.current_p0 = m_robot->link(it->second.target_name)->p; - it->second.current_r0 = m_robot->link(it->second.target_name)->R; + it->second.current_p0 = it->second.target_link->p; + it->second.current_r0 = it->second.target_link->R; } } rel_ref_zmp = input_zmp; @@ -461,8 +463,6 @@ void AutoBalancer::getCurrentParameters() for ( int i = 0; i < m_robot->numJoints(); i++ ){ qorg[i] = m_robot->joint(i)->q; } - ikp["rleg"].getCurrentEndCoords(ikp["rleg"].current_end_coords); - ikp["lleg"].getCurrentEndCoords(ikp["lleg"].current_end_coords); } void AutoBalancer::getTargetParameters() @@ -478,7 +478,6 @@ void AutoBalancer::getTargetParameters() m_robot->calcForwardKinematics(); // if (control_mode != MODE_IDLE) { - coordinates rc, lc; coordinates tmp_fix_coords; if (!zmp_interpolator->isEmpty()) { double default_zmp_offsets_output[6]; @@ -540,17 +539,15 @@ void AutoBalancer::getTargetParameters() { hrp::Vector3 ex = hrp::Vector3::UnitX(); hrp::Vector3 ez = hrp::Vector3::UnitZ(); - hrp::Matrix33 tmpm; - tmpm = OrientRotationMatrix(tmp_fix_coords.rot, (tmp_fix_coords.rot * ez), ez); - hrp::Vector3 xv1 = tmp_fix_coords.rot * ex; - xv1(2)=0.0; + hrp::Vector3 xv1 (tmp_fix_coords.rot * ex); + xv1(2) = 0.0; xv1.normalize(); - hrp::Vector3 xv2 = tmpm * ex; - xv2(2)=0.0; - xv2.normalize(); - tmp_fix_coords.rot = OrientRotationMatrix(tmpm, xv2, xv1); + hrp::Vector3 yv1(ez.cross(xv1)); + tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2); + tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2); + tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2); } - fixLegToCoords(":both", tmp_fix_coords); + fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot); /* update ref_forces ;; sp's absolute -> rmc's absolute */ for (size_t i = 0; i < m_ref_forceIn.size(); i++) { @@ -560,9 +557,12 @@ void AutoBalancer::getTargetParameters() if (sensor) parentlink = sensor->link; else parentlink = m_vfs[sensor_names[i]].link; for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if (it->second.target_name == parentlink->name) eeR = parentlink->R * it->second.localR; + if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR; } - ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]); + // End effector frame + //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]); + // world frame + ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]); } sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset); @@ -570,21 +570,27 @@ void AutoBalancer::getTargetParameters() target_root_R = m_robot->rootLink()->R; for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { if ( control_mode != MODE_ABC || it->first.find("leg") == std::string::npos ) { - it->second.target_p0 = m_robot->link(it->second.target_name)->p; - it->second.target_r0 = m_robot->link(it->second.target_name)->R; + it->second.target_p0 = it->second.target_link->p; + it->second.target_r0 = it->second.target_link->R; } } - ikp["rleg"].getTargetEndCoords(ikp["rleg"].target_end_coords); - ikp["lleg"].getTargetEndCoords(ikp["lleg"].target_end_coords); - ikp["rleg"].getRobotEndCoords(rc, m_robot); - ikp["lleg"].getRobotEndCoords(lc, m_robot); - rc.pos += rc.rot * default_zmp_offsets[0]; /* rleg */ - lc.pos += lc.rot * default_zmp_offsets[1]; /* lleg */ + + hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero()); + for (size_t i = 0; i < leg_names.size(); i++) { + ABCIKparam& tmpikp = ikp[leg_names[i]]; + // get target_end_coords + tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos; + tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR; + // for foot_mid_pos + tmp_foot_mid_pos += tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos + tmpikp.target_link->R * tmpikp.localR * default_zmp_offsets[i]; + } + tmp_foot_mid_pos *= 0.5; + hrp::Vector3 tmp_ref_cog(m_robot->calcCM()); if (gg_is_walking) { ref_cog = gg->get_cog(); } else { - ref_cog = (rc.pos+lc.pos)/2.0; + ref_cog = tmp_foot_mid_pos; } ref_cog(2) = tmp_ref_cog(2); if (gg_is_walking) { @@ -592,7 +598,7 @@ void AutoBalancer::getTargetParameters() } else { ref_zmp(0) = ref_cog(0); ref_zmp(1) = ref_cog(1); - ref_zmp(2) = (rc.pos(2) + lc.pos(2)) / 2.0; + ref_zmp(2) = tmp_foot_mid_pos(2); } } // Just for ik initial value @@ -601,8 +607,8 @@ void AutoBalancer::getTargetParameters() current_root_R = target_root_R; for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { if ( it->first.find("leg") != std::string::npos ) { - it->second.target_p0 = m_robot->link(it->second.target_name)->p; - it->second.target_r0 = m_robot->link(it->second.target_name)->R; + it->second.target_p0 = it->second.target_link->p; + it->second.target_r0 = it->second.target_link->R; } } } @@ -619,26 +625,30 @@ hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, cons } } -void AutoBalancer::fixLegToCoords (const std::string& leg, const coordinates& coords) +void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot) { - coordinates tar, ref, delta, tmp; - coordinates rleg_endcoords, lleg_endcoords; - ikp["rleg"].getRobotEndCoords(rleg_endcoords, m_robot); - ikp["lleg"].getRobotEndCoords(lleg_endcoords, m_robot); - mid_coords(tar, 0.5, rleg_endcoords , lleg_endcoords); - tmp = coords; - ref = coordinates(m_robot->rootLink()->p, m_robot->rootLink()->R); - tar.transformation(delta, ref, ":local"); - tmp.transform(delta, ":local"); - m_robot->rootLink()->p = tmp.pos; - m_robot->rootLink()->R = tmp.rot; + // get current foot mid pos + rot + std::vector foot_pos; + std::vector foot_rot; + for (size_t i = 0; i < leg_names.size(); i++) { + ABCIKparam& tmpikp = ikp[leg_names[i]]; + foot_pos.push_back(tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos); + foot_rot.push_back(tmpikp.target_link->R * tmpikp.localR); + } + hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0); + hrp::Matrix33 current_foot_mid_rot; + mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); + // fix root pos + rot to fix "coords" = "current_foot_mid_xx" + hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose()); + m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos); + rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R); m_robot->calcForwardKinematics(); } bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param) { - param.current_p0 = m_robot->link(param.target_name)->p; - param.current_r0 = m_robot->link(param.target_name)->R; + 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; @@ -765,7 +775,7 @@ void AutoBalancer::startWalking () void AutoBalancer::stopWalking () { mid_coords(fix_leg_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords); - fixLegToCoords(":both", fix_leg_coords); + fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot); gg->clear_footstep_node_list(); if (return_control_mode == MODE_IDLE) stopABCparam(); gg_is_walking = false; @@ -988,8 +998,14 @@ void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footste bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param) { - copyRatscoords2Footstep(i_param.rleg_coords, ikp["rleg"].current_end_coords); - copyRatscoords2Footstep(i_param.lleg_coords, ikp["lleg"].current_end_coords); + 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_coords()); copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_coords()); copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_coords()); @@ -1041,7 +1057,7 @@ void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, if (sensor) parentlink = sensor->link; else parentlink = m_vfs[sensor_names[i]].link; for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if (it->second.target_name == parentlink->name) { + if (it->second.target_link->name == parentlink->name) { hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos; nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) ); denom(j) -= tmp_forces[i](2); diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index ebaaa99033f..cbdb126dfc3 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -167,22 +167,10 @@ class AutoBalancer struct ABCIKparam { hrp::Vector3 target_p0, current_p0, localPos; hrp::Matrix33 target_r0, current_r0, localR; - rats::coordinates target_end_coords, current_end_coords; - std::string target_name, base_name; + rats::coordinates target_end_coords; + hrp::Link* target_link; hrp::JointPathExPtr manip; bool is_active; - void getEndCoords(rats::coordinates& retc, const hrp::Vector3& _pos, const hrp::Matrix33& _rot) - { - retc.pos = _pos; - retc.rot = _rot; - retc.transform(rats::coordinates(localPos, localR)); - }; - void getRobotEndCoords(rats::coordinates& retc, hrp::BodyPtr& _robot) - { - getEndCoords(retc, _robot->link(target_name)->p, _robot->link(target_name)->R); - }; - void getTargetEndCoords(rats::coordinates& retc) { getEndCoords(retc, target_p0, target_r0); }; - void getCurrentEndCoords(rats::coordinates& retc) { getEndCoords(retc, current_p0, current_r0); }; }; void getCurrentParameters(); void getTargetParameters(); @@ -192,7 +180,7 @@ class AutoBalancer void stopABCparam(); void waitABCTransition(); hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2); - void fixLegToCoords (const std::string& leg, const rats::coordinates& coords); + void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot); void startWalking (); void stopWalking (); void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs); @@ -210,7 +198,7 @@ class AutoBalancer std::map ikp; std::map contact_states_index_map; std::map m_vfs; - std::vector sensor_names; + std::vector sensor_names, leg_names; hrp::dvector qorg, qrefv; hrp::Vector3 current_root_p, target_root_p; hrp::Matrix33 current_root_R, target_root_R; diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp index 622f65887f9..1db233ca6d8 100644 --- a/rtc/ImpedanceController/ImpedanceController.cpp +++ b/rtc/ImpedanceController/ImpedanceController.cpp @@ -185,6 +185,7 @@ RTC::ReturnCode_t ImpedanceController::onInitialize() } eet.localR = Eigen::AngleAxis(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle ee_map.insert(std::pair(ee_target , eet)); + ee_name_map.insert(std::pair(ee_name, ee_target));; std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl; std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << std::endl; std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; @@ -307,7 +308,46 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id) } m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z); m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y); - m_robot->calcForwardKinematics(); + m_robot->calcForwardKinematics(); + if (ee_name_map.find("rleg") != ee_name_map.end() && ee_name_map.find("lleg") != ee_name_map.end()) { // if legged robot + // TODO + // Tempolarily modify root coords to fix foot pos rot + // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272 + + // get current foot mid pos + rot + std::vector foot_pos; + std::vector foot_rot; + std::vector leg_names; + leg_names.push_back("rleg"); + leg_names.push_back("lleg"); + for (size_t i = 0; i < leg_names.size(); i++) { + hrp::Link* target_link = m_robot->link(ee_name_map[leg_names[i]]); + foot_pos.push_back(target_link->p + target_link->R * ee_map[target_link->name].localPos); + foot_rot.push_back(target_link->R * ee_map[target_link->name].localR); + } + hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0); + hrp::Matrix33 current_foot_mid_rot; + rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); + // calculate fix pos + rot + hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos); + hrp::Matrix33 new_foot_mid_rot; + { + hrp::Vector3 ex = hrp::Vector3::UnitX(); + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Vector3 xv1 (current_foot_mid_rot * ex); + xv1(2) = 0.0; + xv1.normalize(); + hrp::Vector3 yv1(ez.cross(xv1)); + new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2); + new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2); + new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2); + } + // fix root pos + rot to fix "coords" = "current_foot_mid_xx" + hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose()); + m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos); + rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R); + m_robot->calcForwardKinematics(); + } calcForceMoment(); // set sequencer position to target_p0 @@ -581,9 +621,13 @@ void ImpedanceController::calcForceMoment () } abs_forces[sensor_name] = sensorR * data_p; abs_moments[sensor_name] = sensorR * data_r + parentlink->R * (sensorlocalPos - ee_map[parentlink->name].localPos).cross(abs_forces[sensor_name]); - hrp::Matrix33 eeR (parentlink->R * ee_map[parentlink->name].localR); - abs_ref_forces[sensor_name] = eeR * ref_data_p; - abs_ref_moments[sensor_name] = eeR * ref_data_r; + // End effector local frame + // hrp::Matrix33 eeR (parentlink->R * ee_map[parentlink->name].localR); + // abs_ref_forces[sensor_name] = eeR * ref_data_p; + // abs_ref_moments[sensor_name] = eeR * ref_data_r; + // World frame + abs_ref_forces[sensor_name] = ref_data_p; + abs_ref_moments[sensor_name] = ref_data_r; if ( DEBUGP ) { std::cerr << "[" << m_profile.instance_name << "] abs force = " << abs_forces[sensor_name].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] abs moment = " << abs_moments[sensor_name].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; diff --git a/rtc/ImpedanceController/ImpedanceController.h b/rtc/ImpedanceController/ImpedanceController.h index cf80e5ea67b..425ad10e5db 100644 --- a/rtc/ImpedanceController/ImpedanceController.h +++ b/rtc/ImpedanceController/ImpedanceController.h @@ -176,6 +176,7 @@ class ImpedanceController std::map m_impedance_param; std::map ee_map; + std::map ee_name_map; std::map m_vfs; std::map abs_forces, abs_moments, abs_ref_forces, abs_ref_moments; double m_dt; diff --git a/rtc/ImpedanceController/RatsMatrix.cpp b/rtc/ImpedanceController/RatsMatrix.cpp index 80b166428b1..324500d3b3e 100644 --- a/rtc/ImpedanceController/RatsMatrix.cpp +++ b/rtc/ImpedanceController/RatsMatrix.cpp @@ -56,20 +56,8 @@ namespace rats }; void mid_coords(coordinates& mid_coords, const double p, const coordinates& c1, const coordinates& c2) { - hrp::Vector3 mid_point, omega; - hrp::Matrix33 mid_rot, r; - - mid_point = (1 - p) * c1.pos + p * c2.pos; - r = c1.rot.transpose() * c2.rot; - omega = matrix_log(r); - if (eps_eq(omega.norm(),0.0)) { // c1.rot and c2.rot are same - mid_rot = c1.rot; - } else { - hrp::calcRodrigues(r, omega.normalized(), omega.norm()*p); - //mid_rot = c1.rot * r; - rotm3times(mid_rot, c1.rot, r); - } - mid_coords = coordinates(mid_point, mid_rot); + mid_coords.pos = (1 - p) * c1.pos + p * c2.pos; + mid_rot(mid_coords.rot, p, c1.rot, c2.rot); }; }