RemoveForceSensorLinkOffset.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "RemoveForceSensorLinkOffset.h"
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpModel/ModelLoaderUtil.h>
00013 #include <hrpUtil/MatrixSolvers.h>
00014 #include <hrpModel/Sensor.h>
00015 
00016 typedef coil::Guard<coil::Mutex> Guard;
00017 
00018 // Module specification
00019 // <rtc-template block="module_spec">
00020 static const char* removeforcesensorlinkoffset_spec[] =
00021   {
00022     "implementation_id", "RemoveForceSensorLinkOffset",
00023     "type_name",         "RemoveForceSensorLinkOffset",
00024     "description",       "null component",
00025     "version",           HRPSYS_PACKAGE_VERSION,
00026     "vendor",            "AIST",
00027     "category",          "example",
00028     "activity_type",     "DataFlowComponent",
00029     "max_instance",      "10",
00030     "language",          "C++",
00031     "lang_type",         "compile",
00032     // Configuration variables
00033     "conf.default.debugLevel", "0",
00034     ""
00035   };
00036 // </rtc-template>
00037 
00038 RemoveForceSensorLinkOffset::RemoveForceSensorLinkOffset(RTC::Manager* manager)
00039   : RTC::DataFlowComponentBase(manager),
00040     // <rtc-template block="initializer">
00041     m_qCurrentIn("qCurrent", m_qCurrent),
00042     m_rpyIn("rpy", m_rpy),
00043     m_RemoveForceSensorLinkOffsetServicePort("RemoveForceSensorLinkOffsetService"),
00044     // </rtc-template>
00045     m_debugLevel(0),
00046     max_sensor_offset_calib_counter(0)
00047 {
00048   m_service0.rmfsoff(this);
00049 }
00050 
00051 RemoveForceSensorLinkOffset::~RemoveForceSensorLinkOffset()
00052 {
00053 }
00054 
00055 
00056 
00057 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onInitialize()
00058 {
00059   std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00060   // <rtc-template block="bind_config">
00061   // Bind variables and configuration variable
00062   bindParameter("debugLevel", m_debugLevel, "0");
00063   
00064   // </rtc-template>
00065 
00066   // Registration: InPort/OutPort/Service
00067   // <rtc-template block="registration">
00068   // Set InPort buffers
00069   addInPort("qCurrent", m_qCurrentIn);
00070   addInPort("rpy", m_rpyIn);
00071 
00072   // Set OutPort buffer
00073   
00074   // Set service provider to Ports
00075   m_RemoveForceSensorLinkOffsetServicePort.registerProvider("service0", "RemoveForceSensorLinkOffsetService", m_service0);
00076   
00077   // Set service consumers to Ports
00078   
00079   // Set CORBA Service Ports
00080   addPort(m_RemoveForceSensorLinkOffsetServicePort);
00081   
00082   // </rtc-template>
00083 
00084   RTC::Properties& prop = getProperties();
00085   coil::stringTo(m_dt, prop["dt"].c_str());
00086 
00087   m_robot = hrp::BodyPtr(new hrp::Body());
00088 
00089   RTC::Manager& rtcManager = RTC::Manager::instance();
00090   std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00091   int comPos = nameServer.find(",");
00092   if (comPos < 0){
00093       comPos = nameServer.length();
00094   }
00095   nameServer = nameServer.substr(0, comPos);
00096   RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00097   if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
00098                                CosNaming::NamingContext::_duplicate(naming.getRootContext())
00099           )){
00100       std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
00101       return RTC::RTC_ERROR;
00102   }
00103 
00104   unsigned int nforce = m_robot->numSensors(hrp::Sensor::FORCE);
00105   m_force.resize(nforce);
00106   m_forceOut.resize(nforce);
00107   m_forceIn.resize(nforce);
00108   for (unsigned int i = 0; i < nforce; i++) {
00109     hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i);
00110     m_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string("off_"+s->name).c_str(), m_force[i]);
00111     m_forceIn[i] = new InPort<TimedDoubleSeq>(s->name.c_str(), m_force[i]);
00112     m_force[i].data.length(6);
00113     registerInPort(s->name.c_str(), *m_forceIn[i]);
00114     registerOutPort(std::string("off_"+s->name).c_str(), *m_forceOut[i]);
00115     m_forcemoment_offset_param.insert(std::pair<std::string, ForceMomentOffsetParam>(s->name, ForceMomentOffsetParam()));
00116   }
00117   max_sensor_offset_calib_counter = static_cast<int>(8.0/m_dt); // 8.0[s] by default
00118   return RTC::RTC_OK;
00119 }
00120 
00121 /*
00122 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onFinalize()
00123 {
00124   return RTC::RTC_OK;
00125 }
00126 */
00127 
00128 /*
00129 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onStartup(RTC::UniqueId ec_id)
00130 {
00131   return RTC::RTC_OK;
00132 }
00133 */
00134 
00135 /*
00136 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onShutdown(RTC::UniqueId ec_id)
00137 {
00138   return RTC::RTC_OK;
00139 }
00140 */
00141 
00142 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onActivated(RTC::UniqueId ec_id)
00143 {
00144   std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00145   return RTC::RTC_OK;
00146 }
00147 
00148 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onDeactivated(RTC::UniqueId ec_id)
00149 {
00150   std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00151   return RTC::RTC_OK;
00152 }
00153 
00154 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00155 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onExecute(RTC::UniqueId ec_id)
00156 {
00157   //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00158   static int loop = 0;
00159   loop ++;
00160   for (unsigned int i=0; i<m_forceIn.size(); i++){
00161     if ( m_forceIn[i]->isNew() ) {
00162       m_forceIn[i]->read();
00163     }
00164   }
00165   hrp::Vector3 rpy;
00166   if (m_rpyIn.isNew()) {
00167     m_rpyIn.read();
00168     rpy = hrp::Vector3(m_rpy.data.r, m_rpy.data.p, m_rpy.data.y);
00169   } else {
00170     rpy = hrp::Vector3::Zero();
00171   }
00172   if (m_qCurrentIn.isNew()) {
00173     m_qCurrentIn.read();
00174     for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00175       m_robot->joint(i)->q = m_qCurrent.data[i];
00176     }
00177     //
00178     updateRootLinkPosRot(rpy);
00179     m_robot->calcForwardKinematics();
00180     Guard guard(m_mutex);
00181     for (unsigned int i=0; i<m_forceIn.size(); i++){
00182       if ( m_force[i].data.length()==6 ) {
00183         std::string sensor_name = m_forceIn[i]->name();
00184         hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_name);
00185         hrp::Vector3 data_p(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]);
00186         hrp::Vector3 data_r(m_force[i].data[3], m_force[i].data[4], m_force[i].data[5]);
00187         if ( DEBUGP ) {
00188           std::cerr << "[" << m_profile.instance_name << "] wrench [" << m_forceIn[i]->name() << "]" << std::endl;;
00189           std::cerr << "[" << m_profile.instance_name << "]   raw force = " << data_p.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl;
00190           std::cerr << "[" << m_profile.instance_name << "]   raw moment = " << data_r.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl;
00191         }
00192         if ( sensor ) {
00193           // real force sensor
00194           hrp::Matrix33 sensorR = sensor->link->R * sensor->localR;
00195           ForceMomentOffsetParam& fmp = m_forcemoment_offset_param[sensor_name];
00196           hrp::Vector3 mg = hrp::Vector3(0,0, fmp.link_offset_mass * grav * -1);
00197           hrp::Vector3 cxmg = hrp::Vector3(sensorR * fmp.link_offset_centroid).cross(mg);
00198           // Sensor offset calib
00199           if (fmp.sensor_offset_calib_counter > 0) { // while calibrating
00200               fmp.force_offset_sum += (data_p - sensorR.transpose() * mg);
00201               fmp.moment_offset_sum += (data_r - sensorR.transpose() * cxmg);
00202               fmp.sensor_offset_calib_counter--;
00203               if (fmp.sensor_offset_calib_counter == 0) {
00204                   fmp.force_offset = fmp.force_offset_sum/max_sensor_offset_calib_counter;
00205                   fmp.moment_offset = fmp.moment_offset_sum/max_sensor_offset_calib_counter;
00206                   sem_post(&(fmp.wait_sem));
00207               }
00208           }
00209           // force and moments which do not include offsets
00210           fmp.off_force = sensorR * (data_p - fmp.force_offset) - mg;
00211           fmp.off_moment = sensorR * (data_r - fmp.moment_offset) - cxmg;
00212           // convert absolute force -> sensor local force
00213           fmp.off_force = hrp::Vector3(sensorR.transpose() * fmp.off_force);
00214           fmp.off_moment = hrp::Vector3(sensorR.transpose() * fmp.off_moment);
00215           for (size_t j = 0; j < 3; j++) {
00216             m_force[i].data[j] = fmp.off_force(j);
00217             m_force[i].data[3+j] = fmp.off_moment(j);
00218           }
00219           if ( DEBUGP ) {
00220             std::cerr << "[" << m_profile.instance_name << "]   off force = " << fmp.off_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl;
00221             std::cerr << "[" << m_profile.instance_name << "]   off moment = " << fmp.off_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl;
00222           }
00223         } else {
00224           std::cerr << "[" << m_profile.instance_name << "] unknwon force param " << sensor_name << std::endl;
00225         }
00226       }
00227     }
00228   }
00229   for (unsigned int i=0; i<m_forceOut.size(); i++){
00230     m_forceOut[i]->write();
00231   }
00232   return RTC::RTC_OK;
00233 }
00234 
00235 void RemoveForceSensorLinkOffset::updateRootLinkPosRot (const hrp::Vector3& rpy)
00236 {
00237   if ( m_robot->numSensors(hrp::Sensor::ACCELERATION) > 0) {
00238     hrp::Sensor *sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
00239     hrp::Matrix33 tmpr;
00240     rats::rotm3times(tmpr, hrp::Matrix33(sensor->link->R*sensor->localR).transpose(), m_robot->rootLink()->R);
00241     rats::rotm3times(m_robot->rootLink()->R, hrp::rotFromRpy(rpy(0), rpy(1), rpy(2)), tmpr);
00242   }
00243 }
00244 
00245 void RemoveForceSensorLinkOffset::printForceMomentOffsetParam(const std::string& i_name_)
00246 {
00247   std::cerr << "[" << m_profile.instance_name << "]   force_offset = " << m_forcemoment_offset_param[i_name_].force_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
00248   std::cerr << "[" << m_profile.instance_name << "]   moment_offset = " << m_forcemoment_offset_param[i_name_].moment_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
00249   std::cerr << "[" << m_profile.instance_name << "]   link_offset_centroid = " << m_forcemoment_offset_param[i_name_].link_offset_centroid.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl;
00250   std::cerr << "[" << m_profile.instance_name << "]   link_offset_mass = " << m_forcemoment_offset_param[i_name_].link_offset_mass << "[kg]" << std::endl;
00251 };
00252 
00253 bool RemoveForceSensorLinkOffset::setForceMomentOffsetParam(const std::string& i_name_, const RemoveForceSensorLinkOffsetService::forcemomentOffsetParam& i_param_)
00254 {
00255   std::cerr << "[" << m_profile.instance_name << "] setForceMomentOffsetParam [" << i_name_ << "]" << std::endl;
00256   if (m_forcemoment_offset_param.find(i_name_) != m_forcemoment_offset_param.end()) {
00257     memcpy(m_forcemoment_offset_param[i_name_].force_offset.data(), i_param_.force_offset.get_buffer(), sizeof(double) * 3);
00258     memcpy(m_forcemoment_offset_param[i_name_].moment_offset.data(), i_param_.moment_offset.get_buffer(), sizeof(double) * 3);
00259     memcpy(m_forcemoment_offset_param[i_name_].link_offset_centroid.data(), i_param_.link_offset_centroid.get_buffer(), sizeof(double) * 3);
00260     m_forcemoment_offset_param[i_name_].link_offset_mass = i_param_.link_offset_mass;
00261     printForceMomentOffsetParam(i_name_);
00262     return true;
00263   } else {
00264     std::cerr << "[" << m_profile.instance_name << "]   No such limb"<< std::endl;
00265     return false;
00266   }
00267 }
00268 
00269 bool RemoveForceSensorLinkOffset::getForceMomentOffsetParam(const std::string& i_name_, RemoveForceSensorLinkOffsetService::forcemomentOffsetParam& i_param_)
00270 {
00271   if (m_forcemoment_offset_param.find(i_name_) != m_forcemoment_offset_param.end()) {
00272     // std::cerr << "OK " << i_name_ << " in getForceMomentOffsetParam" << std::endl;
00273     memcpy(i_param_.force_offset.get_buffer(), m_forcemoment_offset_param[i_name_].force_offset.data(), sizeof(double) * 3);
00274     memcpy(i_param_.moment_offset.get_buffer(), m_forcemoment_offset_param[i_name_].moment_offset.data(), sizeof(double) * 3);
00275     memcpy(i_param_.link_offset_centroid.get_buffer(), m_forcemoment_offset_param[i_name_].link_offset_centroid.data(), sizeof(double) * 3);
00276     i_param_.link_offset_mass = m_forcemoment_offset_param[i_name_].link_offset_mass;
00277     return true;
00278   } else {
00279     std::cerr << "[" << m_profile.instance_name << "] No such limb " << i_name_ << " in getForceMomentOffsetParam" << std::endl;
00280     return false;
00281   }
00282 }
00283 
00284 bool RemoveForceSensorLinkOffset::loadForceMomentOffsetParams(const std::string& filename)
00285 {
00286   std::cerr << "[" << m_profile.instance_name << "] loadForceMomentOffsetParams" << std::endl;
00287   std::ifstream ifs(filename.c_str());
00288   if (ifs.is_open()){
00289     while(ifs.eof()==0){
00290       std::string tmps;
00291       ForceMomentOffsetParam tmpp;
00292       if ( ifs >> tmps ) {
00293           if ( m_forcemoment_offset_param.find(tmps) != m_forcemoment_offset_param.end()) {
00294               for (size_t i = 0; i < 3; i++) ifs >> tmpp.force_offset(i);
00295               for (size_t i = 0; i < 3; i++) ifs >> tmpp.moment_offset(i);
00296               for (size_t i = 0; i < 3; i++) ifs >> tmpp.link_offset_centroid(i);
00297               ifs >> tmpp.link_offset_mass;
00298               m_forcemoment_offset_param[tmps] = tmpp;
00299               std::cerr << "[" << m_profile.instance_name << "]   " << tmps << "" << std::endl;
00300               printForceMomentOffsetParam(tmps);
00301           } else {
00302               std::cerr << "[" << m_profile.instance_name << "] no such (" << tmps << ")" << std::endl;
00303               return false;
00304           }
00305       }
00306     }
00307   } else {
00308     std::cerr << "[" << m_profile.instance_name << "] failed to open(" << filename << ")" << std::endl;
00309     return false;
00310   }
00311   return true;
00312 };
00313 
00314 bool RemoveForceSensorLinkOffset::dumpForceMomentOffsetParams(const std::string& filename)
00315 {
00316   std::cerr << "[" << m_profile.instance_name << "] dumpForceMomentOffsetParams" << std::endl;
00317   std::ofstream ofs(filename.c_str());
00318   if (ofs.is_open()){
00319     for ( std::map<std::string, ForceMomentOffsetParam>::iterator it = m_forcemoment_offset_param.begin(); it != m_forcemoment_offset_param.end(); it++ ) {
00320       ofs << it->first << " ";
00321       ofs << it->second.force_offset[0] << " " << it->second.force_offset[1] << " " << it->second.force_offset[2] << " ";
00322       ofs << it->second.moment_offset[0] << " " << it->second.moment_offset[1] << " " << it->second.moment_offset[2] << " ";
00323       ofs << it->second.link_offset_centroid[0] << " " << it->second.link_offset_centroid[1] << " " << it->second.link_offset_centroid[2] << " ";
00324       ofs << it->second.link_offset_mass << std::endl;
00325     }
00326   } else {
00327     std::cerr << "[" << m_profile.instance_name << "] failed to open(" << filename << ")" << std::endl;
00328     return false;
00329   }
00330   return true;
00331 };
00332 
00333 bool RemoveForceSensorLinkOffset::removeForceSensorOffset (const ::OpenHRP::RemoveForceSensorLinkOffsetService::StrSequence& names, const double tm)
00334 {
00335     std::cerr << "[" << m_profile.instance_name << "] removeForceSensorOffset..." << std::endl;
00336 
00337     // Check argument validity
00338     std::vector<std::string> valid_names, invalid_names, calibrating_names;
00339     bool is_valid_argument = true;
00340     {
00341         Guard guard(m_mutex);
00342         if ( names.length() == 0 ) { // If no sensor names are specified, calibrate all sensors.
00343             std::cerr << "[" << m_profile.instance_name << "]   No sensor names are specified, calibrate all sensors = [";
00344             for ( std::map<std::string, ForceMomentOffsetParam>::iterator it = m_forcemoment_offset_param.begin(); it != m_forcemoment_offset_param.end(); it++ ) {
00345                 valid_names.push_back(it->first);
00346                 std::cerr << it->first << " ";
00347             }
00348             std::cerr << "]" << std::endl;
00349         } else {
00350             for (size_t i = 0; i < names.length(); i++) {
00351                 std::string name(names[i]);
00352                 if ( m_forcemoment_offset_param.find(name) != m_forcemoment_offset_param.end() ) {
00353                     if ( m_forcemoment_offset_param[name].sensor_offset_calib_counter == 0 ) {
00354                         valid_names.push_back(name);
00355                     } else {
00356                         calibrating_names.push_back(name);
00357                         is_valid_argument = false;
00358                     }
00359                 } else{
00360                     invalid_names.push_back(name);
00361                     is_valid_argument = false;
00362                 }
00363             }
00364         }
00365     }
00366     // Return if invalid or calibrating
00367     if ( !is_valid_argument ) {
00368         std::cerr << "[" << m_profile.instance_name << "]   Cannot start removeForceSensorOffset, invalid = [";
00369         for (size_t i = 0; i < invalid_names.size(); i++) std::cerr << invalid_names[i] << " ";
00370         std::cerr << "], calibrating = [";
00371         for (size_t i = 0; i < calibrating_names.size(); i++) std::cerr << calibrating_names[i] << " ";
00372         std::cerr << "]" << std::endl;
00373         return false;
00374     }
00375 
00376     // Start calibration
00377     //   Print output force before calib
00378     std::cerr << "[" << m_profile.instance_name << "]   Calibrate sensor names = [";
00379     for (size_t i = 0; i < valid_names.size(); i++) std::cerr << valid_names[i] << " ";
00380     std::cerr << "]" << std::endl;
00381     {
00382         Guard guard(m_mutex);
00383         for (size_t i = 0; i < valid_names.size(); i++) {
00384             std::cerr << "[" << m_profile.instance_name << "]     Offset-removed force before calib [" << valid_names[i] << "], ";
00385             std::cerr << "force = " << m_forcemoment_offset_param[valid_names[i]].off_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][N]")) << ", ";
00386             std::cerr << "moment = " << m_forcemoment_offset_param[valid_names[i]].off_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][Nm]"));
00387             std::cerr << std::endl;
00388         }
00389         max_sensor_offset_calib_counter = static_cast<int>(tm/m_dt);
00390         for (size_t i = 0; i < valid_names.size(); i++) {
00391             m_forcemoment_offset_param[valid_names[i]].force_offset_sum = hrp::Vector3::Zero();
00392             m_forcemoment_offset_param[valid_names[i]].moment_offset_sum = hrp::Vector3::Zero();
00393             m_forcemoment_offset_param[valid_names[i]].sensor_offset_calib_counter = max_sensor_offset_calib_counter;
00394         }
00395     }
00396     //   Wait
00397     for (size_t i = 0; i < valid_names.size(); i++) {
00398         sem_wait(&(m_forcemoment_offset_param[valid_names[i]].wait_sem));
00399     }
00400     //   Print output force and offset after calib
00401     {
00402         Guard guard(m_mutex);
00403         std::cerr << "[" << m_profile.instance_name << "]   Calibrate done (calib time = " << tm << "[s])" << std::endl;
00404         for (size_t i = 0; i < valid_names.size(); i++) {
00405             std::cerr << "[" << m_profile.instance_name << "]     Calibrated offset [" << valid_names[i] << "], ";
00406             std::cerr << "force_offset = " << m_forcemoment_offset_param[valid_names[i]].force_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][N]")) << ", ";
00407             std::cerr << "moment_offset = " << m_forcemoment_offset_param[valid_names[i]].moment_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][Nm]")) << std::endl;
00408             std::cerr << "[" << m_profile.instance_name << "]     Offset-removed force after calib [" << valid_names[i] << "], ";
00409             std::cerr << "force = " << m_forcemoment_offset_param[valid_names[i]].off_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][N]")) << ", ";
00410             std::cerr << "moment = " << m_forcemoment_offset_param[valid_names[i]].off_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][Nm]"));
00411             std::cerr << std::endl;
00412         }
00413     }
00414     std::cerr << "[" << m_profile.instance_name << "] removeForceSensorOffset...done" << std::endl;
00415     return true;
00416 }
00417 
00418 /*
00419 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onAborting(RTC::UniqueId ec_id)
00420 {
00421   return RTC::RTC_OK;
00422 }
00423 */
00424 
00425 /*
00426 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onError(RTC::UniqueId ec_id)
00427 {
00428   return RTC::RTC_OK;
00429 }
00430 */
00431 
00432 /*
00433 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onReset(RTC::UniqueId ec_id)
00434 {
00435   return RTC::RTC_OK;
00436 }
00437 */
00438 
00439 /*
00440 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onStateUpdate(RTC::UniqueId ec_id)
00441 {
00442   return RTC::RTC_OK;
00443 }
00444 */
00445 
00446 /*
00447 RTC::ReturnCode_t RemoveForceSensorLinkOffset::onRateChanged(RTC::UniqueId ec_id)
00448 {
00449   return RTC::RTC_OK;
00450 }
00451 */
00452 
00453 extern "C"
00454 {
00455 
00456   void RemoveForceSensorLinkOffsetInit(RTC::Manager* manager)
00457   {
00458     RTC::Properties profile(removeforcesensorlinkoffset_spec);
00459     manager->registerFactory(profile,
00460                              RTC::Create<RemoveForceSensorLinkOffset>,
00461                              RTC::Delete<RemoveForceSensorLinkOffset>);
00462   }
00463 
00464 };
00465 
00466 


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