ForwardKinematics.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/CorbaNaming.h>
00011 #include "ForwardKinematics.h"
00012 
00013 #include "hrpModel/Link.h"
00014 #include "hrpModel/ModelLoaderUtil.h"
00015 
00016 typedef coil::Guard<coil::Mutex> Guard;
00017 
00018 // Module specification
00019 // <rtc-template block="module_spec">
00020 static const char* nullcomponent_spec[] =
00021   {
00022     "implementation_id", "ForwardKinematics",
00023     "type_name",         "ForwardKinematics",
00024     "description",       "forward kinematics 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.sensorAttachedLink", "",
00034 
00035     ""
00036   };
00037 // </rtc-template>
00038 
00039 ForwardKinematics::ForwardKinematics(RTC::Manager* manager)
00040   : RTC::DataFlowComponentBase(manager),
00041     // <rtc-template block="initializer">
00042     m_qIn("q", m_q),
00043     m_sensorRpyIn("sensorRpy", m_sensorRpy),
00044     m_qRefIn("qRef", m_qRef),
00045     m_basePosRefIn("basePosRef", m_basePosRef),
00046     m_baseRpyRefIn("baseRpyRef", m_baseRpyRef),
00047     m_ForwardKinematicsServicePort("ForwardKinematicsService"),
00048     // </rtc-template>
00049     dummy(0)
00050 {
00051 }
00052 
00053 ForwardKinematics::~ForwardKinematics()
00054 {
00055 }
00056 
00057 
00058 
00059 RTC::ReturnCode_t ForwardKinematics::onInitialize()
00060 {
00061   std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00062   // <rtc-template block="bind_config">
00063   // Bind variables and configuration variable
00064   coil::Properties& ref = getProperties();
00065   bindParameter("sensorAttachedLink", m_sensorAttachedLinkName, ref["conf.default.sensorAttachedLink"].c_str());
00066   
00067   // </rtc-template>
00068 
00069   // Registration: InPort/OutPort/Service
00070   // <rtc-template block="registration">
00071   // Set InPort buffers
00072   addInPort("q", m_qIn);
00073   addInPort("sensorRpy", m_sensorRpyIn);
00074   addInPort("qRef", m_qRefIn);
00075   addInPort("basePosRef", m_basePosRefIn);
00076   addInPort("baseRpyRef", m_baseRpyRefIn);
00077 
00078   // Set OutPort buffer
00079   
00080   // Set service provider to Ports
00081   m_ForwardKinematicsServicePort.registerProvider("service0", "ForwardKinematicsService", m_service0);
00082   addPort(m_ForwardKinematicsServicePort);
00083   m_service0.setComp(this);
00084   
00085   // Set service consumers to Ports
00086   
00087   // Set CORBA Service Ports
00088   
00089   // </rtc-template>
00090 
00091   RTC::Properties& prop = getProperties();
00092 
00093   RTC::Manager& rtcManager = RTC::Manager::instance();
00094   std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00095   int comPos = nameServer.find(",");
00096   if (comPos < 0){
00097       comPos = nameServer.length();
00098   }
00099   nameServer = nameServer.substr(0, comPos);
00100   RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00101   m_refBody = hrp::BodyPtr(new hrp::Body());
00102   if (!loadBodyFromModelLoader(m_refBody, prop["model"].c_str(), 
00103                                CosNaming::NamingContext::_duplicate(naming.getRootContext()))){
00104     std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
00105     return RTC::RTC_ERROR;
00106   }
00107   m_actBody = hrp::BodyPtr(new hrp::Body());
00108   if (!loadBodyFromModelLoader(m_actBody, prop["model"].c_str(), 
00109                                CosNaming::NamingContext::_duplicate(naming.getRootContext()))){
00110     std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
00111     return RTC::RTC_ERROR;
00112   }
00113 
00114   m_refLink = m_refBody->rootLink();
00115   m_actLink = m_actBody->rootLink();
00116 
00117   return RTC::RTC_OK;
00118 }
00119 
00120 
00121 
00122 /*
00123 RTC::ReturnCode_t ForwardKinematics::onFinalize()
00124 {
00125   return RTC::RTC_OK;
00126 }
00127 */
00128 
00129 /*
00130 RTC::ReturnCode_t ForwardKinematics::onStartup(RTC::UniqueId ec_id)
00131 {
00132   return RTC::RTC_OK;
00133 }
00134 */
00135 
00136 /*
00137 RTC::ReturnCode_t ForwardKinematics::onShutdown(RTC::UniqueId ec_id)
00138 {
00139   return RTC::RTC_OK;
00140 }
00141 */
00142 
00143 RTC::ReturnCode_t ForwardKinematics::onActivated(RTC::UniqueId ec_id)
00144 {
00145   std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00146   if (m_sensorAttachedLinkName == ""){
00147     m_sensorAttachedLink = NULL;
00148   }else{
00149     m_sensorAttachedLink = m_actBody->link(m_sensorAttachedLinkName);
00150     if (!m_sensorAttachedLink){
00151       std::cerr << "can't find a link named " << m_sensorAttachedLinkName 
00152                 << std::endl;
00153       return RTC::RTC_ERROR;
00154     }
00155   }
00156   return RTC::RTC_OK;
00157 }
00158 
00159 RTC::ReturnCode_t ForwardKinematics::onDeactivated(RTC::UniqueId ec_id)
00160 {
00161   std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00162   return RTC::RTC_OK;
00163 }
00164 
00165 RTC::ReturnCode_t ForwardKinematics::onExecute(RTC::UniqueId ec_id)
00166 {
00167   //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00168 
00169   coil::TimeValue tm(coil::gettimeofday());
00170   m_tm.sec  = tm.sec();
00171   m_tm.nsec = tm.usec() * 1000;
00172 
00173   if (m_qIn.isNew()) {
00174       m_qIn.read();
00175       for (unsigned int i=0; i<m_actBody->numJoints(); i++){
00176           m_actBody->joint(i)->q = m_q.data[i];
00177       }
00178   }
00179 
00180   if (m_sensorRpyIn.isNew()) {
00181       m_sensorRpyIn.read();
00182       hrp::Matrix33 sensorR = hrp::rotFromRpy(m_sensorRpy.data.r,
00183                                               m_sensorRpy.data.p,
00184                                               m_sensorRpy.data.y);
00185       if (m_sensorAttachedLink){
00186         hrp::Matrix33 sensor2base(m_sensorAttachedLink->R.transpose()*m_actBody->rootLink()->R);
00187         hrp::Matrix33 baseR(sensorR*sensor2base);
00188         // to prevent numerical error
00189         hrp::Vector3 baseRpy = hrp::rpyFromRot(baseR);
00190         // use reference yaw angle instead of estimated one
00191         baseRpy[2] = m_baseRpyRef.data.y;
00192         m_actBody->rootLink()->R = hrp::rotFromRpy(baseRpy);
00193       }else{
00194         m_actBody->rootLink()->R = sensorR;
00195       }
00196   }
00197 
00198   if (m_qRefIn.isNew()) {
00199       m_qRefIn.read();
00200       for (unsigned int i=0; i<m_refBody->numJoints(); i++){
00201           m_refBody->joint(i)->q = m_qRef.data[i];
00202       }
00203   }
00204 
00205   if (m_basePosRefIn.isNew()){
00206       m_basePosRefIn.read();
00207       hrp::Link *root = m_refBody->rootLink();
00208       root->p[0] = m_basePosRef.data.x;
00209       root->p[1] = m_basePosRef.data.y;
00210       root->p[2] = m_basePosRef.data.z;
00211   }
00212 
00213   if (m_baseRpyRefIn.isNew()){
00214       m_baseRpyRefIn.read();
00215       hrp::Vector3 rpy;
00216       rpy[0] = m_baseRpyRef.data.r;
00217       rpy[1] = m_baseRpyRef.data.p;
00218       rpy[2] = m_baseRpyRef.data.y;
00219       m_refBody->rootLink()->R = hrp::rotFromRpy(rpy);
00220 
00221   }
00222 
00223   {
00224       Guard guard(m_bodyMutex);
00225       m_refBody->calcForwardKinematics();
00226       m_actBody->calcForwardKinematics();
00227   }
00228 
00229   return RTC::RTC_OK;
00230 }
00231 
00232 /*
00233 RTC::ReturnCode_t ForwardKinematics::onAborting(RTC::UniqueId ec_id)
00234 {
00235   return RTC::RTC_OK;
00236 }
00237 */
00238 
00239 /*
00240 RTC::ReturnCode_t ForwardKinematics::onError(RTC::UniqueId ec_id)
00241 {
00242   return RTC::RTC_OK;
00243 }
00244 */
00245 
00246 /*
00247 RTC::ReturnCode_t ForwardKinematics::onReset(RTC::UniqueId ec_id)
00248 {
00249   return RTC::RTC_OK;
00250 }
00251 */
00252 
00253 /*
00254 RTC::ReturnCode_t ForwardKinematics::onStateUpdate(RTC::UniqueId ec_id)
00255 {
00256   return RTC::RTC_OK;
00257 }
00258 */
00259 
00260 /*
00261 RTC::ReturnCode_t ForwardKinematics::onRateChanged(RTC::UniqueId ec_id)
00262 {
00263   return RTC::RTC_OK;
00264 }
00265 */
00266 
00267 ::CORBA::Boolean ForwardKinematics::getReferencePose(const char* linkname, RTC::TimedDoubleSeq_out pose,const char* frame_name)
00268 {
00269     pose = new RTC::TimedDoubleSeq();
00270     Guard guard(m_bodyMutex);
00271     hrp::Link *l = m_refBody->link(linkname);
00272     if (!l) return false;
00273     hrp::Link *f = NULL;
00274     if (frame_name) {
00275         f = m_refBody->link(frame_name);
00276         if (!f) {
00277             std::cerr << "[getReferencePose] ERROR Could not find frame_name = " << frame_name << std::endl;
00278             return false;
00279         }
00280     }
00281     std::cerr << "[getReferencePose] linkaname = " << linkname << ", frame_name = " << (frame_name?frame_name:"(null)") << std::endl;
00282     hrp::Vector3 p = l->p;
00283     hrp::Matrix33 R = l->attitude();
00284     if (!!f) {
00285         p = f->attitude().transpose() * ( p - f->p );
00286         R = f->attitude().transpose() * R;
00287     }
00288     pose->tm = m_tm;
00289     pose->data.length(16);
00290     pose->data[ 0]=R(0,0);pose->data[ 1]=R(0,1);pose->data[ 2]=R(0,2);pose->data[ 3]=p[0];
00291     pose->data[ 4]=R(1,0);pose->data[ 5]=R(1,1);pose->data[ 6]=R(1,2);pose->data[ 7]=p[1];
00292     pose->data[ 8]=R(2,0);pose->data[ 9]=R(2,1);pose->data[10]=R(2,2);pose->data[11]=p[2];
00293     pose->data[12]=0;     pose->data[13]=0;     pose->data[14]=0;     pose->data[15]=1;
00294     return true;
00295 }
00296 
00297 ::CORBA::Boolean ForwardKinematics::getCurrentPose(const char* linkname, RTC::TimedDoubleSeq_out pose, const char* frame_name)
00298 {
00299     pose = new RTC::TimedDoubleSeq();
00300     Guard guard(m_bodyMutex);
00301     hrp::Link *l = m_actBody->link(linkname);
00302     if (!l) return false;
00303     hrp::Link *f = NULL;
00304     if (frame_name) {
00305         f = m_actBody->link(frame_name);
00306         if (!f) {
00307             std::cerr << "[getCurrentPose] ERROR Could not find frame_name = " << frame_name << std::endl;
00308             return false;
00309         }
00310     }
00311     std::cerr << "[getCurrentPose] linkaname = " << linkname << ", frame_name = " << (frame_name?frame_name:"(null)") << std::endl;
00312     hrp::Vector3 dp(m_refLink->p - m_actLink->p);
00313 
00314     hrp::Vector3 p(l->p + dp);
00315     hrp::Matrix33 R = l->attitude();
00316     if (!!f) {
00317         p = f->attitude().transpose() * ( p - f->p);
00318         R = f->attitude().transpose() * R;
00319     }
00320     pose->tm = m_tm;
00321     pose->data.length(16);
00322     pose->data[ 0]=R(0,0);pose->data[ 1]=R(0,1);pose->data[ 2]=R(0,2);pose->data[ 3]=p[0];
00323     pose->data[ 4]=R(1,0);pose->data[ 5]=R(1,1);pose->data[ 6]=R(1,2);pose->data[ 7]=p[1];
00324     pose->data[ 8]=R(2,0);pose->data[ 9]=R(2,1);pose->data[10]=R(2,2);pose->data[11]=p[2];
00325     pose->data[12]=0;     pose->data[13]=0;     pose->data[14]=0;     pose->data[15]=1;
00326     return true;
00327 }
00328 
00329 ::CORBA::Boolean ForwardKinematics::getRelativeCurrentPosition(const char* linknameFrom, const char* linknameTo, const OpenHRP::ForwardKinematicsService::position target, OpenHRP::ForwardKinematicsService::position result)
00330 {
00331     Guard guard(m_bodyMutex);
00332     hrp::Link *from = m_actBody->link(linknameFrom);
00333     hrp::Link *to = m_actBody->link(linknameTo);
00334     if (!from || !to) return false;
00335     hrp::Vector3 targetPrel(target[0], target[1], target[2]);
00336     hrp::Vector3 targetPabs(to->p+to->attitude()*targetPrel);
00337     hrp::Matrix33 Rt(from->attitude().transpose());
00338     hrp::Vector3 p(Rt*(targetPabs - from->p));
00339     result[ 0]=p(0);result[ 1]=p(1);result[ 2]=p(2);
00340     return true;
00341 }
00342 
00343 ::CORBA::Boolean ForwardKinematics::selectBaseLink(const char* linkname)
00344 {
00345     Guard guard(m_bodyMutex);
00346     hrp::Link *l = m_refBody->link(linkname);
00347     if (!l) return false;
00348     m_refLink = l;
00349     m_actLink = m_actBody->link(linkname);
00350     return true;
00351 }
00352 
00353 extern "C"
00354 {
00355 
00356   void ForwardKinematicsInit(RTC::Manager* manager)
00357   {
00358     RTC::Properties profile(nullcomponent_spec);
00359     manager->registerFactory(profile,
00360                              RTC::Create<ForwardKinematics>,
00361                              RTC::Delete<ForwardKinematics>);
00362   }
00363 
00364 };
00365 
00366 


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