AutoBalancer.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
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 // Module specification
00024 // <rtc-template block="module_spec">
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         // Configuration variables
00038         "conf.default.debugLevel", "0",
00039         ""
00040     };
00041 // </rtc-template>
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       // <rtc-template block="initializer">
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       // </rtc-template>
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     // Registration: InPort/OutPort/Service
00100     // <rtc-template block="registration">
00101     // Set InPort buffers
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     // Set OutPort buffer
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     // Set service provider to Ports
00129     m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
00130   
00131     // Set service consumers to Ports
00132   
00133     // Set CORBA Service Ports
00134     addPort(m_AutoBalancerServicePort);
00135   
00136     // </rtc-template>
00137     // <rtc-template block="bind_config">
00138     // Bind variables and configuration variable
00139   
00140     // </rtc-template>
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     // allocate memory for outPorts
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     // setting from conf file
00169     // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
00170     // setting leg_pos from conf file
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     // Generate FIK
00176     fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
00177 
00178     // setting from conf file
00179     // rleg,TARGET_LINK,BASE_LINK
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(); // rotation in VRML is represented by axis + angle
00199         // FIK param
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         // Fix for toe joint
00213         //   Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
00214         //   Without toe joints, "end-effector link == force-sensor link" is assumed.
00215         //   With toe joints, "end-effector link != force-sensor link" is assumed.
00216         if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
00217             std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
00218             optw.back() = 0.0; // Set weight = 0 for toe joint by default
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     // setting stride limitations from conf file
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/*[m]*/, stride_outside_y_limit/*[m]*/, stride_outside_th_limit/*[deg]*/,
00295                                           stride_bwd_x_limit/*[m]*/, stride_inside_y_limit/*[m]*/, stride_inside_th_limit/*[m]*/));
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     // load virtual force sensors
00303     readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
00304     // ref force port
00305     unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
00306     unsigned int nvforce = m_vfs.size();
00307     unsigned int nforce  = npforce + nvforce;
00308     // check number of force sensors
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     // set ref force port
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     // set force port
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     // set limb cop offset port
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     //use_force = MODE_NO_FORCE;
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   RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
00400   {
00401   return RTC::RTC_OK;
00402   }
00403 */
00404 
00405 /*
00406   RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
00407   {
00408   return RTC::RTC_OK;
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); // sync in one controller loop
00427   }
00428   return RTC::RTC_OK;
00429 }
00430 
00431 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00432 //#define DEBUGP2 ((loop%200==0))
00433 #define DEBUGP2 (false)
00434 RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
00435 {
00436   // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
00437     loop ++;
00438 
00439     // Read Inport
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         // if (!is_stop_mode) {
00470         //     std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
00471         //     is_stop_mode = true;
00472         //     gg->emergency_stop();
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     // Calculation
00495     Guard guard(m_mutex);
00496     hrp::Vector3 ref_basePos;
00497     hrp::Matrix33 ref_baseRot;
00498     hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
00499     if ( is_legged_robot ) {
00500       // For parameters
00501       fik->storeCurrentParameters();
00502       getTargetParameters();
00503       // Get transition ratio
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 //        /////// Inverse Dynamics /////////
00513 //        if(!idsb.is_initialized){
00514 //          idsb.setInitState(m_robot, m_dt);
00515 //          invdyn_zmp_filters.resize(3);
00516 //          for(int i=0;i<3;i++){
00517 //            invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
00518 //            invdyn_zmp_filters[i].reset(ref_zmp(i));
00519 //          }
00520 //        }
00521 //        calcAccelerationsForInverseDynamics(m_robot, idsb);
00522 //        if(gg_is_walking){
00523 //          calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
00524 //          for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
00525 //        }
00526 //        updateInvDynStateBuffer(idsb);
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       // Transition
00534       if (!is_transition_interpolator_empty) {
00535         // transition_interpolator_ratio 0=>1 : IDLE => ABC
00536         // transition_interpolator_ratio 1=>0 : ABC => IDLE
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             // transition (TODO:set stopABCmode value instead of 0)
00550             m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0;
00551             m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0;
00552             m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0;
00553         }
00554       } else {
00555         ref_basePos = m_robot->rootLink()->p;
00556         ref_baseRot = m_robot->rootLink()->R;
00557       }
00558       // mode change for sync
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     // Write Outport
00569     if ( m_qRef.data.length() != 0 ) { // initialized
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       // basePos
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       // baseRpy
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       // baseTform
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       // basePose
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       // zmp
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       // cog
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       // sbpCogOffset
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       // write
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       // reference acceleration
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           // convert to imu sensor local acceleration
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       // control parameters
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   // joint angles
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   // basepos, rot, zmp
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     // Calculate tmp_fix_coords and something
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     // TODO : see explanation in this function
00694     fixLegToCoords2(tmp_fix_coords);
00695     fix_leg_coords2 = tmp_fix_coords;
00696 
00697     // Get output parameters and target EE coords
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     //   Just for ik initial value
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     // Calculate other parameters
00718     updateTargetCoordsForHandFixMode (tmp_fix_coords);
00719     rotateRefForcesForFixCoords (tmp_fix_coords);
00720     // TODO : see explanation in this function
00721     calculateOutputRefForces ();
00722     // TODO : see explanation in this function
00723     updateWalkingVelocityFromHandError(tmp_fix_coords);
00724     calcReferenceJointAnglesForIK();
00725 
00726     // Calculate ZMP, COG, and sbp targets
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 { // MODE_IDLE
00743       // Update fix_leg_coords based on input basePos and rot if MODE_IDLE
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       // Set force
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   // Just for stop walking
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         // Check whether "it->first" ee_name is included in leg_names. leg_names is equivalent to "swing" + "support" in gg.
00768         if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) {
00769             // Set EE coords
00770             gg->get_swing_support_ee_coords_from_ee_name(it->second.target_p0, it->second.target_r0, it->first);
00771             // Set contactStates
00772             m_contactStates.data[idx] = gg->get_current_support_state_from_ee_name(it->first);
00773             // Set controlSwingSupportTime
00774             m_controlSwingSupportTime.data[idx] = gg->get_current_swing_time_from_ee_name(it->first);
00775             // Set limbCOPOffset
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             // Set toe heel ratio which can be used force moment distribution
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 { // Not included in leg_names
00786             // Set EE coords
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             // contactStates is OFF other than leg_names
00790             m_contactStates.data[idx] = false;
00791             // controlSwingSupportTime is not used while double support period, 1.0 is neglected
00792             m_controlSwingSupportTime.data[idx] = 1.0;
00793             // Set limbCOPOffset
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             // Set toe heel ratio which can be used force moment distribution
00798             m_toeheelRatio.data[idx] = rats::no_using_toe_heel_ratio;
00799         }
00800     }
00801 };
00802 
00803 void AutoBalancer::getOutputParametersForABC ()
00804 {
00805     // double support by default
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         // Set EE coords.
00809         //   If not included in leg_names, calculate EE coords because of not being used in GaitGenerator.
00810         //   Otherwise, keep previous EE coords derived from GaitGenerator or initial value.
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         // Set contactStates
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         // controlSwingSupportTime is not used while double support period, 1.0 is neglected
00823         m_controlSwingSupportTime.data[idx] = 1.0;
00824         // Set limbCOPOffset
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         // Set toe heel ratio is not used while double support
00829         m_toeheelRatio.data[idx] = rats::no_using_toe_heel_ratio;
00830     }
00831 };
00832 
00833 void AutoBalancer::getOutputParametersForIDLE ()
00834 {
00835     // Set contactStates and controlSwingSupportTime
00836     if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
00837         // current optionalData is contactstates x limb and controlSwingSupportTime x limb
00838         //   If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
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         // If two feet have no contact, force set double support contact
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         // Set limbCOPOffset
00852         m_limbCOPOffset[idx].data.x = 0;
00853         m_limbCOPOffset[idx].data.y = 0;
00854         m_limbCOPOffset[idx].data.z = 0;
00855         // Set toe heel ratio is not used while double support
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     //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
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     //fix_leg_coords = tmp_fix_coords;
00899 };
00900 
00901 void AutoBalancer::rotateRefForcesForFixCoords (coordinates& tmp_fix_coords)
00902 {
00903     /* update ref_forces ;; StateHolder's absolute -> AutoBalancer's absolute */
00904     for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
00905       // hrp::Matrix33 eeR;
00906       // hrp::Link* parentlink;
00907       // hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
00908       // if (sensor) parentlink = sensor->link;
00909       // else parentlink = m_vfs[sensor_names[i]].link;
00910       // for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00911       //     if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
00912       // }
00913       // End effector frame
00914       //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
00915       // world frame
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     // Move hand for hand fix mode
00925     //   If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
00926     //   If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
00927     //   If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.    
00928     if (gg_is_walking) {
00929         // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
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         //if (false) { // Disabled temporarily
00939             // Store hand_fix_initial_offset in the initialization of walking
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     // TODO : need to be updated for multicontact and other walking
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 ) { // TODO : use other use_force mode. This should be depends on Stabilizer distribution mode.
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         // for foot_mid_pos
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     // TODO : check frame and robot state for this calculation
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     }//  else {
01007     //     if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
01008     //         gg->set_offset_velocity_param(0,0,0);
01009     //     }
01010     // }
01011 };
01012 
01013 void AutoBalancer::calcReferenceJointAnglesForIK ()
01014 {
01015     fik->overwrite_ref_ja_index_vec.clear();
01016     // Fix for toe joint
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   // get current foot mid pos + rot
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   // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
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     // Tempolarily modify tmp_fix_coords
01062     // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
01063     // Snap input tmp_fix_coords to XY plan projection.
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   // Set ik target params
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 //  fik->current_tm = m_qRef.tm;
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   // Revert
01093   fik->revertRobotStateToCurrent();
01094   // TODO : SBP calculation is outside of solve ik?
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   // Solve IK
01099   fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
01100 }
01101 
01102 
01103 /*
01104   RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
01105   {
01106   return RTC::RTC_OK;
01107   }
01108 */
01109 
01110 /*
01111   RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
01112   {
01113   return RTC::RTC_OK;
01114   }
01115 */
01116 
01117 /*
01118   RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
01119   {
01120   return RTC::RTC_OK;
01121   }
01122 */
01123 
01124 /*
01125   RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
01126   {
01127   return RTC::RTC_OK;
01128   }
01129 */
01130 
01131 /*
01132   RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
01133   {
01134   return RTC::RTC_OK;
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   //Guard guard(m_mutex);
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     //  if ( !gg_is_walking && !is_stop_mode) {
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, // Dummy if gg_is_walking
01259                                                       start_ref_coords,            // Dummy if gg_is_walking
01260                                                       initial_support_legs,        // Dummy if gg_is_walking
01261                                                       (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
01262     if (!ret) {
01263         std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
01264     }
01265     if ( !gg_is_walking ) { // Initializing
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         /* at least one leg shoud be in contact */
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   // is_stop_mode = true;
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   // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
01341   // If gg_is_walking is true, do not set to 0.
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         // Initial footstep Snapping
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                 // If walking, snap initial leg to current ABC foot coords.
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(); /* use only one leg for representation */
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(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
01389                     initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
01390                 }
01391             }
01392         }
01393 
01394         // Get footsteps
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(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
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)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
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   //while (gg_is_walking) usleep(10);
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) { // Support old stride_parameter definitions
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   // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
01492   // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
01493   // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
01494   // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
01495   // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
01496   // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
01497   // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
01498   // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
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 << "]   "; // for set_toe_heel_phase_ratio
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   // print
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()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
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   // Ref force balancing
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   // FIK
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   // IK limb parameters
01783   fik->setIKParam(ee_vec, i_param.ik_limb_parameters);
01784   // Limb stretch avoidance
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   // FIK
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   // IK limb parameters
01859   fik->getIKParam(ee_vec, i_param.ik_limb_parameters);
01860   // Limb stretch avoidance
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       // Get org coords
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       // Get target coords
01928       //   Input : ee coords
01929       //   Output : link coords
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(); // rtc: 
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(); // rtc: 
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       // Fix target pos => org pos, target yaw => org yaw
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       // Calculate rleg and lleg coords
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       // Set interpolator
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         /* go_pos_param_2_footstep_nodes_list_core is const member function  */
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   /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
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; // total ref force at the point without sensors, such as torso
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   // For MODE_REF_FORCE_RFU_EXT_MOMENT, store previous root position to calculate influence from tmp_ext_moment while walking (basically, root link moves while walking).
02049   //   Calculate values via fix_leg_coords2 relative/world values.
02050   static hrp::Vector3 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
02051   //   If not is_hold_value (not hold value), update prev_additional_force_applied_pos
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   // Calculate SBP
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         //nume(j) += (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
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             // Check leg_names. leg_names is assumed to be support limb for locomotion, cannot be used for manipulation. If it->first is not included in leg_names, use it for manipulation and static balance point calculation.
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                 // Force applied point is assumed as end effector
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); // desired arm 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))); // fix for rotation
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     //alias(dp) = foot_mt * dp;
02124     hrp::Vector3 dp2 = foot_mt * dp;
02125     //alias(dr) = foot_mt * dr;
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         /* at least one leg shoud be in contact */
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 // TODO : Use same code as ZMPDistributor.h
02170 // Solve A * x = b => x = W A^T (A W A^T)-1 b
02171 // => x = W^{1/2} Pinv(A W^{1/2}) b
02172 // Copied from ZMPDistributor.h
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     //ret = W2 * Aw.colPivHouseholderQr().solve(b);
02182 };
02183 
02184 void AutoBalancer::distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zmp)
02185 {
02186     // apply inverse system
02187     // TODO : fix 0.055 (zmp delay)
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     // size_t total_fz = m_robot->totalMass() * gg->get_gravitational_acceleration();
02201     size_t total_fz = m_ref_force[0].data[2]+m_ref_force[1].data[2];
02202     //size_t total_wrench_dim = 3;
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     // Set Gmat
02206     //   Fill Fz
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     //   Fill Nx and Ny
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 ) { // Nx
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 ) { // Ny
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     // Set Wmat
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     // Ret is wrench around cop_pos
02236     //   f_cop = f_ee
02237     //   n_ee = (cop_pos - ee_pos) x f_cop + n_cop
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         //hrp::Vector3 tmp_ee_pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
02252         hrp::Vector3 tmp_ee_pos = tmpikp.target_p0;
02253         hrp::Vector3 n_ee = (cop_pos[i]-tmp_ee_pos).cross(f_ee); // n_cop = 0
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 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:17