StateHolder.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "StateHolder.h"
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpUtil/Eigen3d.h>
00013 #include <hrpModel/ModelLoaderUtil.h>
00014 
00015 // Module specification
00016 // <rtc-template block="module_spec">
00017 static const char* stateholder_spec[] =
00018   {
00019     "implementation_id", "StateHolder",
00020     "type_name",         "StateHolder",
00021     "description",       "state holder",
00022     "version",           HRPSYS_PACKAGE_VERSION,
00023     "vendor",            "AIST",
00024     "category",          "example",
00025     "activity_type",     "DataFlowComponent",
00026     "max_instance",      "10",
00027     "language",          "C++",
00028     "lang_type",         "compile",
00029     // Configuration variables
00030 
00031     ""
00032   };
00033 // </rtc-template>
00034 
00035 StateHolder::StateHolder(RTC::Manager* manager)
00036   : RTC::DataFlowComponentBase(manager),
00037     // <rtc-template block="initializer">
00038     m_currentQIn("currentQIn", m_currentQ),
00039     m_qIn("qIn", m_q),
00040     m_tqIn("tqIn", m_tq),
00041     m_basePosIn("basePosIn", m_basePos),
00042     m_baseRpyIn("baseRpyIn", m_baseRpy),
00043     m_zmpIn("zmpIn", m_zmp),
00044     m_optionalDataIn("optionalDataIn", m_optionalData),
00045     m_qOut("qOut", m_q),
00046     m_tqOut("tqOut", m_tq),
00047     m_basePosOut("basePosOut", m_basePos),
00048     m_baseRpyOut("baseRpyOut", m_baseRpy),
00049     m_baseTformOut("baseTformOut", m_baseTform),
00050     m_basePoseOut("basePoseOut", m_basePose),
00051     m_zmpOut("zmpOut", m_zmp),
00052     m_optionalDataOut("optionalDataOut", m_optionalData),
00053     m_StateHolderServicePort("StateHolderService"),
00054     m_TimeKeeperServicePort("TimeKeeperService"),
00055     // </rtc-template>
00056     m_timeCount(0),
00057     dummy(0)
00058 {
00059 
00060   m_service0.setComponent(this);
00061   m_service1.setComponent(this);
00062   m_requestGoActual = false;
00063 
00064   sem_init(&m_waitSem, 0, 0);
00065   sem_init(&m_timeSem, 0, 0);
00066 }
00067 
00068 StateHolder::~StateHolder()
00069 {
00070 }
00071 
00072 
00073 
00074 RTC::ReturnCode_t StateHolder::onInitialize()
00075 {
00076   std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00077   // <rtc-template block="bind_config">
00078   // Bind variables and configuration variable
00079   
00080   // </rtc-template>
00081 
00082   // Registration: InPort/OutPort/Service
00083   // <rtc-template block="registration">
00084   // Set InPort buffers
00085     addInPort("currentQIn", m_currentQIn);
00086     addInPort("qIn", m_qIn);
00087     addInPort("tqIn", m_tqIn);
00088     addInPort("basePosIn", m_basePosIn);
00089     addInPort("baseRpyIn", m_baseRpyIn);
00090     addInPort("zmpIn", m_zmpIn);
00091     addInPort("optionalDataIn", m_optionalDataIn);
00092   
00093   // Set OutPort buffer
00094     addOutPort("qOut", m_qOut);
00095     addOutPort("tqOut", m_tqOut);
00096     addOutPort("basePosOut", m_basePosOut);
00097     addOutPort("baseRpyOut", m_baseRpyOut);
00098     addOutPort("baseTformOut", m_baseTformOut);
00099     addOutPort("basePoseOut", m_basePoseOut);
00100     addOutPort("zmpOut", m_zmpOut);
00101     addOutPort("optionalDataOut", m_optionalDataOut);
00102   
00103   // Set service provider to Ports
00104   m_StateHolderServicePort.registerProvider("service0", "StateHolderService", m_service0);
00105   m_TimeKeeperServicePort.registerProvider("service1", "TimeKeeperService", m_service1);
00106   
00107   // Set service consumers to Ports
00108   
00109   // Set CORBA Service Ports
00110   addPort(m_StateHolderServicePort);
00111   addPort(m_TimeKeeperServicePort);
00112 
00113   // </rtc-template>
00114 
00115   RTC::Properties& prop = getProperties();
00116   coil::stringTo(m_dt, prop["dt"].c_str());
00117   std::cout << "StateHolder: dt = " << m_dt << std::endl;
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   OpenHRP::BodyInfo_var binfo;
00127   binfo = hrp::loadBodyInfo(prop["model"].c_str(),
00128                             CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00129   OpenHRP::LinkInfoSequence_var lis = binfo->links();
00130   const OpenHRP::LinkInfo& li = lis[0];
00131   hrp::Vector3 p, axis;
00132   p << li.translation[0], li.translation[1], li.translation[2];
00133   axis << li.rotation[0], li.rotation[1], li.rotation[2];
00134   hrp::Matrix33 R = hrp::rodrigues(axis, li.rotation[3]);
00135   hrp::Vector3 rpy = hrp::rpyFromRot(R);
00136   
00137   m_baseTform.data.length(12);
00138   double *T = m_baseTform.data.get_buffer();
00139   T[0] = R(0,0); T[1] = R(0,1); T[ 2] = R(0,2); T[ 3] = p[0];
00140   T[4] = R(0,0); T[5] = R(0,1); T[ 6] = R(0,2); T[ 7] = p[1];
00141   T[8] = R(0,0); T[9] = R(0,1); T[10] = R(0,2); T[11] = p[2];
00142   m_basePos.data.x = m_basePose.data.position.x = p[0];
00143   m_basePos.data.y = m_basePose.data.position.y = p[1];
00144   m_basePos.data.z = m_basePose.data.position.z = p[2];
00145   m_baseRpy.data.r = m_basePose.data.orientation.r = rpy[0];
00146   m_baseRpy.data.p = m_basePose.data.orientation.p = rpy[1];
00147   m_baseRpy.data.y = m_basePose.data.orientation.y = rpy[2];
00148   m_zmp.data.x = m_zmp.data.y = m_zmp.data.z = 0.0;
00149 
00150   // Setting for wrench data ports (real + virtual)
00151   std::vector<std::string> fsensor_names;
00152   //   find names for real force sensors
00153   for ( unsigned int k = 0; k < lis->length(); k++ ) {
00154     OpenHRP::SensorInfoSequence& sensors = lis[k].sensors;
00155     for ( unsigned int l = 0; l < sensors.length(); l++ ) {
00156       if ( std::string(sensors[l].type) == "Force" ) {
00157         fsensor_names.push_back(std::string(sensors[l].name));
00158       }
00159     }
00160   }
00161   int npforce = fsensor_names.size();
00162   //   find names for virtual force sensors
00163   coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
00164   unsigned int nvforce = virtual_force_sensor.size()/10;
00165   for (unsigned int i=0; i<nvforce; i++){
00166     fsensor_names.push_back(virtual_force_sensor[i*10+0]);
00167   }
00168   //   add ports for all force sensors
00169   unsigned int nforce  = npforce + nvforce;
00170   m_wrenches.resize(nforce);
00171   m_wrenchesIn.resize(nforce);
00172   m_wrenchesOut.resize(nforce);
00173   for (unsigned int i=0; i<nforce; i++){
00174     m_wrenchesIn[i] = new InPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"In").c_str(), m_wrenches[i]);
00175     m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Out").c_str(), m_wrenches[i]);
00176     m_wrenches[i].data.length(6);
00177     m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
00178     m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
00179     registerInPort(std::string(fsensor_names[i]+"In").c_str(), *m_wrenchesIn[i]);
00180     registerOutPort(std::string(fsensor_names[i]+"Out").c_str(), *m_wrenchesOut[i]);
00181   }
00182 
00183   
00184   return RTC::RTC_OK;
00185 }
00186 
00187 
00188 
00189 /*
00190 RTC::ReturnCode_t StateHolder::onFinalize()
00191 {
00192   return RTC::RTC_OK;
00193 }
00194 */
00195 
00196 /*
00197 RTC::ReturnCode_t StateHolder::onStartup(RTC::UniqueId ec_id)
00198 {
00199   return RTC::RTC_OK;
00200 }
00201 */
00202 
00203 /*
00204 RTC::ReturnCode_t StateHolder::onShutdown(RTC::UniqueId ec_id)
00205 {
00206   return RTC::RTC_OK;
00207 }
00208 */
00209 
00210 /*
00211 RTC::ReturnCode_t StateHolder::onActivated(RTC::UniqueId ec_id)
00212 {
00213   return RTC::RTC_OK;
00214 }
00215 */
00216 
00217 /*
00218 RTC::ReturnCode_t StateHolder::onDeactivated(RTC::UniqueId ec_id)
00219 {
00220   return RTC::RTC_OK;
00221 }
00222 */
00223 
00224 RTC::ReturnCode_t StateHolder::onExecute(RTC::UniqueId ec_id)
00225 {
00226     //std::cout << "StateHolder::onExecute(" << ec_id << ")" << std::endl;
00227     RTC::Time tm;
00228 
00229     if (m_currentQIn.isNew()){
00230         m_currentQIn.read();
00231         tm = m_currentQ.tm;
00232     }else{
00233         coil::TimeValue coiltm(coil::gettimeofday());
00234         tm.sec = coiltm.sec();
00235         tm.nsec = coiltm.usec()*1000;
00236     }
00237 
00238     if (m_qIn.isNew()){
00239         m_qIn.read();
00240     }
00241     if (m_tqIn.isNew()){
00242         m_tqIn.read();
00243     }
00244     if (m_requestGoActual || (m_q.data.length() == 0 && m_currentQ.data.length() > 0)){
00245         m_q = m_currentQ;
00246         if (m_q.data.length() != m_tq.data.length()) m_tq.data.length(m_q.data.length());
00247         // Reset reference wrenches to zero
00248         for (unsigned int i=0; i<m_wrenchesIn.size(); i++){
00249             m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
00250             m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
00251         }
00252     }
00253 
00254     if (m_requestGoActual){
00255         m_requestGoActual = false;
00256         sem_post(&m_waitSem);
00257     }
00258 
00259     if (m_basePosIn.isNew()){
00260         m_basePosIn.read();
00261     }
00262 
00263     if (m_baseRpyIn.isNew()){
00264         m_baseRpyIn.read();
00265     }
00266 
00267     if (m_zmpIn.isNew()){
00268         m_zmpIn.read();
00269     }
00270 
00271     if (m_optionalDataIn.isNew()){
00272         m_optionalDataIn.read();
00273     }
00274 
00275     for (size_t i = 0; i < m_wrenchesIn.size(); i++) {
00276       if ( m_wrenchesIn[i]->isNew() ) {
00277         m_wrenchesIn[i]->read();
00278       }
00279     }
00280 
00281     double *a = m_baseTform.data.get_buffer();
00282     a[0] = m_basePos.data.x;
00283     a[1] = m_basePos.data.y;
00284     a[2] = m_basePos.data.z;
00285     hrp::Matrix33 R = hrp::rotFromRpy(m_baseRpy.data.r, 
00286                                       m_baseRpy.data.p, 
00287                                       m_baseRpy.data.y); 
00288     hrp::setMatrix33ToRowMajorArray(R, a, 3);
00289 
00290     m_basePose.data.position = m_basePos.data;
00291     m_basePose.data.orientation = m_baseRpy.data;
00292 
00293     // put timestamps
00294     m_q.tm         = tm;
00295     m_tq.tm         = tm;
00296     m_baseTform.tm = tm; 
00297     m_basePos.tm   = tm; 
00298     m_baseRpy.tm   = tm; 
00299     m_zmp.tm       = tm; 
00300     m_basePose.tm  = tm;
00301     for (size_t i = 0; i < m_wrenches.size(); i++) {
00302       m_wrenches[i].tm = tm;
00303     }
00304 
00305     // write
00306     if (m_q.data.length() > 0){
00307         m_qOut.write();
00308     }
00309     if (m_tq.data.length() > 0){
00310         m_tqOut.write();
00311     }
00312     m_baseTformOut.write();
00313     m_basePosOut.write();
00314     m_baseRpyOut.write();
00315     m_zmpOut.write();
00316     m_basePoseOut.write();
00317     m_optionalDataOut.write();
00318     for (size_t i = 0; i < m_wrenchesOut.size(); i++) {
00319       m_wrenchesOut[i]->write();
00320     }
00321 
00322     if (m_timeCount > 0){
00323         m_timeCount--;
00324         if (m_timeCount == 0) sem_post(&m_timeSem);
00325     }
00326 
00327     return RTC::RTC_OK;
00328 }
00329 
00330 /*
00331 RTC::ReturnCode_t StateHolder::onAborting(RTC::UniqueId ec_id)
00332 {
00333   return RTC::RTC_OK;
00334 }
00335 */
00336 
00337 /*
00338 RTC::ReturnCode_t StateHolder::onError(RTC::UniqueId ec_id)
00339 {
00340   return RTC::RTC_OK;
00341 }
00342 */
00343 
00344 /*
00345 RTC::ReturnCode_t StateHolder::onReset(RTC::UniqueId ec_id)
00346 {
00347   return RTC::RTC_OK;
00348 }
00349 */
00350 
00351 /*
00352 RTC::ReturnCode_t StateHolder::onStateUpdate(RTC::UniqueId ec_id)
00353 {
00354   return RTC::RTC_OK;
00355 }
00356 */
00357 
00358 /*
00359 RTC::ReturnCode_t StateHolder::onRateChanged(RTC::UniqueId ec_id)
00360 {
00361   return RTC::RTC_OK;
00362 }
00363 */
00364 
00365 
00366 void StateHolder::goActual()
00367 {
00368     std::cout << "StateHolder::goActual()" << std::endl;
00369     m_requestGoActual = true;
00370     sem_wait(&m_waitSem);
00371 }
00372 
00373 void StateHolder::getCommand(StateHolderService::Command &com)
00374 {
00375     com.jointRefs.length(m_q.data.length());
00376     memcpy(com.jointRefs.get_buffer(), m_q.data.get_buffer(), sizeof(double)*m_q.data.length());
00377     com.baseTransform.length(12);
00378     com.baseTransform[0] = m_basePos.data.x;
00379     com.baseTransform[1] = m_basePos.data.y;
00380     com.baseTransform[2] = m_basePos.data.z;
00381     hrp::Matrix33 R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
00382     double *a = com.baseTransform.get_buffer();
00383     hrp::setMatrix33ToRowMajorArray(R, a, 3);
00384     com.zmp.length(3);
00385     com.zmp[0] = m_zmp.data.x; com.zmp[1] = m_zmp.data.y; com.zmp[2] = m_zmp.data.z; 
00386 }
00387 
00388 void StateHolder::wait(CORBA::Double tm)
00389 {
00390     m_timeCount = tm/m_dt;
00391     sem_wait(&m_timeSem);
00392 }
00393  
00394 extern "C"
00395 {
00396 
00397   void StateHolderInit(RTC::Manager* manager)
00398   {
00399     RTC::Properties profile(stateholder_spec);
00400     manager->registerFactory(profile,
00401                              RTC::Create<StateHolder>,
00402                              RTC::Delete<StateHolder>);
00403   }
00404 
00405 };
00406 
00407 


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