SequencePlayer.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/CorbaNaming.h>
00011 #include <hrpModel/Link.h>
00012 #include <hrpModel/ModelLoaderUtil.h>
00013 #include "SequencePlayer.h"
00014 #include "hrpsys/util/VectorConvert.h"
00015 #include <hrpModel/JointPath.h>
00016 #include <hrpUtil/MatrixSolvers.h>
00017 #include "../ImpedanceController/JointPathEx.h"
00018 
00019 typedef coil::Guard<coil::Mutex> Guard;
00020 
00021 // Module specification
00022 // <rtc-template block="module_spec">
00023 static const char* sequenceplayer_spec[] =
00024     {
00025         "implementation_id", "SequencePlayer",
00026         "type_name",         "SequencePlayer",
00027         "description",       "sequence player component",
00028         "version",           HRPSYS_PACKAGE_VERSION,
00029         "vendor",            "AIST",
00030         "category",          "example",
00031         "activity_type",     "DataFlowComponent",
00032         "max_instance",      "10",
00033         "language",          "C++",
00034         "lang_type",         "compile",
00035         // Configuration variables
00036         "conf.default.debugLevel", "0",
00037         "conf.default.fixedLink", "",
00038 
00039         ""
00040     };
00041 // </rtc-template>
00042 
00043 SequencePlayer::SequencePlayer(RTC::Manager* manager)
00044     : RTC::DataFlowComponentBase(manager),
00045       // <rtc-template block="initializer">
00046       m_qInitIn("qInit", m_qInit),
00047       m_basePosInitIn("basePosInit", m_basePosInit),
00048       m_baseRpyInitIn("baseRpyInit", m_baseRpyInit),
00049       m_zmpRefInitIn("zmpRefInit", m_zmpRefInit),
00050       m_qRefOut("qRef", m_qRef),
00051       m_tqRefOut("tqRef", m_tqRef),
00052       m_zmpRefOut("zmpRef", m_zmpRef),
00053       m_accRefOut("accRef", m_accRef),
00054       m_basePosOut("basePos", m_basePos),
00055       m_baseRpyOut("baseRpy", m_baseRpy),
00056       m_optionalDataOut("optionalData", m_optionalData),
00057       m_SequencePlayerServicePort("SequencePlayerService"),
00058       // </rtc-template>
00059       m_robot(hrp::BodyPtr()),
00060       m_debugLevel(0),
00061       m_error_pos(0.0001),
00062       m_error_rot(0.001),
00063       m_iteration(50),
00064       dummy(0)
00065 {
00066     sem_init(&m_waitSem, 0, 0);
00067     m_service0.player(this);
00068     m_clearFlag = false;
00069     m_waitFlag = false;
00070 }
00071 
00072 SequencePlayer::~SequencePlayer()
00073 {
00074 }
00075 
00076 
00077 RTC::ReturnCode_t SequencePlayer::onInitialize()
00078 {
00079     std::cout << "SequencePlayer::onInitialize()" << std::endl;
00080     // Registration: InPort/OutPort/Service
00081     // <rtc-template block="registration">
00082     // Set InPort buffers
00083     addInPort("qInit", m_qInitIn);
00084     addInPort("basePosInit", m_basePosInitIn);
00085     addInPort("baseRpyInit", m_baseRpyInitIn);
00086     addInPort("zmpRefInit", m_zmpRefInitIn);
00087   
00088     // Set OutPort buffer
00089     addOutPort("qRef", m_qRefOut);
00090     addOutPort("tqRef", m_tqRefOut);
00091     addOutPort("zmpRef", m_zmpRefOut);
00092     addOutPort("accRef", m_accRefOut);
00093     addOutPort("basePos", m_basePosOut);
00094     addOutPort("baseRpy", m_baseRpyOut);
00095     addOutPort("optionalData", m_optionalDataOut);
00096   
00097     // Set service provider to Ports
00098     m_SequencePlayerServicePort.registerProvider("service0", "SequencePlayerService", m_service0);
00099   
00100     // Set service consumers to Ports
00101   
00102     // Set CORBA Service Ports
00103     addPort(m_SequencePlayerServicePort);
00104   
00105     // </rtc-template>
00106     // <rtc-template block="bind_config">
00107     // Bind variables and configuration variable
00108   
00109     bindParameter("debugLevel", m_debugLevel, "0");
00110     bindParameter("fixedLink", m_fixedLink, "");
00111     // </rtc-template>
00112 
00113     RTC::Properties& prop = getProperties();
00114     coil::stringTo(dt, prop["dt"].c_str());
00115 
00116     m_robot = hrp::BodyPtr(new Body());
00117 
00118     RTC::Manager& rtcManager = RTC::Manager::instance();
00119     std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00120     int comPos = nameServer.find(",");
00121     if (comPos < 0){
00122         comPos = nameServer.length();
00123     }
00124     nameServer = nameServer.substr(0, comPos);
00125     RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00126     if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
00127                                  CosNaming::NamingContext::_duplicate(naming.getRootContext())
00128                                  )){
00129         std::cerr << "failed to load model[" << prop["model"] << "]" 
00130                   << std::endl;
00131     }
00132 
00133     unsigned int dof = m_robot->numJoints();
00134 
00135 
00136     // Setting for wrench data ports (real + virtual)
00137     std::vector<std::string> fsensor_names;
00138     //   find names for real force sensors
00139     unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
00140     for (unsigned int i=0; i<npforce; i++){
00141       fsensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
00142     }
00143     //   find names for virtual force sensors
00144     coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
00145     unsigned int nvforce = virtual_force_sensor.size()/10;
00146     for (unsigned int i=0; i<nvforce; i++){
00147       fsensor_names.push_back(virtual_force_sensor[i*10+0]);
00148     }
00149     //   add ports for all force sensors
00150     unsigned int nforce  = npforce + nvforce;
00151     m_wrenches.resize(nforce);
00152     m_wrenchesOut.resize(nforce);
00153     for (unsigned int i=0; i<nforce; i++){
00154       m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Ref").c_str(), m_wrenches[i]);
00155       m_wrenches[i].data.length(6);
00156       registerOutPort(std::string(fsensor_names[i]+"Ref").c_str(), *m_wrenchesOut[i]);
00157     }
00158 
00159     if (prop.hasKey("seq_optional_data_dim")) {
00160       coil::stringTo(optional_data_dim, prop["seq_optional_data_dim"].c_str());
00161     } else {
00162       optional_data_dim = 1;
00163     }
00164 
00165     m_seq = new seqplay(dof, dt, nforce, optional_data_dim);
00166 
00167     m_qInit.data.length(dof);
00168     for (unsigned int i=0; i<dof; i++) m_qInit.data[i] = 0.0;
00169     Link *root = m_robot->rootLink();
00170     m_basePosInit.data.x = root->p[0]; m_basePosInit.data.y = root->p[1]; m_basePosInit.data.z = root->p[2];
00171     hrp::Vector3 rpy = hrp::rpyFromRot(root->R);
00172     m_baseRpyInit.data.r = rpy[0]; m_baseRpyInit.data.p = rpy[1]; m_baseRpyInit.data.y = rpy[2];
00173     m_zmpRefInit.data.x = 0; m_zmpRefInit.data.y = 0; m_zmpRefInit.data.z = 0;
00174 
00175     // allocate memory for outPorts
00176     m_qRef.data.length(dof);
00177     m_tqRef.data.length(dof);
00178     m_optionalData.data.length(optional_data_dim);
00179 
00180     return RTC::RTC_OK;
00181 }
00182 
00183 
00184 
00185 RTC::ReturnCode_t SequencePlayer::onFinalize()
00186 {
00187     if ( m_debugLevel > 0 ) {
00188         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00189     }
00190     return RTC::RTC_OK;
00191 }
00192 
00193 /*
00194   RTC::ReturnCode_t SequencePlayer::onStartup(RTC::UniqueId ec_id)
00195   {
00196   return RTC::RTC_OK;
00197   }
00198 */
00199 
00200 /*
00201   RTC::ReturnCode_t SequencePlayer::onShutdown(RTC::UniqueId ec_id)
00202   {
00203   return RTC::RTC_OK;
00204   }
00205 */
00206 
00207 RTC::ReturnCode_t SequencePlayer::onActivated(RTC::UniqueId ec_id)
00208 {
00209     std::cout << "SequencePlayer::onActivated(" << ec_id << ")" << std::endl;
00210     
00211     return RTC::RTC_OK;
00212 }
00213 
00214 /*
00215   RTC::ReturnCode_t SequencePlayer::onDeactivated(RTC::UniqueId ec_id)
00216   {
00217   return RTC::RTC_OK;
00218   }
00219 */
00220 
00221 RTC::ReturnCode_t SequencePlayer::onExecute(RTC::UniqueId ec_id)
00222 {
00223     static int loop = 0;
00224     loop++;
00225     if ( m_debugLevel > 0 && loop % 1000 == 0) {
00226         std::cerr << __PRETTY_FUNCTION__ << "(" << ec_id << ")" << std::endl;
00227     }
00228     if (m_qInitIn.isNew()) m_qInitIn.read();
00229     if (m_basePosInitIn.isNew()) m_basePosInitIn.read();
00230     if (m_baseRpyInitIn.isNew()) m_baseRpyInitIn.read();
00231     if (m_zmpRefInitIn.isNew()) m_zmpRefInitIn.read();
00232 
00233     if (m_gname != "" && m_seq->isEmpty(m_gname.c_str())){
00234         if (m_waitFlag){
00235             m_gname = "";
00236             m_waitFlag = false;
00237             sem_post(&m_waitSem);
00238         }
00239     }
00240     if (m_seq->isEmpty()){
00241         m_clearFlag = false;
00242         if (m_waitFlag){
00243             m_waitFlag = false;
00244             sem_post(&m_waitSem);
00245         }
00246     }else{
00247         Guard guard(m_mutex);
00248 
00249         double zmp[3], acc[3], pos[3], rpy[3], wrenches[6*m_wrenches.size()];
00250         m_seq->get(m_qRef.data.get_buffer(), zmp, acc, pos, rpy, m_tqRef.data.get_buffer(), wrenches, m_optionalData.data.get_buffer());
00251         m_zmpRef.data.x = zmp[0];
00252         m_zmpRef.data.y = zmp[1];
00253         m_zmpRef.data.z = zmp[2];
00254         m_accRef.data.ax = acc[0]; 
00255         m_accRef.data.ay = acc[1]; 
00256         m_accRef.data.az = acc[2];
00257 
00258         if (m_fixedLink != ""){
00259             for (int i=0; i<m_robot->numJoints(); i++){
00260                 m_robot->joint(i)->q = m_qRef.data[i];
00261             }
00262             for (int i=0; i<3; i++){
00263                 m_robot->rootLink()->p[i] = pos[i];
00264             }
00265             m_robot->rootLink()->R = hrp::rotFromRpy(rpy[0], rpy[1], rpy[2]);
00266             m_robot->calcForwardKinematics();
00267             hrp::Link *root = m_robot->rootLink();
00268             hrp::Vector3 rootP;
00269             hrp::Matrix33 rootR;
00270             if (m_timeToStartPlaying > 0){
00271                 m_timeToStartPlaying -= dt;
00272                 hrp::Link *fixed = m_robot->link(m_fixedLink);
00273                 hrp::Matrix33 fixed2rootR = fixed->R.transpose()*root->R;
00274                 hrp::Vector3 fixed2rootP = fixed->R.transpose()*(root->p - fixed->p);
00275                 rootR = m_fixedR*fixed2rootR;
00276                 rootP = m_fixedR*fixed2rootP + m_fixedP;
00277             }else{
00278                 rootR = m_offsetR*m_robot->rootLink()->R;
00279                 rootP = m_offsetR*m_robot->rootLink()->p + m_offsetP;
00280             }
00281             hrp::Vector3 rootRpy = hrp::rpyFromRot(rootR);
00282             pos[0] = rootP[0];
00283             pos[1] = rootP[1];
00284             pos[2] = rootP[2];
00285             rpy[0] = rootRpy[0];
00286             rpy[1] = rootRpy[1];
00287             rpy[2] = rootRpy[2];
00288         }
00289         m_basePos.data.x = pos[0];
00290         m_basePos.data.y = pos[1];
00291         m_basePos.data.z = pos[2];
00292         m_baseRpy.data.r = rpy[0];
00293         m_baseRpy.data.p = rpy[1];
00294         m_baseRpy.data.y = rpy[2];
00295         size_t force_i = 0;
00296         for (size_t i = 0; i < m_wrenches.size(); i++) {
00297           m_wrenches[i].data[0] = wrenches[force_i++];
00298           m_wrenches[i].data[1] = wrenches[force_i++];
00299           m_wrenches[i].data[2] = wrenches[force_i++];
00300           m_wrenches[i].data[3] = wrenches[force_i++];
00301           m_wrenches[i].data[4] = wrenches[force_i++];
00302           m_wrenches[i].data[5] = wrenches[force_i++];
00303         }
00304         m_qRef.tm = m_qInit.tm;
00305         m_qRefOut.write();
00306         m_tqRefOut.write();
00307         m_zmpRefOut.write();
00308         m_accRefOut.write();
00309         m_basePosOut.write();
00310         m_baseRpyOut.write();
00311         m_optionalDataOut.write();
00312         for (size_t i = 0; i < m_wrenchesOut.size(); i++) {
00313           m_wrenchesOut[i]->write();
00314         }
00315 
00316         if (m_clearFlag){
00317             m_seq->clear(0.001);
00318         }
00319     }
00320     return RTC::RTC_OK;
00321 }
00322 
00323 /*
00324   RTC::ReturnCode_t SequencePlayer::onAborting(RTC::UniqueId ec_id)
00325   {
00326   return RTC::RTC_OK;
00327   }
00328 */
00329 
00330 /*
00331   RTC::ReturnCode_t SequencePlayer::onError(RTC::UniqueId ec_id)
00332   {
00333   return RTC::RTC_OK;
00334   }
00335 */
00336 
00337 /*
00338   RTC::ReturnCode_t SequencePlayer::onReset(RTC::UniqueId ec_id)
00339   {
00340   return RTC::RTC_OK;
00341   }
00342 */
00343 
00344 /*
00345   RTC::ReturnCode_t SequencePlayer::onStateUpdate(RTC::UniqueId ec_id)
00346   {
00347   return RTC::RTC_OK;
00348   }
00349 */
00350 
00351 /*
00352   RTC::ReturnCode_t SequencePlayer::onRateChanged(RTC::UniqueId ec_id)
00353   {
00354   return RTC::RTC_OK;
00355   }
00356 */
00357 
00358 void SequencePlayer::setClearFlag()
00359 {
00360     if ( m_debugLevel > 0 ) {
00361         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00362     }
00363     m_clearFlag = true;
00364 }
00365 
00366 void SequencePlayer::waitInterpolation()
00367 {
00368     if ( m_debugLevel > 0 ) {
00369         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00370     }
00371     m_waitFlag = true;
00372     sem_wait(&m_waitSem);
00373 }
00374 
00375 bool SequencePlayer::waitInterpolationOfGroup(const char *gname)
00376 {
00377     if ( m_debugLevel > 0 ) {
00378         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00379     }
00380     m_gname = gname;
00381     m_waitFlag = true;
00382     sem_wait(&m_waitSem);
00383     return true;
00384 }
00385 
00386 
00387 bool SequencePlayer::setJointAngle(short id, double angle, double tm)
00388 {
00389     if ( m_debugLevel > 0 ) {
00390         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00391     }
00392     Guard guard(m_mutex);
00393     if (!setInitialState()) return false;
00394     dvector q(m_robot->numJoints());
00395     m_seq->getJointAngles(q.data());
00396     q[id] = angle;
00397     for (unsigned int i=0; i<m_robot->numJoints(); i++){
00398         hrp::Link *j = m_robot->joint(i);
00399         if (j) j->q = q[i];
00400     }
00401     m_robot->calcForwardKinematics();
00402     hrp::Vector3 absZmp = m_robot->calcCM();
00403     absZmp[2] = 0;
00404     hrp::Link *root = m_robot->rootLink();
00405     hrp::Vector3 relZmp = root->R.transpose()*(absZmp - root->p);
00406     m_seq->setJointAngles(q.data(), tm);
00407     m_seq->setZmp(relZmp.data(), tm);
00408     return true;
00409 }
00410 
00411 bool SequencePlayer::setJointAngles(const double *angles, double tm)
00412 {
00413     if ( m_debugLevel > 0 ) {
00414         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00415     }
00416     Guard guard(m_mutex);
00417     if (!setInitialState()) return false;
00418     for (unsigned int i=0; i<m_robot->numJoints(); i++){
00419         hrp::Link *j = m_robot->joint(i);
00420         if (j) j->q = angles[i];
00421     }
00422     m_robot->calcForwardKinematics();
00423     hrp::Vector3 absZmp = m_robot->calcCM();
00424     absZmp[2] = 0;
00425     hrp::Link *root = m_robot->rootLink();
00426     hrp::Vector3 relZmp = root->R.transpose()*(absZmp - root->p);
00427     std::vector<const double*> v_poss;
00428     std::vector<double> v_tms;
00429     v_poss.push_back(angles);
00430     v_tms.push_back(tm);
00431     m_seq->setJointAnglesSequence(v_poss, v_tms);
00432     m_seq->setZmp(relZmp.data(), tm);
00433     return true;
00434 }
00435 
00436 bool SequencePlayer::setJointAngles(const double *angles, const bool *mask, 
00437                                     double tm)
00438 {
00439     if ( m_debugLevel > 0 ) {
00440         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00441     }
00442     Guard guard(m_mutex);
00443 
00444     if (!setInitialState()) return false;
00445 
00446     double pose[m_robot->numJoints()];
00447     for (unsigned int i=0; i<m_robot->numJoints(); i++){
00448         pose[i] = mask[i] ? angles[i] : m_qInit.data[i];
00449     }
00450     m_seq->setJointAngles(pose, tm);
00451     return true;
00452 }
00453 
00454 bool SequencePlayer::setJointAnglesSequence(const OpenHRP::dSequenceSequence angless, const OpenHRP::bSequence& mask, const OpenHRP::dSequence& times)
00455 {
00456     if ( m_debugLevel > 0 ) {
00457         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00458     }
00459     Guard guard(m_mutex);
00460 
00461     if (!setInitialState()) return false;
00462 
00463     bool tmp_mask[robot()->numJoints()];
00464     if (mask.length() != robot()->numJoints()) {
00465         for (unsigned int i=0; i < robot()->numJoints(); i++) tmp_mask[i] = true;
00466     }else{
00467         for (unsigned int i=0; i < robot()->numJoints(); i++) tmp_mask[i] = mask.get_buffer()[i];
00468     }
00469     int len = angless.length();
00470     std::vector<const double*> v_poss;
00471     std::vector<double> v_tms;
00472     for ( unsigned int i = 0; i < angless.length(); i++ ) v_poss.push_back(angless[i].get_buffer());
00473     for ( unsigned int i = 0; i <  times.length();  i++ )  v_tms.push_back(times[i]);
00474     return m_seq->setJointAnglesSequence(v_poss, v_tms);
00475 }
00476 
00477 bool SequencePlayer::clearJointAngles()
00478 {
00479     if ( m_debugLevel > 0 ) {
00480         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00481     }
00482     Guard guard(m_mutex);
00483 
00484     if (!setInitialState()) return false;
00485 
00486     return m_seq->clearJointAngles();
00487 }
00488 
00489 bool SequencePlayer::setJointAnglesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence angless, const OpenHRP::dSequence& times)
00490 {
00491     if ( m_debugLevel > 0 ) {
00492         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00493     }
00494     Guard guard(m_mutex);
00495     if (!setInitialState()) return false;
00496 
00497     if (!m_seq->resetJointGroup(gname, m_qInit.data.get_buffer())) return false;
00498 
00499     std::vector<const double*> v_poss;
00500     std::vector<double> v_tms;
00501     for ( unsigned int i = 0; i < angless.length(); i++ ) v_poss.push_back(angless[i].get_buffer());
00502     for ( unsigned int i = 0; i <  times.length();  i++ )  v_tms.push_back(times[i]);
00503     return m_seq->setJointAnglesSequenceOfGroup(gname, v_poss, v_tms, angless.length()>0?angless[0].length():0);
00504 }
00505 
00506 bool SequencePlayer::clearJointAnglesOfGroup(const char *gname)
00507 {
00508     if ( m_debugLevel > 0 ) {
00509         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00510     }
00511     Guard guard(m_mutex);
00512     if (!setInitialState()) return false;
00513 
00514     if (!m_seq->resetJointGroup(gname, m_qInit.data.get_buffer())) return false;
00515 
00516     return m_seq->clearJointAnglesOfGroup(gname);
00517 }
00518 
00519 bool SequencePlayer::setJointAnglesSequenceFull(const OpenHRP::dSequenceSequence i_jvss, const OpenHRP::dSequenceSequence i_vels, const OpenHRP::dSequenceSequence i_torques, const OpenHRP::dSequenceSequence i_poss, const OpenHRP::dSequenceSequence i_rpys, const OpenHRP::dSequenceSequence i_accs, const OpenHRP::dSequenceSequence i_zmps, const OpenHRP::dSequenceSequence i_wrenches, const OpenHRP::dSequenceSequence i_optionals, const dSequence i_tms)
00520 {
00521     if ( m_debugLevel > 0 ) {
00522         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00523     }
00524     Guard guard(m_mutex);
00525 
00526     if (!setInitialState()) return false;
00527 
00528     int len = i_jvss.length();
00529     std::vector<const double*> v_jvss, v_vels, v_torques, v_poss, v_rpys, v_accs, v_zmps, v_wrenches, v_optionals;
00530     std::vector<double> v_tms;
00531     for ( unsigned int i = 0; i < i_jvss.length(); i++ ) v_jvss.push_back(i_jvss[i].get_buffer());
00532     for ( unsigned int i = 0; i < i_vels.length(); i++ ) v_vels.push_back(i_vels[i].get_buffer());
00533     for ( unsigned int i = 0; i < i_torques.length(); i++ ) v_torques.push_back(i_torques[i].get_buffer());
00534     for ( unsigned int i = 0; i < i_poss.length(); i++ ) v_poss.push_back(i_poss[i].get_buffer());
00535     for ( unsigned int i = 0; i < i_rpys.length(); i++ ) v_rpys.push_back(i_rpys[i].get_buffer());
00536     for ( unsigned int i = 0; i < i_accs.length(); i++ ) v_accs.push_back(i_accs[i].get_buffer());
00537     for ( unsigned int i = 0; i < i_zmps.length(); i++ ) v_zmps.push_back(i_zmps[i].get_buffer());
00538     for ( unsigned int i = 0; i < i_wrenches.length(); i++ ) v_wrenches.push_back(i_wrenches[i].get_buffer());
00539     for ( unsigned int i = 0; i < i_optionals.length(); i++ ) v_optionals.push_back(i_optionals[i].get_buffer());
00540     for ( unsigned int i = 0; i < i_tms.length();  i++ )  v_tms.push_back(i_tms[i]);
00541     return m_seq->setJointAnglesSequenceFull(v_jvss, v_vels, v_torques, v_poss, v_rpys, v_accs, v_zmps, v_wrenches, v_optionals, v_tms);
00542 }
00543 
00544 bool SequencePlayer::setBasePos(const double *pos, double tm)
00545 {
00546     if ( m_debugLevel > 0 ) {
00547         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00548     }
00549     Guard guard(m_mutex);
00550     m_seq->setBasePos(pos, tm);
00551     return true;
00552 }
00553 
00554 bool SequencePlayer::setBaseRpy(const double *rpy, double tm)
00555 {
00556     if ( m_debugLevel > 0 ) {
00557         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00558     }
00559     Guard guard(m_mutex);
00560     m_seq->setBaseRpy(rpy, tm);
00561     return true;
00562 }
00563 
00564 bool SequencePlayer::setZmp(const double *zmp, double tm)
00565 {
00566     if ( m_debugLevel > 0 ) {
00567         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00568     }
00569     Guard guard(m_mutex);
00570     m_seq->setZmp(zmp, tm);
00571     return true;
00572 }
00573 
00574 bool SequencePlayer::setWrenches(const double *wrenches, double tm)
00575 {
00576     Guard guard(m_mutex);
00577     m_seq->setWrenches(wrenches, tm);
00578     return true;
00579 }
00580 
00581 bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const double *rpy, double tm, const char* frame_name)
00582 {
00583     if ( m_debugLevel > 0 ) {
00584         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00585     }
00586     Guard guard(m_mutex);
00587     if (!setInitialState()) return false;
00588     // setup
00589     std::vector<int> indices;
00590     hrp::dvector start_av, end_av;
00591     std::vector<hrp::dvector> avs;
00592     if (! m_seq->getJointGroup(gname, indices) ) {
00593         std::cerr << "[setTargetPose] Could not find joint group " << gname << std::endl;
00594         return false;
00595     }
00596     start_av.resize(indices.size());
00597     end_av.resize(indices.size());
00598 
00599     //std::cerr << std::endl;
00600     if ( ! m_robot->joint(indices[0])->parent ) {
00601         std::cerr << "[setTargetPose] " << m_robot->joint(indices[0])->name << " does not have parent" << std::endl;
00602         return false;
00603     }
00604     string base_parent_name = m_robot->joint(indices[0])->parent->name;
00605     string target_name = m_robot->joint(indices[indices.size()-1])->name;
00606     // prepare joint path
00607     hrp::JointPathExPtr manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(base_parent_name), m_robot->link(target_name), dt, true, std::string(m_profile.instance_name)));
00608 
00609     // calc fk
00610     for (unsigned int i=0; i<m_robot->numJoints(); i++){
00611         hrp::Link *j = m_robot->joint(i);
00612         if (j) j->q = m_qRef.data.get_buffer()[i];
00613     }
00614     m_robot->calcForwardKinematics();
00615     for ( unsigned int i = 0; i < manip->numJoints(); i++ ){
00616         start_av[i] = manip->joint(i)->q;
00617     }
00618 
00619     // xyz and rpy are relateive to root link, where as pos and rotatoin of manip->calcInverseKinematics are relative to base link
00620 
00621     // ik params
00622     hrp::Vector3 start_p(m_robot->link(target_name)->p);
00623     hrp::Matrix33 start_R(m_robot->link(target_name)->R);
00624     hrp::Vector3 end_p(xyz[0], xyz[1], xyz[2]);
00625     hrp::Matrix33 end_R = m_robot->link(target_name)->calcRfromAttitude(hrp::rotFromRpy(rpy[0], rpy[1], rpy[2]));
00626 
00627     // change start and end must be relative to the frame_name
00628     if ( (frame_name != NULL) && (! m_robot->link(frame_name) ) ) {
00629         std::cerr << "[setTargetPose] Could not find frame_name " << frame_name << std::endl;
00630         return false;
00631     } else if ( frame_name != NULL ) {
00632         hrp::Vector3 frame_p(m_robot->link(frame_name)->p);
00633         hrp::Matrix33 frame_R(m_robot->link(frame_name)->attitude());
00634         // fix start/end references from root to frame;
00635         end_p = frame_R * end_p + frame_p;
00636         end_R = frame_R * end_R;
00637     }
00638     manip->setMaxIKError(m_error_pos,m_error_rot);
00639     manip->setMaxIKIteration(m_iteration);
00640     std::cerr << "[setTargetPose] Solveing IK with frame" << frame_name << ", Error " << m_error_pos << m_error_rot << ", Iteration " << m_iteration << std::endl;
00641     std::cerr << "                Start " << start_p << start_R<< std::endl;
00642     std::cerr << "                End   " << end_p << end_R<< std::endl;
00643 
00644     // interpolate & calc ik
00645     int len = max(((start_p - end_p).norm() / 0.02 ), // 2cm
00646                   ((hrp::omegaFromRot(start_R.transpose() * end_R).norm()) / 0.025)); // 2 deg
00647     len = max(len, 1);
00648 
00649     std::vector<const double *> v_pos;
00650     std::vector<double> v_tm;
00651     v_pos.resize(len);
00652     v_tm.resize(len);
00653 
00654     // do loop
00655     for (int i = 0; i < len; i++ ) {
00656         double a = (1+i)/(double)len;
00657         hrp::Vector3 p = (1-a)*start_p + a*end_p;
00658         hrp::Vector3 omega = hrp::omegaFromRot(start_R.transpose() * end_R);
00659         hrp::Matrix33 R = start_R * rodrigues(omega.isZero()?omega:omega.normalized(), a*omega.norm());
00660         bool ret = manip->calcInverseKinematics2(p, R);
00661 
00662         if ( m_debugLevel > 0 ) {
00663             // for debug
00664             std::cerr << "target pos/rot : " << i << "/" << a << " : "
00665                       << p[0] << " " << p[1] << " " << p[2] << ","
00666                       << omega[0] << " " << omega[1] << " " << omega[2] << std::endl;
00667         }
00668         if ( ! ret ) {
00669             std::cerr << "[setTargetPose] IK failed" << std::endl;
00670             return false;
00671         }
00672         v_pos[i] = (const double *)malloc(sizeof(double)*manip->numJoints());
00673         for ( unsigned int j = 0; j < manip->numJoints(); j++ ){
00674             ((double *)v_pos[i])[j] = manip->joint(j)->q;
00675         }
00676         v_tm[i] = tm/len;
00677     }
00678 
00679     if ( m_debugLevel > 0 ) {
00680         // for debug
00681         for(int i = 0; i < len; i++ ) {
00682             std::cerr << v_tm[i] << ":";
00683             for(int j = 0; j < start_av.size(); j++ ) {
00684                 std::cerr << v_pos[i][j] << " ";
00685             }
00686             std::cerr << std::endl;
00687         }
00688     }
00689 
00690     if (!m_seq->resetJointGroup(gname, m_qInit.data.get_buffer())) return false; // reset sequencer
00691     bool ret = m_seq->playPatternOfGroup(gname, v_pos, v_tm, m_qInit.data.get_buffer(), v_pos.size()>0?indices.size():0);
00692 
00693     // clean up memory, need to improve
00694     for (int i = 0; i < len; i++ ) {
00695         free((double *)v_pos[i]);
00696     }
00697 
00698     return ret;
00699 }
00700 
00701 void SequencePlayer::loadPattern(const char *basename, double tm)
00702 {
00703     if ( m_debugLevel > 0 ) {
00704         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00705     }
00706     Guard guard(m_mutex);
00707     if (setInitialState()){
00708         if (m_fixedLink != ""){
00709             hrp::Link *l = m_robot->link(m_fixedLink);
00710             if (!l) {
00711                 std::cerr << __PRETTY_FUNCTION__ << "can't find a fixed link("
00712                           << m_fixedLink << ")" << std::endl;
00713                 m_fixedLink = ""; 
00714                 return;
00715             }
00716             m_robot->calcForwardKinematics(); // this is not called by setinitialstate()
00717             m_fixedP = l->p;
00718             m_fixedR = l->R;
00719 
00720             std::string pos = std::string(basename)+".pos";
00721             std::string wst = std::string(basename)+".waist";
00722             std::ifstream ifspos(pos.c_str());
00723             std::ifstream ifswst(wst.c_str());
00724             if (!ifspos.is_open() || !ifswst.is_open()){
00725                 std::cerr << __PRETTY_FUNCTION__ << "can't open " << pos << " or "
00726                           << wst << ")" << std::endl;
00727                 m_fixedLink = ""; 
00728                 return;
00729             }
00730             double time;
00731             ifspos >> time;
00732             for (int i=0; i<m_robot->numJoints(); i++){
00733                 ifspos >> m_robot->joint(i)->q; 
00734             }
00735             ifswst >> time;
00736             for (int i=0; i<3; i++) ifswst >> m_robot->rootLink()->p[i];
00737             hrp::Vector3 rpy;
00738             for (int i=0; i<3; i++) ifswst >> rpy[i];
00739             m_robot->rootLink()->R = hrp::rotFromRpy(rpy);
00740             m_robot->calcForwardKinematics();
00741 
00742             m_offsetR = m_fixedR*l->R.transpose();
00743             m_offsetP = m_fixedP - m_offsetR*l->p;
00744             m_timeToStartPlaying = tm;
00745         }
00746         m_seq->loadPattern(basename, tm);
00747     }
00748 }
00749 
00750 bool SequencePlayer::setInitialState(double tm)
00751 {
00752     if ( m_debugLevel > 0 ) {
00753         std::cerr << __PRETTY_FUNCTION__ << "m_seq-isEmpty() " << m_seq->isEmpty() << ", m_Init.data.length() " << m_qInit.data.length() << std::endl;
00754     }
00755     if (!m_seq->isEmpty()) return true;
00756 
00757     if (m_qInit.data.length() == 0){
00758         std::cerr << "can't determine initial posture" << std::endl;
00759         return false;
00760     }else{
00761         m_seq->setJointAngles(m_qInit.data.get_buffer(), tm);
00762         for (unsigned int i=0; i<m_robot->numJoints(); i++){
00763             Link *l = m_robot->joint(i);
00764             l->q = m_qInit.data[i];
00765             m_qRef.data[i] = m_qInit.data[i]; // update m_qRef for setTargetPose()
00766         }
00767 
00768         Link *root = m_robot->rootLink();
00769 
00770         root->p << m_basePosInit.data.x,
00771             m_basePosInit.data.y,
00772             m_basePosInit.data.z;
00773         m_seq->setBasePos(root->p.data(), tm);
00774 
00775         double rpy[] = {m_baseRpyInit.data.r,
00776                         m_baseRpyInit.data.p,
00777                         m_baseRpyInit.data.y};
00778         m_seq->setBaseRpy(rpy, tm);
00779         calcRotFromRpy(root->R, rpy[0], rpy[1], rpy[2]);
00780 
00781         double zmp[] = {m_zmpRefInit.data.x, m_zmpRefInit.data.y, m_zmpRefInit.data.z};
00782         m_seq->setZmp(zmp, tm);
00783         double zero[] = {0,0,0};
00784         m_seq->setBaseAcc(zero, tm);
00785         return true;
00786     }
00787 }
00788 
00789 void SequencePlayer::playPattern(const dSequenceSequence& pos, const dSequenceSequence& rpy, const dSequenceSequence& zmp, const dSequence& tm)
00790 {
00791     if ( m_debugLevel > 0 ) {
00792         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00793     }
00794     Guard guard(m_mutex);
00795     if (!setInitialState()) return;
00796 
00797     std::vector<const double *> v_pos, v_rpy, v_zmp;
00798     std::vector<double> v_tm;
00799     for ( unsigned int i = 0; i < pos.length(); i++ ) v_pos.push_back(pos[i].get_buffer());
00800     for ( unsigned int i = 0; i < rpy.length(); i++ ) v_rpy.push_back(rpy[i].get_buffer());
00801     for ( unsigned int i = 0; i < zmp.length(); i++ ) v_zmp.push_back(zmp[i].get_buffer());
00802     for ( unsigned int i = 0; i < tm.length() ; i++ ) v_tm.push_back(tm[i]);
00803     return m_seq->playPattern(v_pos, v_rpy, v_zmp, v_tm, m_qInit.data.get_buffer(), pos.length()>0?pos[0].length():0);
00804 }
00805 
00806 bool SequencePlayer::setInterpolationMode(OpenHRP::SequencePlayerService::interpolationMode i_mode_)
00807 {
00808     if ( m_debugLevel > 0 ) {
00809         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00810     }
00811     Guard guard(m_mutex);
00812     interpolator::interpolation_mode new_mode;
00813     if (i_mode_ == OpenHRP::SequencePlayerService::LINEAR){
00814         new_mode = interpolator::LINEAR;
00815     }else if (i_mode_ == OpenHRP::SequencePlayerService::HOFFARBIB){
00816         new_mode = interpolator::HOFFARBIB;
00817     }else{
00818         return false;
00819     }
00820     return m_seq->setInterpolationMode(new_mode);
00821 }
00822 
00823 bool SequencePlayer::addJointGroup(const char *gname, const OpenHRP::SequencePlayerService::StrSequence& jnames)
00824 {
00825     std::cerr << "[addJointGroup] group name = " << gname << std::endl;
00826     if ( m_debugLevel > 0 ) {
00827         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00828     }
00829     if (!waitInterpolationOfGroup(gname)) return false;
00830 
00831     Guard guard(m_mutex);
00832     std::vector<int> indices;
00833     for (size_t i=0; i<jnames.length(); i++){
00834         hrp::Link *l = m_robot->link(std::string(jnames[i]));
00835         if (l){
00836             indices.push_back(l->jointId);
00837         }else{
00838             std::cerr << "[addJointGroup] link name " << jnames[i] << "is not found" << std::endl;
00839             return false;
00840         }
00841     }
00842     return m_seq->addJointGroup(gname, indices);
00843 }
00844 
00845 bool SequencePlayer::removeJointGroup(const char *gname)
00846 {
00847     std::cerr << "[removeJointGroup] group name = " << gname << std::endl;
00848     if (!waitInterpolationOfGroup(gname)) return false;
00849     bool ret;
00850     {
00851         Guard guard(m_mutex);
00852         ret = m_seq->removeJointGroup(gname);
00853     }
00854     return ret;
00855 }
00856 
00857 bool SequencePlayer::setJointAnglesOfGroup(const char *gname, const dSequence& jvs, double tm)
00858 {
00859     if ( m_debugLevel > 0 ) {
00860         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00861     }
00862     Guard guard(m_mutex);
00863     if (!setInitialState()) return false;
00864 
00865     if (!m_seq->resetJointGroup(gname, m_qInit.data.get_buffer())) return false;
00866     return m_seq->setJointAnglesOfGroup(gname, jvs.get_buffer(), jvs.length(), tm);
00867 }
00868 
00869 bool SequencePlayer::playPatternOfGroup(const char *gname, const dSequenceSequence& pos, const dSequence& tm)
00870 {
00871     if ( m_debugLevel > 0 ) {
00872         std::cerr << __PRETTY_FUNCTION__ << std::endl;
00873     }
00874     Guard guard(m_mutex);
00875     if (!setInitialState()) return false;
00876 
00877     std::vector<const double *> v_pos;
00878     std::vector<double> v_tm;
00879     for ( unsigned int i = 0; i < pos.length(); i++ ) v_pos.push_back(pos[i].get_buffer());
00880     for ( unsigned int i = 0; i < tm.length() ; i++ ) v_tm.push_back(tm[i]);
00881     return m_seq->playPatternOfGroup(gname, v_pos, v_tm, m_qInit.data.get_buffer(), pos.length()>0?pos[0].length():0);
00882 }
00883 
00884 void SequencePlayer::setMaxIKError(double pos, double rot){
00885     m_error_pos = pos;
00886     m_error_rot = rot;
00887 }
00888 
00889 void SequencePlayer::setMaxIKIteration(short iter){
00890     m_iteration= iter;
00891 }
00892 
00893 
00894 extern "C"
00895 {
00896 
00897     void SequencePlayerInit(RTC::Manager* manager)
00898     {
00899         RTC::Properties profile(sequenceplayer_spec);
00900         manager->registerFactory(profile,
00901                                  RTC::Create<SequencePlayer>,
00902                                  RTC::Delete<SequencePlayer>);
00903     }
00904 
00905 };
00906 
00907 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56