00001
00010 #include <rtm/CorbaNaming.h>
00011 #include <hrpModel/Link.h>
00012 #include <hrpModel/Sensor.h>
00013 #include <hrpModel/ModelLoaderUtil.h>
00014 #include "AutoBalancer.h"
00015 #include <hrpModel/JointPath.h>
00016 #include <hrpUtil/MatrixSolvers.h>
00017 #include "hrpsys/util/Hrpsys.h"
00018
00019
00020 typedef coil::Guard<coil::Mutex> Guard;
00021 using namespace rats;
00022
00023
00024
00025 static const char* autobalancer_spec[] =
00026 {
00027 "implementation_id", "AutoBalancer",
00028 "type_name", "AutoBalancer",
00029 "description", "autobalancer component",
00030 "version", HRPSYS_PACKAGE_VERSION,
00031 "vendor", "AIST",
00032 "category", "example",
00033 "activity_type", "DataFlowComponent",
00034 "max_instance", "10",
00035 "language", "C++",
00036 "lang_type", "compile",
00037
00038 "conf.default.debugLevel", "0",
00039 ""
00040 };
00041
00042
00043 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
00044 {
00045 int pre = os.precision();
00046 os.setf(std::ios::fixed);
00047 os << std::setprecision(6)
00048 << (tm.sec + tm.nsec/1e9)
00049 << std::setprecision(pre);
00050 os.unsetf(std::ios::fixed);
00051 return os;
00052 }
00053
00054 AutoBalancer::AutoBalancer(RTC::Manager* manager)
00055 : RTC::DataFlowComponentBase(manager),
00056
00057 m_qRefIn("qRef", m_qRef),
00058 m_basePosIn("basePosIn", m_basePos),
00059 m_baseRpyIn("baseRpyIn", m_baseRpy),
00060 m_zmpIn("zmpIn", m_zmp),
00061 m_optionalDataIn("optionalData", m_optionalData),
00062 m_emergencySignalIn("emergencySignal", m_emergencySignal),
00063 m_diffCPIn("diffCapturePoint", m_diffCP),
00064 m_refFootOriginExtMomentIn("refFootOriginExtMoment", m_refFootOriginExtMoment),
00065 m_refFootOriginExtMomentIsHoldValueIn("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue),
00066 m_actContactStatesIn("actContactStates", m_actContactStates),
00067 m_qOut("q", m_qRef),
00068 m_zmpOut("zmpOut", m_zmp),
00069 m_basePosOut("basePosOut", m_basePos),
00070 m_baseRpyOut("baseRpyOut", m_baseRpy),
00071 m_baseTformOut("baseTformOut", m_baseTform),
00072 m_basePoseOut("basePoseOut", m_basePose),
00073 m_accRefOut("accRef", m_accRef),
00074 m_contactStatesOut("contactStates", m_contactStates),
00075 m_toeheelRatioOut("toeheelRatio", m_toeheelRatio),
00076 m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
00077 m_walkingStatesOut("walkingStates", m_walkingStates),
00078 m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
00079 m_cogOut("cogOut", m_cog),
00080 m_AutoBalancerServicePort("AutoBalancerService"),
00081
00082 gait_type(BIPED),
00083 m_robot(hrp::BodyPtr()),
00084 m_debugLevel(0)
00085 {
00086 m_service0.autobalancer(this);
00087 }
00088
00089 AutoBalancer::~AutoBalancer()
00090 {
00091 }
00092
00093
00094 RTC::ReturnCode_t AutoBalancer::onInitialize()
00095 {
00096 std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00097 bindParameter("debugLevel", m_debugLevel, "0");
00098
00099
00100
00101
00102 addInPort("qRef", m_qRefIn);
00103 addInPort("basePosIn", m_basePosIn);
00104 addInPort("baseRpyIn", m_baseRpyIn);
00105 addInPort("zmpIn", m_zmpIn);
00106 addInPort("optionalData", m_optionalDataIn);
00107 addInPort("emergencySignal", m_emergencySignalIn);
00108 addInPort("diffCapturePoint", m_diffCPIn);
00109 addInPort("actContactStates", m_actContactStatesIn);
00110 addInPort("refFootOriginExtMoment", m_refFootOriginExtMomentIn);
00111 addInPort("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValueIn);
00112
00113
00114 addOutPort("q", m_qOut);
00115 addOutPort("zmpOut", m_zmpOut);
00116 addOutPort("basePosOut", m_basePosOut);
00117 addOutPort("baseRpyOut", m_baseRpyOut);
00118 addOutPort("baseTformOut", m_baseTformOut);
00119 addOutPort("basePoseOut", m_basePoseOut);
00120 addOutPort("accRef", m_accRefOut);
00121 addOutPort("contactStates", m_contactStatesOut);
00122 addOutPort("toeheelRatio", m_toeheelRatioOut);
00123 addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
00124 addOutPort("cogOut", m_cogOut);
00125 addOutPort("walkingStates", m_walkingStatesOut);
00126 addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
00127
00128
00129 m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
00130
00131
00132
00133
00134 addPort(m_AutoBalancerServicePort);
00135
00136
00137
00138
00139
00140
00141
00142 RTC::Properties& prop = getProperties();
00143 coil::stringTo(m_dt, prop["dt"].c_str());
00144
00145 m_robot = hrp::BodyPtr(new hrp::Body());
00146 RTC::Manager& rtcManager = RTC::Manager::instance();
00147 std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00148 int comPos = nameServer.find(",");
00149 if (comPos < 0){
00150 comPos = nameServer.length();
00151 }
00152 nameServer = nameServer.substr(0, comPos);
00153 RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00154 if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
00155 CosNaming::NamingContext::_duplicate(naming.getRootContext())
00156 )){
00157 std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
00158 return RTC::RTC_ERROR;
00159 }
00160
00161
00162 m_qRef.data.length(m_robot->numJoints());
00163 m_baseTform.data.length(12);
00164
00165 control_mode = MODE_IDLE;
00166 loop = 0;
00167
00168
00169
00170
00171 coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
00172 leg_names.push_back("rleg");
00173 leg_names.push_back("lleg");
00174
00175
00176 fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
00177
00178
00179
00180 coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
00181 size_t prop_num = 10;
00182 if (end_effectors_str.size() > 0) {
00183 size_t num = end_effectors_str.size()/prop_num;
00184 for (size_t i = 0; i < num; i++) {
00185 std::string ee_name, ee_target, ee_base;
00186 coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
00187 coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
00188 coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
00189 ABCIKparam tp;
00190 hrp::Link* root = m_robot->link(ee_target);
00191 for (size_t j = 0; j < 3; j++) {
00192 coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
00193 }
00194 double tmpv[4];
00195 for (int j = 0; j < 4; j++ ) {
00196 coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
00197 }
00198 tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix();
00199
00200 SimpleFullbodyInverseKinematicsSolver::IKparam tmp_fikp;
00201 tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
00202 tmp_fikp.target_link = m_robot->link(ee_target);
00203 tmp_fikp.localPos = tp.localPos;
00204 tmp_fikp.localR = tp.localR;
00205 tmp_fikp.max_limb_length = 0.0;
00206 while (!root->isRoot()) {
00207 tmp_fikp.max_limb_length += root->b.norm();
00208 tmp_fikp.parent_name = root->name;
00209 root = root->parent;
00210 }
00211 fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
00212
00213
00214
00215
00216 if (m_robot->link(ee_target)->sensors.size() == 0) {
00217 std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
00218 optw.back() = 0.0;
00219 fik->ikp[ee_name].manip->setOptionalWeightVector(optw);
00220 tp.has_toe_joint = true;
00221 } else {
00222 tp.has_toe_joint = false;
00223 }
00224 tp.target_link = m_robot->link(ee_target);
00225 ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
00226 ee_vec.push_back(ee_name);
00227 std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
00228 std::cerr << "[" << m_profile.instance_name << "] target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
00229 std::cerr << "[" << m_profile.instance_name << "] offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
00230 std::cerr << "[" << m_profile.instance_name << "] has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
00231 contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
00232 }
00233 m_contactStates.data.length(num);
00234 m_toeheelRatio.data.length(num);
00235 if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
00236 m_contactStates.data[contact_states_index_map["rleg"]] = true;
00237 m_contactStates.data[contact_states_index_map["lleg"]] = true;
00238 }
00239 if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
00240 m_contactStates.data[contact_states_index_map["rarm"]] = false;
00241 m_contactStates.data[contact_states_index_map["larm"]] = false;
00242 }
00243 m_controlSwingSupportTime.data.length(num);
00244 for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 1.0;
00245 for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio;
00246 }
00247 std::vector<hrp::Vector3> leg_pos;
00248 if (leg_offset_str.size() > 0) {
00249 hrp::Vector3 leg_offset;
00250 for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
00251 std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
00252 leg_pos.push_back(hrp::Vector3(-1*leg_offset));
00253 leg_pos.push_back(hrp::Vector3(leg_offset));
00254 }
00255 if (leg_pos.size() < ikp.size()) {
00256 size_t tmp_leg_pos_size = leg_pos.size();
00257 for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
00258 leg_pos.push_back(hrp::Vector3::Zero());
00259 }
00260 }
00261
00262 std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
00263 readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
00264 if (interlocking_joints.size() > 0) {
00265 fik->initializeInterlockingJoints(interlocking_joints);
00266 }
00267
00268 zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
00269 zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
00270 zmp_transition_time = 1.0;
00271 transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
00272 transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
00273 transition_interpolator_ratio = 0.0;
00274 adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
00275 adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
00276 transition_time = 2.0;
00277 adjust_footstep_transition_time = 2.0;
00278 leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1);
00279 leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
00280 leg_names_interpolator_ratio = 1.0;
00281
00282
00283 double stride_fwd_x_limit = 0.15;
00284 double stride_outside_y_limit = 0.05;
00285 double stride_outside_th_limit = 10;
00286 double stride_bwd_x_limit = 0.05;
00287 double stride_inside_y_limit = stride_outside_y_limit*0.5;
00288 double stride_inside_th_limit = stride_outside_th_limit*0.5;
00289 std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_outside_y_limit << "[m], " << stride_outside_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
00290 if (default_zmp_offsets.size() == 0) {
00291 for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
00292 }
00293 if (leg_offset_str.size() > 0) {
00294 gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit, stride_outside_y_limit, stride_outside_th_limit,
00295 stride_bwd_x_limit, stride_inside_y_limit, stride_inside_th_limit));
00296 gg->set_default_zmp_offsets(default_zmp_offsets);
00297 }
00298 gg_is_walking = gg_solved = false;
00299 m_walkingStates.data = false;
00300 fix_leg_coords = coordinates();
00301
00302
00303 readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
00304
00305 unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
00306 unsigned int nvforce = m_vfs.size();
00307 unsigned int nforce = npforce + nvforce;
00308
00309 if (nforce < m_contactStates.data.length()) {
00310 std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has less force sensors(" << nforce;
00311 std::cerr << ") than end-effector settings(" << m_contactStates.data.length() << ") !" << std::endl;
00312 }
00313
00314 m_ref_force.resize(nforce);
00315 m_ref_forceIn.resize(nforce);
00316 m_force.resize(nforce);
00317 m_ref_forceOut.resize(nforce);
00318 m_limbCOPOffset.resize(nforce);
00319 m_limbCOPOffsetOut.resize(nforce);
00320 for (unsigned int i=0; i<npforce; i++){
00321 sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
00322 }
00323 for (unsigned int i=0; i<nvforce; i++){
00324 for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
00325 if (it->second.id == (int)i) sensor_names.push_back(it->first);
00326 }
00327 }
00328
00329 std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
00330 for (unsigned int i=0; i<nforce; i++){
00331 m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
00332 m_ref_force[i].data.length(6);
00333 registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
00334 std::cerr << "[" << m_profile.instance_name << "] name = " << std::string("ref_"+sensor_names[i]) << std::endl;
00335 ref_forces.push_back(hrp::Vector3(0,0,0));
00336 ref_moments.push_back(hrp::Vector3(0,0,0));
00337 }
00338
00339 for (unsigned int i=0; i<nforce; i++){
00340 m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
00341 m_force[i].data.length(6);
00342 m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
00343 m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
00344 registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
00345 std::cerr << "[" << m_profile.instance_name << "] name = " << std::string(sensor_names[i]) << std::endl;
00346 }
00347
00348 std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
00349 for (unsigned int i=0; i<nforce; i++){
00350 std::string nm("limbCOPOffset_"+sensor_names[i]);
00351 m_limbCOPOffsetOut[i] = new OutPort<TimedPoint3D>(nm.c_str(), m_limbCOPOffset[i]);
00352 registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
00353 m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
00354 std::cerr << "[" << m_profile.instance_name << "] name = " << nm << std::endl;
00355 }
00356 sbp_offset = hrp::Vector3(0,0,0);
00357 sbp_cog_offset = hrp::Vector3(0,0,0);
00358
00359 use_force = MODE_REF_FORCE;
00360
00361 if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
00362 is_legged_robot = true;
00363 } else {
00364 is_legged_robot = false;
00365 }
00366
00367 m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
00368 prev_imu_sensor_vel = hrp::Vector3::Zero();
00369
00370 graspless_manip_mode = false;
00371 graspless_manip_arm = "arms";
00372 graspless_manip_p_gain = hrp::Vector3::Zero();
00373
00374 is_stop_mode = false;
00375 is_hand_fix_mode = false;
00376
00377 hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
00378 if (sen == NULL) {
00379 std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl;
00380 }
00381
00382 additional_force_applied_link = m_robot->rootLink();
00383 additional_force_applied_point_offset = hrp::Vector3::Zero();
00384 return RTC::RTC_OK;
00385 }
00386
00387
00388
00389 RTC::ReturnCode_t AutoBalancer::onFinalize()
00390 {
00391 delete zmp_offset_interpolator;
00392 delete transition_interpolator;
00393 delete adjust_footstep_interpolator;
00394 delete leg_names_interpolator;
00395 return RTC::RTC_OK;
00396 }
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412 RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
00413 {
00414 std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00415
00416 return RTC::RTC_OK;
00417 }
00418
00419 RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id)
00420 {
00421 std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00422 Guard guard(m_mutex);
00423 if (control_mode == MODE_ABC) {
00424 control_mode = MODE_SYNC_TO_IDLE;
00425 double tmp_ratio = 0.0;
00426 transition_interpolator->setGoal(&tmp_ratio, m_dt, true);
00427 }
00428 return RTC::RTC_OK;
00429 }
00430
00431 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00432
00433 #define DEBUGP2 (false)
00434 RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
00435 {
00436
00437 loop ++;
00438
00439
00440 if (m_qRefIn.isNew()) {
00441 m_qRefIn.read();
00442 }
00443 if (m_basePosIn.isNew()) {
00444 m_basePosIn.read();
00445 input_basePos(0) = m_basePos.data.x;
00446 input_basePos(1) = m_basePos.data.y;
00447 input_basePos(2) = m_basePos.data.z;
00448 }
00449 if (m_baseRpyIn.isNew()) {
00450 m_baseRpyIn.read();
00451 input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
00452 }
00453 if (m_zmpIn.isNew()) {
00454 m_zmpIn.read();
00455 input_zmp(0) = m_zmp.data.x;
00456 input_zmp(1) = m_zmp.data.y;
00457 input_zmp(2) = m_zmp.data.z;
00458 }
00459 for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
00460 if ( m_ref_forceIn[i]->isNew() ) {
00461 m_ref_forceIn[i]->read();
00462 }
00463 }
00464 if (m_optionalDataIn.isNew()) {
00465 m_optionalDataIn.read();
00466 }
00467 if (m_emergencySignalIn.isNew()){
00468 m_emergencySignalIn.read();
00469
00470
00471
00472
00473
00474 }
00475 if (m_diffCPIn.isNew()) {
00476 m_diffCPIn.read();
00477 gg->set_diff_cp(hrp::Vector3(m_diffCP.data.x, m_diffCP.data.y, m_diffCP.data.z));
00478 }
00479 if (m_refFootOriginExtMomentIn.isNew()) {
00480 m_refFootOriginExtMomentIn.read();
00481 }
00482 if (m_refFootOriginExtMomentIsHoldValueIn.isNew()) {
00483 m_refFootOriginExtMomentIsHoldValueIn.read();
00484 }
00485 if (m_actContactStatesIn.isNew()) {
00486 m_actContactStatesIn.read();
00487 std::vector<bool> tmp_contacts(m_actContactStates.data.length());
00488 for (size_t i = 0; i < m_actContactStates.data.length(); i++) {
00489 tmp_contacts[i] = m_actContactStates.data[i];
00490 }
00491 gg->set_act_contact_states(tmp_contacts);
00492 }
00493
00494
00495 Guard guard(m_mutex);
00496 hrp::Vector3 ref_basePos;
00497 hrp::Matrix33 ref_baseRot;
00498 hrp::Vector3 rel_ref_zmp;
00499 if ( is_legged_robot ) {
00500
00501 fik->storeCurrentParameters();
00502 getTargetParameters();
00503
00504 bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
00505 if (!is_transition_interpolator_empty) {
00506 transition_interpolator->get(&transition_interpolator_ratio, true);
00507 } else {
00508 transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0;
00509 }
00510 if (control_mode != MODE_IDLE ) {
00511 solveFullbodyIK();
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528 rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
00529 } else {
00530 rel_ref_zmp = input_zmp;
00531 fik->d_root_height = 0.0;
00532 }
00533
00534 if (!is_transition_interpolator_empty) {
00535
00536
00537 ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p;
00538 rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
00539 rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
00540 for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
00541 m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
00542 }
00543 for (unsigned int i=0; i< m_force.size(); i++) {
00544 for (unsigned int j=0; j<6; j++) {
00545 m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j];
00546 }
00547 }
00548 for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) {
00549
00550 m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;
00551 m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;
00552 m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;
00553 }
00554 } else {
00555 ref_basePos = m_robot->rootLink()->p;
00556 ref_baseRot = m_robot->rootLink()->R;
00557 }
00558
00559 if (control_mode == MODE_SYNC_TO_ABC) {
00560 control_mode = MODE_ABC;
00561 } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) {
00562 std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
00563 << "] Finished cleanup" << std::endl;
00564 control_mode = MODE_IDLE;
00565 }
00566 }
00567
00568
00569 if ( m_qRef.data.length() != 0 ) {
00570 if (is_legged_robot) {
00571 for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00572 m_qRef.data[i] = m_robot->joint(i)->q;
00573 }
00574 }
00575 m_qOut.write();
00576 }
00577 if (is_legged_robot) {
00578
00579 m_basePos.data.x = ref_basePos(0);
00580 m_basePos.data.y = ref_basePos(1);
00581 m_basePos.data.z = ref_basePos(2);
00582 m_basePos.tm = m_qRef.tm;
00583
00584 hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
00585 m_baseRpy.data.r = baseRpy(0);
00586 m_baseRpy.data.p = baseRpy(1);
00587 m_baseRpy.data.y = baseRpy(2);
00588 m_baseRpy.tm = m_qRef.tm;
00589
00590 double *tform_arr = m_baseTform.data.get_buffer();
00591 tform_arr[0] = m_basePos.data.x;
00592 tform_arr[1] = m_basePos.data.y;
00593 tform_arr[2] = m_basePos.data.z;
00594 hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
00595 m_baseTform.tm = m_qRef.tm;
00596
00597 m_basePose.data.position.x = m_basePos.data.x;
00598 m_basePose.data.position.y = m_basePos.data.y;
00599 m_basePose.data.position.z = m_basePos.data.z;
00600 m_basePose.data.orientation.r = m_baseRpy.data.r;
00601 m_basePose.data.orientation.p = m_baseRpy.data.p;
00602 m_basePose.data.orientation.y = m_baseRpy.data.y;
00603 m_basePose.tm = m_qRef.tm;
00604
00605 m_zmp.data.x = rel_ref_zmp(0);
00606 m_zmp.data.y = rel_ref_zmp(1);
00607 m_zmp.data.z = rel_ref_zmp(2);
00608 m_zmp.tm = m_qRef.tm;
00609
00610 m_cog.data.x = ref_cog(0);
00611 m_cog.data.y = ref_cog(1);
00612 m_cog.data.z = ref_cog(2);
00613 m_cog.tm = m_qRef.tm;
00614
00615 m_sbpCogOffset.data.x = sbp_cog_offset(0);
00616 m_sbpCogOffset.data.y = sbp_cog_offset(1);
00617 m_sbpCogOffset.data.z = sbp_cog_offset(2);
00618 m_sbpCogOffset.tm = m_qRef.tm;
00619
00620 m_basePosOut.write();
00621 m_baseRpyOut.write();
00622 m_baseTformOut.write();
00623 m_basePoseOut.write();
00624 m_zmpOut.write();
00625 m_cogOut.write();
00626 m_sbpCogOffsetOut.write();
00627
00628
00629 hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
00630 if (sen != NULL) {
00631 hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
00632 hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
00633
00634 hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
00635 m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
00636 m_accRefOut.write();
00637 prev_imu_sensor_pos = imu_sensor_pos;
00638 prev_imu_sensor_vel = imu_sensor_vel;
00639 }
00640
00641
00642 m_contactStates.tm = m_qRef.tm;
00643 m_contactStatesOut.write();
00644 m_controlSwingSupportTime.tm = m_qRef.tm;
00645 m_controlSwingSupportTimeOut.write();
00646 m_toeheelRatio.tm = m_qRef.tm;
00647 m_toeheelRatioOut.write();
00648 m_walkingStates.data = gg_is_walking;
00649 m_walkingStates.tm = m_qRef.tm;
00650 m_walkingStatesOut.write();
00651
00652 for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
00653 m_force[i].tm = m_qRef.tm;
00654 m_ref_forceOut[i]->write();
00655 }
00656
00657 for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
00658 m_limbCOPOffset[i].tm = m_qRef.tm;
00659 m_limbCOPOffsetOut[i]->write();
00660 }
00661 }
00662
00663 return RTC::RTC_OK;
00664 }
00665
00666 void AutoBalancer::getTargetParameters()
00667 {
00668
00669 for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00670 m_robot->joint(i)->q = m_qRef.data[i];
00671 }
00672 fik->setReferenceJointAngles();
00673
00674 m_robot->rootLink()->p = input_basePos;
00675 m_robot->rootLink()->R = input_baseRot;
00676 m_robot->calcForwardKinematics();
00677 gg->proc_zmp_weight_map_interpolation();
00678 if (control_mode != MODE_IDLE) {
00679 interpolateLegNamesAndZMPOffsets();
00680
00681 coordinates tmp_fix_coords;
00682 if ( gg_is_walking ) {
00683 gg->set_default_zmp_offsets(default_zmp_offsets);
00684 gg_solved = gg->proc_one_tick();
00685 gg->get_swing_support_mid_coords(tmp_fix_coords);
00686 } else {
00687 tmp_fix_coords = fix_leg_coords;
00688 }
00689 if (!adjust_footstep_interpolator->isEmpty()) {
00690 calcFixCoordsForAdjustFootstep(tmp_fix_coords);
00691 fix_leg_coords = tmp_fix_coords;
00692 }
00693
00694 fixLegToCoords2(tmp_fix_coords);
00695 fix_leg_coords2 = tmp_fix_coords;
00696
00697
00698 target_root_p = m_robot->rootLink()->p;
00699 target_root_R = m_robot->rootLink()->R;
00700 if ( gg_is_walking ) {
00701 getOutputParametersForWalking();
00702 } else {
00703 getOutputParametersForABC();
00704 }
00705
00706 if (control_mode == MODE_SYNC_TO_ABC) {
00707 fik->current_root_p = target_root_p;
00708 fik->current_root_R = target_root_R;
00709 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00710 if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
00711 it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
00712 it->second.target_r0 = it->second.target_link->R * it->second.localR;
00713 }
00714 }
00715 }
00716
00717
00718 updateTargetCoordsForHandFixMode (tmp_fix_coords);
00719 rotateRefForcesForFixCoords (tmp_fix_coords);
00720
00721 calculateOutputRefForces ();
00722
00723 updateWalkingVelocityFromHandError(tmp_fix_coords);
00724 calcReferenceJointAnglesForIK();
00725
00726
00727 hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
00728 hrp::Vector3 tmp_foot_mid_pos = calcFootMidPosUsingZMPWeightMap ();
00729 if (gg_is_walking) {
00730 ref_cog = gg->get_cog();
00731 } else {
00732 ref_cog = tmp_foot_mid_pos;
00733 }
00734 ref_cog(2) = tmp_ref_cog(2);
00735 if (gg_is_walking) {
00736 ref_zmp = gg->get_refzmp();
00737 } else {
00738 ref_zmp(0) = ref_cog(0);
00739 ref_zmp(1) = ref_cog(1);
00740 ref_zmp(2) = tmp_foot_mid_pos(2);
00741 }
00742 } else {
00743
00744 std::vector<coordinates> tmp_end_coords_list;
00745 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00746 if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
00747 tmp_end_coords_list.push_back(coordinates(it->second.target_link->p+it->second.target_link->R*it->second.localPos, it->second.target_link->R*it->second.localR));
00748 }
00749 }
00750 multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
00751 getOutputParametersForIDLE();
00752
00753 for (unsigned int i=0; i< m_force.size(); i++) {
00754 for (unsigned int j=0; j<6; j++) {
00755 m_force[i].data[j] = m_ref_force[i].data[j];
00756 }
00757 }
00758 }
00759
00760 if (gg_is_walking && !gg_solved) stopWalking ();
00761 };
00762
00763 void AutoBalancer::getOutputParametersForWalking ()
00764 {
00765 for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
00766 size_t idx = contact_states_index_map[it->first];
00767
00768 if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) {
00769
00770 gg->get_swing_support_ee_coords_from_ee_name(it->second.target_p0, it->second.target_r0, it->first);
00771
00772 m_contactStates.data[idx] = gg->get_current_support_state_from_ee_name(it->first);
00773
00774 m_controlSwingSupportTime.data[idx] = gg->get_current_swing_time_from_ee_name(it->first);
00775
00776 hrp::Vector3 tmpzmpoff(m_limbCOPOffset[idx].data.x, m_limbCOPOffset[idx].data.y, m_limbCOPOffset[idx].data.z);
00777 gg->get_swing_support_foot_zmp_offsets_from_ee_name(tmpzmpoff, it->first);
00778 m_limbCOPOffset[idx].data.x = tmpzmpoff(0);
00779 m_limbCOPOffset[idx].data.y = tmpzmpoff(1);
00780 m_limbCOPOffset[idx].data.z = tmpzmpoff(2);
00781
00782 double tmp = m_toeheelRatio.data[idx];
00783 gg->get_current_toe_heel_ratio_from_ee_name(tmp, it->first);
00784 m_toeheelRatio.data[idx] = tmp;
00785 } else {
00786
00787 it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
00788 it->second.target_r0 = it->second.target_link->R * it->second.localR;
00789
00790 m_contactStates.data[idx] = false;
00791
00792 m_controlSwingSupportTime.data[idx] = 1.0;
00793
00794 m_limbCOPOffset[idx].data.x = default_zmp_offsets[idx](0);
00795 m_limbCOPOffset[idx].data.y = default_zmp_offsets[idx](1);
00796 m_limbCOPOffset[idx].data.z = default_zmp_offsets[idx](2);
00797
00798 m_toeheelRatio.data[idx] = rats::no_using_toe_heel_ratio;
00799 }
00800 }
00801 };
00802
00803 void AutoBalancer::getOutputParametersForABC ()
00804 {
00805
00806 for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
00807 size_t idx = contact_states_index_map[it->first];
00808
00809
00810
00811 if ( std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
00812 it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
00813 it->second.target_r0 = it->second.target_link->R * it->second.localR;
00814 }
00815
00816 std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
00817 if (dst != leg_names.end()) {
00818 m_contactStates.data[idx] = true;
00819 } else {
00820 m_contactStates.data[idx] = false;
00821 }
00822
00823 m_controlSwingSupportTime.data[idx] = 1.0;
00824
00825 m_limbCOPOffset[idx].data.x = default_zmp_offsets[idx](0);
00826 m_limbCOPOffset[idx].data.y = default_zmp_offsets[idx](1);
00827 m_limbCOPOffset[idx].data.z = default_zmp_offsets[idx](2);
00828
00829 m_toeheelRatio.data[idx] = rats::no_using_toe_heel_ratio;
00830 }
00831 };
00832
00833 void AutoBalancer::getOutputParametersForIDLE ()
00834 {
00835
00836 if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
00837
00838
00839 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00840 m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
00841 m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()];
00842 }
00843
00844 if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) {
00845 m_contactStates.data[contact_states_index_map["rleg"]] = true;
00846 m_contactStates.data[contact_states_index_map["lleg"]] = true;
00847 }
00848 }
00849 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00850 size_t idx = contact_states_index_map[it->first];
00851
00852 m_limbCOPOffset[idx].data.x = 0;
00853 m_limbCOPOffset[idx].data.y = 0;
00854 m_limbCOPOffset[idx].data.z = 0;
00855
00856 m_toeheelRatio.data[idx] = rats::no_using_toe_heel_ratio;
00857 }
00858 };
00859
00860 void AutoBalancer::interpolateLegNamesAndZMPOffsets()
00861 {
00862 if (!zmp_offset_interpolator->isEmpty()) {
00863 double *default_zmp_offsets_output = new double[ikp.size()*3];
00864 zmp_offset_interpolator->get(default_zmp_offsets_output, true);
00865 for (size_t i = 0; i < ikp.size(); i++)
00866 for (size_t j = 0; j < 3; j++)
00867 default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
00868 delete[] default_zmp_offsets_output;
00869 if (DEBUGP) {
00870 std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
00871 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00872 std::cerr << "[" << m_profile.instance_name << "] " << it->first << " = " << default_zmp_offsets[contact_states_index_map[it->first]].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
00873 }
00874 }
00875 }
00876 if (!leg_names_interpolator->isEmpty()) {
00877 leg_names_interpolator->get(&leg_names_interpolator_ratio, true);
00878 }else {
00879 leg_names_interpolator_ratio = 1.0;
00880 }
00881 };
00882
00883 void AutoBalancer::calcFixCoordsForAdjustFootstep (coordinates& tmp_fix_coords)
00884 {
00885 double tmp = 0.0;
00886 adjust_footstep_interpolator->get(&tmp, true);
00887
00888 ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
00889 ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
00890 rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
00891 rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
00892 coordinates tmprc, tmplc;
00893 tmprc.pos = ikp["rleg"].target_p0;
00894 tmprc.rot = ikp["rleg"].target_r0;
00895 tmplc.pos = ikp["lleg"].target_p0;
00896 tmplc.rot = ikp["lleg"].target_r0;
00897 rats::mid_coords(tmp_fix_coords, 0.5, tmprc, tmplc);
00898
00899 };
00900
00901 void AutoBalancer::rotateRefForcesForFixCoords (coordinates& tmp_fix_coords)
00902 {
00903
00904 for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
00905
00906
00907
00908
00909
00910
00911
00912
00913
00914
00915
00916 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]);
00917 ref_moments[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[3], m_ref_force[i].data[4], m_ref_force[i].data[5]);
00918 }
00919 sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
00920 };
00921
00922 void AutoBalancer::updateTargetCoordsForHandFixMode (coordinates& tmp_fix_coords)
00923 {
00924
00925
00926
00927
00928 if (gg_is_walking) {
00929
00930 bool is_hand_control_while_walking = false;
00931 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00932 if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
00933 && it->first.find("arm") != std::string::npos ) {
00934 is_hand_control_while_walking = true;
00935 }
00936 }
00937 if (is_hand_control_while_walking) {
00938
00939
00940 if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
00941 is_hand_fix_initial = false;
00942 hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
00943 if (is_hand_fix_mode) {
00944 dif_p = tmp_fix_coords.rot.transpose() * dif_p;
00945 dif_p(1) = 0;
00946 dif_p = tmp_fix_coords.rot * dif_p;
00947 }
00948 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00949 if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
00950 && it->first.find("arm") != std::string::npos ) {
00951 it->second.target_p0 = it->second.target_p0 + dif_p;
00952 }
00953 }
00954 }
00955 }
00956 };
00957
00958 void AutoBalancer::calculateOutputRefForces ()
00959 {
00960
00961 if (leg_names.size() == 2) {
00962 std::vector<hrp::Vector3> ee_pos;
00963 for (size_t i = 0 ; i < leg_names.size(); i++) {
00964 ABCIKparam& tmpikp = ikp[leg_names[i]];
00965 ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * default_zmp_offsets[i]);
00966 }
00967 double alpha = (ref_zmp - ee_pos[1]).norm() / ((ee_pos[0] - ref_zmp).norm() + (ee_pos[1] - ref_zmp).norm());
00968 if (alpha>1.0) alpha = 1.0;
00969 if (alpha<0.0) alpha = 0.0;
00970 if (DEBUGP) {
00971 std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
00972 }
00973 double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
00974 m_force[0].data[2] = alpha * mg;
00975 m_force[1].data[2] = (1-alpha) * mg;
00976 }
00977 if ( use_force == MODE_REF_FORCE_WITH_FOOT || use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) {
00978 distributeReferenceZMPToWrenches (ref_zmp);
00979 }
00980 prev_ref_zmp = ref_zmp;
00981 };
00982
00983 hrp::Vector3 AutoBalancer::calcFootMidPosUsingZMPWeightMap ()
00984 {
00985 hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
00986 std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
00987 std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
00988 double sum_of_weight = 0.0;
00989 for (size_t i = 0; i < leg_names.size(); i++) {
00990 ABCIKparam& tmpikp = ikp[leg_names[i]];
00991
00992 std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
00993 tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
00994 sum_of_weight += zmp_weight_map[dst->first];
00995 }
00996 tmp_foot_mid_pos *= (1.0 / sum_of_weight);
00997 return tmp_foot_mid_pos;
00998 };
00999
01000 void AutoBalancer::updateWalkingVelocityFromHandError (coordinates& tmp_fix_coords)
01001 {
01002
01003 if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
01004 hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
01005 gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
01006 }
01007
01008
01009
01010
01011 };
01012
01013 void AutoBalancer::calcReferenceJointAnglesForIK ()
01014 {
01015 fik->overwrite_ref_ja_index_vec.clear();
01016
01017 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
01018 if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
01019 int i = it->second.target_link->jointId;
01020 if (gg->get_swing_leg_names().front() == it->first) {
01021 fik->qrefv[i] = fik->qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
01022 }
01023 fik->overwrite_ref_ja_index_vec.push_back(i);
01024 }
01025 }
01026 };
01027
01028 hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2)
01029 {
01030 hrp::Vector3 vv = axis1.cross(axis2);
01031 if (fabs(vv.norm()-0.0) < 1e-5) {
01032 return rot;
01033 } else {
01034 Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
01035 return tmpr.toRotationMatrix() * rot;
01036 }
01037 }
01038
01039 void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
01040 {
01041
01042 std::vector<coordinates> foot_coords;
01043 for (size_t i = 0; i < leg_names.size(); i++) {
01044 ABCIKparam& tmpikp = ikp[leg_names[i]];
01045 if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
01046 (tmpikp.target_link->R * tmpikp.localR)));
01047 }
01048 coordinates current_foot_mid_coords;
01049 multi_mid_coords(current_foot_mid_coords, foot_coords);
01050 hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
01051 hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
01052
01053 hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
01054 m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
01055 rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
01056 m_robot->calcForwardKinematics();
01057 }
01058
01059 void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords)
01060 {
01061
01062
01063
01064 {
01065 hrp::Vector3 ex = hrp::Vector3::UnitX();
01066 hrp::Vector3 ez = hrp::Vector3::UnitZ();
01067 hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
01068 xv1(2) = 0.0;
01069 xv1.normalize();
01070 hrp::Vector3 yv1(ez.cross(xv1));
01071 tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
01072 tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
01073 tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
01074 }
01075 fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
01076 }
01077
01078 void AutoBalancer::solveFullbodyIK ()
01079 {
01080
01081 fik->target_root_p = target_root_p;
01082 fik->target_root_R = target_root_R;
01083 for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
01084 it->second.target_p0 = ikp[it->first].target_p0;
01085 it->second.target_r0 = ikp[it->first].target_r0;
01086 }
01087 fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio;
01088
01089 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
01090 fik->ikp[it->first].is_ik_enable = it->second.is_active;
01091 }
01092
01093 fik->revertRobotStateToCurrent();
01094
01095 hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
01096 static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
01097 hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
01098
01099 fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
01100 }
01101
01102
01103
01104
01105
01106
01107
01108
01109
01110
01111
01112
01113
01114
01115
01116
01117
01118
01119
01120
01121
01122
01123
01124
01125
01126
01127
01128
01129
01130
01131
01132
01133
01134
01135
01136
01137
01138 void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
01139 {
01140 std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
01141 Guard guard(m_mutex);
01142 double tmp_ratio = 0.0;
01143 transition_interpolator->clear();
01144 transition_interpolator->set(&tmp_ratio);
01145 tmp_ratio = 1.0;
01146 transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
01147 prev_ref_zmp = ref_zmp;
01148 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
01149 it->second.is_active = false;
01150 }
01151
01152 for (size_t i = 0; i < limbs.length(); i++) {
01153 ABCIKparam& tmp = ikp[std::string(limbs[i])];
01154 tmp.is_active = true;
01155 std::cerr << "[" << m_profile.instance_name << "] limb [" << std::string(limbs[i]) << "]" << std::endl;
01156 }
01157
01158 control_mode = MODE_SYNC_TO_ABC;
01159 }
01160
01161 void AutoBalancer::stopABCparam()
01162 {
01163 std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
01164
01165 double tmp_ratio = 1.0;
01166 transition_interpolator->clear();
01167 transition_interpolator->set(&tmp_ratio);
01168 tmp_ratio = 0.0;
01169 transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
01170 control_mode = MODE_SYNC_TO_IDLE;
01171 }
01172
01173 bool AutoBalancer::startWalking ()
01174 {
01175 if ( control_mode != MODE_ABC ) {
01176 std::cerr << "[" << m_profile.instance_name << "] Cannot start walking without MODE_ABC. Please startAutoBalancer." << std::endl;
01177 return false;
01178 }
01179 {
01180 Guard guard(m_mutex);
01181 fik->resetIKFailParam();
01182 std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
01183 std::vector<std::string> tmp_all_limbs(leg_names);
01184 std::vector<std::string> init_support_leg_names;
01185 std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
01186 std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
01187 std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
01188 init_swing_leg_names.begin(), init_swing_leg_names.end(),
01189 std::back_inserter(init_support_leg_names));
01190 std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
01191 for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
01192 init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
01193 for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
01194 init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
01195 gg->set_default_zmp_offsets(default_zmp_offsets);
01196 gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
01197 }
01198 is_hand_fix_initial = true;
01199 while ( !gg->proc_one_tick() );
01200 {
01201 Guard guard(m_mutex);
01202 gg_is_walking = gg_solved = true;
01203 }
01204 return true;
01205 }
01206
01207 void AutoBalancer::stopWalking ()
01208 {
01209 std::vector<coordinates> tmp_end_coords_list;
01210 for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
01211 if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(coordinates(ikp[*it].target_p0, ikp[*it].target_r0));
01212 }
01213 multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
01214 fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot);
01215 gg->clear_footstep_nodes_list();
01216 gg_is_walking = false;
01217 }
01218
01219 bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
01220 {
01221 if (control_mode == MODE_IDLE) {
01222 fik->resetIKFailParam();
01223 startABCparam(limbs);
01224 waitABCTransition();
01225 return true;
01226 } else {
01227 return false;
01228 }
01229 }
01230
01231 bool AutoBalancer::stopAutoBalancer ()
01232 {
01233 if (control_mode == MODE_ABC) {
01234 stopABCparam();
01235 waitABCTransition();
01236 return true;
01237 } else {
01238 return false;
01239 }
01240 }
01241
01242 void AutoBalancer::waitABCTransition()
01243 {
01244 while (!transition_interpolator->isEmpty()) usleep(1000);
01245 usleep(1000);
01246 }
01247 bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
01248 {
01249
01250 if ( !is_stop_mode) {
01251 gg->set_all_limbs(leg_names);
01252 coordinates start_ref_coords;
01253 std::vector<coordinates> initial_support_legs_coords;
01254 std::vector<leg_type> initial_support_legs;
01255 bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
01256 if (is_valid_gait_type == false) return false;
01257 bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
01258 initial_support_legs_coords,
01259 start_ref_coords,
01260 initial_support_legs,
01261 (!gg_is_walking));
01262 if (!ret) {
01263 std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
01264 }
01265 if ( !gg_is_walking ) {
01266 ret = startWalking();
01267 }
01268 return ret;
01269 } else {
01270 std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
01271 return false;
01272 }
01273 }
01274
01275 bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
01276 {
01277 gg->set_all_limbs(leg_names);
01278 bool ret = true;
01279 if (gg_is_walking && gg_solved) {
01280 gg->set_velocity_param(vx, vy, vth);
01281 } else {
01282 coordinates ref_coords;
01283 ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
01284 mid_rot(ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
01285 std::vector<leg_type> current_legs;
01286 switch(gait_type) {
01287 case BIPED:
01288 current_legs.assign (1, vy > 0 ? RLEG : LLEG);
01289 break;
01290 case TROT:
01291 current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM)).convert_to_container < std::vector<leg_type> > ();
01292 break;
01293 case PACE:
01294 current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM)).convert_to_container < std::vector<leg_type> > ();
01295 break;
01296 case CRAWL:
01297 std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
01298 return false;
01299 case GALLOP:
01300
01301 std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
01302 return false;
01303 default: break;
01304 }
01305 gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
01306 ret = startWalking();
01307 }
01308 return ret;
01309 }
01310
01311 bool AutoBalancer::goStop ()
01312 {
01313 gg->finalize_velocity_mode();
01314 waitFootSteps();
01315 return true;
01316 }
01317
01318 bool AutoBalancer::emergencyStop ()
01319 {
01320 std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
01321
01322 gg->emergency_stop();
01323 waitFootSteps();
01324 return true;
01325 }
01326
01327 bool AutoBalancer::releaseEmergencyStop ()
01328 {
01329 if (is_stop_mode) {
01330 std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
01331 is_stop_mode = false;
01332 }
01333 return true;
01334 }
01335
01336 bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
01337 {
01338 OpenHRP::AutoBalancerService::StepParamsSequence spss;
01339 spss.length(fss.length());
01340
01341
01342 for (size_t i = 0; i < spss.length(); i++) {
01343 spss[i].sps.length(fss[i].fs.length());
01344 for (size_t j = 0; j < spss[i].sps.length(); j++) {
01345 spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
01346 spss[i].sps[j].step_time = gg->get_default_step_time();
01347 spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
01348 spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
01349 }
01350 }
01351 return setFootStepsWithParam(fss, spss, overwrite_fs_idx);
01352 }
01353
01354 bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
01355 {
01356 if (!is_stop_mode) {
01357 std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
01358
01359
01360 coordinates tmpfs, fstrans;
01361 step_node initial_support_step, initial_input_step;
01362 {
01363 std::vector<step_node> initial_support_steps;
01364 if (gg_is_walking) {
01365 if (overwrite_fs_idx <= 0) {
01366 std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
01367 return false;
01368 }
01369 if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
01370 std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
01371 return false;
01372 }
01373 } else {
01374
01375 for (size_t i = 0; i < fss[0].fs.length(); i++) {
01376 ABCIKparam& tmpikp = ikp[std::string(fss[0].fs[i].leg)];
01377 initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), coordinates(tmpikp.target_p0, tmpikp.target_r0), 0, 0, 0, 0));
01378 }
01379 }
01380 initial_support_step = initial_support_steps.front();
01381 }
01382 {
01383 std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
01384 for (size_t i = 0; i < fss[0].fs.length(); i++) {
01385 if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
01386 coordinates tmp;
01387 memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
01388 tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix();
01389 initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
01390 }
01391 }
01392 }
01393
01394
01395 std::vector< std::vector<coordinates> > fs_vec_list;
01396 std::vector< std::vector<std::string> > leg_name_vec_list;
01397 for (size_t i = 0; i < fss.length(); i++) {
01398 std::vector<coordinates> fs_vec;
01399 std::vector<std::string> leg_name_vec;
01400 for (size_t j = 0; j < fss[i].fs.length(); j++) {
01401 std::string leg(fss[i].fs[j].leg);
01402 if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
01403 memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
01404 tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix();
01405 initial_input_step.worldcoords.transformation(fstrans, tmpfs);
01406 tmpfs = initial_support_step.worldcoords;
01407 tmpfs.transform(fstrans);
01408 } else {
01409 std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl;
01410 return false;
01411 }
01412 leg_name_vec.push_back(leg);
01413 fs_vec.push_back(tmpfs);
01414 }
01415 leg_name_vec_list.push_back(leg_name_vec);
01416 fs_vec_list.push_back(fs_vec);
01417 }
01418 if (spss.length() != fs_vec_list.size()) {
01419 std::cerr << "[" << m_profile.instance_name << "] StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
01420 return false;
01421 }
01422 std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
01423 std::vector< std::vector<step_node> > fnsl;
01424 for (size_t i = 0; i < fs_vec_list.size(); i++) {
01425 if (!(gg_is_walking && i == 0)) {
01426 std::vector<step_node> tmp_fns;
01427 for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
01428 tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
01429 }
01430 fnsl.push_back(tmp_fns);
01431 }
01432 }
01433 bool ret = true;
01434 if (gg_is_walking) {
01435 std::cerr << "[" << m_profile.instance_name << "] Set overwrite footsteps" << std::endl;
01436 gg->set_overwrite_foot_steps_list(fnsl);
01437 gg->set_overwrite_foot_step_index(overwrite_fs_idx);
01438 } else {
01439 std::cerr << "[" << m_profile.instance_name << "] Set normal footsteps" << std::endl;
01440 gg->set_foot_steps_list(fnsl);
01441 ret = startWalking();
01442 }
01443 return ret;
01444 } else {
01445 std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
01446 return false;
01447 }
01448 }
01449
01450 void AutoBalancer::waitFootSteps()
01451 {
01452
01453 while (gg_is_walking || !transition_interpolator->isEmpty() )
01454 usleep(1000);
01455 usleep(1000);
01456 gg->set_offset_velocity_param(0,0,0);
01457 }
01458
01459 void AutoBalancer::waitFootStepsEarly(const double tm)
01460 {
01461 if (!gg_is_walking) { return;}
01462 while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
01463 usleep(1000);
01464 usleep(1000);
01465 gg->set_offset_velocity_param(0,0,0);
01466 }
01467
01468 bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
01469 {
01470 std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
01471 if (i_param.stride_parameter.length() == 4) {
01472 gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
01473 i_param.stride_parameter[1]*0.5, i_param.stride_parameter[2]*0.5);
01474 } else {
01475 gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
01476 i_param.stride_parameter[4], i_param.stride_parameter[5]);
01477 }
01478 std::vector<hrp::Vector3> off;
01479 for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
01480 off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
01481 }
01482 gg->set_leg_default_translate_pos(off);
01483 gg->set_default_step_time(i_param.default_step_time);
01484 gg->set_default_step_height(i_param.default_step_height);
01485 gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
01486 gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
01487 gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
01488 gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
01489 gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
01490 gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
01491
01492
01493
01494
01495
01496
01497
01498
01499 if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
01500 gg->set_default_orbit_type(SHUFFLING);
01501 } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
01502 gg->set_default_orbit_type(CYCLOID);
01503 } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
01504 gg->set_default_orbit_type(RECTANGLE);
01505 } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
01506 gg->set_default_orbit_type(STAIR);
01507 } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
01508 gg->set_default_orbit_type(CYCLOIDDELAY);
01509 } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
01510 gg->set_default_orbit_type(CYCLOIDDELAYKICK);
01511 } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
01512 gg->set_default_orbit_type(CROSS);
01513 }
01514 gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
01515 gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
01516 gg->set_swing_trajectory_time_offset_xy2z(i_param.swing_trajectory_time_offset_xy2z);
01517 gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
01518 gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));
01519 gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
01520 gg->set_toe_angle(i_param.toe_angle);
01521 gg->set_heel_angle(i_param.heel_angle);
01522 gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
01523 gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
01524 gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
01525 gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
01526 gg->set_toe_check_thre(i_param.toe_check_thre);
01527 gg->set_heel_check_thre(i_param.heel_check_thre);
01528 std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
01529 std::cerr << "[" << m_profile.instance_name << "] ";
01530 gg->set_toe_heel_phase_ratio(tmp_ratio);
01531 gg->set_use_toe_joint(i_param.use_toe_joint);
01532 gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
01533 gg->set_use_toe_heel_auto_set(i_param.use_toe_heel_auto_set);
01534 gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
01535 gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
01536 gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
01537 gg->set_leg_margin(i_param.leg_margin);
01538 gg->set_stride_limitation_for_circle_type(i_param.stride_limitation_for_circle_type);
01539 gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
01540 gg->set_use_stride_limitation(i_param.use_stride_limitation);
01541 gg->set_footstep_modification_gain(i_param.footstep_modification_gain);
01542 gg->set_modify_footsteps(i_param.modify_footsteps);
01543 gg->set_cp_check_margin(i_param.cp_check_margin);
01544 gg->set_margin_time_ratio(i_param.margin_time_ratio);
01545 if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
01546 gg->set_stride_limitation_type(SQUARE);
01547 } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
01548 gg->set_stride_limitation_type(CIRCLE);
01549 }
01550
01551
01552 gg->print_param(std::string(m_profile.instance_name));
01553 return true;
01554 };
01555
01556 bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
01557 {
01558 gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], i_param.stride_parameter[4], i_param.stride_parameter[5]);
01559 std::vector<hrp::Vector3> off;
01560 gg->get_leg_default_translate_pos(off);
01561 i_param.leg_default_translate_pos.length(off.size());
01562 for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
01563 i_param.leg_default_translate_pos[i].length(3);
01564 i_param.leg_default_translate_pos[i][0] = off[i](0);
01565 i_param.leg_default_translate_pos[i][1] = off[i](1);
01566 i_param.leg_default_translate_pos[i][2] = off[i](2);
01567 }
01568 i_param.default_step_time = gg->get_default_step_time();
01569 i_param.default_step_height = gg->get_default_step_height();
01570 i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
01571 i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
01572 i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
01573 i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
01574 i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
01575 i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
01576 i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
01577 i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
01578 if (gg->get_default_orbit_type() == SHUFFLING) {
01579 i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
01580 } else if (gg->get_default_orbit_type() == CYCLOID) {
01581 i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
01582 } else if (gg->get_default_orbit_type() == RECTANGLE) {
01583 i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
01584 } else if (gg->get_default_orbit_type() == STAIR) {
01585 i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
01586 } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
01587 i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
01588 } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
01589 i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
01590 } else if (gg->get_default_orbit_type() == CROSS) {
01591 i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
01592 }
01593
01594 hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
01595 for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
01596 tmpv = gg->get_cycloid_delay_kick_point_offset();
01597 for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
01598 i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
01599 i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
01600 i_param.swing_trajectory_time_offset_xy2z = gg->get_swing_trajectory_time_offset_xy2z();
01601 i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
01602 i_param.toe_angle = gg->get_toe_angle();
01603 i_param.heel_angle = gg->get_heel_angle();
01604 i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
01605 i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
01606 i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
01607 i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
01608 i_param.toe_check_thre = gg->get_toe_check_thre();
01609 i_param.heel_check_thre = gg->get_heel_check_thre();
01610 std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
01611 gg->get_toe_heel_phase_ratio(ratio);
01612 for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
01613 i_param.use_toe_joint = gg->get_use_toe_joint();
01614 i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
01615 i_param.use_toe_heel_auto_set = gg->get_use_toe_heel_auto_set();
01616 std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
01617 i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
01618 i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
01619 i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
01620 i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
01621 i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
01622 i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
01623 for (size_t i=0; i<4; i++) {
01624 i_param.leg_margin[i] = gg->get_leg_margin(i);
01625 }
01626 for (size_t i=0; i<5; i++) {
01627 i_param.stride_limitation_for_circle_type[i] = gg->get_stride_limitation_for_circle_type(i);
01628 }
01629 for (size_t i=0; i<5; i++) {
01630 i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
01631 }
01632 i_param.use_stride_limitation = gg->get_use_stride_limitation();
01633 i_param.footstep_modification_gain = gg->get_footstep_modification_gain();
01634 i_param.modify_footsteps = gg->get_modify_footsteps();
01635 for (size_t i=0; i<2; i++) {
01636 i_param.cp_check_margin[i] = gg->get_cp_check_margin(i);
01637 }
01638 i_param.margin_time_ratio = gg->get_margin_time_ratio();
01639 if (gg->get_stride_limitation_type() == SQUARE) {
01640 i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
01641 } else if (gg->get_stride_limitation_type() == CIRCLE) {
01642 i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE;
01643 }
01644 return true;
01645 };
01646
01647 bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
01648 {
01649 Guard guard(m_mutex);
01650 std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
01651 double *default_zmp_offsets_array = new double[ikp.size()*3];
01652 for (size_t i = 0; i < ikp.size(); i++)
01653 for (size_t j = 0; j < 3; j++)
01654 default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
01655 zmp_transition_time = i_param.zmp_transition_time;
01656 adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
01657 if (zmp_offset_interpolator->isEmpty()) {
01658 zmp_offset_interpolator->clear();
01659 zmp_offset_interpolator->setGoal(default_zmp_offsets_array, zmp_transition_time, true);
01660 } else {
01661 std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets cannot be set because interpolating." << std::endl;
01662 }
01663 if (control_mode == MODE_IDLE) {
01664 switch (i_param.use_force_mode) {
01665 case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
01666 use_force = MODE_NO_FORCE;
01667 break;
01668 case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
01669 use_force = MODE_REF_FORCE;
01670 break;
01671 case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
01672 use_force = MODE_REF_FORCE_WITH_FOOT;
01673 break;
01674 case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
01675 use_force = MODE_REF_FORCE_RFU_EXT_MOMENT;
01676 break;
01677 default:
01678 break;
01679 }
01680 } else {
01681 std::cerr << "[" << m_profile.instance_name << "] use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
01682 }
01683 graspless_manip_mode = i_param.graspless_manip_mode;
01684 graspless_manip_arm = std::string(i_param.graspless_manip_arm);
01685 for (size_t j = 0; j < 3; j++)
01686 graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
01687 for (size_t j = 0; j < 3; j++)
01688 graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
01689 graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
01690 i_param.graspless_manip_reference_trans_rot[1],
01691 i_param.graspless_manip_reference_trans_rot[2],
01692 i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix());
01693 transition_time = i_param.transition_time;
01694 std::vector<std::string> cur_leg_names, dst_leg_names;
01695 cur_leg_names = leg_names;
01696 for (size_t i = 0; i < i_param.leg_names.length(); i++) {
01697 dst_leg_names.push_back(std::string(i_param.leg_names[i]));
01698 }
01699 std::sort(cur_leg_names.begin(), cur_leg_names.end());
01700 std::sort(dst_leg_names.begin(), dst_leg_names.end());
01701 if (cur_leg_names != dst_leg_names) {
01702 if (leg_names_interpolator->isEmpty()) {
01703 leg_names.clear();
01704 leg_names = dst_leg_names;
01705 if (control_mode == MODE_ABC) {
01706 double tmp_ratio = 0.0;
01707 leg_names_interpolator->set(&tmp_ratio);
01708 tmp_ratio = 1.0;
01709 leg_names_interpolator->setGoal(&tmp_ratio, 5.0, true);
01710 control_mode = MODE_SYNC_TO_ABC;
01711 }
01712 }
01713 } else {
01714 std::cerr << "[" << m_profile.instance_name << "] leg_names cannot be set because interpolating." << std::endl;
01715 }
01716 if (!gg_is_walking) {
01717 is_hand_fix_mode = i_param.is_hand_fix_mode;
01718 std::cerr << "[" << m_profile.instance_name << "] is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
01719 } else {
01720 std::cerr << "[" << m_profile.instance_name << "] is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
01721 }
01722 if (control_mode == MODE_IDLE) {
01723 for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
01724 std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
01725 memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
01726 it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
01727 }
01728 } else {
01729 std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
01730 }
01731 if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
01732 gait_type = BIPED;
01733 } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
01734 gait_type = TROT;
01735 } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
01736 gait_type = PACE;
01737 } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
01738 gait_type = CRAWL;
01739 } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
01740 gait_type = GALLOP;
01741 }
01742
01743 std::cerr << "[" << m_profile.instance_name << "] Ref force balancing" << std::endl;
01744 if ( use_force == MODE_REF_FORCE_WITH_FOOT && control_mode != MODE_IDLE ) {
01745 std::cerr << "[" << m_profile.instance_name << "] additional_force_applied_point_offset and additional_force_applied_link_name cannot be updated during MODE_REF_FORCE_WITH_FOOT and non-MODE_IDLE"<< std::endl;
01746 } else if ( !m_robot->link(std::string(i_param.additional_force_applied_link_name)) ) {
01747 std::cerr << "[" << m_profile.instance_name << "] Invalid link name for additional_force_applied_link_name = " << i_param.additional_force_applied_link_name << std::endl;
01748 } else {
01749 additional_force_applied_link = m_robot->link(std::string(i_param.additional_force_applied_link_name));
01750 for (size_t i = 0; i < 3; i++) {
01751 additional_force_applied_point_offset(i) = i_param.additional_force_applied_point_offset[i];
01752 }
01753 std::cerr << "[" << m_profile.instance_name << "] Link name for additional_force_applied_link_name = " << additional_force_applied_link->name << ", additional_force_applied_point_offset = " << additional_force_applied_point_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
01754 }
01755
01756 for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
01757 std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
01758 std::cerr << "[" << m_profile.instance_name << "] localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
01759 std::cerr << "[" << m_profile.instance_name << "] localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl;
01760 }
01761
01762 std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets = ";
01763 for (size_t i = 0; i < ikp.size() * 3; i++) {
01764 std::cerr << default_zmp_offsets_array[i] << " ";
01765 }
01766 std::cerr << std::endl;
01767 delete[] default_zmp_offsets_array;
01768 std::cerr << "[" << m_profile.instance_name << "] use_force_mode = " << getUseForceModeString() << std::endl;
01769 std::cerr << "[" << m_profile.instance_name << "] graspless_manip_mode = " << graspless_manip_mode << std::endl;
01770 std::cerr << "[" << m_profile.instance_name << "] graspless_manip_arm = " << graspless_manip_arm << std::endl;
01771 std::cerr << "[" << m_profile.instance_name << "] graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl;
01772 std::cerr << "[" << m_profile.instance_name << "] graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl;
01773 std::cerr << "[" << m_profile.instance_name << "] graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl;
01774 std::cerr << "[" << m_profile.instance_name << "] transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
01775 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;
01776 std::cerr << "[" << m_profile.instance_name << "] default_gait_type = " << gait_type << std::endl;
01777
01778 fik->move_base_gain = i_param.move_base_gain;
01779 fik->pos_ik_thre = i_param.pos_ik_thre;
01780 fik->rot_ik_thre = i_param.rot_ik_thre;
01781 fik->printParam();
01782
01783 fik->setIKParam(ee_vec, i_param.ik_limb_parameters);
01784
01785 fik->use_limb_stretch_avoidance = i_param.use_limb_stretch_avoidance;
01786 fik->limb_stretch_avoidance_time_const = i_param.limb_stretch_avoidance_time_const;
01787 for (size_t i = 0; i < 2; i++) {
01788 fik->limb_stretch_avoidance_vlimit[i] = i_param.limb_stretch_avoidance_vlimit[i];
01789 }
01790 for (size_t i = 0; i < fik->ikp.size(); i++) {
01791 fik->ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i];
01792 }
01793 return true;
01794 };
01795
01796 bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
01797 {
01798 i_param.default_zmp_offsets.length(ikp.size());
01799 for (size_t i = 0; i < ikp.size(); i++) {
01800 i_param.default_zmp_offsets[i].length(3);
01801 for (size_t j = 0; j < 3; j++) {
01802 i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
01803 }
01804 }
01805 switch(control_mode) {
01806 case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
01807 case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
01808 case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
01809 case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
01810 default: break;
01811 }
01812 switch(use_force) {
01813 case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
01814 case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
01815 case MODE_REF_FORCE_WITH_FOOT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT; break;
01816 case MODE_REF_FORCE_RFU_EXT_MOMENT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT; break;
01817 default: break;
01818 }
01819 i_param.graspless_manip_mode = graspless_manip_mode;
01820 i_param.graspless_manip_arm = graspless_manip_arm.c_str();
01821 for (size_t j = 0; j < 3; j++)
01822 i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
01823 for (size_t j = 0; j < 3; j++)
01824 i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
01825 Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
01826 i_param.graspless_manip_reference_trans_rot[0] = qt.w();
01827 i_param.graspless_manip_reference_trans_rot[1] = qt.x();
01828 i_param.graspless_manip_reference_trans_rot[2] = qt.y();
01829 i_param.graspless_manip_reference_trans_rot[3] = qt.z();
01830 i_param.transition_time = transition_time;
01831 i_param.zmp_transition_time = zmp_transition_time;
01832 i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
01833 i_param.leg_names.length(leg_names.size());
01834 for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
01835 i_param.is_hand_fix_mode = is_hand_fix_mode;
01836 i_param.end_effector_list.length(ikp.size());
01837 {
01838 size_t i = 0;
01839 for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
01840 copyRatscoords2Footstep(i_param.end_effector_list[i],
01841 coordinates(it->second.localPos, it->second.localR));
01842 i_param.end_effector_list[i].leg = it->first.c_str();
01843 i++;
01844 }
01845 }
01846 switch(gait_type) {
01847 case BIPED: i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED; break;
01848 case TROT: i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT; break;
01849 case PACE: i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE; break;
01850 case CRAWL: i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL; break;
01851 case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
01852 default: break;
01853 }
01854
01855 i_param.move_base_gain = fik->move_base_gain;
01856 i_param.pos_ik_thre = fik->pos_ik_thre;
01857 i_param.rot_ik_thre = fik->rot_ik_thre;
01858
01859 fik->getIKParam(ee_vec, i_param.ik_limb_parameters);
01860
01861 i_param.use_limb_stretch_avoidance = fik->use_limb_stretch_avoidance;
01862 i_param.limb_stretch_avoidance_time_const = fik->limb_stretch_avoidance_time_const;
01863 i_param.limb_length_margin.length(fik->ikp.size());
01864 for (size_t i = 0; i < 2; i++) {
01865 i_param.limb_stretch_avoidance_vlimit[i] = fik->limb_stretch_avoidance_vlimit[i];
01866 }
01867 for (size_t i = 0; i < ikp.size(); i++) {
01868 i_param.limb_length_margin[i] = fik->ikp[ee_vec[i]].limb_length_margin;
01869 }
01870 i_param.additional_force_applied_link_name = additional_force_applied_link->name.c_str();
01871 for (size_t i = 0; i < 3; i++) {
01872 i_param.additional_force_applied_point_offset[i] = additional_force_applied_point_offset(i);
01873 }
01874 return true;
01875 };
01876
01877 void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
01878 {
01879 memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
01880 Eigen::Quaternion<double> qt(in_fs.rot);
01881 out_fs.rot[0] = qt.w();
01882 out_fs.rot[1] = qt.x();
01883 out_fs.rot[2] = qt.y();
01884 out_fs.rot[3] = qt.z();
01885 };
01886
01887 bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
01888 {
01889 copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
01890 copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
01891 copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
01892 copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
01893 copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
01894 if (gg->get_support_leg_names().front() == "rleg") {
01895 i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
01896 } else {
01897 i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
01898 }
01899 switch ( gg->get_current_support_states().front() ) {
01900 case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
01901 case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
01902 case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
01903 default: break;
01904 }
01905 return true;
01906 };
01907
01908 bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
01909 {
01910 std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
01911 << "] adjustFootSteps" << std::endl;
01912 if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) {
01913 Guard guard(m_mutex);
01914
01915 hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
01916 hrp::Matrix33 eerot, tmprot;
01917 coordinates org_mid_coords, target_mid_coords;
01918
01919 ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
01920 ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
01921 ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
01922 ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
01923 mid_coords(org_mid_coords, 0.5,
01924 coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
01925 coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
01926 org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
01927
01928
01929
01930 memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
01931 eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix();
01932 ikp["rleg"].adjust_interpolation_target_r0 = eerot;
01933 ikp["rleg"].adjust_interpolation_target_p0 = eepos;
01934 memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
01935 eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix();
01936 ikp["lleg"].adjust_interpolation_target_r0 = eerot;
01937 ikp["lleg"].adjust_interpolation_target_p0 = eepos;
01938 mid_coords(target_mid_coords, 0.5,
01939 coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
01940 coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
01941 coordinates rtrans, ltrans;
01942 target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
01943 target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
01944 target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
01945
01946 target_mid_rpy(2) = org_mid_rpy(2);
01947 target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
01948 target_mid_coords.pos = org_mid_coords.pos;
01949
01950 coordinates tmpc;
01951 tmpc = target_mid_coords;
01952 tmpc.transform(rtrans);
01953 ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
01954 ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
01955 tmpc = target_mid_coords;
01956 tmpc.transform(ltrans);
01957 ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
01958 ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
01959
01960 adjust_footstep_interpolator->clear();
01961 double tmp = 0.0;
01962 adjust_footstep_interpolator->set(&tmp);
01963 tmp = 1.0;
01964 adjust_footstep_interpolator->setGoal(&tmp, adjust_footstep_transition_time, true);
01965 }
01966 while (!adjust_footstep_interpolator->isEmpty() )
01967 usleep(1000);
01968 usleep(1000);
01969 return true;
01970 };
01971
01972 bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
01973 {
01974 std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
01975 o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
01976 if (gg_is_walking) {
01977 std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
01978 o_current_fs_idx = gg->get_footstep_index();
01979 o_footstep->length(fsnl.size());
01980 for (size_t i = 0; i < fsnl.size(); i++) {
01981 o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
01982 copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
01983 }
01984 }
01985 return true;
01986 };
01987
01988 bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
01989 {
01990 std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
01991 o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
01992 if (gg_is_walking) {
01993 std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
01994 return false;
01995 } else {
01996 gg->set_all_limbs(leg_names);
01997 std::vector< std::vector<step_node> > new_footstep_nodes_list;
01998 coordinates start_ref_coords;
01999 std::vector<coordinates> initial_support_legs_coords;
02000 std::vector<leg_type> initial_support_legs;
02001 bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
02002 if (is_valid_gait_type == false) return false;
02003
02004 gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
02005 initial_support_legs_coords, start_ref_coords, initial_support_legs,
02006 new_footstep_nodes_list, true, 0);
02007 o_footstep->length(new_footstep_nodes_list.size());
02008 for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
02009 o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
02010 for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
02011 leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
02012 o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
02013 (tmp_leg_type == LLEG) ? "lleg":
02014 (tmp_leg_type == RARM) ? "rarm":
02015 "larm");
02016 copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
02017 }
02018 }
02019 return true;
02020 }
02021 };
02022
02023 void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
02024 {
02025 hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
02026 hrp::Vector3 tmpcog = m_robot->calcCM();
02027 if ( use_force == MODE_NO_FORCE ) {
02028 tmp_input_sbp = tmpcog + sbp_cog_offset;
02029 } else {
02030 calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height);
02031 tmp_input_sbp = target_sbp - sbp_offset;
02032 sbp_cog_offset = tmp_input_sbp - tmpcog;
02033 }
02034 };
02035
02036 void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height)
02037 {
02038 hrp::Vector3 denom, nume;
02039
02040 double mass = m_robot->totalMass();
02041 double mg = mass * gg->get_gravitational_acceleration();
02042 hrp::Vector3 total_sensor_ref_force = hrp::Vector3::Zero();
02043 for (size_t i = 0; i < ref_forces.size(); i++) {
02044 total_sensor_ref_force += ref_forces[i];
02045 }
02046 hrp::Vector3 total_nosensor_ref_force = mg * hrp::Vector3::UnitZ() - total_sensor_ref_force;
02047 hrp::Vector3 tmp_ext_moment = fix_leg_coords2.pos.cross(total_nosensor_ref_force) + fix_leg_coords2.rot * hrp::Vector3(m_refFootOriginExtMoment.data.x, m_refFootOriginExtMoment.data.y, m_refFootOriginExtMoment.data.z);
02048
02049
02050 static hrp::Vector3 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
02051
02052 if ( !m_refFootOriginExtMomentIsHoldValue.data ) {
02053 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
02054 }
02055 hrp::Vector3 tmp_prev_additional_force_applied_pos = fix_leg_coords2.rot * prev_additional_force_applied_pos + fix_leg_coords2.pos;
02056
02057 for (size_t j = 0; j < 2; j++) {
02058 nume(j) = mg * tmpcog(j);
02059 denom(j) = mg;
02060 if ( use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) {
02061
02062 nume(j) += (tmp_prev_additional_force_applied_pos(j)-additional_force_applied_link->p(j))*total_nosensor_ref_force(2) + (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
02063 denom(j) -= total_nosensor_ref_force(2);
02064 } else {
02065 for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
02066
02067 if (std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()) {
02068 size_t idx = contact_states_index_map[it->first];
02069
02070 hrp::Vector3 fpos = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
02071 nume(j) += ( (fpos(2) - ref_com_height) * ref_forces[idx](j) - fpos(j) * ref_forces[idx](2) );
02072 nume(j) += (j==0 ? ref_moments[idx](1):-ref_moments[idx](0));
02073 denom(j) -= ref_forces[idx](2);
02074 }
02075 }
02076 if ( use_force == MODE_REF_FORCE_WITH_FOOT ) {
02077 hrp::Vector3 fpos(additional_force_applied_link->p+additional_force_applied_point_offset);
02078 nume(j) += ( (fpos(2) - ref_com_height) * total_nosensor_ref_force(j) - fpos(j) * total_nosensor_ref_force(2) );
02079 denom(j) -= total_nosensor_ref_force(2);
02080 }
02081 }
02082 sb_point(j) = nume(j) / denom(j);
02083 }
02084 sb_point(2) = ref_com_height;
02085 };
02086
02087 #ifndef rad2deg
02088 #define rad2deg(rad) (rad * 180 / M_PI)
02089 #endif
02090 #ifndef deg2rad
02091 #define deg2rad(deg) (deg * M_PI / 180)
02092 #endif
02093
02094 hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_coords)
02095 {
02096 if (graspless_manip_mode) {
02097 hrp::Vector3 dp,dr;
02098 coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
02099 ref_hand_coords.transform(graspless_manip_reference_trans_coords);
02100 hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
02101 if ( graspless_manip_arm == "arms" ) {
02102 hrp::Vector3 rarm_pos = ikp["rarm"].target_p0;
02103 hrp::Vector3 larm_pos = ikp["larm"].target_p0;
02104 act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
02105 hrp::Vector3 act_y = larm_pos-rarm_pos;
02106 act_y(2) = 0;
02107 act_y.normalize();
02108 hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
02109 ref_y(2) = 0;
02110 ref_y.normalize();
02111 dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y)));
02112 } else {
02113 ABCIKparam& tmpikp = ikp[graspless_manip_arm];
02114 act_hand_coords = rats::coordinates(tmpikp.target_p0,
02115 tmpikp.target_r0);
02116 rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
02117 dr(0) = 0; dr(1) = 0;
02118 }
02119 dp = act_hand_coords.pos - ref_hand_coords.pos
02120 + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
02121 dp(2) = 0;
02122 hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
02123
02124 hrp::Vector3 dp2 = foot_mt * dp;
02125
02126 return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
02127 graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
02128 graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
02129 } else {
02130 return hrp::Vector3::Zero();
02131 }
02132 };
02133
02134 bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
02135 switch(gait_type) {
02136 case BIPED:
02137 initial_support_legs_coords.assign (1,
02138 y > 0 ?
02139 coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0)
02140 : coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0));
02141 initial_support_legs.assign (1, y > 0 ? RLEG : LLEG);
02142 break;
02143 case TROT:
02144 initial_support_legs_coords = (y > 0 ?
02145 boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))
02146 : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))).convert_to_container < std::vector<coordinates> > ();
02147 initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM)).convert_to_container < std::vector<leg_type> > ();
02148 break;
02149 case PACE:
02150 initial_support_legs_coords = (y > 0 ?
02151 boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))
02152 : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))).convert_to_container < std::vector<coordinates> > ();
02153 initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM)).convert_to_container < std::vector<leg_type> > ();
02154 break;
02155 case CRAWL:
02156 std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
02157 return false;
02158 case GALLOP:
02159
02160 std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
02161 return false;
02162 default: break;
02163 }
02164 start_ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
02165 mid_rot(start_ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
02166 return true;
02167 };
02168
02169
02170
02171
02172
02173 void calcWeightedLinearEquation(hrp::dvector& ret, const hrp::dmatrix& A, const hrp::dmatrix& W, const hrp::dvector& b)
02174 {
02175 hrp::dmatrix W2 = hrp::dmatrix::Zero(W.rows(), W.cols());
02176 for (size_t i = 0; i < W.rows(); i++) W2(i,i) = std::sqrt(W(i,i));
02177 hrp::dmatrix Aw = A*W2;
02178 hrp::dmatrix Aw_inv = hrp::dmatrix::Zero(A.cols(), A.rows());
02179 hrp::calcPseudoInverse(Aw, Aw_inv);
02180 ret = W2 * Aw_inv * b;
02181
02182 };
02183
02184 void AutoBalancer::distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zmp)
02185 {
02186
02187
02188 hrp::Vector3 tmp_ref_zmp = _ref_zmp + 0.055 * (_ref_zmp - prev_ref_zmp) / m_dt;
02189
02190 std::vector<hrp::Vector3> cop_pos;
02191 std::vector<double> limb_gains;
02192 for (size_t i = 0 ; i < leg_names.size(); i++) {
02193 ABCIKparam& tmpikp = ikp[leg_names[i]];
02194 cop_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
02195 limb_gains.push_back(m_contactStates.data[contact_states_index_map[leg_names[i]]] ? 1.0 : 0.0);
02196 }
02197 size_t ee_num = leg_names.size();
02198 size_t state_dim = 6*ee_num;
02199 size_t total_wrench_dim = 5;
02200
02201 size_t total_fz = m_ref_force[0].data[2]+m_ref_force[1].data[2];
02202
02203 hrp::dmatrix Wmat = hrp::dmatrix::Identity(state_dim/2, state_dim/2);
02204 hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim/2);
02205
02206
02207 for (size_t j = 0; j < ee_num; j++) {
02208 if (total_wrench_dim == 3) {
02209 Gmat(0,3*j+2) = 1.0;
02210 } else {
02211 for (size_t k = 0; k < 3; k++) Gmat(k,3*j+k) = 1.0;
02212 }
02213 }
02214
02215 for (size_t i = 0; i < total_wrench_dim; i++) {
02216 for (size_t j = 0; j < ee_num; j++) {
02217 if ( i == total_wrench_dim-2 ) {
02218 Gmat(i,3*j+1) = -(cop_pos[j](2) - tmp_ref_zmp(2));
02219 Gmat(i,3*j+2) = (cop_pos[j](1) - tmp_ref_zmp(1));
02220 } else if ( i == total_wrench_dim-1 ) {
02221 Gmat(i,3*j) = (cop_pos[j](2) - tmp_ref_zmp(2));
02222 Gmat(i,3*j+2) = -(cop_pos[j](0) - tmp_ref_zmp(0));
02223 }
02224 }
02225 }
02226
02227 for (size_t j = 0; j < ee_num; j++) {
02228 for (size_t i = 0; i < 3; i++) {
02229 if (ee_num == 2)
02230 Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j] * (i==2? 1.0 : 0.01);
02231 else
02232 Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j];
02233 }
02234 }
02235
02236
02237
02238 hrp::dvector ret(state_dim/2);
02239 hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim);
02240 total_wrench(total_wrench_dim-5) = m_ref_force[0].data[0]+m_ref_force[1].data[0];
02241 total_wrench(total_wrench_dim-4) = m_ref_force[0].data[1]+m_ref_force[1].data[1];
02242 total_wrench(total_wrench_dim-3) = total_fz;
02243 calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench);
02244 if (DEBUGP) {
02245 std::cerr << "[" << m_profile.instance_name << "] distributeReferenceZMPToWrenches" << std::endl;
02246 }
02247 for (size_t i = 0 ; i < leg_names.size(); i++) {
02248 size_t fidx = contact_states_index_map[leg_names[i]];
02249 ABCIKparam& tmpikp = ikp[leg_names[i]];
02250 hrp::Vector3 f_ee(ret(3*i), ret(3*i+1), ret(3*i+2));
02251
02252 hrp::Vector3 tmp_ee_pos = tmpikp.target_p0;
02253 hrp::Vector3 n_ee = (cop_pos[i]-tmp_ee_pos).cross(f_ee);
02254 m_force[fidx].data[0] = f_ee(0);
02255 m_force[fidx].data[1] = f_ee(1);
02256 m_force[fidx].data[2] = f_ee(2);
02257 m_force[fidx].data[3] = n_ee(0);
02258 m_force[fidx].data[4] = n_ee(1);
02259 m_force[fidx].data[5] = n_ee(2);
02260 if (DEBUGP) {
02261 std::cerr << "[" << m_profile.instance_name << "] "
02262 << "ref_force [" << leg_names[i] << "] " << f_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
02263 << "ref_moment [" << leg_names[i] << "] " << n_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
02264 }
02265 }
02266 if (DEBUGP) {
02267 std::cerr << "[" << m_profile.instance_name << "] Gmat = " << Gmat.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
02268 std::cerr << "[" << m_profile.instance_name << "] total_wrench = " << total_wrench.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
02269 hrp::dvector tmp(total_wrench.size());
02270 tmp = Gmat*ret;
02271 std::cerr << "[" << m_profile.instance_name << "] Gmat*ret = " << tmp.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
02272 std::cerr << "[" << m_profile.instance_name << "] (Gmat*ret-total_wrench) = " << (tmp-total_wrench).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
02273 std::cerr << "[" << m_profile.instance_name << "] ret = " << ret.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
02274 std::cerr << "[" << m_profile.instance_name << "] Wmat(diag) = [";
02275 for (size_t j = 0; j < ee_num; j++) {
02276 for (size_t i = 0; i < 3; i++) {
02277 std::cerr << Wmat(i+j*3, i+j*3) << " ";
02278 }
02279 }
02280 std::cerr << "]" << std::endl;
02281 }
02282 };
02283
02284 std::string AutoBalancer::getUseForceModeString ()
02285 {
02286 switch (use_force) {
02287 case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
02288 return "MODE_NO_FORCE";
02289 case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
02290 return "MODE_REF_FORCE";
02291 case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
02292 return "MODE_REF_FORCE_WITH_FOOT";
02293 case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
02294 return "MODE_REF_FORCE_RFU_EXT_MOMENT";
02295 default:
02296 return "";
02297 }
02298 };
02299
02300
02301 extern "C"
02302 {
02303
02304 void AutoBalancerInit(RTC::Manager* manager)
02305 {
02306 RTC::Properties profile(autobalancer_spec);
02307 manager->registerFactory(profile,
02308 RTC::Create<AutoBalancer>,
02309 RTC::Delete<AutoBalancer>);
02310 }
02311
02312 };
02313
02314