VirtualForceSensor.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "VirtualForceSensor.h"
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpModel/ModelLoaderUtil.h>
00013 #include <hrpUtil/MatrixSolvers.h>
00014 
00015 // Module specification
00016 // <rtc-template block="module_spec">
00017 static const char* virtualforcesensor_spec[] =
00018   {
00019     "implementation_id", "VirtualForceSensor",
00020     "type_name",         "VirtualForceSensor",
00021     "description",       "null component",
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     "conf.default.debugLevel", "0",
00031     ""
00032   };
00033 // </rtc-template>
00034 
00035 VirtualForceSensor::VirtualForceSensor(RTC::Manager* manager)
00036   : RTC::DataFlowComponentBase(manager),
00037     // <rtc-template block="initializer">
00038     m_qCurrentIn("qCurrent", m_qCurrent),
00039     m_tauInIn("tauIn", m_tauIn),
00040     m_VirtualForceSensorServicePort("VirtualForceSensorService"),
00041     // </rtc-template>
00042     m_debugLevel(0)
00043 {
00044   m_service0.vfsensor(this);
00045 }
00046 
00047 VirtualForceSensor::~VirtualForceSensor()
00048 {
00049 }
00050 
00051 
00052 
00053 RTC::ReturnCode_t VirtualForceSensor::onInitialize()
00054 {
00055   std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00056   // <rtc-template block="bind_config">
00057   // Bind variables and configuration variable
00058   bindParameter("debugLevel", m_debugLevel, "0");
00059   
00060   // </rtc-template>
00061 
00062   // Registration: InPort/OutPort/Service
00063   // <rtc-template block="registration">
00064   // Set InPort buffers
00065   addInPort("qCurrent", m_qCurrentIn);
00066   addInPort("tauIn", m_tauInIn);
00067 
00068   // Set OutPort buffer
00069   
00070   // Set service provider to Ports
00071   m_VirtualForceSensorServicePort.registerProvider("service0", "VirtualForceSensorService", m_service0);
00072   
00073   // Set service consumers to Ports
00074   
00075   // Set CORBA Service Ports
00076   addPort(m_VirtualForceSensorServicePort);
00077   
00078   // </rtc-template>
00079 
00080   RTC::Properties& prop = getProperties();
00081   coil::stringTo(m_dt, prop["dt"].c_str());
00082 
00083   m_robot = hrp::BodyPtr(new hrp::Body());
00084 
00085   RTC::Manager& rtcManager = RTC::Manager::instance();
00086   std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00087   int comPos = nameServer.find(",");
00088   if (comPos < 0){
00089       comPos = nameServer.length();
00090   }
00091   nameServer = nameServer.substr(0, comPos);
00092   RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00093   if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
00094                                CosNaming::NamingContext::_duplicate(naming.getRootContext())
00095           )){
00096     std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
00097               << m_profile.instance_name << std::endl;
00098     return RTC::RTC_ERROR;
00099   }
00100 
00101   // virtual_force_sensor: <name>, <base>, <target>, 0, 0, 0,  0, 0, 1, 0
00102   coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
00103   for(unsigned int i = 0; i < virtual_force_sensor.size()/10; i++ ){
00104     std::string name = virtual_force_sensor[i*10+0];
00105     VirtualForceSensorParam p;
00106     p.base_name = virtual_force_sensor[i*10+1];
00107     p.target_name = virtual_force_sensor[i*10+2];
00108     hrp::dvector tr(7);
00109     for (int j = 0; j < 7; j++ ) {
00110       coil::stringTo(tr[j], virtual_force_sensor[i*10+3+j].c_str());
00111     }
00112     p.p = hrp::Vector3(tr[0], tr[1], tr[2]);
00113     p.R = Eigen::AngleAxis<double>(tr[6], hrp::Vector3(tr[3],tr[4],tr[5])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
00114     p.forceOffset = hrp::Vector3(0, 0, 0);
00115     p.momentOffset = hrp::Vector3(0, 0, 0);
00116     std::cerr << "[" << m_profile.instance_name << "] virtual force sensor : " << name << std::endl;
00117     std::cerr << "[" << m_profile.instance_name << "]                 base : " << p.base_name << std::endl;
00118     std::cerr << "[" << m_profile.instance_name << "]               target : " << p.target_name << std::endl;
00119     std::cerr << "[" << m_profile.instance_name << "]                 T, R : " << p.p[0] << " " << p.p[1] << " " << p.p[2] << std::endl << p.R << std::endl;
00120     p.path = hrp::JointPathPtr(new hrp::JointPath(m_robot->link(p.base_name), m_robot->link(p.target_name)));
00121     m_sensors[name] = p;
00122     if ( m_sensors[name].path->numJoints() == 0 ) {
00123       std::cerr << "[" << m_profile.instance_name << "] ERROR : Unknown link path " << m_sensors[name].base_name << " " << m_sensors[name].target_name  << std::endl;
00124       return RTC::RTC_ERROR;
00125     }
00126   }
00127   int nforce = m_sensors.size();
00128   m_force.resize(nforce);
00129   m_forceOut.resize(nforce);
00130   int i = 0;
00131   std::map<std::string, VirtualForceSensorParam>::iterator it = m_sensors.begin();
00132   while ( it != m_sensors.end() ) {
00133     m_forceOut[i] = new OutPort<TimedDoubleSeq>((*it).first.c_str(), m_force[i]);
00134     m_force[i].data.length(6);
00135     registerOutPort((*it).first.c_str(), *m_forceOut[i]);
00136     it++; i++;
00137   }
00138   
00139   return RTC::RTC_OK;
00140 }
00141 
00142 
00143 
00144 /*
00145 RTC::ReturnCode_t VirtualForceSensor::onFinalize()
00146 {
00147   return RTC::RTC_OK;
00148 }
00149 */
00150 
00151 /*
00152 RTC::ReturnCode_t VirtualForceSensor::onStartup(RTC::UniqueId ec_id)
00153 {
00154   return RTC::RTC_OK;
00155 }
00156 */
00157 
00158 /*
00159 RTC::ReturnCode_t VirtualForceSensor::onShutdown(RTC::UniqueId ec_id)
00160 {
00161   return RTC::RTC_OK;
00162 }
00163 */
00164 
00165 RTC::ReturnCode_t VirtualForceSensor::onActivated(RTC::UniqueId ec_id)
00166 {
00167   std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
00168   return RTC::RTC_OK;
00169 }
00170 
00171 RTC::ReturnCode_t VirtualForceSensor::onDeactivated(RTC::UniqueId ec_id)
00172 {
00173   std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
00174   return RTC::RTC_OK;
00175 }
00176 
00177 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
00178 RTC::ReturnCode_t VirtualForceSensor::onExecute(RTC::UniqueId ec_id)
00179 {
00180   //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
00181   static int loop = 0;
00182   loop ++;
00183 
00184   coil::TimeValue coiltm(coil::gettimeofday());
00185   RTC::Time tm;
00186   tm.sec = coiltm.sec();
00187   tm.nsec = coiltm.usec()*1000;
00188 
00189   if (m_qCurrentIn.isNew()) {
00190     m_qCurrentIn.read();
00191   }
00192   if (m_tauInIn.isNew()) {
00193     m_tauInIn.read();
00194   }
00195 
00196   if ( m_qCurrent.data.length() ==  m_robot->numJoints() &&
00197        m_tauIn.data.length() ==  m_robot->numJoints() ) {
00198     // reference model
00199     for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00200       m_robot->joint(i)->q = m_qCurrent.data[i];
00201     }
00202     m_robot->calcForwardKinematics();
00203     m_robot->calcCM();
00204     m_robot->rootLink()->calcSubMassCM();
00205 
00206     std::map<std::string, VirtualForceSensorParam>::iterator it = m_sensors.begin();
00207     int i = 0;
00208     while ( it != m_sensors.end() ) {
00209 
00210       hrp::JointPathPtr path = (*it).second.path;
00211       int n = path->numJoints();
00212       
00213       if ( DEBUGP ) {
00214         std::cerr << "  sensor name  : " << (*it).first << std::endl;
00215         std::cerr << "sensor torque  : ";
00216         for (int j = 0; j < n; j++) {
00217           std::cerr << " " << m_tauIn.data[path->joint(j)->jointId] ;
00218         }
00219         std::cerr << std::endl;
00220       }
00221 
00222       hrp::dvector force(6);
00223       calcRawVirtualForce((*it).first, force);
00224 
00225       if ( DEBUGP ) {
00226         std::cerr << "    raw force  : ";
00227         for ( int j = 0; j < 6; j ++ ) {
00228           std::cerr << " " << force[j] ;
00229         }
00230         std::cerr << std::endl;
00231       }
00232       
00233       hrp::dvector force_p(3), force_r(3);
00234       for ( int j = 0; j < 3; j ++ ) {
00235         force_p[j] = force[j];
00236         force_r[j] = force[j+3];
00237       }
00238       force_p = force_p - (*it).second.forceOffset;
00239       force_r = force_r - (*it).second.momentOffset;
00240       for ( int j = 0; j < 3; j ++ ) {
00241         m_force[i].data[j+0] = force_p[j];
00242         m_force[i].data[j+3] = force_r[j];
00243       }
00244 
00245       if ( DEBUGP ) {
00246         std::cerr << "  output force  : ";
00247         for (int j = 0; j < 6; j++) {
00248           std::cerr << " " << m_force[i].data[j];
00249         }
00250         std::cerr << std::endl;
00251       }
00252 
00253       m_force[i].tm = tm; // put timestamp
00254       m_forceOut[i]->write();
00255 
00256       it++; i++;
00257     }
00258     //
00259 #if 0
00260   (:calc-force-from-joint-torque
00261    (limb all-torque &key (move-target (send self limb :end-coords)) (use-torso))
00262    (let* ((link-list
00263            (send self :link-list
00264                  (send move-target :parent)
00265                  (unless use-torso (car (send self limb :links)))))
00266           (jacobian
00267            (send self :calc-jacobian-from-link-list
00268                  link-list
00269                  :move-target move-target
00270                  :rotation-axis (list t)
00271                  :translation-axis (list t)))
00272           (torque (instantiate float-vector (length link-list))))
00273      (dotimes (i (length link-list))
00274        (setf (elt torque i)
00275              (elt all-torque (position (send (elt link-list i) :joint) (send self :joint-list)))))
00276      (transform (send self :calc-inverse-jacobian (transpose jacobian))
00277                 torque)))
00278 #endif
00279 
00280   }
00281   return RTC::RTC_OK;
00282 }
00283 
00284 /*
00285 RTC::ReturnCode_t VirtualForceSensor::onAborting(RTC::UniqueId ec_id)
00286 {
00287   return RTC::RTC_OK;
00288 }
00289 */
00290 
00291 /*
00292 RTC::ReturnCode_t VirtualForceSensor::onError(RTC::UniqueId ec_id)
00293 {
00294   return RTC::RTC_OK;
00295 }
00296 */
00297 
00298 /*
00299 RTC::ReturnCode_t VirtualForceSensor::onReset(RTC::UniqueId ec_id)
00300 {
00301   return RTC::RTC_OK;
00302 }
00303 */
00304 
00305 /*
00306 RTC::ReturnCode_t VirtualForceSensor::onStateUpdate(RTC::UniqueId ec_id)
00307 {
00308   return RTC::RTC_OK;
00309 }
00310 */
00311 
00312 /*
00313 RTC::ReturnCode_t VirtualForceSensor::onRateChanged(RTC::UniqueId ec_id)
00314 {
00315   return RTC::RTC_OK;
00316 }
00317 */
00318 
00319 bool VirtualForceSensor::removeVirtualForceSensorOffset(std::string sensorName)
00320 {
00321   std::map<std::string, VirtualForceSensorParam>::iterator it;
00322   for (it = m_sensors.begin(); it != m_sensors.end(); it++) {
00323     if ((*it).first != sensorName){
00324       continue;
00325     } else {
00326       hrp::JointPathPtr path = (*it).second.path;
00327       hrp::dvector force(6);
00328       if(!calcRawVirtualForce(sensorName, force)){
00329         return false;
00330       }
00331       hrp::Vector3 force_p, force_r;
00332       for ( int i = 0; i < 3; i ++ ) {
00333         force_p[i] = force[i];
00334         force_r[i] = force[i+3];
00335       }
00336       (*it).second.forceOffset = force_p;
00337       (*it).second.momentOffset = force_r;
00338       return true;
00339     }
00340   }
00341   std::cerr << "removeVirtualForceSensorOffset: No sensor " << sensorName << std::endl;
00342   return false;
00343 }
00344 
00345 bool VirtualForceSensor::calcRawVirtualForce(std::string sensorName, hrp::dvector &outputForce)
00346 {
00347   std::map<std::string, VirtualForceSensorParam>::iterator it;
00348   for (it = m_sensors.begin(); it != m_sensors.end(); ++it) {
00349     if ((*it).first != sensorName){
00350       continue;
00351     } else {
00352       hrp::JointPathPtr path = (*it).second.path;
00353       int n = path->numJoints();
00354       hrp::dmatrix J(6, n);
00355       hrp::dmatrix Jtinv(6, n);
00356       path->calcJacobian(J);
00357       hrp::calcPseudoInverse(J.transpose(), Jtinv);
00358       // use sr inverse of J.transpose()
00359       // hrp::dmatrix Jt = J.transpose();
00360       // double manipulability = sqrt((Jt*J).determinant());
00361       // hrp::calcPseudoInverse((Jt * J + 0.1 * hrp::dmatrix::Identity(n,n)), Jtinv);
00362       hrp::dvector torque(n);
00363       hrp::dvector force(6);
00364           
00365       // get gear torque
00366       for (int i = 0; i < n; i++) {
00367         torque[i] = -m_tauIn.data[path->joint(i)->jointId]; // passive torque from external force
00368       }
00369 
00370       // calc estimated force from torque vector
00371       force = Jtinv * torque;
00372       // force = J * torque;
00373 
00374       // trans to localcoords and set offset
00375       hrp::dvector force_p(3), force_r(3);
00376       for (int i = 0; i < 3; i++) {
00377         force_p[i] = force[i];
00378         force_r[i] = force[i + 3];
00379       }
00380       force_p = (*it).second.R.transpose() * path->endLink()->R.transpose() * force_p;
00381       force_r = (*it).second.R.transpose() * path->endLink()->R.transpose() * force_r;
00382       
00383       outputForce.resize(6);
00384       for(int i = 0; i < 3; i++) {
00385         outputForce[i] = force_p[i];
00386         outputForce[i + 3] = force_r[i];
00387       }
00388       return true;
00389     }
00390   }
00391 
00392   std::cerr << "calcVirtualForce: No sensor " << sensorName << std::endl;
00393   return false;
00394   
00395 }
00396 
00397 extern "C"
00398 {
00399 
00400   void VirtualForceSensorInit(RTC::Manager* manager)
00401   {
00402     RTC::Properties profile(virtualforcesensor_spec);
00403     manager->registerFactory(profile,
00404                              RTC::Create<VirtualForceSensor>,
00405                              RTC::Delete<VirtualForceSensor>);
00406   }
00407 
00408 };
00409 
00410 


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