VirtualForceSensor.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include "VirtualForceSensor.h"
11 #include <rtm/CorbaNaming.h>
12 #include <hrpModel/ModelLoaderUtil.h>
13 #include <hrpUtil/MatrixSolvers.h>
14 
15 // Module specification
16 // <rtc-template block="module_spec">
17 static const char* virtualforcesensor_spec[] =
18  {
19  "implementation_id", "VirtualForceSensor",
20  "type_name", "VirtualForceSensor",
21  "description", "null component",
22  "version", HRPSYS_PACKAGE_VERSION,
23  "vendor", "AIST",
24  "category", "example",
25  "activity_type", "DataFlowComponent",
26  "max_instance", "10",
27  "language", "C++",
28  "lang_type", "compile",
29  // Configuration variables
30  "conf.default.debugLevel", "0",
31  ""
32  };
33 // </rtc-template>
34 
36  : RTC::DataFlowComponentBase(manager),
37  // <rtc-template block="initializer">
38  m_qCurrentIn("qCurrent", m_qCurrent),
39  m_tauInIn("tauIn", m_tauIn),
40  m_VirtualForceSensorServicePort("VirtualForceSensorService"),
41  // </rtc-template>
42  m_debugLevel(0)
43 {
44  m_service0.vfsensor(this);
45 }
46 
48 {
49 }
50 
51 
52 
54 {
55  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
56  // <rtc-template block="bind_config">
57  // Bind variables and configuration variable
58  bindParameter("debugLevel", m_debugLevel, "0");
59 
60  // </rtc-template>
61 
62  // Registration: InPort/OutPort/Service
63  // <rtc-template block="registration">
64  // Set InPort buffers
65  addInPort("qCurrent", m_qCurrentIn);
66  addInPort("tauIn", m_tauInIn);
67 
68  // Set OutPort buffer
69 
70  // Set service provider to Ports
71  m_VirtualForceSensorServicePort.registerProvider("service0", "VirtualForceSensorService", m_service0);
72 
73  // Set service consumers to Ports
74 
75  // Set CORBA Service Ports
77 
78  // </rtc-template>
79 
81  coil::stringTo(m_dt, prop["dt"].c_str());
82 
84 
85  RTC::Manager& rtcManager = RTC::Manager::instance();
86  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
87  int comPos = nameServer.find(",");
88  if (comPos < 0){
89  comPos = nameServer.length();
90  }
91  nameServer = nameServer.substr(0, comPos);
92  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
93  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
94  CosNaming::NamingContext::_duplicate(naming.getRootContext())
95  )){
96  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "] in "
97  << m_profile.instance_name << std::endl;
98  return RTC::RTC_ERROR;
99  }
100 
101  // virtual_force_sensor: <name>, <base>, <target>, 0, 0, 0, 0, 0, 1, 0
102  coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
103  for(unsigned int i = 0; i < virtual_force_sensor.size()/10; i++ ){
104  std::string name = virtual_force_sensor[i*10+0];
106  p.base_name = virtual_force_sensor[i*10+1];
107  p.target_name = virtual_force_sensor[i*10+2];
108  hrp::dvector tr(7);
109  for (int j = 0; j < 7; j++ ) {
110  coil::stringTo(tr[j], virtual_force_sensor[i*10+3+j].c_str());
111  }
112  p.p = hrp::Vector3(tr[0], tr[1], tr[2]);
113  p.R = Eigen::AngleAxis<double>(tr[6], hrp::Vector3(tr[3],tr[4],tr[5])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
114  p.forceOffset = hrp::Vector3(0, 0, 0);
115  p.momentOffset = hrp::Vector3(0, 0, 0);
116  std::cerr << "[" << m_profile.instance_name << "] virtual force sensor : " << name << std::endl;
117  std::cerr << "[" << m_profile.instance_name << "] base : " << p.base_name << std::endl;
118  std::cerr << "[" << m_profile.instance_name << "] target : " << p.target_name << std::endl;
119  std::cerr << "[" << m_profile.instance_name << "] T, R : " << p.p[0] << " " << p.p[1] << " " << p.p[2] << std::endl << p.R << std::endl;
121  m_sensors[name] = p;
122  if ( m_sensors[name].path->numJoints() == 0 ) {
123  std::cerr << "[" << m_profile.instance_name << "] ERROR : Unknown link path " << m_sensors[name].base_name << " " << m_sensors[name].target_name << std::endl;
124  return RTC::RTC_ERROR;
125  }
126  }
127  int nforce = m_sensors.size();
128  m_force.resize(nforce);
129  m_forceOut.resize(nforce);
130  int i = 0;
131  std::map<std::string, VirtualForceSensorParam>::iterator it = m_sensors.begin();
132  while ( it != m_sensors.end() ) {
133  m_forceOut[i] = new OutPort<TimedDoubleSeq>((*it).first.c_str(), m_force[i]);
134  m_force[i].data.length(6);
135  registerOutPort((*it).first.c_str(), *m_forceOut[i]);
136  it++; i++;
137  }
138 
139  return RTC::RTC_OK;
140 }
141 
142 
143 
144 /*
145 RTC::ReturnCode_t VirtualForceSensor::onFinalize()
146 {
147  return RTC::RTC_OK;
148 }
149 */
150 
151 /*
152 RTC::ReturnCode_t VirtualForceSensor::onStartup(RTC::UniqueId ec_id)
153 {
154  return RTC::RTC_OK;
155 }
156 */
157 
158 /*
159 RTC::ReturnCode_t VirtualForceSensor::onShutdown(RTC::UniqueId ec_id)
160 {
161  return RTC::RTC_OK;
162 }
163 */
164 
166 {
167  std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
168  return RTC::RTC_OK;
169 }
170 
172 {
173  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
174  return RTC::RTC_OK;
175 }
176 
177 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
179 {
180  //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
181  static int loop = 0;
182  loop ++;
183 
185  RTC::Time tm;
186  tm.sec = coiltm.sec();
187  tm.nsec = coiltm.usec()*1000;
188 
189  if (m_qCurrentIn.isNew()) {
190  m_qCurrentIn.read();
191  }
192  if (m_tauInIn.isNew()) {
193  m_tauInIn.read();
194  }
195 
196  if ( m_qCurrent.data.length() == m_robot->numJoints() &&
197  m_tauIn.data.length() == m_robot->numJoints() ) {
198  // reference model
199  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
200  m_robot->joint(i)->q = m_qCurrent.data[i];
201  }
202  m_robot->calcForwardKinematics();
203  m_robot->calcCM();
204  m_robot->rootLink()->calcSubMassCM();
205 
206  std::map<std::string, VirtualForceSensorParam>::iterator it = m_sensors.begin();
207  int i = 0;
208  while ( it != m_sensors.end() ) {
209 
210  hrp::JointPathPtr path = (*it).second.path;
211  int n = path->numJoints();
212 
213  if ( DEBUGP ) {
214  std::cerr << " sensor name : " << (*it).first << std::endl;
215  std::cerr << "sensor torque : ";
216  for (int j = 0; j < n; j++) {
217  std::cerr << " " << m_tauIn.data[path->joint(j)->jointId] ;
218  }
219  std::cerr << std::endl;
220  }
221 
222  hrp::dvector force(6);
223  calcRawVirtualForce((*it).first, force);
224 
225  if ( DEBUGP ) {
226  std::cerr << " raw force : ";
227  for ( int j = 0; j < 6; j ++ ) {
228  std::cerr << " " << force[j] ;
229  }
230  std::cerr << std::endl;
231  }
232 
233  hrp::dvector force_p(3), force_r(3);
234  for ( int j = 0; j < 3; j ++ ) {
235  force_p[j] = force[j];
236  force_r[j] = force[j+3];
237  }
238  force_p = force_p - (*it).second.forceOffset;
239  force_r = force_r - (*it).second.momentOffset;
240  for ( int j = 0; j < 3; j ++ ) {
241  m_force[i].data[j+0] = force_p[j];
242  m_force[i].data[j+3] = force_r[j];
243  }
244 
245  if ( DEBUGP ) {
246  std::cerr << " output force : ";
247  for (int j = 0; j < 6; j++) {
248  std::cerr << " " << m_force[i].data[j];
249  }
250  std::cerr << std::endl;
251  }
252 
253  m_force[i].tm = tm; // put timestamp
254  m_forceOut[i]->write();
255 
256  it++; i++;
257  }
258  //
259 #if 0
260  (:calc-force-from-joint-torque
261  (limb all-torque &key (move-target (send self limb :end-coords)) (use-torso))
262  (let* ((link-list
263  (send self :link-list
264  (send move-target :parent)
265  (unless use-torso (car (send self limb :links)))))
266  (jacobian
267  (send self :calc-jacobian-from-link-list
268  link-list
269  :move-target move-target
270  :rotation-axis (list t)
271  :translation-axis (list t)))
272  (torque (instantiate float-vector (length link-list))))
273  (dotimes (i (length link-list))
274  (setf (elt torque i)
275  (elt all-torque (position (send (elt link-list i) :joint) (send self :joint-list)))))
276  (transform (send self :calc-inverse-jacobian (transpose jacobian))
277  torque)))
278 #endif
279 
280  }
281  return RTC::RTC_OK;
282 }
283 
284 /*
285 RTC::ReturnCode_t VirtualForceSensor::onAborting(RTC::UniqueId ec_id)
286 {
287  return RTC::RTC_OK;
288 }
289 */
290 
291 /*
292 RTC::ReturnCode_t VirtualForceSensor::onError(RTC::UniqueId ec_id)
293 {
294  return RTC::RTC_OK;
295 }
296 */
297 
298 /*
299 RTC::ReturnCode_t VirtualForceSensor::onReset(RTC::UniqueId ec_id)
300 {
301  return RTC::RTC_OK;
302 }
303 */
304 
305 /*
306 RTC::ReturnCode_t VirtualForceSensor::onStateUpdate(RTC::UniqueId ec_id)
307 {
308  return RTC::RTC_OK;
309 }
310 */
311 
312 /*
313 RTC::ReturnCode_t VirtualForceSensor::onRateChanged(RTC::UniqueId ec_id)
314 {
315  return RTC::RTC_OK;
316 }
317 */
318 
320 {
321  std::map<std::string, VirtualForceSensorParam>::iterator it;
322  for (it = m_sensors.begin(); it != m_sensors.end(); it++) {
323  if ((*it).first != sensorName){
324  continue;
325  } else {
326  hrp::JointPathPtr path = (*it).second.path;
327  hrp::dvector force(6);
328  if(!calcRawVirtualForce(sensorName, force)){
329  return false;
330  }
331  hrp::Vector3 force_p, force_r;
332  for ( int i = 0; i < 3; i ++ ) {
333  force_p[i] = force[i];
334  force_r[i] = force[i+3];
335  }
336  (*it).second.forceOffset = force_p;
337  (*it).second.momentOffset = force_r;
338  return true;
339  }
340  }
341  std::cerr << "removeVirtualForceSensorOffset: No sensor " << sensorName << std::endl;
342  return false;
343 }
344 
345 bool VirtualForceSensor::calcRawVirtualForce(std::string sensorName, hrp::dvector &outputForce)
346 {
347  std::map<std::string, VirtualForceSensorParam>::iterator it;
348  for (it = m_sensors.begin(); it != m_sensors.end(); ++it) {
349  if ((*it).first != sensorName){
350  continue;
351  } else {
352  hrp::JointPathPtr path = (*it).second.path;
353  int n = path->numJoints();
354  hrp::dmatrix J(6, n);
355  hrp::dmatrix Jtinv(6, n);
356  path->calcJacobian(J);
357  hrp::calcPseudoInverse(J.transpose(), Jtinv);
358  // use sr inverse of J.transpose()
359  // hrp::dmatrix Jt = J.transpose();
360  // double manipulability = sqrt((Jt*J).determinant());
361  // hrp::calcPseudoInverse((Jt * J + 0.1 * hrp::dmatrix::Identity(n,n)), Jtinv);
362  hrp::dvector torque(n);
363  hrp::dvector force(6);
364 
365  // get gear torque
366  for (int i = 0; i < n; i++) {
367  torque[i] = -m_tauIn.data[path->joint(i)->jointId]; // passive torque from external force
368  }
369 
370  // calc estimated force from torque vector
371  force = Jtinv * torque;
372  // force = J * torque;
373 
374  // trans to localcoords and set offset
375  hrp::dvector force_p(3), force_r(3);
376  for (int i = 0; i < 3; i++) {
377  force_p[i] = force[i];
378  force_r[i] = force[i + 3];
379  }
380  force_p = (*it).second.R.transpose() * path->endLink()->R.transpose() * force_p;
381  force_r = (*it).second.R.transpose() * path->endLink()->R.transpose() * force_r;
382 
383  outputForce.resize(6);
384  for(int i = 0; i < 3; i++) {
385  outputForce[i] = force_p[i];
386  outputForce[i + 3] = force_r[i];
387  }
388  return true;
389  }
390  }
391 
392  std::cerr << "calcVirtualForce: No sensor " << sensorName << std::endl;
393  return false;
394 
395 }
396 
397 extern "C"
398 {
399 
401  {
403  manager->registerFactory(profile,
404  RTC::Create<VirtualForceSensor>,
405  RTC::Delete<VirtualForceSensor>);
406  }
407 
408 };
409 
410 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
InPort< TimedDoubleSeq > m_tauInIn
Eigen::MatrixXd dmatrix
bool calcRawVirtualForce(std::string sensorName, hrp::dvector &outputForce)
bool stringTo(To &val, const char *str)
TimedDoubleSeq m_tauIn
png_infop png_charpp name
virtual ~VirtualForceSensor()
Destructor.
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
CORBA::ORB_ptr getORB()
png_bytep png_bytep png_size_t length
png_uint_32 i
Eigen::VectorXd dvector
dmatrix inverse(const dmatrix &M)
coil::Properties & getProperties()
static Manager & instance()
static const char * virtualforcesensor_spec[]
boost::shared_ptr< Body > BodyPtr
Eigen::Vector3d Vector3
#define DEBUGP
std::vector< std::string > vstring
int gettimeofday(struct timeval *tv, struct timezone *tz)
coil::Properties & getConfig()
std::vector< TimedDoubleSeq > m_force
ExecutionContextHandle_t UniqueId
bool removeVirtualForceSensorOffset(std::string sensorName)
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
null component
std::vector< OutPort< TimedDoubleSeq > * > m_forceOut
t
n
int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio)
path
prop
position
naming
InPort< TimedDoubleSeq > m_qCurrentIn
long int sec() const
virtual bool isNew()
virtual RTC::ReturnCode_t onInitialize()
bool addPort(PortBase &port)
long int usec() const
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
void vfsensor(VirtualForceSensor *i_vfsensor)
VirtualForceSensorService_impl m_service0
boost::shared_ptr< JointPath > JointPathPtr
RTC::CorbaPort m_VirtualForceSensorServicePort
VirtualForceSensor(RTC::Manager *manager)
Constructor.
bool addInPort(const char *name, InPortBase &inport)
void registerOutPort(const char *name, OutPortBase &outport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void VirtualForceSensorInit(RTC::Manager *manager)
TimedDoubleSeq m_qCurrent
std::map< std::string, VirtualForceSensorParam > m_sensors
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:21