iCub-main
iDynBody.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010-2011 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Serena Ivaldi, Matteo Fumagalli
4 * email: serena.ivaldi@iit.it, matteo.fumagalli@iit.it
5 * website: www.robotcub.org
6 * Permission is granted to copy, distribute, and/or modify this program
7 * under the terms of the GNU General Public License, version 2 or any
8 * later version published by the Free Software Foundation.
9 *
10 * A copy of the license can be found at
11 * http://www.robotcub.org/icub/license/gpl.txt
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16 * Public License for more details
17 */
18 
19 #include <cstdio>
20 #include <iostream>
21 #include <iomanip>
22 #include <cmath>
23 
24 #include <iCub/iDyn/iDyn.h>
25 #include <iCub/iDyn/iDynBody.h>
26 
27 using namespace std;
28 using namespace yarp::os;
29 using namespace yarp::sig;
30 using namespace yarp::math;
31 using namespace iCub::ctrl;
32 using namespace iCub::iKin;
33 using namespace iCub::iDyn;
34 using namespace iCub::skinDynLib;
35 
36 // #define DEBUG_FOOT_COM
37 //====================================
38 //
39 // RIGID BODY TRANSFORMATION
40 //
41 //====================================
42 
43 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
44 RigidBodyTransformation::RigidBodyTransformation(iDynLimb *_limb, const yarp::sig::Matrix &_H, const string &_info, bool _hasSensor, const FlowType kin, const FlowType wre, const NewEulMode _mode, unsigned int verb)
45 {
46  limb = _limb;
47  kinFlow = kin;
48  wreFlow = wre;
49  mode = _mode;
50  info = _info;
51  hasSensor=_hasSensor;
52  F.resize(3); F.zero();
53  Mu.resize(3); Mu.zero();
54  w.resize(3); w.zero();
55  dw.resize(3); dw.zero();
56  ddp.resize(3); ddp.zero();
57  H.resize(4,4); H.eye();
58  setRBT(_H);
59 }
60 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
61 RigidBodyTransformation::~RigidBodyTransformation()
62 {
63  // never delete the limb!! only stop pointing at it
64  limb = NULL;
65 }
66 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
67 bool RigidBodyTransformation::setRBT(const Matrix &_H)
68 {
69  if((_H.cols()==4)&&(_H.rows()==4))
70  {
71  H=_H;
72  return true;
73  }
74  else
75  {
76  if(verbose) fprintf(stderr,"RigidBodyTransformation: could not set RBT due to wrong sized matrix H: %zu,%zu instead of (4,4). Setting identity as default. \n",_H.cols(),_H.rows());
77  H.resize(4,4);
78  H.eye();
79  return false;
80  }
81 }
82 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
83 bool RigidBodyTransformation::setKinematic(const Vector &wNode, const Vector &dwNode, const Vector &ddpNode)
84 {
85  // set the RBT kinematic variables to the ones of the node
86  w = wNode;
87  dw = dwNode;
88  ddp = ddpNode;
89  //now apply the RBT transformation
90  computeKinematic();
91  // send the kinematic information to the limb - the limb knows whether it is on base or on end-eff
92  return limb->initKinematicNewtonEuler(w,dw,ddp);
93 }
94 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
95 bool RigidBodyTransformation::setKinematicMeasure(const Vector &w0, const Vector &dw0, const Vector &ddp0)
96 {
97  return limb->initKinematicNewtonEuler(w0,dw0,ddp0);
98 }
99 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
100 bool RigidBodyTransformation::setWrench(const Vector &FNode, const Vector &MuNode)
101 {
102  //set the RBT force/moment with the one of the node
103  F = FNode;
104  Mu = MuNode;
105  // now apply the RBT transformation
106  computeWrench();
107  // send the wrench to the limb - the limb knows whether it is on base or on end-eff
108  return limb->initWrenchNewtonEuler(F,Mu);
109 }
110 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
111 bool RigidBodyTransformation::setWrenchMeasure(const Vector &F0, const Vector &Mu0)
112 {
113  return limb->initWrenchNewtonEuler(F0,Mu0);
114 }
115 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
116 bool RigidBodyTransformation::setWrenchMeasure(iDynSensor *sensor, const Vector &Fsens, const Vector &Musens)
117 {
118  return sensor->setSensorMeasures(Fsens,Musens);
119 }
120 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
121 Matrix RigidBodyTransformation::getRBT() const
122 {
123  return H;
124 }
125 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
126 Matrix RigidBodyTransformation::getR()
127 {
128  return H.submatrix(0,2,0,2);
129 }
130 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
131 Matrix RigidBodyTransformation::getR6() const
132 {
133  Matrix R(6,6); R.zero();
134  for(int i=0;i<3;i++)
135  {
136  for(int j=0;j<3;j++)
137  {
138  R(i,j) = H(i,j);
139  R(i+3,j+3) = R(i,j);
140  }
141  }
142  return R;
143 }
144 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
145 Vector RigidBodyTransformation::getr(bool proj)
146 {
147  if(proj==false)
148  return H.submatrix(0,2,0,3).getCol(3);
149  return getR().transposed() * H.getCol(3).subVector(0,2);
150 }
151 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
152 void RigidBodyTransformation::getKinematic(Vector &wNode, Vector &dwNode, Vector &ddpNode)
153 {
154  //read w,dw,ddp from the limb and stores them into the RBT variables
155  limb->getKinematicNewtonEuler(w,dw,ddp);
156 
157  //now compute according to transformation
158  computeKinematic();
159 
160  //apply the kinematics computations to the node
161  wNode = w;
162  dwNode = dw;
163  ddpNode = ddp;
164 }
165 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
166 void RigidBodyTransformation::getWrench(Vector &FNode, Vector &MuNode)
167 {
168  //read F,Mu from the limb and stores them into the RBT variables
169  limb->getWrenchNewtonEuler(F,Mu);
170 
171  //now compute according to transformation
172  computeWrench();
173 
174  //apply the wrench computations to the node
175  FNode = FNode + F;
176  MuNode = MuNode + Mu;
177 }
178 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
179 void RigidBodyTransformation::setInfoFlow(const FlowType kin, const FlowType wre)
180 {
181  kinFlow=kin;
182  wreFlow=wre;
183 }
184 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
185 FlowType RigidBodyTransformation::getKinematicFlow() const
186 {
187  return kinFlow;
188 }
189 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
190 FlowType RigidBodyTransformation::getWrenchFlow() const
191 {
192  return wreFlow;
193 }
194 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
195 void RigidBodyTransformation::computeKinematic()
196 {
197  if(this->kinFlow== RBT_NODE_IN)
198  {
199  // note: these computations are similar to the Backward ones in
200  // OneLinkNewtonEuler: compute AngVel/AngAcc/LinAcc Backward
201  // but here are fastened and adapted to the RBT
202  // note: w,dw,ddp are already set with the ones coming from the LIMB
203 
204  switch(mode)
205  {
206  case DYNAMIC:
208  case DYNAMIC_W_ROTOR:
209  ddp = getR() * ( ddp - cross(dw,getr(true)) - cross(w,cross(w,getr(true))) ) ;
210  w = getR() * w ;
211  dw = getR() * dw ;
212  break;
213  case STATIC:
214  w = 0.0;
215  dw = 0.0;
216  ddp = getR() * ddp;
217  break;
218  }
219  }
220  else
221  {
222  // note: these computations are similar to the Forward ones in
223  // OneLinkNewtonEuler: compute AngVel/AngAcc/LinAcc
224  // but here are fastened and adapted to the RBT
225  // note: w,dw,ddp are already set with the ones coming from the NODE
226 
227  switch(mode)
228  {
229  case DYNAMIC:
231  case DYNAMIC_W_ROTOR:
232  w = getR().transposed() * w ;
233  dw = getR().transposed() * dw;
234  ddp = getR().transposed() * (ddp) + cross(dw,getr(true)) + cross(w,cross(w,getr(true))) ;
235  break;
236  case STATIC:
237  w = 0.0;
238  dw = 0.0;
239  ddp = getR().transposed() * ddp ;
240  break;
241  }
242 
243  }
244 }
245 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
246 void RigidBodyTransformation::computeWrench()
247 {
248  if(this->wreFlow== RBT_NODE_IN)
249  {
250  // note: these computations are similar to the Backward ones in
251  // OneLinkNewtonEuler: compute Force/Moment Backward
252  // but here are fastened and adapted to the RBT
253  // note: no switch(mode) is necessary because all modes have the same formula
254  // note: F,Mu are already set with the ones coming from the LIMB
255 
256  Mu = cross( getr(), getR()*F ) + getR() * Mu ;
257  F = getR() * F;
258  }
259  else
260  {
261  // note: these computations are similar to the Forward ones in
262  // OneLinkNewtonEuler: compute Force/Moment Forward
263  // but here are fastened and adapted to the RBT
264  // note: no switch(mode) is necessary because all modes have the same formula
265  // note: F,Mu are already set with the ones coming from the NODE
266 
267  Mu = getR().transposed() * ( Mu - cross(getr(),getR() * F ));
268  F = getR().transposed() * F ;
269 
270  }
271 }
272 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
273 bool RigidBodyTransformation::isSensorized() const
274 {
275  return hasSensor;
276 }
277 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
278 void RigidBodyTransformation::computeLimbKinematic()
279 {
280  limb->computeKinematicNewtonEuler();
281 }
282 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
283 void RigidBodyTransformation::computeLimbWrench()
284 {
285  limb->computeWrenchNewtonEuler();
286 }
287 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
288 unsigned int RigidBodyTransformation::getNLinks() const
289 {
290  return limb->getN();
291 }
292 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
293 unsigned int RigidBodyTransformation::getDOF() const
294 {
295  return limb->getDOF();
296 }
297 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
298 Matrix RigidBodyTransformation::getH(const unsigned int iLink, const bool allLink)
299 {
300  return limb->getH(iLink,allLink);
301 }
302 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
303 Matrix RigidBodyTransformation::getH()
304 {
305  return limb->getH();
306 }
307 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
308 Vector RigidBodyTransformation::getEndEffPose(const bool axisRep)
309 {
310  return limb->EndEffPose(axisRep);
311 }
312 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
313 Matrix RigidBodyTransformation::computeGeoJacobian(const unsigned int iLink, const Matrix &Pn, bool rbtRoto)
314 {
315  if(rbtRoto==false)
316  return limb->computeGeoJacobian(iLink,Pn);
317  else
318  return getR6() * limb->computeGeoJacobian(iLink,Pn);
319 }
320 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
321 Matrix RigidBodyTransformation::computeGeoJacobian(const unsigned int iLink, const Matrix &Pn, const Matrix &H0, bool rbtRoto)
322 {
323  if(rbtRoto==false)
324  return limb->computeGeoJacobian(iLink,Pn,H0);
325  else
326  return getR6() * limb->computeGeoJacobian(iLink,Pn,H0);
327 }
328 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
329 Matrix RigidBodyTransformation::computeGeoJacobian(const Matrix &Pn, bool rbtRoto)
330 {
331  if(rbtRoto==false)
332  return limb->computeGeoJacobian(Pn);
333  else
334  return getR6() * limb->computeGeoJacobian(Pn);
335 }
336 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
337 Matrix RigidBodyTransformation::computeGeoJacobian(const Matrix &Pn, const Matrix &H0, bool rbtRoto)
338 {
339  if(rbtRoto==false)
340  return limb->computeGeoJacobian(Pn,H0);
341  else
342  return getR6() * limb->computeGeoJacobian(Pn,H0);
343 }
344 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
345 Matrix RigidBodyTransformation::computeGeoJacobian(bool rbtRoto)
346 {
347  if(rbtRoto==false)
348  return limb->GeoJacobian();
349  else
350  return getR6() * limb->GeoJacobian();
351 }
352 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
353 Matrix RigidBodyTransformation::computeGeoJacobian(const unsigned int iLink, bool rbtRoto)
354 {
355  if(rbtRoto==false)
356  return limb->GeoJacobian(iLink);
357  else
358  return getR6() * limb->GeoJacobian(iLink);
359 }
360 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
361 Matrix RigidBodyTransformation::getH0() const
362 {
363  return limb->getH0();
364 }
365 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
366 bool RigidBodyTransformation::setH0(const Matrix &_H0)
367 {
368  return limb->setH0(_H0);
369 }
370 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
371 
372  //---------------
373  // jacobians COM
374  //---------------
375 
376 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
377 Matrix RigidBodyTransformation::TESTING_computeCOMJacobian(const unsigned int iLink, bool rbtRoto)
378 {
379  if(rbtRoto==false)
380  return limb->TESTING_computeCOMJacobian(iLink);
381  else
382  return getR6() * limb->TESTING_computeCOMJacobian(iLink);
383 }
384 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
385 Matrix RigidBodyTransformation::TESTING_computeCOMJacobian(const unsigned int iLink, const Matrix &Pn, bool rbtRoto)
386 {
387  if(rbtRoto==false)
388  return limb->TESTING_computeCOMJacobian(iLink, Pn);
389  else
390  return getR6() * limb->TESTING_computeCOMJacobian(iLink, Pn);
391 }
392 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
393 Matrix RigidBodyTransformation::TESTING_computeCOMJacobian(const unsigned int iLink, const Matrix &Pn, const Matrix &_H0, bool rbtRoto)
394 {
395  if(rbtRoto==false)
396  return limb->TESTING_computeCOMJacobian(iLink, Pn, _H0);
397  else
398  return getR6() * limb->TESTING_computeCOMJacobian(iLink, Pn, _H0);
399 }
400 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
401 Matrix RigidBodyTransformation::getHCOM(unsigned int iLink)
402 {
403  return limb->getHCOM(iLink);
404 }
405 
406 
407 
408 
409 //====================================
410 //
411 // i DYN NODE
412 //
413 //====================================
414 
415 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
416 iDynNode::iDynNode(const NewEulMode _mode)
417 {
418  rbtList.clear();
419  mode = _mode;
420  verbose = iCub::skinDynLib::VERBOSE;
421  zero();
422 }
423 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
424 iDynNode::iDynNode(const string &_info, const NewEulMode _mode, unsigned int verb)
425 {
426  info=_info;
427  rbtList.clear();
428  mode = _mode;
429  verbose = verb;
430  zero();
431 }
432 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
434 {
435  w.resize(3); w.zero();
436  dw.resize(3); dw.zero();
437  ddp.resize(3); ddp.zero();
438  F.resize(3); F.zero();
439  Mu.resize(3); Mu.zero();
440 }
441 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
442 void iDynNode::addLimb(iDynLimb *limb, const Matrix &H, const FlowType kinFlow, const FlowType wreFlow, bool hasSensor)
443 {
444  string infoRbt = limb->getType() + " to node";
445  RigidBodyTransformation rbt(limb,H,infoRbt,hasSensor,kinFlow,wreFlow,mode,verbose);
446  rbtList.push_back(rbt);
447 }
448 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
449 Matrix iDynNode::getRBT(unsigned int iLimb) const
450 {
451  if(iLimb<rbtList.size())
452  {
453  return rbtList[iLimb].getRBT();
454  }
455  else
456  {
457  if(verbose) fprintf(stderr,"iDynNode: error, could not getRBT() due to out of range index: %d , while we have %d limbs. \n", iLimb, (int)rbtList.size() );
458  return Matrix(0,0);
459  }
460 }
461 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
462 bool iDynNode::solveKinematics()
463 {
464  unsigned int inputNode=0;
465 
466  //first find the limb (one!) which must get the measured kinematics data
467  // e.g. the head gets this information from the inertial sensor on the head
468  for(unsigned int i=0; i<rbtList.size(); i++)
469  {
470  if(rbtList[i].getKinematicFlow()==RBT_NODE_IN)
471  {
472  // measures are already set
473  //then compute the kinematics pass in that limb
474  rbtList[i].computeLimbKinematic();
475  // and retrieve the kinematics data in the base/end
476  rbtList[i].getKinematic(w,dw,ddp);
477  //check
478  inputNode++;
479  }
480  }
481 
482  //just check if the input node is only one (as it should be)
483  if(inputNode==1)
484  {
485  //now forward the kinematic input from limbs whose kinematic flow is input type
486  for(unsigned int i=0; i<rbtList.size(); i++)
487  {
488  if(rbtList[i].getKinematicFlow()==RBT_NODE_OUT)
489  {
490  //init the kinematics with the node information
491  rbtList[i].setKinematic(w,dw,ddp);
492  //solve kinematics in that limb/chain
493  rbtList[i].computeLimbKinematic();
494  }
495  }
496  return true;
497 
498  }
499  else
500  {
501  if(verbose)
502  {
503  fprintf(stderr,"iDynNode error: there are %d limbs with Kinematic Flow = Input. Only one limb must have Kinematic Input from outside measurements/computations. \n",inputNode);
504  fprintf(stderr,"Please check the coherence of the limb configuration in the node <%s> \n",info.c_str());
505  }
506  return false;
507  }
508 
509 }
510 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
511 bool iDynNode::solveKinematics(const Vector &w0, const Vector &dw0, const Vector &ddp0)
512 {
513  unsigned int inputNode=0;
514 
515  //first find the limb (one!) which must get the measured kinematics data
516  // e.g. the head gets this information from the inertial sensor on the head
517  for(unsigned int i=0; i<rbtList.size(); i++)
518  {
519  if(rbtList[i].getKinematicFlow()==RBT_NODE_IN)
520  {
521  rbtList[i].setKinematicMeasure(w0,dw0,ddp0);
522  //then compute the kinematics pass in that limb
523  rbtList[i].computeLimbKinematic();
524  // and retrieve the kinematics data in the base/end
525  rbtList[i].getKinematic(w,dw,ddp);
526  //check
527  inputNode++;
528  }
529  }
530 
531  //just check if the input node is only one (as it should be)
532  if(inputNode==1)
533  {
534  //now forward the kinematic input from limbs whose kinematic flow is input type
535  for(unsigned int i=0; i<rbtList.size(); i++)
536  {
537  if(rbtList[i].getKinematicFlow()==RBT_NODE_OUT)
538  {
539  //init the kinematics with the node information
540  rbtList[i].setKinematic(w,dw,ddp);
541  //solve kinematics in that limb/chain
542  rbtList[i].computeLimbKinematic();
543  }
544  }
545  return true;
546 
547  }
548  else
549  {
550  if(verbose)
551  {
552  fprintf(stderr,"iDynNode: error: there are %d limbs with Kinematic Flow = Input. ",inputNode);
553  fprintf(stderr," Only one limb must have Kinematic Input from outside measurements/computations. ");
554  fprintf(stderr,"Please check the coherence of the limb configuration in the node <%s> \n", info.c_str());
555  }
556  return false;
557  }
558 
559 }
560 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
561 bool iDynNode::setKinematicMeasure(const Vector &w0, const Vector &dw0, const Vector &ddp0)
562 {
563  if( (w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3))
564  {
565  // find the limb (one!) which must get the measured kinematics data
566  // e.g. the head gets this information from the inertial sensor on the head
567  for(unsigned int i=0; i<rbtList.size(); i++)
568  {
569  if(rbtList[i].getKinematicFlow()==RBT_NODE_IN)
570  {
571  rbtList[i].setKinematicMeasure(w0,dw0,ddp0);
572  return true;
573  }
574  }
575  if(verbose)
576  fprintf(stderr,"iDynNode: error, there is not a node to set kinematic measure! \n");
577  return false;
578  }
579  else
580  {
581  if(verbose)
582  {
583  fprintf(stderr,"iDynNode: error, could not set Kinematic measures due to wrong sized vectors: \n");
584  fprintf(stderr," w,dw,ddp have length %d,%d,%d instead of 3,3,3. \n",(int)w0.length(),(int)dw0.length(),(int)ddp0.length());
585  }
586  return false;
587  }
588 }
589 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
590 bool iDynNode::solveWrench()
591 {
592  unsigned int outputNode = 0;
593  F.zero(); Mu.zero();
594 
595  //first get the forces/moments from each limb
596  //assuming that each limb has been properly set with the outcoming measured
597  //forces/moments which are necessary for the wrench computation
598  for(unsigned int i=0; i<rbtList.size(); i++)
599  {
600  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
601  {
602  //compute the wrench pass in that limb
603  rbtList[i].computeLimbWrench();
604  //update the node force/moment with the wrench coming from the limb base/end
605  // note that getWrench sum the result to F,Mu - because they are passed by reference
606  // F = F + F[i], Mu = Mu + Mu[i]
607  rbtList[i].getWrench(F,Mu);
608  //check
609  outputNode++;
610  }
611  }
612 
613  // node summation: already performed by each RBT
614  // F = F + F[i], Mu = Mu + Mu[i]
615 
616  // at least one output node should exist
617  // however if for testing purposes only one limb is attached to the node,
618  // we can't avoid the computeWrench phase, but still we must remember that
619  // it is not correct, because the node must be in balance
620  if(outputNode==rbtList.size())
621  {
622  if(verbose>1)
623  {
624  fprintf(stderr,"iDynNode: warning: there are no limbs with Wrench Flow = Output. ");
625  fprintf(stderr," At least one limb must have Wrench Output for balancing forces in the node. ");
626  fprintf(stderr,"Please check the coherence of the limb configuration in the node <%s> \n",info.c_str());
627  }
628  }
629 
630  //now forward the wrench output from the node to limbs whose wrench flow is output type
631  for(unsigned int i=0; i<rbtList.size(); i++)
632  {
633  if(rbtList[i].getWrenchFlow()==RBT_NODE_OUT)
634  {
635  //init the wrench with the node information
636  rbtList[i].setWrench(F,Mu);
637  //solve wrench in that limb/chain
638  rbtList[i].computeLimbWrench();
639  }
640  }
641  return true;
642 }
643 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
644 bool iDynNode::solveWrench(const Matrix &FM)
645 {
646  bool inputWasOk = setWrenchMeasure(FM);
647  //now that all is set, we can really solveWrench()
648  return ( solveWrench() && inputWasOk );
649 }
650 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
651 bool iDynNode::solveWrench(const Matrix &Fm, const Matrix &Mm)
652 {
653  bool inputWasOk = setWrenchMeasure(Fm,Mm);
654  //now that all is set, we can really solveWrench()
655  return ( solveWrench() && inputWasOk );
656 }
657 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
658 bool iDynNode::setWrenchMeasure(const Matrix &FM)
659 {
660  Vector fi(3); fi.zero();
661  Vector mi(3); mi.zero();
662  Vector FMi(6);FMi.zero();
663  bool inputWasOk = true;
664 
665  //check how many limbs have wrench input
666  int inputNode = howManyWrenchInputs(false);
667 
668  // input (eg measured) wrenches are stored in a 6xN matrix: each column is a 6x1 vector
669  // with force/moment; N is the number of columns, ie the number of measured/input wrenches to the limb
670  // the order is assumed coherent with the one built when adding limbs
671  // eg:
672  // adding limbs: addLimb(limb1), addLimb(limb2), addLimb(limb3)
673  // where limb1, limb3 have wrench flow input
674  // passing wrenches: Matrix FM(6,2), FM.setcol(0,fm1), FM.setcol(1,fm3)
675  if(FM.cols()<inputNode)
676  {
677  if(verbose)
678  {
679  fprintf(stderr,"iDynNode: could not solveWrench due to missing wrenches to initialize the computations: ");
680  fprintf(stderr," only %zu f/m available instead of %d. Using default values, all zero. \n ",FM.cols(),inputNode);
681  }
682  inputWasOk = false;
683  }
684  if(FM.rows()!=6)
685  {
686  if(verbose)
687  {
688  fprintf(stderr,"iDynNode: could not solveWrench due to wrong sized init wrenches: ");
689  fprintf(stderr," %zu instead of 6 (3+3). Using default values, all zero. \n",FM.rows());
690  }
691  inputWasOk = false;
692  }
693 
694  //set the measured/input forces/moments from each limb
695  if(inputWasOk)
696  {
697  inputNode = 0;
698  for(unsigned int i=0; i<rbtList.size(); i++)
699  {
700  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
701  {
702  // from the input matrix - read the input wrench
703  FMi = FM.getCol(inputNode);
704  fi[0]=FMi[0];fi[1]=FMi[1];fi[2]=FMi[2];
705  mi[0]=FMi[3];mi[1]=FMi[4];mi[2]=FMi[5];
706  inputNode++;
707  //set the input wrench in the RBT->limb
708  rbtList[i].setWrenchMeasure(fi,mi);
709  }
710  }
711  }
712  else
713  {
714  // default zero values if inputs are wrong sized
715  for(unsigned int i=0; i<rbtList.size(); i++)
716  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
717  rbtList[i].setWrenchMeasure(fi,mi);
718  }
719 
720  //now that all is set, we can really solveWrench()
721  return inputWasOk;
722 }
723 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
724 bool iDynNode::setWrenchMeasure(const Matrix &Fm, const Matrix &Mm)
725 {
726  bool inputWasOk = true;
727 
728  //check how many limbs have wrench input
729  int inputNode = howManyWrenchInputs(false);
730 
731  // input (eg measured) wrenches are stored in two 3xN matrix: each column is a 3x1 vector
732  // with force/moment; N is the number of columns, ie the number of measured/input wrenches to the limb
733  // the order is assumed coherent with the one built when adding limbs
734  // eg:
735  // adding limbs: addLimb(limb1), addLimb(limb2), addLimb(limb3)
736  // where limb1, limb3 have wrench flow input
737  // passing wrenches: Matrix Fm(3,2), Fm.setcol(0,f1), Fm.setcol(1,f3) and similar for moment
738  if((Fm.cols()<inputNode)||(Mm.cols()<inputNode))
739  {
740  if(verbose)
741  {
742  fprintf(stderr,"iDynNode: could not setWrenchMeasure due to missing wrenches to initialize the computations: ");
743  fprintf(stderr," only %zu/%zu f/m available instead of %d/%d. Using default values, all zero. \n",Fm.cols(),Mm.cols(),inputNode,inputNode);
744  }
745  inputWasOk = false;
746  }
747  if((Fm.rows()!=3)||(Mm.rows()!=3))
748  {
749  if(verbose)
750  {
751  fprintf(stderr,"iDynNode: could not setWrenchMeasure due to wrong sized init f/m: ");
752  fprintf(stderr," %zu/%zu instead of 3/3. Using default values, all zero. \n",Fm.rows(),Mm.rows());
753  }
754  inputWasOk = false;
755  }
756 
757  //set the measured/input forces/moments from each limb
758  if(inputWasOk)
759  {
760  inputNode = 0;
761  for(unsigned int i=0; i<rbtList.size(); i++)
762  {
763  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
764  {
765  // from the input matrix
766  //set the input wrench in the RBT->limb
767  rbtList[i].setWrenchMeasure(Fm.getCol(inputNode),Mm.getCol(inputNode));
768  inputNode++;
769  }
770  }
771  }
772  else
773  {
774  // default zero values if inputs are wrong sized
775  Vector fi(3), mi(3); fi.zero(); mi.zero();
776  for(unsigned int i=0; i<rbtList.size(); i++)
777  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
778  rbtList[i].setWrenchMeasure(fi,mi);
779  }
780 
781  //now that all is set, we can really solveWrench()
782  return inputWasOk ;
783 }
784 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
785 unsigned int iDynNode::howManyWrenchInputs(bool afterAttach) const
786 {
787  unsigned int inputNode = 0;
788 
789  //check how many limbs have wrench input
790  for(unsigned int i=0; i<rbtList.size(); i++)
791  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
792  inputNode++;
793 
794  // if an attach has been done, we already set one wrench measure, so we don't have to do it again
795  // and we expect FM (or F,M) to be smaller of 1
796  if(afterAttach==true) inputNode--;
797 
798  return inputNode;
799 }
800 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
801 unsigned int iDynNode::howManyKinematicInputs(bool afterAttach) const
802 {
803  unsigned int inputNode = 0;
804 
805  //check how many limbs have kinematic input
806  for(unsigned int i=0; i<rbtList.size(); i++)
807  if(rbtList[i].getKinematicFlow()==RBT_NODE_IN)
808  inputNode++;
809 
810  // if an attach has been done, we already set one kinematic measure, so we don't have to do it again
811  // actually we expect the number to be zero, because only one limb has kinematic measures
812  if(afterAttach==true) inputNode--;
813 
814  return inputNode;
815 }
816 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
817 Vector iDynNode::getForce() const { return F;}
818 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
819 Vector iDynNode::getMoment() const {return Mu;}
820 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
821 Vector iDynNode::getAngVel() const {return w;}
822 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
823 Vector iDynNode::getAngAcc() const {return dw;}
824 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
825 Vector iDynNode::getLinAcc() const {return ddp;}
826 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
827 
828  //-----------------
829  // jacobians
830  //-----------------
831 
832 
833 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
834 Matrix iDynNode::computeJacobian(unsigned int iChain)
835 {
836  if(iChain<=rbtList.size())
837  {
838  return rbtList[iChain].computeGeoJacobian(false);
839  }
840  else
841  {
842  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to out of range index: input limb has index %d > %d. Returning a null matrix. \n",iChain,(int)rbtList.size());
843  return Matrix(0,0);
844  }
845 }
846 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
847 Matrix iDynNode::computeJacobian(unsigned int iChain, unsigned int iLink)
848 {
849  if(iChain<=rbtList.size())
850  {
851  // the check on iLink<=N is performed by iKinChain when calling GeoJacobian(iLink)
852  // from the RBT
853  return rbtList[iChain].computeGeoJacobian(iLink,false);
854  }
855  else
856  {
857  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to out of range index: input limb has index %d > %d. Returning a null matrix. \n",iChain,(int)rbtList.size());
858  return Matrix(0,0);
859  }
860 }
861 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
862 Matrix iDynNode::computeJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB)
863 {
864  //first check param coherence:
865  // - wrong limb index
866  if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
867  {
868  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix. \n",iChainA,iChainB,(int)rbtList.size());
869  return Matrix(0,0);
870  }
871  // - jacobian .. in the same limb @_@
872  if( iChainA==iChainB )
873  {
874  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to weird index for chains %d: same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix. \n",iChainA);
875  return Matrix(0,0);
876  }
877 
878  // params are ok, go on..
879 
880  // total number of joints = Ndof_A + Ndof_B
881  // the total jacobian matrix
882  Matrix J(6,rbtList[iChainA].getDOF() + rbtList[iChainB].getDOF()); J.zero();
883  //the vector from the base-link (for the jac.) of limb A to the end-link (for the jac.) of limb B
884  Matrix Pn;
885  // the two jacobians
886  Matrix J_A; Matrix J_B;
887  // from base-link (for the jac.) of limb A to base (for the jac.) of limb B
888  Matrix H_A_Node;
889 
890  // compute the roto-transf matrix between the base of limb A and the base of limb B
891  // this is used to set the correct reference of vector Pn
892  // H_A_Node is used to initialize J_B properly
893  compute_Pn_HAN(iChainA, dirA, iChainB, dirB, Pn, H_A_Node);
894 
895  // now compute jacobian of first and second limb, setting the correct Pn
896  // JA
897  J_A = rbtList[iChainA].computeGeoJacobian(Pn,false);
898  // JB
899  // note: set H_A_node as the H0 of the B chain, but does not modify the chain original H0
900  J_B = rbtList[iChainB].computeGeoJacobian(Pn,H_A_Node,false);
901 
902  // finally start building the Jacobian J=[J_A|J_B]
903  unsigned int c=0;
904  unsigned int JAcols = J_A.cols();
905  unsigned int JBcols = J_B.cols();
906 
907  if(JAcols+JBcols!=J.cols())
908  {
909  fprintf(stderr,"iDynNode error: Jacobian should be 6x%zu instead is 6x%d \n",J.cols(),(JAcols+JBcols));
910  fprintf(stderr,"Note: limb A: N=%d DOF=%d \n",rbtList[iChainA].getNLinks(),rbtList[iChainA].getDOF());
911  fprintf(stderr,"Note: limb B: N=%d DOF=%d \n",rbtList[iChainB].getNLinks(),rbtList[iChainB].getDOF());
912  J.resize(6,JAcols+JBcols);
913  }
914 
915  for(unsigned int r=0; r<6; r++)
916  {
917  for(c=0;c<JAcols;c++)
918  J(r,c)=J_A(r,c);
919  for(c=0;c<JBcols;c++)
920  J(r,JAcols+c)=J_B(r,c);
921  }
922 
923  // now return the jacobian
924  return J;
925 }
926 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
927 Matrix iDynNode::computeJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB)
928 {
929  //first check param coherence:
930  // - wrong limb index
931  if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
932  {
933  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix.\n",iChainA,iChainB,(int)rbtList.size());
934  return Matrix(0,0);
935  }
936  // - jacobian .. in the same limb @_@
937  if( iChainA==iChainB )
938  {
939  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to weird index for chains %d : same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix.\n",iChainA);
940  return Matrix(0,0);
941  }
942  // - there's not a link with index iLink in that chain
943  if( iLinkB >= rbtList[iChainB].getNLinks())
944  {
945  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to out of range index for chain %d: the selected link %d > %d. Returning a null matrix. \n",iChainB,iLinkB,rbtList[iChainB].getNLinks());
946  return Matrix(0,0);
947  }
948 
949  // params are ok, go on..
950 
951  // total number of joints = Ndof_A + iLinkB
952  // note that we are taking all the links in chainB, not only the DOF until iLinkB
953  // the total jacobian matrix
954  Matrix J(6,rbtList[iChainA].getDOF() + iLinkB ); J.zero();
955  //the vector from the base-link (for the jac.) of limb A to the end-link (for the jac.) of limb B
956  Matrix Pn;
957  // the two jacobians
958  Matrix J_A; Matrix J_B;
959  // from base-link (for the jac.) of limb A to base (for the jac.) of limb B
960  Matrix H_A_Node;
961 
962  // compute the roto-transf matrix between the base of limb A and the base of limb B
963  // this is used to set the correct reference of vector Pn
964  // H_A_Node is used to initialize J_B properly
965  compute_Pn_HAN(iChainA, dirA, iChainB, iLinkB, dirB, Pn, H_A_Node);
966 
967  // now compute jacobian of first and second limb, setting the correct Pn
968  // JA
969  J_A = rbtList[iChainA].computeGeoJacobian(Pn,false);
970  // JB
971  // note: set H_A_node as the H0 of the B chain, but does not modify the chain original H0
972  J_B = rbtList[iChainB].computeGeoJacobian(iLinkB,Pn,H_A_Node,false);
973 
974  // finally start building the Jacobian J=[J_A|J_B]
975  unsigned int c=0;
976  unsigned int JAcols = J_A.cols();
977  unsigned int JBcols = J_B.cols();
978 
979  if(JAcols+JBcols!=J.cols())
980  {
981  fprintf(stderr,"iDynNode error: Jacobian should be 6x%zu instead is 6x%d \n",J.cols(),(JAcols+JBcols));
982  fprintf(stderr,"Note: limb A: N=%d DOF=%d \n",rbtList[iChainA].getNLinks(),rbtList[iChainA].getDOF());
983  fprintf(stderr,"Note: limb B: N=%d DOF=%d iLinkB=%d \n",rbtList[iChainB].getNLinks(),rbtList[iChainB].getDOF(),iLinkB);
984  J.resize(6,JAcols+JBcols);
985  }
986 
987  for(unsigned int r=0; r<6; r++)
988  {
989  for(c=0;c<JAcols;c++)
990  J(r,c)=J_A(r,c);
991  for(c=0;c<JBcols;c++)
992  J(r,JAcols+c)=J_B(r,c);
993  }
994 
995  // now return the jacobian
996  return J;
997 }
998 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
999 void iDynNode::compute_Pn_HAN(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, Matrix &Pn, Matrix &H_A_Node)
1000 {
1001  // compute the roto-transf matrix between the base of limb A and the base of limb B
1002  // this is used to set the correct reference of vector Pn
1003  if(dirA==JAC_KIN)
1004  {
1005  // H_A_Node = H_A * RBT_A^T * RBT_B
1006  // note: RBT_A is transposed because we're going in the opposite direction wrt to one of the RBT
1007  H_A_Node = rbtList[iChainA].getH() * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1008  }
1009  else //dirA==JAC_IKIN
1010  {
1011  // H_A_Node = H_A^-1 * RBT_A^T * RBT_B
1012  H_A_Node = SE3inv(rbtList[iChainA].getH()) * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1013  }
1014 
1015  if(dirB==JAC_KIN)
1016  {
1017  // Pn = H_A_Node * H_B
1018  // Pn is the roto-transf matrix between base (of jac. in limb A) and end (of jac. in limb B)
1019  // it is needed because the two jacobians must refer to a common Pn
1020  Pn = H_A_Node * rbtList[iChainB].getH();
1021  }
1022  else //dirB==JAC_IKIN
1023  {
1024  // Pn = H_A_Node * H_B^-1
1025  Pn = H_A_Node * SE3inv(rbtList[iChainB].getH());
1026  }
1027 }
1028 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1029 void iDynNode::compute_Pn_HAN(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, Matrix &Pn, Matrix &H_A_Node)
1030 {
1031  // compute the roto-transf matrix between the base of limb A and the base of limb B
1032  // this is used to set the correct reference of vector Pn
1033  if(dirA==JAC_KIN)
1034  {
1035  // H_A_Node = H_A * RBT_A^T * RBT_B
1036  // note: RBT_A is transposed because we're going in the opposite direction wrt to one of the RBT
1037  H_A_Node = rbtList[iChainA].getH() * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1038  }
1039  else //dirA==JAC_IKIN
1040  {
1041  // H_A_Node = H_A^-1 * RBT_A^T * RBT_B
1042  H_A_Node = SE3inv(rbtList[iChainA].getH()) * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1043  }
1044 
1045  if(dirB==JAC_KIN)
1046  {
1047  // Pn = H_A_Node * H_B(iLinkB)
1048  // Pn is the roto-transf matrix between base (of jac. in limb A) and end (of jac. in limb B)
1049  // it is needed because the two jacobians must refer to a common Pn
1050  // here for chain B we stop at the iLinkB link
1051  // allLink=true because we deal with all the links (even blocked ones): see motivation in the corresponding
1052  // jacobian with iLinkB
1053  Pn = H_A_Node * rbtList[iChainB].getH(iLinkB,true);
1054  }
1055  else //dirB==JAC_IKIN
1056  {
1057  // Pn = H_A_Node * H_B^-1
1058  Pn = H_A_Node * SE3inv(rbtList[iChainB].getH(iLinkB,true));
1059  }
1060 }
1061 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1062 Vector iDynNode::computePose(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, const bool axisRep)
1063 {
1064  //first check param coherence:
1065  // - wrong limb index
1066  if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
1067  {
1068  if(verbose) fprintf(stderr,"iDynNode: error, could not computePose() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix. \n",iChainA,iChainB,(int)rbtList.size());
1069  return Vector(0);
1070  }
1071  // - jacobian .. in the same limb @_@
1072  if( iChainA==iChainB )
1073  {
1074  if(verbose) fprintf(stderr,"iDynNode: error, could not computePose() due to weird index for chains %d: same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix. \n",iChainA);
1075  return Vector(0);
1076  }
1077 
1078  // params are ok, go on..
1079 
1080  //the vector from the base-link (for the jac.) of limb A to the end-link (for the jac.) of limb B
1081  Matrix Pn, H_A_Node;
1082  // the pose vector
1083  Vector v;
1084 
1085  // compute the roto-transf matrix between the base of limb A and the base of limb B
1086  // this is used to set the correct reference of vector Pn
1087  // just ignore H_A_Node
1088  compute_Pn_HAN(iChainA, dirA, iChainB, dirB, Pn, H_A_Node);
1089 
1090  // now compute the pose vector v (see iKin for more details)
1091  if (axisRep)
1092  {
1093  v.resize(7);
1094  Vector r=dcm2axis(Pn);
1095  v[0]=Pn(0,3);
1096  v[1]=Pn(1,3);
1097  v[2]=Pn(2,3);
1098  v[3]=r[0];
1099  v[4]=r[1];
1100  v[5]=r[2];
1101  v[6]=r[3];
1102  }
1103  else
1104  {
1105  v.resize(6);
1106  Vector r(3); r.zero();
1107  // Euler Angles as XYZ (see dcm2angle.m)
1108  r[0]=atan2(-Pn(2,1),Pn(2,2));
1109  r[1]=asin(Pn(2,0));
1110  r[2]=atan2(-Pn(1,0),Pn(0,0));
1111  v[0]=Pn(0,3);
1112  v[1]=Pn(1,3);
1113  v[2]=Pn(2,3);
1114  v[3]=r[0];
1115  v[4]=r[1];
1116  v[5]=r[2];
1117  }
1118 
1119  return v;
1120 }
1121 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1122 Vector iDynNode::computePose(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, const bool axisRep)
1123 {
1124  //first check param coherence:
1125  // - wrong limb index
1126  if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
1127  {
1128  if(verbose) fprintf(stderr,"iDynNode: error, could not computePose() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix. \n",iChainA,iChainB,(int)rbtList.size());
1129  return Vector(0);
1130  }
1131  // - jacobian .. in the same limb @_@
1132  if( iChainA==iChainB )
1133  {
1134  if(verbose) fprintf(stderr,"iDynNode: error, could not computePose() due to weird index for chains %d: same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix. \n",iChainA);
1135  return Vector(0);
1136  }
1137  // - there's not a link with index iLink in that chain
1138  if( iLinkB >= rbtList[iChainB].getNLinks())
1139  {
1140  if(verbose) fprintf(stderr,"iDynNode: error, could not computePose() due to out of range index for chain %d: the selected link is %d > %d. Returning a null matrix. \n",iChainB,iLinkB,rbtList[iChainB].getNLinks());
1141  return Vector(0);
1142  }
1143 
1144  // params are ok, go on..
1145 
1146  //the vector from the base-link (for the jac.) of limb A to the end-link (for the jac.) of limb B
1147  Matrix Pn, H_A_Node;
1148  // the pose vector
1149  Vector v;
1150 
1151  // compute the roto-transf matrix between the base of limb A and the base of limb B
1152  // this is used to set the correct reference of vector Pn
1153  // just ignore H_A_Node
1154  compute_Pn_HAN(iChainA, dirA, iChainB, iLinkB, dirB, Pn, H_A_Node);
1155 
1156  // now compute the pose vector v (see iKin for more details)
1157  if (axisRep)
1158  {
1159  v.resize(7);
1160  Vector r=dcm2axis(Pn);
1161  v[0]=Pn(0,3);
1162  v[1]=Pn(1,3);
1163  v[2]=Pn(2,3);
1164  v[3]=r[0];
1165  v[4]=r[1];
1166  v[5]=r[2];
1167  v[6]=r[3];
1168  }
1169  else
1170  {
1171  v.resize(6);
1172  Vector r(3); r.zero();
1173  // Euler Angles as XYZ (see dcm2angle.m)
1174  r[0]=atan2(-Pn(2,1),Pn(2,2));
1175  r[1]=asin(Pn(2,0));
1176  r[2]=atan2(-Pn(1,0),Pn(0,0));
1177  v[0]=Pn(0,3);
1178  v[1]=Pn(1,3);
1179  v[2]=Pn(2,3);
1180  v[3]=r[0];
1181  v[4]=r[1];
1182  v[5]=r[2];
1183  }
1184 
1185  return v;
1186 }
1187 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1188 
1189  //---------------
1190  // jacobians COM
1191  //---------------
1192 
1193 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1194 Matrix iDynNode::TESTING_computeCOMJacobian(unsigned int iChain, unsigned int iLink)
1195 {
1196  if(iChain<=rbtList.size())
1197  {
1198  // the check on iLink<=N is performed by iDynChain when calling computeCOMJacobian(iLink)
1199  // from the RBT
1200  return rbtList[iChain].TESTING_computeCOMJacobian(iLink,false);
1201  }
1202  else
1203  {
1204  if(verbose) fprintf(stderr,"iDynNode: error, could not computeCOMJacobian() due to out of range index: input limb has index %d > %d. Returning a null matrix. \n",iChain,(int)rbtList.size());
1205  return Matrix(0,0);
1206  }
1207 }
1208 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1209 Matrix iDynNode::TESTING_computeCOMJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB)
1210 {
1211  //first check param coherence:
1212  // - wrong limb index
1213  if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
1214  {
1215  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix. \n",iChainA,iChainB,(int)rbtList.size());
1216  return Matrix(0,0);
1217  }
1218  // - jacobian .. in the same limb @_@
1219  if( iChainA==iChainB )
1220  {
1221  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to weird index for chains %d: same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix. \n",iChainA);
1222  return Matrix(0,0);
1223  }
1224  // - there's not a link with index iLink in that chain
1225  if( iLinkB >= rbtList[iChainB].getNLinks())
1226  {
1227  if(verbose) fprintf(stderr,"iDynNode: error, could not computeJacobian() due to out of range index for chain %d: the selected link is %d > %d. Returning a null matrix. \n",iChainB,iLinkB,rbtList[iChainB].getNLinks());
1228  return Matrix(0,0);
1229  }
1230 
1231  // params are ok, go on..
1232 
1233  // total number of joints = Ndof_A + iLinkB
1234  // note that we are taking all the links in chainB, not only the DOF until iLinkB
1235  // the total jacobian matrix
1236  Matrix J(6,rbtList[iChainA].getDOF() + iLinkB ); J.zero();
1237  //the vector from the base-link (for the jac.) of limb A to the end-link (for the jac.) of limb B
1238  Matrix Pn;
1239  // the two jacobians
1240  Matrix J_A; Matrix J_B;
1241  // from base-link (for the jac.) of limb A to base (for the jac.) of limb B
1242  Matrix H_A_Node;
1243 
1244  // compute the roto-transf matrix between the base of limb A and the base of limb B
1245  // H_A_Node is used to initialize J_B properly
1246  // Pn consider chain A, chain B until iLinkB, then the COM of iLinkB
1247  compute_Pn_HAN_COM(iChainA, dirA, iChainB, iLinkB, dirB, Pn, H_A_Node);
1248 
1249  // now compute jacobian of first and second limb, setting the correct Pn
1250  // JA
1251  J_A = rbtList[iChainA].computeGeoJacobian(Pn,false);
1252  // JB
1253  // note: set H_A_node as the H0 of the B chain, but does not modify the chain original H0
1254  J_B = rbtList[iChainB].TESTING_computeCOMJacobian(iLinkB,Pn,H_A_Node,false);
1255 
1256  // finally start building the Jacobian J=[J_A|J_B]
1257  unsigned int c=0;
1258  unsigned int JAcols = J_A.cols();
1259  unsigned int JBcols = J_B.cols();
1260 
1261  if(JAcols+JBcols!=J.cols())
1262  {
1263  fprintf(stderr,"iDynNode error: Jacobian should be 6x%zu instead is 6x%d \n",J.cols(),(JAcols+JBcols));
1264  fprintf(stderr,"Note: limb A: N=%d DOF=%d \n",rbtList[iChainA].getNLinks(),rbtList[iChainA].getDOF());
1265  fprintf(stderr,"Note: limb B: N=%d DOF=%d iLinkB=%d \n",rbtList[iChainB].getNLinks(),rbtList[iChainB].getDOF(),iLinkB);
1266  J.resize(6,JAcols+JBcols);
1267  }
1268 
1269  for(unsigned int r=0; r<6; r++)
1270  {
1271  for(c=0;c<JAcols;c++)
1272  J(r,c)=J_A(r,c);
1273  for(c=0;c<JBcols;c++)
1274  J(r,JAcols+c)=J_B(r,c);
1275  }
1276 
1277  // now return the jacobian
1278  return J;
1279 }
1280 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1281 void iDynNode::compute_Pn_HAN_COM(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, Matrix &Pn, Matrix &H_A_Node)
1282 {
1283  // compute the roto-transf matrix between the base of limb A and the base of limb B
1284  // this is used to set the correct reference of vector Pn
1285  if(dirA==JAC_KIN)
1286  {
1287  // H_A_Node = H_A * RBT_A^T * RBT_B
1288  // note: RBT_A is transposed because we're going in the opposite direction wrt to one of the RBT
1289  H_A_Node = rbtList[iChainA].getH() * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1290  }
1291  else //dirA==JAC_IKIN
1292  {
1293  // H_A_Node = H_A^-1 * RBT_A^T * RBT_B
1294  H_A_Node = SE3inv(rbtList[iChainA].getH()) * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1295  }
1296 
1297  if(dirB==JAC_KIN)
1298  {
1299  // Pn = H_A_Node * H_B(iLinkB)
1300  // Pn is the roto-transf matrix between base (of jac. in limb A) and end (of jac. in limb B)
1301  // it is needed because the two jacobians must refer to a common Pn
1302  // here for chain B we stop at the iLinkB link
1303  // allLink=true because we deal with all the links (even blocked ones): see motivation in the corresponding
1304  // jacobian with iLinkB
1305  Pn = H_A_Node * rbtList[iChainB].getHCOM(iLinkB);
1306  }
1307  else //dirB==JAC_IKIN
1308  {
1309  // Pn = H_A_Node * H_B^-1
1310  Pn = H_A_Node * SE3inv(rbtList[iChainB].getHCOM(iLinkB));
1311  }
1312 }
1313 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1314 
1315 
1316 
1317 
1318 
1319 
1320 //====================================
1321 //
1322 // i DYN SENSOR NODE
1323 //
1324 //====================================
1325 
1326 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1327 iDynSensorNode::iDynSensorNode(const NewEulMode _mode)
1328 :iDynNode(_mode)
1329 {
1330  sensorList.clear();
1331 }
1332 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1333 iDynSensorNode::iDynSensorNode(const string &_info, const NewEulMode _mode, unsigned int verb)
1334 :iDynNode(_info,_mode,verb)
1335 {
1336  sensorList.clear();
1337 }
1338 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1339 void iDynSensorNode::addLimb(iDynLimb *limb, const Matrix &H, const FlowType kinFlow, const FlowType wreFlow)
1340 {
1341  iDynNode::addLimb(limb,H,kinFlow,wreFlow,false);
1342  iDynSensor *noSens = NULL;
1343  sensorList.push_back(noSens);
1344 }
1345 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1346 void iDynSensorNode::addLimb(iDynLimb *limb, const Matrix &H, iDynSensor *sensor, const FlowType kinFlow, const FlowType wreFlow)
1347 {
1348  iDynNode::addLimb(limb,H,kinFlow,wreFlow,true);
1349  sensorList.push_back(sensor);
1350 }
1351 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1353 {
1354  unsigned int outputNode = 0;
1355  // set to zero the node force/moment
1356  F.zero(); Mu.zero();
1357 
1358  //first get the forces/moments from each limb
1359  //assuming that each limb has been properly set with the outcoming measured
1360  //forces/moments which are necessary for the wrench computation
1361  for(unsigned int i=0; i<rbtList.size(); i++)
1362  {
1363  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
1364  {
1365  //compute the wrench pass in that limb
1366  // if there's a sensor, we must use iDynSensor
1367  // otherwise we use the limb method as usual
1368  if(rbtList[i].isSensorized()==true)
1369  sensorList[i]->computeWrenchFromSensorNewtonEuler();
1370  else
1371  rbtList[i].computeLimbWrench();
1372 
1373  //update the node force/moment with the wrench coming from the limb base/end
1374  // note that getWrench sum the result to F,Mu - because they are passed by reference
1375  // F = F + F[i], Mu = Mu + Mu[i]
1376  rbtList[i].getWrench(F,Mu);
1377  //check
1378  outputNode++;
1379  }
1380  }
1381 
1382  // node summation: already performed by each RBT
1383  // F = F + F[i], Mu = Mu + Mu[i]
1384 
1385  // at least one output node should exist
1386  // however if for testing purposes only one limb is attached to the node,
1387  // we can't avoid the computeWrench phase, but still we must remember that
1388  // it is not correct, because the node must be in balanc
1389  if(outputNode==rbtList.size())
1390  {
1391  if(verbose>1)
1392  {
1393  fprintf(stderr,"iDynNode: warning: there are no limbs with Wrench Flow = Output. At least one limb must have Wrench Output for balancing forces in the node. \n");
1394  fprintf(stderr,"Please check the coherence of the limb configuration in the node <%s>\n",info.c_str());
1395  }
1396  }
1397 
1398  //now forward the wrench output from the node to limbs whose wrench flow is output type
1399  // assuming they don't have a FT sensor
1400  for(unsigned int i=0; i<rbtList.size(); i++)
1401  {
1402  if(rbtList[i].getWrenchFlow()==RBT_NODE_OUT)
1403  {
1404  //init the wrench with the node information
1405  rbtList[i].setWrench(F,Mu);
1406  //solve wrench in that limb/chain
1407  rbtList[i].computeLimbWrench();
1408  }
1409  }
1410  return true;
1411 }
1412 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1413 bool iDynSensorNode::setWrenchMeasure(const Matrix &FM, bool afterAttach)
1414 {
1415  Vector fi(3); fi.zero();
1416  Vector mi(3); mi.zero();
1417  Vector FMi(6);FMi.zero();
1418  bool inputWasOk = true;
1419 
1420  //check how many limbs have wrench input
1421  int inputNode = howManyWrenchInputs(afterAttach);
1422 
1423  // input (eg measured) wrenches are stored in a 6xN matrix: each column is a 6x1 vector
1424  // with force/moment; N is the number of columns, ie the number of measured/input wrenches to the limb
1425  // the order is assumed coherent with the one built when adding limbs
1426  // eg:
1427  // adding limbs: addLimb(limb1), addLimb(limb2), addLimb(limb3)
1428  // where limb1, limb3 have wrench flow input
1429  // passing wrenches: Matrix FM(6,2), FM.setcol(0,fm1), FM.setcol(1,fm3)
1430  if(FM.cols()<inputNode)
1431  {
1432  if(verbose)
1433  {
1434  fprintf(stderr,"iDynNode: could not setWrenchMeasure due to missing wrenches to initialize the computations: only %zu f/m available instead of %d. Using default values, all zero.\n",FM.cols(),inputNode);
1435  if(afterAttach==true)
1436  fprintf(stderr," Remember that the first limb receives wrench input during an attach from another node. \n");
1437  }
1438  inputWasOk = false;
1439  }
1440  if(FM.rows()!=6)
1441  {
1442  if(verbose)
1443  fprintf(stderr,"iDynNode: could not setWrenchMeasure due to wrong sized init wrenches: %zu instead of 6 (3+3). Using default values, all zero.\n",FM.rows());
1444  inputWasOk = false;
1445  }
1446 
1447  //set the input forces/moments from each limb at base/end
1448  // note: if afterAttach=true we set the wrench only in limbs 1,2,..
1449  // if afterAttach=false, we set the wrench in all limbs 0,1,2,..
1450  unsigned int startLimb=0;
1451  if(afterAttach) startLimb=1;
1452 
1453  //set the measured/input forces/moments from each limb
1454  if(inputWasOk)
1455  {
1456  inputNode = 0;
1457  for(unsigned int i=startLimb; i<rbtList.size(); i++)
1458  {
1459  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
1460  {
1461  // from the input matrix - read the input wrench
1462  FMi = FM.getCol(inputNode);
1463  fi[0]=FMi[0];fi[1]=FMi[1];fi[2]=FMi[2];
1464  mi[0]=FMi[3];mi[1]=FMi[4];mi[2]=FMi[5];
1465  inputNode++;
1466  //set the input wrench in the RBT->limb
1467  // if there's a sensor, set on the sensor
1468  // otherwise on base/end as usual
1469  if(rbtList[i].isSensorized()==true)
1470  rbtList[i].setWrenchMeasure(sensorList[i],fi,mi);
1471  else
1472  rbtList[i].setWrenchMeasure(fi,mi);
1473  }
1474  }
1475  }
1476  else
1477  {
1478  // default zero values if inputs are wrong sized
1479  for(unsigned int i=startLimb; i<rbtList.size(); i++)
1480  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
1481  {
1482  if(rbtList[i].isSensorized()==true)
1483  rbtList[i].setWrenchMeasure(sensorList[i],fi,mi);
1484  else
1485  rbtList[i].setWrenchMeasure(fi,mi);
1486  }
1487  }
1488 
1489  //now that all is set, we can really solveWrench()
1490  return inputWasOk ;
1491 }
1492 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1493 bool iDynSensorNode::setWrenchMeasure(const Matrix &Fm, const Matrix &Mm, bool afterAttach)
1494 {
1495  bool inputWasOk = true;
1496 
1497  //check how many limbs have wrench input
1498  int inputNode = howManyWrenchInputs(afterAttach);
1499 
1500  // input (eg measured) wrenches are stored in two 3xN matrix: each column is a 3x1 vector
1501  // with force/moment; N is the number of columns, ie the number of measured/input wrenches to the limb
1502  // the order is assumed coherent with the one built when adding limbs
1503  // eg:
1504  // adding limbs: addLimb(limb1), addLimb(limb2), addLimb(limb3)
1505  // where limb1, limb3 have wrench flow input
1506  // passing wrenches: Matrix Fm(3,2), Fm.setcol(0,f1), Fm.setcol(1,f3) and similar for moment
1507  if((Fm.cols()<inputNode)||(Mm.cols()<inputNode))
1508  {
1509  if(verbose)
1510  {
1511  fprintf(stderr,"iDynNode: could not setWrenchMeasure due to missing wrenches to initialize the computations: only %zu/%zu f/m available instead of %d/%d. Using default values, all zero.\n",Fm.cols(),Mm.cols(),inputNode,inputNode);
1512  if(afterAttach==true)
1513  fprintf(stderr," Remember that the first limb receives wrench input during an attach from another node.\n");
1514  }
1515  inputWasOk = false;
1516  }
1517  if((Fm.rows()!=3)||(Mm.rows()!=3))
1518  {
1519  if(verbose) fprintf(stderr,"iDynNode: could not setWrenchMeasure due to wrong sized init f/m: %zu/%zu instead of 3/3. Using default values, all zero.\n",Fm.rows(),Mm.rows());
1520  inputWasOk = false;
1521  }
1522 
1523  //set the input forces/moments from each limb at base/end
1524  // note: if afterAttach=true we set the wrench only in limbs 1,2,..
1525  // if afterAttach=false, we set the wrench in all limbs 0,1,2,..
1526  unsigned int startLimb=0;
1527  if(afterAttach) startLimb=1;
1528 
1529  //set the measured/input forces/moments from each limb
1530  if(inputWasOk)
1531  {
1532  inputNode = 0;
1533  for(unsigned int i=startLimb; i<rbtList.size(); i++)
1534  {
1535  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
1536  {
1537  // from the input matrix
1538  //set the input wrench in the RBT->limb
1539  // if there's a sensor, set on the sensor
1540  // otherwise on base/end as usual
1541  if(rbtList[i].isSensorized()==true)
1542  rbtList[i].setWrenchMeasure(sensorList[i],Fm.getCol(inputNode),Mm.getCol(inputNode));
1543  else
1544  rbtList[i].setWrenchMeasure(Fm.getCol(inputNode),Mm.getCol(inputNode));
1545  inputNode++;
1546  }
1547  }
1548  }
1549  else
1550  {
1551  // default zero values if inputs are wrong sized
1552  Vector fi(3), mi(3); fi.zero(); mi.zero();
1553  for(unsigned int i=startLimb; i<rbtList.size(); i++)
1554  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
1555  rbtList[i].setWrenchMeasure(fi,mi);
1556  }
1557 
1558  //now that all is set, we can call solveWrench() or update()...
1559  return inputWasOk ;
1560 }
1561 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1562 Matrix iDynSensorNode::estimateSensorsWrench(const Matrix &FM, bool afterAttach)
1563 {
1564  unsigned int inputNode = 0;
1565  unsigned int outputNode = 0;
1566  unsigned int numSensor = 0;
1567  bool inputWasOk = true;
1568  Vector fi(3); fi.zero();
1569  Vector mi(3); mi.zero();
1570  Vector FMi(6);FMi.zero();
1571  Matrix ret;
1572 
1573  //reset node wrench
1574  F.zero(); Mu.zero();
1575 
1576  // solve kinematics
1577  solveKinematics();
1578 
1579  //check how many limbs have wrench input
1580  inputNode = howManyWrenchInputs(afterAttach);
1581 
1582  //first check if the input is correct
1583  if(FM.rows()!=6)
1584  {
1585  if(verbose) fprintf(stderr,"iDynSensorNode: could not setWrenchMeasure due to wrong sized init wrenches matrix: %zu rows instead of 6 (3+3). Using default values, all zero.\n",FM.rows());
1586  inputWasOk = false;
1587  }
1588  if(FM.cols()!=inputNode)
1589  {
1590  if(verbose)
1591  fprintf(stderr,"iDynSensorNode: could not setWrenchMeasure due to wrong sized init wrenches: %zu instead of %d. Using default values, all zero. \n",FM.cols(),inputNode);
1592  if(afterAttach==true)
1593  fprintf(stderr," Remember that the first limb receives wrench input during an attach from another node.\n");
1594  inputWasOk = false;
1595  }
1596 
1597  //set the input forces/moments from each limb at base/end
1598  // note: if afterAttach=true we set the wrench only in limbs 1,2,..
1599  // if afterAttach=false, we set the wrench in all limbs 0,1,2,..
1600  unsigned int startLimb=0;
1601  if(afterAttach) startLimb=1;
1602 
1603  if(inputWasOk)
1604  {
1605  inputNode = 0;
1606  for(unsigned int i=startLimb; i<rbtList.size(); i++)
1607  {
1608  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
1609  {
1610  // from the input matrix - read the input wrench
1611  FMi = FM.getCol(inputNode);
1612  fi[0]=FMi[0];fi[1]=FMi[1];fi[2]=FMi[2];
1613  mi[0]=FMi[3];mi[1]=FMi[4];mi[2]=FMi[5];
1614  inputNode++;
1615  //set the input wrench in the RBT->limb
1616  // set on base/end as usual
1617  rbtList[i].setWrenchMeasure(fi,mi);
1618  }
1619  }
1620  }
1621  else
1622  {
1623  // default zero values if inputs are wrong sized
1624  for(unsigned int i=startLimb; i<rbtList.size(); i++)
1625  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
1626  rbtList[i].setWrenchMeasure(fi,mi);
1627  }
1628 
1629  //first get the forces/moments from each limb
1630  //assuming that each limb has been properly set with the outcoming measured
1631  //forces/moments which are necessary for the wrench computation
1632  for(unsigned int i=0; i<rbtList.size(); i++)
1633  {
1634  if(rbtList[i].getWrenchFlow()==RBT_NODE_IN)
1635  {
1636  //compute the wrench pass in that limb
1637  // if there's a sensor, it's the same because here we estimate its value
1638  rbtList[i].computeLimbWrench();
1639 
1640  //update the node force/moment with the wrench coming from the limb base/end
1641  // note that getWrench sum the result to F,Mu - because they are passed by reference
1642  // F = F + F[i], Mu = Mu + Mu[i]
1643  rbtList[i].getWrench(F,Mu);
1644  //check
1645  outputNode++;
1646  }
1647  }
1648 
1649  // node summation: already performed by each RBT
1650  // F = F + F[i], Mu = Mu + Mu[i]
1651 
1652  // at least one output node should exist
1653  // however if for testing purposes only one limb is attached to the node,
1654  // we can't avoid the computeWrench phase, but still we must remember that
1655  // it is not correct, because the node must be in balanc
1656  if(outputNode==rbtList.size())
1657  {
1658  if(verbose>1)
1659  {
1660  fprintf(stderr,"iDynSensorNode: warning: there are no limbs with Wrench Flow = Output. ");
1661  fprintf(stderr," At least one limb must have Wrench Output for balancing forces in the node. ");
1662  fprintf(stderr,"Please check the coherence of the limb configuration in the node <%s> \n",info.c_str());
1663  }
1664  }
1665 
1666  //now forward the wrench output from the node to limbs whose wrench flow is output type
1667  // assuming they don't have a FT sensor
1668  for(unsigned int i=0; i<rbtList.size(); i++)
1669  {
1670  if(rbtList[i].getWrenchFlow()==RBT_NODE_OUT)
1671  {
1672  //init the wrench with the node information
1673  rbtList[i].setWrench(F,Mu);
1674  //solve wrench in that limb/chain
1675  rbtList[i].computeLimbWrench();
1676  }
1677  }
1678 
1679  // now look for sensors
1680  numSensor = 0;
1681  for(unsigned int i=0; i<rbtList.size(); i++)
1682  {
1683  //now we can estimate the sensor wrench if there's a sensor
1684  // if has sensor, compute its wrench
1685  if(rbtList[i].isSensorized()==true)
1686  {
1687  sensorList[i]->computeSensorForceMoment();
1688  numSensor++;
1689  }
1690  }
1691 
1692  //now build the matrix with wrenches
1693  if(numSensor>0)
1694  {
1695  ret.resize(6,numSensor);
1696  numSensor=0;
1697  for(unsigned int i=0; i<rbtList.size(); i++)
1698  if(rbtList[i].isSensorized()==true)
1699  {
1700  ret.setCol(numSensor,sensorList[i]->getSensorForceMoment());
1701  numSensor++;
1702  }
1703  }
1704  return ret;
1705 }
1706 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1708 {
1709  unsigned int numSensor = 0;
1710  for(unsigned int i=0; i<rbtList.size(); i++)
1711  {
1712  if(rbtList[i].isSensorized()==true)
1713  numSensor++;
1714  }
1715  return numSensor;
1716 }
1717 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1718 
1719 
1720 
1721 //====================================
1722 //
1723 // iDYN SENSOR TORSO NODE
1724 //
1725 //====================================
1726 
1727 
1728 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1730 :iDynSensorNode("torso_node",_mode,verb)
1731 {
1732 
1733 }
1734 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1735 iDynSensorTorsoNode::iDynSensorTorsoNode(const string &_info,const NewEulMode _mode, unsigned int verb)
1736 :iDynSensorNode(_info,_mode,verb)
1737 {
1738 
1739 }
1740 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1742 {
1743  delete rightSensor; rightSensor = NULL;
1744  delete leftSensor; leftSensor = NULL;
1745  delete up; up = NULL;
1746  delete right; right = NULL;
1747  delete left; left = NULL;
1748 
1749  rbtList.clear();
1750  sensorList.clear();
1751 }
1752 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1753 bool iDynSensorTorsoNode::setInertialMeasure(const Vector &w0, const Vector &dw0, const Vector &ddp0)
1754 {
1755  return setKinematicMeasure(w0,dw0,ddp0);
1756 }
1757 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1758 bool iDynSensorTorsoNode::setSensorMeasurement(const Vector &FM_right, const Vector &FM_left)
1759 {
1760  Matrix FM(6,2); FM.zero();
1761  if((FM_right.length()==6)&&(FM_left.length()==6))
1762  {
1763  FM.setCol(0,FM_right);
1764  FM.setCol(1,FM_left);
1765  return setWrenchMeasure(FM,true);
1766  }
1767  else
1768  {
1769  if(verbose) fprintf(stderr,"Node <%s> could not set sensor measurements properly due to wrong sized vectors. FM right/left have length %d,%d instead of 6,6. Setting everything to zero. \n",name.c_str(),(int)FM_right.length(),(int)FM_left.length());
1770  setWrenchMeasure(FM,true);
1771  return false;
1772  }
1773 }
1774 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1775 bool iDynSensorTorsoNode::setSensorMeasurement(const Vector &FM_right, const Vector &FM_left, const Vector &FM_up)
1776 {
1777  Matrix FM(6,3); FM.zero();
1778  if((FM_right.length()==6)&&(FM_left.length()==6)&&(FM_up.length()==6))
1779  {
1780  // order: up 0 - right 1 - left 2
1781  FM.setCol(0,FM_up);
1782  FM.setCol(1,FM_right);
1783  FM.setCol(2,FM_left);
1784  return setWrenchMeasure(FM,false);
1785  }
1786  else
1787  {
1788  if(verbose) fprintf(stderr,"Node <%s> could not set sensor measurements properly due to wrong sized vectors. FM up/right/left have length %d,%d,%d instead of 6,6. Setting everything to zero. \n",name.c_str(),(int)FM_up.length(),(int)FM_right.length(),(int)FM_left.length());
1789  setWrenchMeasure(FM);
1790  return false;
1791  }
1792 }
1793 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1795 {
1796  bool isOk = true;
1797  isOk = solveKinematics();
1798  isOk = isOk && solveWrench();
1799  return isOk;
1800 }
1801 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1802 bool iDynSensorTorsoNode::update(const Vector &w0, const Vector &dw0, const Vector &ddp0, const Vector &FM_right, const Vector &FM_left, const Vector &FM_up)
1803 {
1804  bool inputOk = true;
1805 
1806  if((FM_right.length()==6)&&(FM_left.length()==6)&&(FM_up.length()==6)&&(w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3))
1807  {
1808  setInertialMeasure(w0,dw0,ddp0);
1809  setSensorMeasurement(FM_right,FM_left,FM_up);
1810  return update();
1811  }
1812  else
1813  {
1814  if(verbose)
1815  {
1816  fprintf(stderr,"Node <%s> error, could not update() due to wrong sized vectors. ",name.c_str());
1817  fprintf(stderr," w0,dw0,ddp0 have size %d,%d,%d instead of 3,3,3. \n",(int)w0.length(),(int)dw0.length(),(int)ddp0.length());
1818  fprintf(stderr," FM up/right/left have size %d,%d,%d instead of 6,6,6. \n",(int)FM_up.length(),(int)FM_right.length(),(int)FM_left.length());
1819  fprintf(stderr," Updating without new values.\n");
1820  }
1821  update();
1822  return false;
1823  }
1824 }
1825 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1826 bool iDynSensorTorsoNode::update(const Vector &FM_right, const Vector &FM_left, bool afterAttach)
1827 {
1828  if(afterAttach==false)
1829  {
1830  if(verbose)
1831  {
1832  fprintf(stderr,"Node <%s> error, could not update() due to missing wrench vectors. ",name.c_str());
1833  fprintf(stderr,"This type of update() only works after an attachTorso() or after having set the central limb wrench and kinematics variables. ");
1834  fprintf(stderr,"You should try with the other update() methods. ");
1835  fprintf(stderr,"Could not perform update(): exiting. \n");
1836  }
1837  return false;
1838  }
1839 
1840  if((FM_right.length()==6)&&(FM_left.length()==6))
1841  {
1842  Matrix FM(6,2);
1843  FM.setCol(0,FM_right);
1844  FM.setCol(1,FM_left);
1845  setWrenchMeasure(FM,true);
1846  return update();
1847  }
1848  else
1849  {
1850  if(verbose)
1851  {
1852  fprintf(stderr,"Node <%s> error, could not update() due to wrong sized vectors. ",name.c_str());
1853  fprintf(stderr,"FM right/left have size %d,%d instead of 6,6. Updating without new values. \n",(int)FM_right.length(),(int)FM_left.length());
1854  }
1855  update();
1856  return false;
1857  }
1858 }
1859 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1860 
1861  //----------------
1862  // GET
1863  //----------------
1864 
1865 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1866 Matrix iDynSensorTorsoNode::getForces(const string &limbType)
1867 {
1868  if(limbType==up_name) return up->getForces();
1869  else if(limbType==left_name) return left->getForces();
1870  else if(limbType==right_name) return right->getForces();
1871  else
1872  {
1873  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
1874  return Matrix(0,0);
1875  }
1876 }
1877 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1878 Matrix iDynSensorTorsoNode::getMoments(const string &limbType)
1879 {
1880  if(limbType==up_name) return up->getMoments();
1881  else if(limbType==left_name) return left->getMoments();
1882  else if(limbType==right_name) return right->getMoments();
1883  else
1884  {
1885  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
1886  return Matrix(0,0);
1887  }
1888 }
1889 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1890 Vector iDynSensorTorsoNode::getTorques(const string &limbType)
1891 {
1892  if(limbType==up_name) return up->getTorques();
1893  else if(limbType==left_name) return left->getTorques();
1894  else if(limbType==right_name) return right->getTorques();
1895  else
1896  {
1897  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
1898  return Vector(0);
1899  }
1900 }
1901 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1902 Vector iDynSensorTorsoNode::getTorsoForce() const { return F;}
1903 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1904 Vector iDynSensorTorsoNode::getTorsoMoment() const{ return Mu;}
1905 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1906 Vector iDynSensorTorsoNode::getTorsoAngVel() const{ return w;}
1907 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1908 Vector iDynSensorTorsoNode::getTorsoAngAcc() const{ return dw;}
1909 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1911 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1913 {
1914  unsigned int i=0;
1915  yarp::sig::Vector COM_UP(4); COM_UP.zero();
1916  yarp::sig::Vector COM_RT(4); COM_RT.zero();
1917  yarp::sig::Vector COM_LF(4); COM_LF.zero();
1918  total_COM_UP.resize(4);total_COM_UP.zero();
1919  total_COM_RT.resize(4);total_COM_RT.zero();
1920  total_COM_LF.resize(4);total_COM_LF.zero();
1921  total_mass_UP=0.0;
1922  total_mass_RT=0.0;
1923  total_mass_LF=0.0;
1924 
1925  iCub::iDyn::iDynLimb *limb = 0;
1926  yarp::sig::Matrix *orig = 0;
1927  yarp::sig::Vector *total_COM = 0;
1928  double *total_mass = 0;
1929 
1930  for (int n=0; n<3; n++)
1931  {
1932  switch (n)
1933  {
1934  case 0:
1935  limb = (this->left);
1936  orig = &(this->HLeft);
1937  total_COM = &(total_COM_LF);
1938  total_mass = &(total_mass_LF);
1939  break;
1940  case 1:
1941  limb = (this->right);
1942  orig = &(this->HRight);
1943  total_COM = &(total_COM_RT);
1944  total_mass = &(total_mass_RT);
1945  break;
1946  case 2:
1947  limb = (this->up);
1948  orig = &(this->HUp);
1949  total_COM = &(total_COM_UP);
1950  total_mass = &(total_mass_UP);
1951  break;
1952  }
1953 
1954  for (i=0; i<limb->getN(); i++)
1955  {
1956  (*total_mass)=(*total_mass)+limb->getMass(i);
1957  COM=limb->getCOM(i).getCol(3);
1958  yarp::sig::Matrix m = limb->getH(i, true);
1959  (*total_COM) = (*total_COM) + ((*orig) * m * COM) * limb->getMass(i);
1960  }
1961  if (fabs(*total_mass) > 0.00001)
1962  {(*total_COM)=(*total_COM)/(*total_mass); }
1963  else
1964  {(*total_COM).zero();}
1965  }
1966  return true;
1967 }
1968 
1969 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1971 {
1972  iCub::iDyn::iDynLimb *limb = 0;
1973  yarp::sig::Matrix *orig = 0;
1974  yarp::sig::Matrix *COM_jacob = 0;
1975  double *total_mass = 0;
1976 
1977 
1978  for (int n=0; n<3; n++)
1979  {
1980  switch (n)
1981  {
1982  case 0:
1983  limb = (this->left);
1984  orig = &(this->HLeft);
1985  COM_jacob = &(COM_jacob_LF);
1986  total_mass = &(total_mass_LF);
1987  break;
1988  case 1:
1989  limb = (this->right);
1990  orig = &(this->HRight);
1991  COM_jacob = &(COM_jacob_RT);
1992  total_mass = &(total_mass_RT);
1993  break;
1994  case 2:
1995  limb = (this->up);
1996  orig = &(this->HUp);
1997  COM_jacob = &(COM_jacob_UP);
1998  total_mass = &(total_mass_UP);
1999  break;
2000  }
2001 
2002  //here do the computation
2003  double* partial_mass = 0;
2004  Vector* partial_COM = 0;
2005  partial_mass = new double [limb->getN()];
2006  partial_COM = new Vector [limb->getN()];
2007  COM_jacob->resize(6,limb->getN());
2008 
2009  //obtain the list of Z vectors (used in the final step)
2010  deque<Matrix> intH;
2011  intH.push_back(*orig);
2012  for (unsigned int i=0; i<limb->getN(); i++)
2013  intH.push_back((*orig)*limb->getH(i,true));
2014 
2015  for (unsigned int iLink=0; iLink<limb->getN(); iLink++)
2016  {
2017  partial_mass[iLink]=0;
2018  partial_COM [iLink].resize(4);
2019  partial_COM [iLink].zero();
2020 
2021  // this is the transformation from the link root to current link iLink
2022  yarp::sig::Matrix m = (*orig) * limb->getH(iLink, true);
2023 
2024  //partial COMs are computed in this block
2025  for (unsigned int i=iLink; i<limb->getN(); i++)
2026  {
2027  yarp::sig::Matrix m_i = (*orig) * limb->getH(i, true);
2028  partial_COM[iLink] =partial_COM[iLink] + m_i * limb->getCOM(i).getCol(3) * limb->getMass(i);
2029  partial_mass[iLink]=partial_mass[iLink]+limb->getMass(i);
2030  if(partial_mass[iLink]==0) partial_mass[iLink]=1e-15;
2031  }
2032  partial_COM[iLink] = (partial_COM[iLink]/partial_mass[iLink])-intH[iLink].getCol(3);
2033 
2034 #ifdef DEBUG_P_COM
2035  printf ("parCOM: %d %+3.3f %+3.3f %+3.3f \n", iLink, partial_COM[iLink](0), partial_COM[iLink](1), partial_COM[iLink](2));
2036  printf ("n : %d %+3.3f %+3.3f %+3.3f \n", iLink, intH[iLink].getCol(3)(0),intH[iLink].getCol(3)(1),intH[iLink].getCol(3)(2));
2037  printf ("p-nCOM: %d %+3.3f %+3.3f %+3.3f \n", iLink, partial_COM[iLink](0), partial_COM[iLink](1), partial_COM[iLink](2));
2038 #endif
2039  //partial COM jacobian are computed in this block
2040  double mass_coeff = partial_mass[iLink]/(*total_mass);
2041 
2042  Vector Z = intH[iLink].subcol(0,2,3);
2043  Vector w = cross(Z,partial_COM[iLink].subVector(0,2));
2044 
2045  (*COM_jacob)(0,iLink)=mass_coeff*w[0];
2046  (*COM_jacob)(1,iLink)=mass_coeff*w[1];
2047  (*COM_jacob)(2,iLink)=mass_coeff*w[2];
2048  (*COM_jacob)(3,iLink)=Z(0);
2049  (*COM_jacob)(4,iLink)=Z(1);
2050  (*COM_jacob)(5,iLink)=Z(2);
2051  }
2052 
2053  delete [] partial_mass;
2054  delete [] partial_COM;
2055  }
2056  return true;
2057 }
2058 
2059  //------------------
2060  // LIMB CALLS
2061  //------------------
2062 
2063 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2064 Vector iDynSensorTorsoNode::setAng(const string &limbType, const Vector &_q)
2065 {
2066  if(limbType==up_name) return up->setAng(_q);
2067  else if(limbType==left_name) return left->setAng(_q);
2068  else if(limbType==right_name) return right->setAng(_q);
2069  else
2070  {
2071  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2072  return Vector(0);
2073  }
2074 }
2075 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2076 Vector iDynSensorTorsoNode::getAng(const string &limbType)
2077 {
2078  if(limbType==up_name) return up->getAng();
2079  else if(limbType==left_name) return left->getAng();
2080  else if(limbType==right_name) return right->getAng();
2081  else
2082  {
2083  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2084  return Vector(0);
2085  }
2086 }
2087 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2088 double iDynSensorTorsoNode::setAng(const string &limbType, const unsigned int i, double _q)
2089 {
2090  if(limbType==up_name) return up->setAng(i,_q);
2091  else if(limbType==left_name) return left->setAng(i,_q);
2092  else if(limbType==right_name) return right->setAng(i,_q);
2093  else
2094  {
2095  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2096  return 0.0;
2097  }
2098 }
2099 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2100 double iDynSensorTorsoNode::getAng(const string &limbType, const unsigned int i)
2101 {
2102  if(limbType==up_name) return up->getAng(i);
2103  else if(limbType==left_name) return left->getAng(i);
2104  else if(limbType==right_name) return right->getAng(i);
2105  else
2106  {
2107  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2108  return 0.0;
2109  }
2110 }
2111 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2112 Vector iDynSensorTorsoNode::setDAng(const string &limbType, const Vector &_dq)
2113 {
2114  if(limbType==up_name) return up->setDAng(_dq);
2115  else if(limbType==left_name) return left->setDAng(_dq);
2116  else if(limbType==right_name) return right->setDAng(_dq);
2117  else
2118  {
2119  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2120  return Vector(0);
2121  }
2122 }
2123 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2124 Vector iDynSensorTorsoNode::getDAng(const string &limbType)
2125 {
2126  if(limbType==up_name) return up->getDAng();
2127  else if(limbType==left_name) return left->getDAng();
2128  else if(limbType==right_name) return right->getDAng();
2129  else
2130  {
2131  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2132  return Vector(0);
2133  }
2134 }
2135 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2136 double iDynSensorTorsoNode::setDAng(const string &limbType, const unsigned int i, double _dq)
2137 {
2138  if(limbType==up_name) return up->setDAng(i,_dq);
2139  else if(limbType==left_name) return left->setDAng(i,_dq);
2140  else if(limbType==right_name) return right->setDAng(i,_dq);
2141  else
2142  {
2143  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2144  return 0.0;
2145  }
2146 }
2147 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2148 double iDynSensorTorsoNode::getDAng(const string &limbType, const unsigned int i)
2149 {
2150  if(limbType==up_name) return up->getDAng(i);
2151  else if(limbType==left_name) return left->getDAng(i);
2152  else if(limbType==right_name) return right->getDAng(i);
2153  else
2154  {
2155  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2156  return 0.0;
2157  }
2158 }
2159 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2160 Vector iDynSensorTorsoNode::setD2Ang(const string &limbType, const Vector &_ddq)
2161 {
2162  if(limbType==up_name) return up->setD2Ang(_ddq);
2163  else if(limbType==left_name) return left->setD2Ang(_ddq);
2164  else if(limbType==right_name) return right->setD2Ang(_ddq);
2165  else
2166  {
2167  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2168  return Vector(0);
2169  }
2170 }
2171 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2172 Vector iDynSensorTorsoNode::getD2Ang(const string &limbType)
2173 {
2174  if(limbType==up_name) return up->getD2Ang();
2175  else if(limbType==left_name) return left->getD2Ang();
2176  else if(limbType==right_name) return right->getD2Ang();
2177  else
2178  {
2179  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2180  return Vector(0);
2181  }
2182 }
2183 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2184 double iDynSensorTorsoNode::setD2Ang(const string &limbType, const unsigned int i, double _ddq)
2185 {
2186  if(limbType==up_name) return up->setD2Ang(i,_ddq);
2187  else if(limbType==left_name) return left->setD2Ang(i,_ddq);
2188  else if(limbType==right_name) return right->setD2Ang(i,_ddq);
2189  else
2190  {
2191  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2192  return 0.0;
2193  }
2194 }
2195 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2196 double iDynSensorTorsoNode::getD2Ang(const string &limbType, const unsigned int i)
2197 {
2198  if(limbType==up_name) return up->getD2Ang(i);
2199  else if(limbType==left_name) return left->getD2Ang(i);
2200  else if(limbType==right_name) return right->getD2Ang(i);
2201  else
2202  {
2203  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2204  return 0.0;
2205  }
2206 }
2207 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2208 unsigned int iDynSensorTorsoNode::getNLinks(const string &limbType) const
2209 {
2210  if(limbType==up_name) return up->getN();
2211  else if(limbType==left_name) return left->getN();
2212  else if(limbType==right_name) return right->getN();
2213  else
2214  {
2215  if(verbose) fprintf(stderr,"Node <%s> there's not a limb named %s. Only %s,%s,%s are available. \n",name.c_str(),limbType.c_str(),left_name.c_str(),right_name.c_str(),up_name.c_str());
2216  return 0;
2217  }
2218 }
2219 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2221 {
2224 }
2225 
2226 
2227 
2228 //====================================
2229 //
2230 // UPPER TORSO
2231 //
2232 //====================================
2233 
2234 
2235 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2236 iCubUpperTorso::iCubUpperTorso(version_tag _tag, const NewEulMode _mode, unsigned int verb)
2237 :iDynSensorTorsoNode("upper-torso",_mode,verb)
2238 {
2239  tag=_tag;
2240  build();
2241 }
2242 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2244 {
2245  left = new iCubArmNoTorsoDyn("left",KINFWD_WREBWD);
2246  right = new iCubArmNoTorsoDyn("right",KINFWD_WREBWD);
2247  if (tag.head_version == 2) {
2248  std::string ver {"v2."};
2249  ver += std::to_string(tag.head_subversion);
2251  }
2252  else {
2254  }
2255 
2256  SensorLinkNewtonEuler* armLeftSensor = new iCubArmSensorLink("left", mode, verbose);
2257  SensorLinkNewtonEuler* armRightSensor = new iCubArmSensorLink("right", mode, verbose);
2258  // FT sensor is in position 2 in the kinematic chain in both arms
2259  // note position 5 if with torso, 2 without torso
2260  unsigned int SENSOR_LINK_INDEX = 2;
2261 
2262  leftSensor = new iDynContactSolver(dynamic_cast<iCubArmNoTorsoDyn*>(left), SENSOR_LINK_INDEX, armLeftSensor,
2263  "leftArmContactSolver",mode,LEFT_ARM, verbose);
2264  rightSensor = new iDynContactSolver(dynamic_cast<iCubArmNoTorsoDyn*>(right), SENSOR_LINK_INDEX, armRightSensor,
2265  "rightArmContactSolver",mode,RIGHT_ARM, verbose);
2266 
2267  HUp.resize(4,4); HUp.eye();
2268  HLeft.resize(4,4); HLeft.zero();
2269  HRight.resize(4,4); HRight.zero();
2270 
2271  double theta = CTRL_DEG2RAD * (180.0-15.0);
2272  /*
2273  HLeft(0,0) = cos(theta); HLeft(0,1) = 0.0; HLeft(0,2) = sin(theta); HLeft(0,3) = -24.8786e-3;
2274  HLeft(1,0) = 0.0; HLeft(1,1) = 1.0; HLeft(1,2) = 0.0; HLeft(1,3) = -50.0000e-3;
2275  HLeft(2,0) = -sin(theta); HLeft(2,1) = 0.0; HLeft(2,2) = cos(theta); HLeft(2,3) = -6.0472e-3;
2276  HLeft(3,3) = 1.0;
2277  HRight(0,0) = -cos(theta); HRight(0,1) = 0.0; HRight(0,2) = -sin(theta); HRight(0,3) = -24.8786e-3;
2278  HRight(1,0) = 0.0; HRight(1,1) = -1.0; HRight(1,2) = 0.0; HRight(1,3) = -50.0000e-3;
2279  HRight(2,0) = -sin(theta); HRight(2,1) = 0.0; HRight(2,2) = cos(theta); HRight(2,3) = 6.0472e-3;
2280  HRight(3,3) = 1.0;
2281  */
2282 
2283  HLeft(0,0) = cos(theta); HLeft(0,1) = 0.0; HLeft(0,2) = sin(theta); HLeft(0,3) = 0.00294;
2284  HLeft(1,0) = 0.0; HLeft(1,1) = 1.0; HLeft(1,2) = 0.0; HLeft(1,3) = -0.050;
2285  HLeft(2,0) = -sin(theta); HLeft(2,1) = 0.0; HLeft(2,2) = cos(theta); HLeft(2,3) = -0.11026;
2286  HLeft(3,3) = 1.0;
2287  HRight(0,0) = -cos(theta); HRight(0,1) = 0.0; HRight(0,2) = -sin(theta); HRight(0,3) = 0.00294;
2288  HRight(1,0) = 0.0; HRight(1,1) = -1.0; HRight(1,2) = 0.0; HRight(1,3) = -0.050;
2289  HRight(2,0) = -sin(theta); HRight(2,1) = 0.0; HRight(2,2) = cos(theta); HRight(2,3) = 0.11026;
2290  HRight(3,3) = 1.0;
2291 
2292  // order: head - right arm - left arm
2296 
2297  left_name = "left_arm";
2298  right_name= "right_arm";
2299  up_name = "head";
2300  name = "upper_torso";
2301 }
2302 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2304 {
2305 }
2306 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2307 
2308 
2309 //====================================
2310 //
2311 // LOWER TORSO
2312 //
2313 //====================================
2314 
2315 
2316 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2317 iCubLowerTorso::iCubLowerTorso(version_tag _tag, const NewEulMode _mode, unsigned int verb)
2318 :iDynSensorTorsoNode("lower-torso",_mode,verb)
2319 {
2320  tag=_tag;
2321  build();
2322 }
2323 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2325 {
2326 
2327  SensorLinkNewtonEuler* legLeftSensor = new iCubLegSensorLink("left", mode, verbose);
2328  SensorLinkNewtonEuler* legRightSensor = new iCubLegSensorLink("right", mode, verbose);
2329  unsigned int SENSOR_LINK_INDEX = 1;
2330 
2331  if (tag.legs_version == 2)
2332  {
2333  left = new iCubLegDynV2("left",KINFWD_WREBWD);
2334  right = new iCubLegDynV2("right",KINFWD_WREBWD);
2335 
2336  leftSensor = new iDynContactSolver(dynamic_cast<iCubLegDynV2*>(left), SENSOR_LINK_INDEX, legLeftSensor,
2337  "leftLegContactSolver",mode,LEFT_LEG,verbose);
2338  rightSensor = new iDynContactSolver(dynamic_cast<iCubLegDynV2*>(right), SENSOR_LINK_INDEX, legRightSensor,
2339  "rightLegContactSolver",mode,RIGHT_LEG,verbose);
2340  }
2341  else
2342  {
2343  left = new iCubLegDyn("left",KINFWD_WREBWD);
2344  right = new iCubLegDyn("right",KINFWD_WREBWD);
2345 
2346  leftSensor = new iDynContactSolver(dynamic_cast<iCubLegDyn*>(left), SENSOR_LINK_INDEX, legLeftSensor,
2347  "leftLegContactSolver",mode,LEFT_LEG,verbose);
2348  rightSensor = new iDynContactSolver(dynamic_cast<iCubLegDyn*>(right), SENSOR_LINK_INDEX, legRightSensor,
2349  "rightLegContactSolver",mode,RIGHT_LEG,verbose);
2350  }
2351 
2352  up = new iCubTorsoDyn("lower",KINBWD_WREBWD);
2353 
2354 
2355  HUp.resize(4,4); HUp.zero();
2356  HUp(0,1)=-1.0; // 0 -1 0
2357  HUp(1,2)=-1.0; // 0 0 -1
2358  HUp(2,0)= 1.0; // 1 0 0
2359  HUp(3,3)= 1.0;
2360 
2361  HLeft.resize(4,4); HRight.resize(4,4);
2362  HLeft.zero(); HRight.zero();
2363 
2364  HLeft(0,0)=1.0; // 1 0 0
2365  HLeft(1,2)=1.0; // 0 0 1
2366  HLeft(2,1)=-1.0; // 0 -1 0
2367  HLeft(3,3)=1.0; //
2368  HLeft(2,3)=-0.1199; HLeft(1,3)=-0.0681;
2369 
2370  HRight(0,0)=1.0; // 1 0 0
2371  HRight(1,2)=1.0; // 0 0 1
2372  HRight(2,1)=-1.0; // 0 -1 0
2373  HRight(3,3)=1.0; //
2374  HRight(2,3)=-0.1199;HRight(1,3)=0.0681;
2375 
2376  // order: torso - right leg - left leg
2380 
2381  left_name = "left_leg";
2382  right_name= "right_leg";
2383  up_name = "torso";
2384  name = "lower_torso";
2385 }
2386 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2388 {
2389 }
2390 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2391 
2392 
2393 //====================================
2394 //
2395 // iCUB WHOLE BODY
2396 //
2397 //====================================
2398 
2399 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2400 iCubWholeBody::iCubWholeBody(version_tag _tag, const NewEulMode mode, unsigned int verbose)
2401 {
2402  //create all limbs
2403  tag = _tag;
2404  upperTorso = new iCubUpperTorso(tag,mode,verbose);
2405  lowerTorso = new iCubLowerTorso(tag,mode,verbose);
2406 
2407  //now create a connection between upperTorso node and Torso ( lowerTorso->up == Torso )
2408  Matrix H(4,4);
2409  H.eye();
2410  //H is no used currently since the transformation is an identity
2411  rbt = new RigidBodyTransformation(lowerTorso->up,H,"connection between lower and upper torso",false,RBT_NODE_OUT,RBT_NODE_OUT,mode,verbose);
2412 }
2413 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2415 {
2416  if (upperTorso) delete upperTorso; upperTorso = NULL;
2417  if (lowerTorso) delete lowerTorso; lowerTorso = NULL;
2418  if (rbt) delete rbt; rbt = NULL;
2419 }
2420 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2421 void iCubWholeBody::attachLowerTorso(const Vector &FM_right_leg, const Vector &FM_left_leg)
2422 {
2423  Vector in_w = upperTorso->getTorsoAngVel();
2424  Vector in_dw = upperTorso->getTorsoAngAcc();
2425  Vector in_ddp = upperTorso->getTorsoLinAcc();
2426  Vector in_F = upperTorso->getTorsoForce();
2427  Vector in_M = upperTorso->getTorsoMoment();
2428 
2429  Vector out_w; out_w.resize(3);
2430  Vector out_dw; out_dw.resize(3);
2431  Vector out_ddp; out_ddp.resize(3);
2432  Vector out_F; out_F.resize(3);
2433  Vector out_M; out_M.resize(3);
2434 
2435  //kinematics:
2436  out_w[0]= in_w[0];
2437  out_w[1]= in_w[1];
2438  out_w[2]= in_w[2];
2439 
2440  out_dw[0]= in_dw[0];
2441  out_dw[1]= in_dw[1];
2442  out_dw[2]= in_dw[2];
2443 
2444  out_ddp[0]= in_ddp[0];
2445  out_ddp[1]= in_ddp[1];
2446  out_ddp[2]= in_ddp[2];
2447 
2448  //wrenches:
2449  out_F[0]= in_F[0];
2450  out_F[1]= in_F[1];
2451  out_F[2]= in_F[2];
2452 
2453  out_M[0]= in_M[0];
2454  out_M[1]= in_M[1];
2455  out_M[2]= in_M[2];
2456 
2457  Vector FUP ;
2458  Vector FLL ;
2459  Vector FRL ;
2460  FUP.resize(6);
2461  FUP[0] = out_F[0];
2462  FUP[1] = out_F[1];
2463  FUP[2] = out_F[2];
2464  FUP[3] = out_M[0];
2465  FUP[4] = out_M[1];
2466  FUP[5] = out_M[2];
2467  lowerTorso->setKinematicMeasure(out_w,out_dw,out_ddp);
2468  lowerTorso->setSensorMeasurement(FM_right_leg,FM_left_leg,FUP);
2469 
2470 }
2471 
2472 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2474 {
2475  //upper torso COM computation
2476  yarp::sig::Matrix T0 = lowerTorso->HUp;
2477  yarp::sig::Matrix T1 = lowerTorso->up->getH(2,true);
2478 
2480 
2484 
2488  //upper_COM.push_back(1.0);
2489  upper_COM= T0 * T1 * upper_COM;
2490 
2491  //lower torso COM computation
2493 
2497 
2501 
2504  whole_COM.pop_back();
2505 
2506  sw_getcom = 1; //USEFUL FOR THE DOUBLE CALL TO getCOMjacobian()
2507  return true;
2508 }
2509 
2510 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2511 bool iCubWholeBody::getCOM(BodyPart which_part, Vector &COM, double & mass)
2512 {
2513  COM.zero();
2514  mass=0.0;
2515  yarp::sig::Matrix T0;
2516  yarp::sig::Matrix T1;
2517  yarp::sig::Matrix hright;
2518 
2519  // hright.resize(4,4); hright.zero();
2520  // double theta = CTRL_DEG2RAD * (180.0-15.0);
2521  // hright(0,0)=1.0; // 1 0 0
2522  // hright(1,2)=1.0; // 0 0 1
2523  // hright(2,1)=-1.0; // 0 -1 0
2524  // hright(3,3)=1.0;
2525  // hright(2,3)=-0.1199;
2526  // hright(1,3)=0.0681;
2527  // yarp::sig::Matrix rot6x6; rot6x6.resize(6,6); rot6x6.zero();
2528  // rot6x6.setSubmatrix(hright.submatrix(0,2,0,2),0,0);
2529  // rot6x6.setSubmatrix(hright.submatrix(0,2,0,2),3,3);
2530 
2531  switch (which_part)
2532  {
2533  case BODY_PART_ALL:
2534  COM=whole_COM;
2535  mass=whole_mass;
2536 
2537  #ifdef DEBUG_FOOT_COM
2538  fprintf(stderr, "Whole COM being computed w.r.t Foot Reference Frame... \n");
2539  #endif
2540 
2541  break;
2542  case LOWER_BODY_PARTS:
2543  COM=this->lower_COM;
2544  mass=lower_mass;
2545  COM.pop_back();
2546  break;
2547  case UPPER_BODY_PARTS:
2548  //T0 = lowerTorso->HUp;
2549  //T1 = lowerTorso->up->getH(2,true);
2550  //COM=this->upper_COM; //respect to neck
2551  //COM= T0 * T1 * this->upper_COM; //respect to root
2552  COM= this->upper_COM; //rotation is done aslread in computation?????
2553  mass=upper_mass;
2554  COM.pop_back();
2555  break;
2556  case LEFT_LEG:
2557  COM=this->lowerTorso->total_COM_LF;
2558  mass=this->lowerTorso->total_mass_LF;
2559  COM.pop_back();
2560  break;
2561  case RIGHT_LEG:
2562  COM=this->lowerTorso->total_COM_RT;
2563  mass=this->lowerTorso->total_mass_RT;
2564  COM.pop_back();
2565  break;
2566  case TORSO:
2567  COM=this->lowerTorso->total_COM_UP;
2568  mass=this->lowerTorso->total_mass_UP;
2569  COM.pop_back();
2570  break;
2571  case LEFT_ARM:
2572  T0 = lowerTorso->HUp;
2573  T1 = lowerTorso->up->getH(2,true);
2574  mass=this->upperTorso->total_mass_LF;
2575  COM=this->upperTorso->total_COM_LF;
2576  //COM.push_back(1.0);
2577  COM= T0 * T1 * COM;
2578  COM.pop_back();
2579  break;
2580  case RIGHT_ARM:
2581  T0 = lowerTorso->HUp;
2582  T1 = lowerTorso->up->getH(2,true);
2583  mass=this->upperTorso->total_mass_RT;
2584  COM=this->upperTorso->total_COM_RT;
2585  //COM.push_back(1.0);
2586  COM= T0 * T1 * COM;
2587  COM.pop_back();
2588  break;
2589  case HEAD:
2590  T0 = lowerTorso->HUp;
2591  T1 = lowerTorso->up->getH(2,true);
2592  mass=this->upperTorso->total_mass_UP;
2593  COM=this->upperTorso->total_COM_UP;
2594  //COM.push_back(1.0);
2595  COM= T0 * T1 * COM;
2596  COM.pop_back();
2597  break;
2598  default:
2599  COM.zero();
2600  mass=0.0;
2601  break;
2602  }
2603  return true;
2604 }
2605 
2606 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2608 {
2609  Vector ll = this->lowerTorso->left->getAng();
2610  Vector rl = this->lowerTorso->right->getAng();
2611  Vector to = this->lowerTorso->up->getAng();
2612  Vector la = this->upperTorso->left->getAng();
2613  Vector ra = this->upperTorso->right->getAng();
2614  Vector hd = this->upperTorso->up->getAng();
2615  pos.clear();
2616  pos.resize(32,0.0);
2617  int i=0; int j=0;
2618  for (i=0; i<6; i++, j++) pos[j] = ll[i];
2619  for (i=0; i<6; i++, j++) pos[j] = rl[i];
2620  for (i=0; i<3; i++, j++) pos[j] = to[i];
2621  for (i=0; i<7; i++, j++) pos[j] = la[i];
2622  for (i=0; i<7; i++, j++) pos[j] = ra[i];
2623  for (i=0; i<3; i++, j++) pos[j] = hd[i];
2624 
2625  return true;
2626 }
2627 
2628 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2630 {
2631  Vector ll = this->lowerTorso->left->getDAng();
2632  Vector rl = this->lowerTorso->right->getDAng();
2633  Vector to = this->lowerTorso->up->getDAng();
2634  Vector la = this->upperTorso->left->getDAng();
2635  Vector ra = this->upperTorso->right->getDAng();
2636  Vector hd = this->upperTorso->up->getDAng();
2637  vel.clear();
2638  vel.resize(32,0.0);
2639  int i=0; int j=0;
2640  for (i=0; i<6; i++, j++) vel[j] = ll[i];
2641  for (i=0; i<6; i++, j++) vel[j] = rl[i];
2642  for (i=0; i<3; i++, j++) vel[j] = to[i];
2643  for (i=0; i<7; i++, j++) vel[j] = la[i];
2644  for (i=0; i<7; i++, j++) vel[j] = ra[i];
2645  for (i=0; i<3; i++, j++) vel[j] = hd[i];
2646 
2647  return true;
2648 }
2649 
2650 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2651 /*
2652 bool iCubWholeBody::getAllAccelerations(Vector &acc)
2653 {
2654  Vector ll = this->lowerTorso->left->getD2Ang();
2655  Vector rl = this->lowerTorso->right->getD2Ang();
2656  Vector to = this->lowerTorso->up->getD2Ang();
2657  Vector la = this->upperTorso->left->getD2Ang();
2658  Vector ra = this->upperTorso->right->getD2Ang();
2659  Vector hd = this->upperTorso->up->getD2Ang();
2660  acc.clear();
2661  acc.resize(32,0.0);
2662  int i=0; int j=0;
2663  for (i=0; i<6; i++, j++) acc[j] = ll[i];
2664  for (i=0; i<6; i++, j++) acc[j] = rl[i];
2665  for (i=0; i<3; i++, j++) acc[j] = to[i];
2666  for (i=0; i<7; i++, j++) acc[j] = la[i];
2667  for (i=0; i<7; i++, j++) acc[j] = ra[i];
2668  for (i=0; i<3; i++, j++) acc[j] = hd[i];
2669 
2670  return true;
2671 }
2672 */
2673 
2674 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2676 {
2677  //prepare the transformation matrices
2678  yarp::sig::Matrix T0 = lowerTorso->HUp;
2679  yarp::sig::Matrix T1 = lowerTorso->up->getH(2,true);
2680 
2681  int r=0;
2682  int ct=0;
2683  int c=0;
2684 
2685  //upper torso COM jacobian computation
2687 
2688  //lower torso COM jacobian computation
2690 
2691  //order the partial jacobians in the main jacobian.
2692  this->COM_Jacob.resize(6,32);
2693  COM_Jacob.zero();
2694 
2695  //Adding Lower Torso columns
2696  for (r=0; r<3; r++)
2697  {
2698  ct = 0;
2699  //left_leg
2700  for (c=0; c<6; c++, ct++)
2701  COM_Jacob(r,ct) = lowerTorso->COM_jacob_LF(r,c);
2702  //right leg
2703  for (c=0; c<6; c++, ct++)
2704  COM_Jacob(r,ct) = lowerTorso->COM_jacob_RT(r,c);
2705  //torso
2706  for (c=0; c<3; c++, ct++)
2707  COM_Jacob(r,ct) = lowerTorso->COM_jacob_UP(r,c);
2708  }
2709 
2710  Matrix tmp_Jac;
2711  tmp_Jac.resize(6,32);
2712 
2713 
2714  /*Upper body Jacobian Matrices (arms and head) require a transformation from NECK reference frame to ROOT
2715  reference frame. In order to get these velocity transform Jacobians to ROOT we have to multiply each by
2716  a 6x6 matrix containing the rotational matrix from T0*T1 (Transformation matrix from NECK to ROOT). */
2717 
2718  //Velocity Transform Jacobian
2719  Matrix T_ad;
2720  T_ad.resize(6,6);
2721  T_ad = adjoint(T0*T1);
2722  Matrix RotZero; RotZero.resize(3,3); RotZero.zero();
2723  T_ad.setSubmatrix(RotZero,0,3);
2724 
2725  tmp_Jac.zero();
2726  tmp_Jac.setSubmatrix(T_ad*upperTorso->COM_jacob_LF,0,15);
2727  Matrix larm = T_ad*upperTorso->COM_jacob_LF;
2728  COM_Jacob+=tmp_Jac;
2729 
2730  tmp_Jac.zero();
2731  tmp_Jac.setSubmatrix(T_ad*upperTorso->COM_jacob_RT,0,22);
2732  Matrix rarm = T_ad*upperTorso->COM_jacob_RT;
2733  COM_Jacob+=tmp_Jac;
2734 
2735  tmp_Jac.zero();
2736  yarp::sig::Matrix COMHead = upperTorso->COM_jacob_UP;
2737  tmp_Jac.setSubmatrix(T_ad*COMHead.submatrix(0,5,0,2),0,29);
2738 
2739  Matrix mm = T_ad*upperTorso->COM_jacob_UP;
2740  COM_Jacob+=tmp_Jac;
2741 
2742  return true;
2743 }
2744 
2745 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
2747 {
2748  jac = COM_Jacob;
2749  Matrix T_rotT0 = adjoint(lowerTorso->HUp);
2750  Matrix RotZero; RotZero.resize(3,3); RotZero.zero();
2751  T_rotT0.setSubmatrix(RotZero,0,3);
2752  // T_rotT0 is a similar Transformation matrix for putting the Torso GeoJacobian from its base link
2753  // reference frame to the ROOT reference frame.
2754  Matrix Jac_Torso = T_rotT0*lowerTorso->up->GeoJacobian();
2755 
2756  Matrix T0 = lowerTorso->HUp;
2757  Matrix T1 = lowerTorso->up->getH(2,true);
2758  Matrix H_T0T1; H_T0T1.resize(4,4); H_T0T1 = T0*T1;
2759  Matrix rotT0T1; rotT0T1.resize(3,3); rotT0T1 = H_T0T1.submatrix(0,2,0,2);
2760  // Left Arm COM seen from Root RcL i.e. R*NcL where NcL is the COM vector of the left arm w.r.t. Neck
2761  Vector LFcom; LFcom = upperTorso->total_COM_LF;
2762  Vector RcL = rotT0T1*(LFcom.subVector(0,2));
2763 
2764  // Right Arm COM seen from Root RcR i.e. R*NcR where NcR is the COM vector of the left arm w.r.t. Neck
2765  Vector RTcom; RTcom = upperTorso->total_COM_RT;
2766  Vector RcR = rotT0T1*(RTcom.subVector(0,2));
2767 
2768  //Head COM Seen from ROOT RcH i.e. R*NcH where NcH is the com of the head w.r.t the Neck
2769  Vector HDcom; HDcom = upperTorso->total_COM_UP;
2770  Vector RcH = rotT0T1*(HDcom.subVector(0,2));
2771 
2772  // First additional matrix RIGHT ARM
2773  Matrix L1; L1.resize(6,3); L1.zero();
2774  L1.setRow(0, -1*(Jac_Torso.getRow(5)*RcR(1)) + Jac_Torso.getRow(4)*RcR(2));
2775  L1.setRow(1, Jac_Torso.getRow(5)*RcR(0) + -1*Jac_Torso.getRow(3)*RcR(2));
2776  L1.setRow(2, -1*(Jac_Torso.getRow(4)*RcR(0)) + Jac_Torso.getRow(3)*RcR(1));
2777  // Second additional matrix LEFT ARM
2778  Matrix L2; L2.resize(6,3); L2.zero();
2779  L2.setRow(0, -1*(Jac_Torso.getRow(5)*RcL(1)) + Jac_Torso.getRow(4)*RcL(2));
2780  L2.setRow(1, Jac_Torso.getRow(5)*RcL(0) + -1*Jac_Torso.getRow(3)*RcL(2));
2781  L2.setRow(2, -1*(Jac_Torso.getRow(4)*RcL(0)) + Jac_Torso.getRow(3)*RcL(1));
2782  // Third addition matrix HEAD
2783  Matrix L3; L3.resize(6,3); L3.zero();
2784  L3.setRow(0, -1*(Jac_Torso.getRow(5)*RcH(1)) + Jac_Torso.getRow(4)*RcH(2));
2785  L3.setRow(1, Jac_Torso.getRow(5)*RcH(0) + -1*Jac_Torso.getRow(3)*RcH(2));
2786  L3.setRow(2, -1*(Jac_Torso.getRow(4)*RcH(0)) + Jac_Torso.getRow(3)*RcH(1));
2787 
2788  //Preparing the hright matrix for passing COM Jacobian toRIGHT FOOT REFERENCE FRAME
2789  Matrix hright;
2790  hright.resize(4,4); hright.zero();
2791  hright(0,0)=1.0; // 1 0 0
2792  hright(1,2)=1.0; // 0 0 1
2793  hright(2,1)=-1.0; // 0 -1 0
2794  hright(3,3)=1.0; //
2795  hright(2,3)=-0.1199;hright(1,3)=0.0681;
2796 
2797  Matrix rRf; rRf.resize(6,6); rRf.zero();
2798  Matrix rTf; rTf.resize(4,4); rTf.zero();
2799  Matrix fTr; fTr.resize(4,4); fTr.zero();
2800  Matrix fLr; fLr.resize(6,32); fLr.zero();
2801  Matrix fRr; fRr.resize(6,6); fRr.zero();
2802  Vector rCw; rCw.resize(3); rCw.zero(); //Whole Body COM w.r.t. ROOT.
2803  rCw = whole_COM;
2804  Vector fCw; fCw.resize(3); fCw.zero();
2805 
2806  Matrix fJr; fJr.resize(6,32); fJr.zero(); //Right Leg GeoJacobian from Root to Foot.
2807  Matrix rJf; rJf.resize(6,32); rJf.zero(); //Right Leg GeoJacobianf rom Foot to Root.
2808  Matrix HH; HH.resize(6,32); HH.zero(); //To be used to obtain fJr.
2809  Matrix LL; LL.resize(3,3); LL.zero(); //Used to build HH.
2810  Matrix hright_rot6x6; hright_rot6x6.resize(6,6); hright_rot6x6.zero(); //used to rotate right leg jacobian to root
2811 
2812 #ifdef DEBUG_FOOT_COM
2813 //DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING
2814 //DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING
2815 
2816  //Transforming the Whole Body (WB) COM Jacobian to the right foot reference frame
2817  rTf = lowerTorso->right->getH(); //Here we get rTf
2818  rTf = hright*rTf; //CHECKED
2819 
2820  hright_rot6x6.setSubmatrix(hright.submatrix(0,2,0,2), 0,0); //CHECKED.
2821  hright_rot6x6.setSubmatrix(hright.submatrix(0,2,0,2), 3,3);
2822 
2823  rRf.setSubmatrix(rTf.submatrix(0,2,0,2), 0,0); //CHECKED
2824  rRf.setSubmatrix(rTf.submatrix(0,2,0,2), 3,3); //CHECKED
2825  //Now we need the geometric Jacobian of the right leg from FOOT to ROOT.
2826  rJf.setSubmatrix( hright_rot6x6*lowerTorso->right->GeoJacobian(),0,6); //We first get the GeoJacobian from Foot to Root.
2827 
2828  //Now we build the H matrix that will be summed up to rJf in order to get fJr.
2829  //But first we obtain individual elements of H for which we build matrix LL
2830 
2831  //ALL LL LINES CHECKED.
2832  LL(0,0) = rRf(1,0)*rTf(2,3) - rRf(2,0)*rTf(1,3);
2833  LL(0,1) = rRf(2,0)*rTf(0,3) - rRf(0,0)*rTf(2,3);
2834  LL(0,2) = rRf(0,0)*rTf(1,3) - rRf(1,0)*rTf(0,3);
2835 
2836  LL(1,0) = rRf(1,1)*rTf(2,3) - rRf(2,1)*rTf(1,3);
2837  LL(1,1) = rRf(2,1)*rTf(0,3) - rRf(0,1)*rTf(2,3);
2838  LL(1,2) = rRf(0,1)*rTf(1,3) - rRf(1,1)*rTf(0,3);
2839 
2840  LL(2,0) = rRf(1,2)*rTf(2,3) - rRf(2,2)*rTf(1,3);
2841  LL(2,1) = rRf(2,2)*rTf(0,3) - rRf(0,2)*rTf(2,3);
2842  LL(2,2) = rRf(0,2)*rTf(1,3) - rRf(1,2)*rTf(0,3);
2843 
2844  //ALL HHs CHECKED
2845  HH.setSubrow( LL(0,0)*rJf.getRow(3) + LL(0,1)*rJf.getRow(4) + LL(0,2)*rJf.getRow(5) , 0,6);
2846  HH.setSubrow( LL(1,0)*rJf.getRow(3) + LL(1,1)*rJf.getRow(4) + LL(1,2)*rJf.getRow(5) , 1,6);
2847  HH.setSubrow( LL(2,0)*rJf.getRow(3) + LL(2,1)*rJf.getRow(4) + LL(2,2)*rJf.getRow(5) , 2,6);
2848 
2849  //Now we add up the HH matrix to rRf.transposed()*rJf to fintally obtain fJr.
2850  fJr = -1*HH - (rRf.transposed())*rJf; //TO BE ADDED TO fLr + fRr * rJw
2851 
2852  // Next matrix to fill in is fLr which is a 6x32 matrix filled with zeros except for the columns for the
2853  // right leg. This matrix will be added to our 'rotated' jac.
2854  // Thus, we first get fTr
2855  fTr.setSubmatrix(rTf.submatrix(0,2,0,2).transposed() ,0,0); //CHECKED
2856  fTr.setSubcol(-1*fTr.submatrix(0,2,0,2)*rTf.subcol(0,3,3) ,0,3); //CHECKED
2857  fTr(3,3) = 1;
2858 
2859  fRr.setSubmatrix(fTr.submatrix(0,2,0,2),0,0); //CHECKED
2860  fRr.setSubmatrix(fTr.submatrix(0,2,0,2),3,3); //CHECKED
2861  // Next we prepare the fLr matrix
2862  fCw = fTr.submatrix(0,2,0,2)*rCw; //NEEDED LIKE THIS FROM THE EQUATION
2863  fLr.setSubrow(-fCw(1)*fJr.getRow(5) + fCw(2)*fJr.getRow(4), 0,6);
2864  fLr.setSubrow( fCw(0)*fJr.getRow(5) - fCw(2)*fJr.getRow(3), 1,6);
2865  fLr.setSubrow(-fCw(0)*fJr.getRow(4) + fCw(1)*fJr.getRow(3), 2,6);
2866 
2867 //DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING
2868 //DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING DEBUGGING
2869 #endif
2870 
2871  Vector temp_com; temp_com.resize(4,1); temp_com.zero();
2872 
2873  unsigned int r,c,ct=0;
2874  double tmp, tmp2; tmp = tmp2 = 0.0;
2875  switch (which_part)
2876  {
2877  case BODY_PART_ALL:
2878  for (r=0; r<6; r++)
2879  {
2880  ct = 0;
2881  tmp = lowerTorso->total_mass_LF / whole_mass; for (c=0; c<6; c++, ct++) jac(r,ct) *= tmp;
2882  tmp = lowerTorso->total_mass_RT / whole_mass; for (c=0; c<6; c++, ct++) jac(r,ct) *= tmp;
2885  for (c=0; c<3; c++, ct++){
2886  jac(r,ct) *= tmp;
2887  jac(r,ct) += tmp2*Jac_Torso(r,c);
2888  jac(r,ct) += (upperTorso->total_mass_RT/whole_mass)*L1(r,c) + (upperTorso->total_mass_LF/whole_mass)*L2(r,c) + (upperTorso->total_mass_UP/whole_mass)*L3(r,c);
2889  }
2890  tmp = upperTorso->total_mass_LF / whole_mass; for (c=0; c<7; c++, ct++) jac(r,ct) *= tmp;
2891  tmp = upperTorso->total_mass_RT / whole_mass; for (c=0; c<7; c++, ct++) jac(r,ct) *= tmp;
2892  tmp = upperTorso->total_mass_UP / whole_mass; for (c=0; c<3; c++, ct++) jac(r,ct) *= tmp;
2893  }
2894 
2895  #ifdef DEBUG_FOOT_COM
2896  //Now we can finally add all the matricial terms that compose fJw
2897  jac = fJr + fLr + fRr*jac;
2898  temp_com.setSubvector(0,whole_COM); //CHECKED
2899  temp_com(3)=1; //CHECKED
2900  if (sw_getcom)
2901  {
2902  temp_com = fTr*temp_com; //CHECKED
2903  whole_COM = temp_com.subVector(0,2); //CHECKED
2904  }
2905  // whole_COM(0) = -temp_com(2);
2906  // whole_COM(2) = temp_com(0);
2907  #endif
2908 
2909  break;
2910  case LOWER_BODY_PARTS:
2911  for (r=0; r<6; r++)
2912  {
2913  ct = 0;
2914  tmp = lowerTorso->total_mass_LF / lower_mass; for (c=0; c<6; c++, ct++) jac(r,ct) *= tmp;
2915  tmp = lowerTorso->total_mass_RT / lower_mass; for (c=0; c<6; c++, ct++) jac(r,ct) *= tmp;
2916  tmp = lowerTorso->total_mass_UP / lower_mass; for (c=0; c<3; c++, ct++) jac(r,ct) *= tmp;
2917  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2918  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2919  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2920  }
2921 
2922 
2923  break;
2924  case UPPER_BODY_PARTS:
2925  for (r=0; r<6; r++)
2926  {
2927  ct = 0;
2928  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2929  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2930  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2931  tmp = upperTorso->total_mass_LF / upper_mass; for (c=0; c<7; c++, ct++) jac(r,ct) *= tmp;
2932  tmp = upperTorso->total_mass_RT / upper_mass; for (c=0; c<7; c++, ct++) jac(r,ct) *= tmp;
2933  tmp = upperTorso->total_mass_UP / upper_mass; for (c=0; c<3; c++, ct++) jac(r,ct) *= tmp;
2934 
2935  fprintf(stderr, "COM_Jacob2\n %s\n\n", COM_Jacob.submatrix(0,5,29,31).toString().c_str());
2936 
2937  }
2938  break;
2939  case LEFT_LEG:
2940  for (r=0; r<6; r++)
2941  {
2942  ct = 0;
2943  //for (c=0; c<6; c++, ct++) jac(r,ct) *= 1;
2944  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2945  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2946  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2947  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2948  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2949  }
2950  break;
2951  case RIGHT_LEG:
2952  for (r=0; r<6; r++)
2953  {
2954  ct = 0;
2955  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2956  //for (c=0; c<6; c++, ct++) jac(r,ct) *= 1;
2957  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2958  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2959  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2960  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2961  }
2962  break;
2963  case TORSO:
2964  for (r=0; r<6; r++)
2965  {
2966  ct = 0;
2967  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2968  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2969  //for (c=0; c<3; c++, ct++) jac(r,ct) *= 1;
2970  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2971  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2972  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2973  }
2974  break;
2975  case LEFT_ARM:
2976  for (r=0; r<6; r++)
2977  {
2978  ct = 0;
2979  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2980  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2981  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2982  //for (c=0; c<7; c++, ct++) jac(r,ct) *= 1;
2983  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2984  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2985  }
2986  break;
2987  case RIGHT_ARM:
2988  for (r=0; r<6; r++)
2989  {
2990  ct = 0;
2991  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2992  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2993  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2994  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2995  //for (c=0; c<7; c++, ct++) jac(r,ct) *= 1;
2996  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2997  }
2998  break;
2999  case HEAD:
3000  for (r=0; r<6; r++)
3001  {
3002  ct = 0;
3003  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
3004  for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
3005  for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
3006  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
3007  for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
3008  //for (c=0; c<3; c++, ct++) jac(r,ct) *= 1;
3009  }
3010  break;
3011  }
3012  return true;
3013 }
3014 
3015 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
3016 bool iCubWholeBody::EXPERIMENTAL_getCOMvelocity(BodyPart which_part, Vector &com_vel, Vector &dq)
3017 {
3018  Matrix jac;
3019  sw_getcom = 0;
3020  EXPERIMENTAL_getCOMjacobian(which_part,jac);
3021  sw_getcom = 1;
3022 
3023  Vector jvel;
3024  getAllVelocities(jvel);
3025  dq = jvel;
3026 
3027  com_vel = jac*jvel;
3028  return true;
3029 }
3030 
3031 
3032 
iCub::iDyn::iDynNode::dw
yarp::sig::Vector dw
angular acceleration
Definition: iDynBody.h:550
iCub::iDyn::iCubWholeBody::~iCubWholeBody
~iCubWholeBody()
Standard destructor.
Definition: iDynBody.cpp:2414
iCub::iDyn::iDynSensorTorsoNode::getTorsoAngAcc
yarp::sig::Vector getTorsoAngAcc() const
Return the torso angular acceleration.
Definition: iDynBody.cpp:1908
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
iCub::iDyn::JacobType
JacobType
Definition: iDynBody.h:99
iCub::iDyn::iDynChain::getForces
yarp::sig::Matrix getForces() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
Definition: iDyn.cpp:859
iCub::iDyn::iDynSensorTorsoNode::clearContactList
void clearContactList()
Definition: iDynBody.cpp:2220
iCub::iDyn::version_tag::head_subversion
int head_subversion
Definition: iDynBody.h:111
iCub::iDyn::iDynSensorNode::addLimb
virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN)
Add one limb to the node, defining its RigidBodyTransformation.
iCub::skinDynLib
Definition: common.h:35
H
H
Definition: compute_ekf_fast.m:27
iCub::iDyn::iDynSensorTorsoNode::total_COM_LF
yarp::sig::Vector total_COM_LF
Definition: iDynBody.h:1087
iCub::iDyn::iDynNode::w
yarp::sig::Vector w
angular velocity
Definition: iDynBody.h:548
iCub::iDyn::iCubTorsoDyn
Definition: iDyn.h:1409
iCub::iDyn::iDynNode::F
yarp::sig::Vector F
force
Definition: iDynBody.h:554
iCub::iDyn::iDynContactSolver::clearContactList
void clearContactList()
Clear the contact list.
Definition: iDynContact.cpp:78
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
iCub::iDyn::iCubWholeBody::getAllPositions
bool getAllPositions(yarp::sig::Vector &pos)
Retrieves a vector containing the anglular position of all the iCub joints, ordered in this way: left...
Definition: iDynBody.cpp:2607
iCub::iDyn::iCubWholeBody::COM_Jacob
yarp::sig::Matrix COM_Jacob
Definition: iDynBody.h:1493
iCub::iDyn::iDynNode::ddp
yarp::sig::Vector ddp
linear acceleration
Definition: iDynBody.h:552
iCub::iDyn::iCubLowerTorso::build
void build()
Definition: iDynBody.cpp:2324
iCub::iDyn::RBT_NODE_OUT
@ RBT_NODE_OUT
Definition: iDynBody.h:92
pos
const dReal * pos
Definition: iCub_Sim.cpp:62
e
e
Definition: compute_ekf_fast.m:13
iCub::iDyn::iCubWholeBody::EXPERIMENTAL_computeCOMjacobian
bool EXPERIMENTAL_computeCOMjacobian()
Performs the computation of the center of mass jacobian of the whole iCub.
Definition: iDynBody.cpp:2675
iCub::iDyn::iDynNode::verbose
unsigned int verbose
verbosity flag
Definition: iDynBody.h:545
iCub::iDyn::version_tag::legs_version
int legs_version
Definition: iDynBody.h:112
iCub::iDyn::NewEulMode
NewEulMode
Definition: iDynInv.h:64
iDyn.h
iCub::iDyn::iDynSensorNode::setWrenchMeasure
virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M, bool afterAttach=false)
Set the Wrench measures on the limbs attached to the node.
iCub::iDyn::iDynSensorTorsoNode::right
iDyn::iDynLimb * right
right limb
Definition: iDynBody.h:1081
iCub::iDyn::iDynSensorTorsoNode::HRight
yarp::sig::Matrix HRight
roto-translational matrix defining the right limb base frame with respect to the torso node
Definition: iDynBody.h:1055
iCub::iDyn::iDynChain::getMass
double getMass(const unsigned int i) const
Returns the i-th link mass.
Definition: iDyn.cpp:822
iCub::iDyn::iDynSensorTorsoNode::setSensorMeasurement
bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left)
Set the FT sensor measurements on the sensor in right and left limb.
iCub::skinDynLib::RIGHT_LEG
@ RIGHT_LEG
Definition: common.h:46
iCub::iDyn::iDynSensorNode::sensorList
std::deque< iDyn::iDynSensor * > sensorList
the list of iDynSensors used to solve each limb after FT sensor measurements
Definition: iDynBody.h:922
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
iCub::iDyn::iDynSensorTorsoNode::computeCOM
bool computeCOM()
Performs the computation of the center of mass (COM) of the node.
Definition: iDynBody.cpp:1912
iCub::iDyn::iCubLegDyn
Definition: iDyn.h:1451
iCub::iDyn::iDynSensorTorsoNode::up
iDyn::iDynLimb * up
central-up limb
Definition: iDynBody.h:1083
iCub::iDyn::iCubWholeBody::iCubWholeBody
iCubWholeBody(version_tag _tag, const NewEulMode mode=DYNAMIC, unsigned int verbose=iCub::skinDynLib::VERBOSE)
Constructor: build the nodes and creates the whole body.
Definition: iDynBody.cpp:2400
iCub::iDyn::iDynNode
Definition: iDynBody.h:531
iCub::iDyn::iCubWholeBody::EXPERIMENTAL_getCOMvelocity
bool EXPERIMENTAL_getCOMvelocity(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &vel, yarp::sig::Vector &dq)
Retrieves a 3x1 vector containing the velocity of the robot COM.
Definition: iDynBody.cpp:3016
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:740
iCub::iDyn::iDynSensorTorsoNode
Definition: iDynBody.h:1046
iCub::iDyn::iCubLegDynV2
Definition: iDyn.h:1491
iCub::iKin::iKinChain::GeoJacobian
yarp::sig::Matrix GeoJacobian(const unsigned int i)
Returns the geometric Jacobian of the ith link.
Definition: iKinFwd.cpp:1020
iCub::iDyn::iDynSensorTorsoNode::getTorques
yarp::sig::Vector getTorques(const std::string &limbType)
Return the chosen limb torques, as a Nx1 vector.
Definition: iDynBody.cpp:1890
iCub::iDyn::iDynNode::solveKinematics
bool solveKinematics()
Main function to manage the exchange of kinematic information among the limbs attached to the node.
Definition: iDynBody.cpp:462
iCub::iDyn::iCubNeckInertialDyn
Definition: iDyn.h:1532
iCub::iDyn::iDynChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::skinDynLib::LEFT_ARM
@ LEFT_ARM
Definition: common.h:46
iCub::iDyn::iCubUpperTorso::build
void build()
Definition: iDynBody.cpp:2243
iCub::iDyn::iCubUpperTorso::~iCubUpperTorso
~iCubUpperTorso()
Destructor.
Definition: iDynBody.cpp:2303
iCub::iDyn::iDynNode::setKinematicMeasure
bool setKinematicMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the kinematic measurement (w,dw,ddp) on the limb where the kinematic flow is of type RBT_NODE_IN.
Definition: iDynBody.cpp:561
iCub::iDyn::iDynSensorTorsoNode::total_mass_UP
double total_mass_UP
Definition: iDynBody.h:1089
iCub::iDyn::iDynSensorNode
Definition: iDynBody.h:916
iCub::skinDynLib::RIGHT_ARM
@ RIGHT_ARM
Definition: common.h:46
iCub::iDyn::iCubWholeBody::computeCOM
bool computeCOM()
Performs the computation of the center of mass (COM) of the whole iCub.
Definition: iDynBody.cpp:2473
iCub::iDyn::iDynSensorNode::solveWrench
virtual bool solveWrench()
Main function to manage the exchange of wrench information among the limbs attached to the node.
Definition: iDynBody.cpp:1352
F
F
Definition: compute_ekf_fast.m:22
iCub::iDyn::iCubLowerTorso::iCubLowerTorso
iCubLowerTorso(version_tag _tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:2317
iCub::iDyn::iCubWholeBody::attachLowerTorso
void attachLowerTorso(const yarp::sig::Vector &FM_right_leg, const yarp::sig::Vector &FM_left_leg)
Connect upper and lower torso: this procedure handles the exchange of kinematic and wrench variables ...
Definition: iDynBody.cpp:2421
iCub::iDyn::iDynSensorTorsoNode::getMoments
yarp::sig::Matrix getMoments(const std::string &limbType)
Return the chosen limb moments, as a 6xN matrix.
Definition: iDynBody.cpp:1878
iCub::iDyn::version_tag::head_version
int head_version
Definition: iDynBody.h:110
iCub::iDyn::iDynSensorTorsoNode::getTorsoMoment
yarp::sig::Vector getTorsoMoment() const
Return the torso moment.
Definition: iDynBody.cpp:1904
iCub::iKin::iKinChain::getAng
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:619
iCub::iDyn::iDynSensorTorsoNode::COM_jacob_LF
yarp::sig::Matrix COM_jacob_LF
Definition: iDynBody.h:1093
iCub::iDyn::DYNAMIC
@ DYNAMIC
Definition: iDynInv.h:64
iCub::skinDynLib::BODY_PART_ALL
@ BODY_PART_ALL
Definition: common.h:47
iCub::iDyn
Definition: iDyn.h:81
iCub::iDyn::iDynSensorTorsoNode::HUp
yarp::sig::Matrix HUp
roto-translational matrix defining the central-up base frame with respect to the torso node
Definition: iDynBody.h:1051
iCub::iDyn::iDynSensorTorsoNode::getTorsoAngVel
yarp::sig::Vector getTorsoAngVel() const
Return the torso angular velocity.
Definition: iDynBody.cpp:1906
iCub::iDyn::STATIC
@ STATIC
Definition: iDynInv.h:64
iCub::iDyn::iCubWholeBody::upper_mass
double upper_mass
Definition: iDynBody.h:1489
iCub::iDyn::iCubWholeBody::lowerTorso
iCubLowerTorso * lowerTorso
pointer to LowerTorso = torso + right leg + left leg
Definition: iDynBody.h:1484
iCub::iKin
Definition: iKinFwd.h:71
iCub::iDyn::SensorLinkNewtonEuler
Definition: iDynInv.h:718
iCub::iDyn::iDynSensorTorsoNode::right_name
std::string right_name
name of right limb
Definition: iDynBody.h:1060
mode
GLenum mode
Definition: rendering.cpp:48
iCub::iDyn::iCubUpperTorso
Definition: iDynBody.h:1401
iCub::iDyn::iDynNode::COM
yarp::sig::Vector COM
COM position of the node.
Definition: iDynBody.h:558
iCub::iDyn::iDynSensorTorsoNode::total_COM_UP
yarp::sig::Vector total_COM_UP
COMs and masses of the limbs.
Definition: iDynBody.h:1086
iCub::iDyn::iDynSensorTorsoNode::getTorsoLinAcc
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
Definition: iDynBody.cpp:1910
iCub::iDyn::iDynSensorNode::iDynSensorNode
iDynSensorNode(const NewEulMode _mode=DYNAMIC)
Default constructor.
Definition: iDynBody.cpp:1327
iCub::iDyn::DYNAMIC_CORIOLIS_GRAVITY
@ DYNAMIC_CORIOLIS_GRAVITY
Definition: iDynInv.h:64
iCub::iDyn::iDynSensorTorsoNode::iDynSensorTorsoNode
iDynSensorTorsoNode(const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:1729
iCub::iDyn::iDynSensorTorsoNode::getTorsoForce
yarp::sig::Vector getTorsoForce() const
Return the torso force.
Definition: iDynBody.cpp:1902
iCub::iDyn::iDynSensorTorsoNode::setAng
yarp::sig::Vector setAng(const std::string &limbType, const yarp::sig::Vector &_q)
Set joints angle position in the chosen limb.
iCub::iDyn::iCubLowerTorso
Definition: iDynBody.h:1437
iCub::skinDynLib::UPPER_BODY_PARTS
@ UPPER_BODY_PARTS
Definition: common.h:47
iCub::iDyn::iDynLimb
Definition: iDyn.h:1176
iCub::iDyn::iDynSensorTorsoNode::left
iDyn::iDynLimb * left
left limb
Definition: iDynBody.h:1079
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::iDyn::iCubWholeBody::whole_COM
yarp::sig::Vector whole_COM
Definition: iDynBody.h:1490
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
n
int n
Definition: debugFunctions.cpp:58
iCub::iDyn::RigidBodyTransformation
Definition: iDynBody.h:131
iCub::iDyn::iDynSensorTorsoNode::getNLinks
unsigned int getNLinks(const std::string &limbType) const
Definition: iDynBody.cpp:2208
iCub::iDyn::iCubWholeBody::EXPERIMENTAL_getCOMjacobian
bool EXPERIMENTAL_getCOMjacobian(iCub::skinDynLib::BodyPart which_part, yarp::sig::Matrix &jac)
Retrieves the result of the last COM jacobian computation.
Definition: iDynBody.cpp:2746
iCub::iDyn::iDynChain::setD2Ang
yarp::sig::Vector setD2Ang(const yarp::sig::Vector &ddq)
Sets the free joint angles acceleration to values of ddq[i].
iCub::iDyn::iDynSensorTorsoNode::~iDynSensorTorsoNode
~iDynSensorTorsoNode()
Destructor.
Definition: iDynBody.cpp:1741
iCub::iDyn::iDynNode::info
std::string info
info or useful notes
Definition: iDynBody.h:542
iCub::iDyn::iDynSensorTorsoNode::name
std::string name
the torso node name
Definition: iDynBody.h:1064
iCub::iDyn::iDynSensorTorsoNode::COM_jacob_UP
yarp::sig::Matrix COM_jacob_UP
Definition: iDynBody.h:1092
iCub::iDyn::iDynSensor
Definition: iDynInv.h:1577
iCub::iDyn::iCubWholeBody::getAllVelocities
bool getAllVelocities(yarp::sig::Vector &vel)
Retrieves a vector containing the velocities of all the iCub joints, ordered in this way: left leg (6...
Definition: iDynBody.cpp:2629
iCub::iDyn::iCubWholeBody::sw_getcom
double sw_getcom
Definition: iDynBody.h:1494
iCub::iDyn::iDynNode::rbtList
std::deque< RigidBodyTransformation > rbtList
the list of RBT
Definition: iDynBody.h:536
iCub::iDyn::iDynSensorTorsoNode::left_name
std::string left_name
name of left limb
Definition: iDynBody.h:1058
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::skinDynLib::VERBOSE
@ VERBOSE
Definition: common.h:38
iCub::iDyn::iDynNode::mode
NewEulMode mode
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
Definition: iDynBody.h:539
iCub::iDyn::iDynSensorTorsoNode::EXPERIMENTAL_computeCOMjacobian
bool EXPERIMENTAL_computeCOMjacobian()
Performs the computation of the center of mass jacobian of the node.
Definition: iDynBody.cpp:1970
iCub::iDyn::iDynSensorTorsoNode::update
bool update()
Main method for solving kinematics and wrench among limbs, where information are shared.
Definition: iDynBody.cpp:1794
iCub::iDyn::iCubLowerTorso::~iCubLowerTorso
~iCubLowerTorso()
Destructor.
Definition: iDynBody.cpp:2387
iCub::skinDynLib::LEFT_LEG
@ LEFT_LEG
Definition: common.h:46
iCub::iDyn::iCubWholeBody::whole_mass
double whole_mass
masses and position of the center of mass of the iCub sub-parts
Definition: iDynBody.h:1487
iCub::skinDynLib::LOWER_BODY_PARTS
@ LOWER_BODY_PARTS
Definition: common.h:47
iCub::iDyn::iDynChain::getDAng
yarp::sig::Vector getDAng()
Returns the current free joint angles velocity.
Definition: iDyn.cpp:584
iCub::ctrl::cross
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
iCub::iDyn::iDynSensorTorsoNode::total_mass_LF
double total_mass_LF
Definition: iDynBody.h:1090
iCub::iDyn::iDynNode::addLimb
virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN, bool hasSensor=false)
Add one limb to the node, defining its RigidBodyTransformation.
Definition: iDynBody.cpp:442
iCub::iDyn::version_tag
Definition: iDynBody.h:108
iCub::iDyn::iDynSensorNode::estimateSensorsWrench
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Exploit iDynInvSensor methods to retrieve FT sensor measurements after solving wrenches in the limbs.
Definition: iDynBody.cpp:1562
iCub::iDyn::iDynSensorTorsoNode::total_mass_RT
double total_mass_RT
Definition: iDynBody.h:1091
iCub::iDyn::iCubWholeBody::upperTorso
iCubUpperTorso * upperTorso
pointer to UpperTorso = head + right arm + left arm
Definition: iDynBody.h:1482
iDynBody.h
iCub::skinDynLib::BodyPart
BodyPart
Definition: common.h:44
iCub::iDyn::iCubArmNoTorsoDyn
Definition: iDyn.h:1368
iCub::iDyn::iDynNode::Mu
yarp::sig::Vector Mu
moment
Definition: iDynBody.h:556
iCub::iDyn::iCubWholeBody::lower_mass
double lower_mass
Definition: iDynBody.h:1488
iCub::iDyn::iDynSensorNode::howManySensors
unsigned int howManySensors() const
Definition: iDynBody.cpp:1707
iCub::iDyn::iDynContactSolver
Definition: iDynContact.h:52
iCub::iDyn::iCubUpperTorso::iCubUpperTorso
iCubUpperTorso(version_tag tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:2236
v
static int v
Definition: iCub_Sim.cpp:42
iCub::iDyn::iCubWholeBody::rbt
RigidBodyTransformation * rbt
the rigid body transformation linking the UpperTorso node with the final link of the iCubTorsoDyn cha...
Definition: iDynBody.h:1476
iCub::skinDynLib::TORSO
@ TORSO
Definition: common.h:46
iCub::iDyn::KINBWD_WREBWD
@ KINBWD_WREBWD
Definition: iDynInv.h:69
iCub::iDyn::iCubWholeBody::tag
version_tag tag
Definition: iDynBody.h:1477
iCub::iDyn::FlowType
FlowType
Definition: iDynBody.h:92
iCub::iDyn::iCubWholeBody::upper_COM
yarp::sig::Vector upper_COM
Definition: iDynBody.h:1491
iCub::iDyn::iDynSensorTorsoNode::up_name
std::string up_name
name of central-up limb
Definition: iDynBody.h:1062
iCub::iDyn::iCubWholeBody::lower_COM
yarp::sig::Vector lower_COM
Definition: iDynBody.h:1492
iCub::iDyn::iDynSensorTorsoNode::HLeft
yarp::sig::Matrix HLeft
roto-translational matrix defining the left limb base frame with respect to the torso node
Definition: iDynBody.h:1053
iCub::iDyn::iDynSensorTorsoNode::setInertialMeasure
bool setInertialMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the inertial sensor measurements on the central-up limb.
Definition: iDynBody.cpp:1753
iCub::iDyn::KINFWD_WREBWD
@ KINFWD_WREBWD
Definition: iDynInv.h:69
iCub::iDyn::iCubLowerTorso::tag
version_tag tag
Build the node.
Definition: iDynBody.h:1443
iCub::iDyn::iCubNeckInertialDynV2
Definition: iDyn.h:1565
iCub::iDyn::iDynChain::getD2Ang
yarp::sig::Vector getD2Ang()
Returns the current free joint angles acceleration.
Definition: iDyn.cpp:597
fprintf
fprintf(fid,'\n')
iCub::iDyn::iDynSensorTorsoNode::setDAng
yarp::sig::Vector setDAng(const std::string &limbType, const yarp::sig::Vector &_dq)
Set joints angle velocity in the chosen limb.
iCub::iDyn::iDynLimb::getType
std::string getType()
Returns the Limb type as a string.
Definition: iDyn.h:1300
iCub::iDyn::iDynSensorTorsoNode::total_COM_RT
yarp::sig::Vector total_COM_RT
Definition: iDynBody.h:1088
iCub::iDyn::iDynSensorTorsoNode::getForces
yarp::sig::Matrix getForces(const std::string &limbType)
Return the chosen limb forces, as a 6xN matrix.
Definition: iDynBody.cpp:1866
strain::dsp::q15::zero
const Q15 zero
Definition: strain.h:67
iCub::iDyn::iDynChain::getCOM
yarp::sig::Matrix getCOM(unsigned int iLink)
Return the COM matrix of the i-th link.
Definition: iDyn.cpp:1585
iCub::iDyn::DYNAMIC_W_ROTOR
@ DYNAMIC_W_ROTOR
Definition: iDynInv.h:64
iCub::iDyn::JAC_KIN
@ JAC_KIN
Definition: iDynBody.h:99
iCub::iDyn::iCubWholeBody::getCOM
bool getCOM(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &COM, double &mass)
Retrieves the result of the last COM computation.
Definition: iDynBody.cpp:2511
iCub::iDyn::iDynSensorTorsoNode::COM_jacob_RT
yarp::sig::Matrix COM_jacob_RT
Definition: iDynBody.h:1094
iCub::iDyn::iDynSensorTorsoNode::setD2Ang
yarp::sig::Vector setD2Ang(const std::string &limbType, const yarp::sig::Vector &_ddq)
Set joints angle acceleration in the chosen limb.
iCub::skinDynLib::HEAD
@ HEAD
Definition: common.h:46
iCub::iDyn::iDynChain::getMoments
yarp::sig::Matrix getMoments() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
Definition: iDyn.cpp:867
iCub::iDyn::iDynSensorTorsoNode::rightSensor
iDyn::iDynContactSolver * rightSensor
right FT sensor and solver
Definition: iDynBody.h:1075
iCub::iDyn::iDynChain::getTorques
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDyn.cpp:875
iCub::iDyn::iDynSensorTorsoNode::getD2Ang
yarp::sig::Vector getD2Ang(const std::string &limbType)
Get joints angle acceleration in the chosen limb.
Definition: iDynBody.cpp:2172
iCub::iDyn::iDynNode::howManyWrenchInputs
unsigned int howManyWrenchInputs(bool afterAttach=false) const
Return the number of limbs with wrench input, i.e.
Definition: iDynBody.cpp:785
iCub::iDyn::RBT_NODE_IN
@ RBT_NODE_IN
Definition: iDynBody.h:92
iCub::iDyn::iDynSensor::setSensorMeasures
bool setSensorMeasures(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
Set the sensor measured force and moment.
iCub::iDyn::iDynSensorTorsoNode::getDAng
yarp::sig::Vector getDAng(const std::string &limbType)
Get joints angle velocity in the chosen limb.
Definition: iDynBody.cpp:2124
iCub::iDyn::iDynSensorTorsoNode::getAng
yarp::sig::Vector getAng(const std::string &limbType)
Get joints angle position in the chosen limb.
Definition: iDynBody.cpp:2076
iCub::iDyn::iDynChain::setDAng
yarp::sig::Vector setDAng(const yarp::sig::Vector &dq)
Sets the free joint angles velocity to values of dq[i].
iCub::iDyn::iCubUpperTorso::tag
version_tag tag
Build the node.
Definition: iDynBody.h:1407
iCub::iDyn::iDynSensorTorsoNode::leftSensor
iDyn::iDynContactSolver * leftSensor
Build the node.
Definition: iDynBody.h:1073