iCub-main
iDynInv.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2011 RobotCub Consortium
3  * Author: Serena Ivaldi, Matteo Fumagalli
4  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
5  *
6  */
7 
43 #ifndef __IDYNINV_H__
44 #define __IDYNINV_H__
45 
46 #include <yarp/sig/Vector.h>
47 #include <yarp/sig/Matrix.h>
48 #include <iCub/ctrl/math.h>
49 #include <iCub/iKin/iKinFwd.h>
50 #include <iCub/iDyn/iDyn.h>
51 #include <iCub/skinDynLib/common.h>
52 #include <deque>
53 #include <string>
54 
55 
56 namespace iCub
57 {
58 
59 namespace iDyn
60 {
61 
62 // Newton-Euler types
63 // useful for iDyn, iDynInv, iDynBody
64 enum NewEulMode {STATIC,DYNAMIC,DYNAMIC_W_ROTOR,DYNAMIC_CORIOLIS_GRAVITY};
65 const std::string NewEulMode_s[4] = {"static","dynamic","dynamic with motor/rotor","dynamic with only Coriolis and gravitational terms"};
66 
67 // Kinematic and Wrench modes
68 enum ChainIterationMode { FORWARD, BACKWARD };
69 enum ChainComputationMode { KINFWD_WREFWD, KINFWD_WREBWD, KINBWD_WREFWD, KINBWD_WREBWD };
70 const std::string ChainIterationMode_s[2] = {"Forward (Base To End)","Backward (End To Base)"};
71 const std::string ChainComputationMode_s[4] = {"Kinematic Forward - Wrench Forward","Kinematic Forward - Wrench Backward","Kinematic Backward - Wrench Forward","Kinematic Backward - Wrench Backward"};
72 
73  class iDynLink;
74  class iDynChain;
75  class iDynLimb;
76  class iCubArmDyn;
77  class iCubArmNoTorsoDyn;
78  class iCubLegDyn;
79  class iCubEyeDyn;
80  class iCubEyeNeckRefDyn;
81  class iCubInertialSensorDyn;
82  class iDynSensor;
83  class iDynSensorLeg;
84  class iDynSensorArm;
85  class iGenericFrame;
86  class iFrameOnLink;
87  class iFTransformation;
88 
89 
95 class OneLinkNewtonEuler
96 {
97 protected:
98 
100  NewEulMode mode;
102  std::string info;
104  unsigned int verbose;
106  yarp::sig::Vector z0;
108  yarp::sig::Vector zm;
110  iDyn::iDynLink *link;
111 
112  //~~~~~~~~~~~~~~~~~~~~~~
113  // set methods
114  //~~~~~~~~~~~~~~~~~~~~~~
115 
122  virtual bool setForce(const yarp::sig::Vector &_F);
123 
130  virtual bool setMoment(const yarp::sig::Vector &_Mu);
131 
137  virtual void setTorque(const double _Tau);
138 
144  virtual bool setAngVel(const yarp::sig::Vector &_w);
145 
151  virtual bool setAngAcc(const yarp::sig::Vector &_dw);
157  virtual bool setLinAcc(const yarp::sig::Vector &_ddp);
164  virtual bool setLinAccC(const yarp::sig::Vector &_ddpC);
171  virtual bool setAngAccM(const yarp::sig::Vector &_dwM);
172 
173  //~~~~~~~~~~~~~~~~~~~~~~
174  // core computation
175  //~~~~~~~~~~~~~~~~~~~~~~
176 
181  void computeAngVel( OneLinkNewtonEuler *prev);
182 
188  void computeAngVelBackward( OneLinkNewtonEuler *prev);
189 
194  void computeAngAcc( OneLinkNewtonEuler *prev);
195 
200  void computeAngAccBackward( OneLinkNewtonEuler *prev);
201 
206  void computeLinAcc( OneLinkNewtonEuler *prev);
207 
212  void computeLinAccBackward( OneLinkNewtonEuler *prev);
213 
218  void computeLinAccC();
219 
225  void computeAngAccM( OneLinkNewtonEuler *prev);
226 
231  void computeForceBackward( OneLinkNewtonEuler *next);
232 
237  void computeForceForward( OneLinkNewtonEuler *prev);
238 
243  void computeMomentBackward( OneLinkNewtonEuler *next);
244 
249  void computeMomentForward( OneLinkNewtonEuler *prev);
250 
251 
252 public:
253 
257  OneLinkNewtonEuler(iDyn::iDynLink *dlink=NULL);
258 
262  OneLinkNewtonEuler(const NewEulMode _mode, unsigned int verb = iCub::skinDynLib::NO_VERBOSE, iDyn::iDynLink *dlink=NULL);
263 
267  virtual ~OneLinkNewtonEuler() {};
268 
272  void zero();
273 
278  virtual bool setAsBase(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp);
279  virtual bool setAsBase(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
280 
287  virtual bool setAsFinal(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
288 
296  virtual bool setAsFinal(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp);
297 
304  virtual bool setMeasuredFMu(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
305 
310  virtual bool setMeasuredTorque(const double _Tau);
311 
315  virtual std::string toString() const;
316 
317 
318  //~~~~~~~~~~~~~~~~~~~~~~
319  // set methods
320  //~~~~~~~~~~~~~~~~~~~~~~
321 
326  void setVerbose(unsigned int verb = iCub::skinDynLib::VERBOSE);
331  void setMode(const NewEulMode _mode);
337  bool setZM(const yarp::sig::Vector &_zm);
342  void setInfo(const std::string &_info);
343 
344 
345  //~~~~~~~~~~~~~~~~~~~~~~
346  // get methods
347  //~~~~~~~~~~~~~~~~~~~~~~
348 
352  NewEulMode getMode() const;
356  yarp::sig::Vector getZM() const;
360  std::string getInfo() const;
364  virtual const yarp::sig::Vector& getAngVel() const;
368  virtual const yarp::sig::Vector& getAngAcc() const;
372  virtual const yarp::sig::Vector& getAngAccM() const;
376  virtual const yarp::sig::Vector& getLinAcc() const;
380  virtual const yarp::sig::Vector& getLinAccC() const;
384  virtual const yarp::sig::Vector& getForce() const;
388  virtual const yarp::sig::Vector& getMoment(bool isBase = false) const;
392  virtual double getTorque() const;
396  virtual double getIm() const;
400  virtual double getFs() const;
404  virtual double getFv() const;
408  virtual double getD2q() const;
412  virtual double getDq() const;
416  virtual double getKr() const;
420  virtual double getMass() const;
424  virtual const yarp::sig::Matrix& getInertia() const;
428  virtual const yarp::sig::Matrix& getH();
432  virtual const yarp::sig::Matrix& getR();
436  virtual const yarp::sig::Matrix& getRC();
440  virtual const yarp::sig::Vector& getr(bool proj=false);
444  virtual const yarp::sig::Vector& getrC(bool proj=false);
445 
446 
447  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
448  // main computation methods
449  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
450 
455  void ForwardKinematics( OneLinkNewtonEuler *prev);
456 
461  void BackwardKinematics( OneLinkNewtonEuler *prev);
462 
467  void BackwardWrench( OneLinkNewtonEuler *next);
468 
473  void ForwardWrench( OneLinkNewtonEuler *prev);
474 
479  virtual void computeTorque(OneLinkNewtonEuler *prev);
480 
481 };
482 
491 class BaseLinkNewtonEuler : public OneLinkNewtonEuler
492 {
493 protected:
495  yarp::sig::Vector w;
497  yarp::sig::Vector dw;
499  yarp::sig::Vector ddp;
501  yarp::sig::Matrix H0;
503  yarp::sig::Vector F;
505  yarp::sig::Vector Mu;
507  yarp::sig::Vector Mu0;
509  double Tau;
510 
511  // dummy matrixes/vectors
512  const yarp::sig::Matrix eye3x3;
513  const yarp::sig::Matrix zeros3x3;
514  const yarp::sig::Vector zeros3;
515 
516 public:
517 
524  BaseLinkNewtonEuler(const yarp::sig::Matrix &_H0, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
525 
535  BaseLinkNewtonEuler(const yarp::sig::Matrix &_H0, const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw,
536  const yarp::sig::Vector &_ddp, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
537 
544  bool setAsBase(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp);
550  bool setAsBase(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
551 
552  //~~~~~~~~~~~~~~~~~~~~~~
553  // get methods
554  //~~~~~~~~~~~~~~~~~~~~~~
555 
556  const yarp::sig::Vector& getAngVel() const;
557  const yarp::sig::Vector& getAngAcc() const;
558  const yarp::sig::Vector& getAngAccM() const;
559  const yarp::sig::Vector& getLinAcc() const;
560  const yarp::sig::Vector& getLinAccC() const;
561 
562  // redefine the other unuseful methods
563  // to avoid errors due to missing link
564 
565  const yarp::sig::Vector& getForce() const;
566  const yarp::sig::Vector& getMoment(bool isBase = false) const;
567  double getTorque() const;
568  const yarp::sig::Matrix& getR();
569  const yarp::sig::Matrix& getRC();
570  double getIm() const;
571  double getD2q() const;
572  double getDq() const;
573  double getKr() const;
574  double getFs() const;
575  double getFv() const;
576  double getMass() const;
577  const yarp::sig::Matrix& getInertia()const;
578  const yarp::sig::Vector& getr(bool proj=false);
579  const yarp::sig::Vector& getrC(bool proj=false);
580 
581  //~~~~~~~~~~~~~~~~~~~~~~
582  // set methods
583  //~~~~~~~~~~~~~~~~~~~~~~
584 
585  bool setForce(const yarp::sig::Vector &_F);
586  bool setMoment(const yarp::sig::Vector &_Mu);
587  void setTorque(const double _Tau);
588  bool setAngVel(const yarp::sig::Vector &_w);
589  bool setAngAcc(const yarp::sig::Vector &_dw);
590  bool setLinAcc(const yarp::sig::Vector &_ddp);
591  bool setLinAccC(const yarp::sig::Vector &_ddpC);
592  bool setAngAccM(const yarp::sig::Vector &_dwM);
593 
594  // other methods
595 
596  std::string toString() const;
597 
598 
599 };
600 
601 
610 class FinalLinkNewtonEuler : public OneLinkNewtonEuler
611 {
612 protected:
614  yarp::sig::Vector w;
616  yarp::sig::Vector dw;
618  yarp::sig::Vector ddp;
620  yarp::sig::Vector F;
622  yarp::sig::Vector Mu;
624  yarp::sig::Matrix HN;
625 
626  // dummy matrices/vectors
627  const yarp::sig::Matrix eye4x4;
628  const yarp::sig::Matrix eye3x3;
629  const yarp::sig::Matrix zeros3x3;
630  const yarp::sig::Vector zeros3;
631 
632 public:
633 
639  FinalLinkNewtonEuler(const yarp::sig::Matrix &_HN, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
640 
648  FinalLinkNewtonEuler(const yarp::sig::Matrix &_HN, const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
649 
656  bool setAsFinal(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
657 
665  bool setAsFinal(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp);
666 
667 
668  //~~~~~~~~~~~~~~~~~~~~~~
669  // get methods
670  //~~~~~~~~~~~~~~~~~~~~~~
671  const yarp::sig::Vector& getForce() const;
672  const yarp::sig::Vector& getMoment(bool isBase = false) const;
673 
674  // redefine the other unuseful methods to avoid errors due to missing link
675  const yarp::sig::Vector& getAngVel() const;
676  const yarp::sig::Vector& getAngAcc() const;
677  const yarp::sig::Vector& getAngAccM() const;
678  const yarp::sig::Vector& getLinAcc() const;
679  const yarp::sig::Vector& getLinAccC() const;
680  double getTorque() const;
681  const yarp::sig::Matrix& getH();
682  const yarp::sig::Matrix& getR();
683  const yarp::sig::Matrix& getRC();
684  double getIm() const;
685  double getFs() const;
686  double getFv() const;
687  double getD2q() const;
688  double getDq() const;
689  double getKr() const;
690  double getMass() const;
691  const yarp::sig::Matrix& getInertia()const;
692  const yarp::sig::Vector& getr(bool proj=false);
693  const yarp::sig::Vector& getrC(bool proj=false);
694  bool setForce(const yarp::sig::Vector &_F);
695  bool setMoment(const yarp::sig::Vector &_Mu);
696  void setTorque(const double _Tau);
697  bool setAngVel(const yarp::sig::Vector &_w);
698  bool setAngAcc(const yarp::sig::Vector &_dw);
699  bool setLinAcc(const yarp::sig::Vector &_ddp);
700  bool setLinAccC(const yarp::sig::Vector &_ddpC);
701  bool setAngAccM(const yarp::sig::Vector &_dwM);
702 
703  //other
704  std::string toString() const;
705 };
706 
707 
718 class SensorLinkNewtonEuler : public OneLinkNewtonEuler
719 {
720 protected:
722  yarp::sig::Vector F;
724  yarp::sig::Vector Mu;
725 
727  yarp::sig::Vector w;
729  yarp::sig::Vector dw;
731  yarp::sig::Vector ddp;
733  yarp::sig::Vector ddpC;
735  yarp::sig::Matrix H;
737  yarp::sig::Matrix COM;
739  yarp::sig::Matrix I;
741  double m;
742 
743  yarp::sig::Matrix R;
744  yarp::sig::Matrix RC;
745  yarp::sig::Vector r;
746  yarp::sig::Vector r_proj;
747  yarp::sig::Vector rc;
748  yarp::sig::Vector rc_proj;
749  const yarp::sig::Vector zeros0;
750 
751 public:
752 
758  SensorLinkNewtonEuler(const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
759 
765  SensorLinkNewtonEuler(const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_COM, const double _m, const yarp::sig::Matrix &_I, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
766 
770  virtual ~SensorLinkNewtonEuler() {};
771 
778  bool setMeasuredFMu(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
779 
787  bool setSensor(const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I);
788 
793  void ForwardAttachToLink( iDynLink *link);
794 
799  void BackwardAttachToLink( iDynLink *link);
800 
807  void ForwardForcesMomentsToLink( iDynLink *link);
808 
813  yarp::sig::Vector getForceMoment() const;
814 
815 
816  //~~~~~~~~~~~~~~~~~~~~~~
817  // get methods
818  //~~~~~~~~~~~~~~~~~~~~~~
819  const yarp::sig::Vector& getAngVel() const;
820  const yarp::sig::Vector& getAngAcc() const;
821  const yarp::sig::Vector& getLinAcc() const;
822  const yarp::sig::Vector& getLinAccC() const;
823 
824  const yarp::sig::Vector& getForce() const;
825  const yarp::sig::Vector& getMoment(bool isBase = false) const;
826 
827  double getIm() const;
828  double getFs() const;
829  double getFv() const;
830  double getD2q() const;
831  double getDq() const;
832  double getKr() const;
833  const yarp::sig::Vector& getAngAccM()const;
834  double getTorque() const;
835  const yarp::sig::Matrix& getH() const;
836 
837  double getMass() const;
838  const yarp::sig::Matrix& getInertia()const;
839  const yarp::sig::Matrix& getR();
840  const yarp::sig::Matrix& getRC();
841  const yarp::sig::Vector& getr(bool proj=false);
842  const yarp::sig::Vector& getrC(bool proj=false);
843 
844  bool setForce(const yarp::sig::Vector &_F);
845  bool setMoment(const yarp::sig::Vector &_Mu);
846  void setTorque(const double _Tau);
847  bool setAngVel(const yarp::sig::Vector &_w);
848  bool setAngAcc(const yarp::sig::Vector &_dw);
849  bool setLinAcc(const yarp::sig::Vector &_ddp);
850  bool setLinAccC(const yarp::sig::Vector &_ddpC);
851  bool setAngAccM(const yarp::sig::Vector &_dwM);
852 
856  std::string toString() const;
857 
858  // redefinitions but using iDynLink instead of OneLinkNeutonEuler
859  // that because it is used by IDynInvSensor
860 
861  void computeAngVel ( iDynLink *link);
862  void computeAngAcc ( iDynLink *link);
863  void computeLinAcc ( iDynLink *link);
864  void computeLinAccC( );
865  void computeForce ( iDynLink *link);
866  void computeMoment ( iDynLink *link);
867 
868  void computeForceToLink ( iDynLink *link);
869  void computeMomentToLink( iDynLink *link);
870 
871  // virtual function to be called by specific sensorLinks
872  virtual std::string getType() const;
873 
874 };
875 
881 class OneChainNewtonEuler
882 {
883  friend class iDynChain;
884 
885 protected:
886 
888  iDyn::iDynChain *chain;
890  OneLinkNewtonEuler ** neChain;
891 
893  unsigned int nLinks;
895  unsigned int nEndEff;
896 
898  NewEulMode mode;
900  std::string info;
902  unsigned int verbose;
903 
904 public:
905 
909  OneChainNewtonEuler(iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
910 
914  ~OneChainNewtonEuler();
915 
919  std::string toString() const;
920 
924  bool getVelAccAfterForward(unsigned int i, yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &dwM, yarp::sig::Vector &ddp, yarp::sig::Vector &ddpC) const;
925 
929  bool getWrenchAfterForward(unsigned int i, yarp::sig::Vector &F, yarp::sig::Vector &Mu) const;
930 
935  void getVelAccBase(yarp::sig::Vector &w, yarp::sig::Vector &dw,yarp::sig::Vector &ddp) const;
936 
941  void getVelAccEnd(yarp::sig::Vector &w, yarp::sig::Vector &dw,yarp::sig::Vector &ddp) const;
942 
947  void getWrenchBase(yarp::sig::Vector &F, yarp::sig::Vector &Mu) const;
948 
953  void getWrenchEnd(yarp::sig::Vector &F, yarp::sig::Vector &Mu) const;
954 
955  //~~~~~~~~~~~~~~~~~~~~~~
956  // set methods
957  //~~~~~~~~~~~~~~~~~~~~~~
958 
959  void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE);
960  void setMode(const NewEulMode _mode);
961  void setInfo(const std::string _info);
962 
970  bool initKinematicBase(const yarp::sig::Vector &w0,const yarp::sig::Vector &dw0,const yarp::sig::Vector &ddp0);
971 
979  bool initKinematicEnd(const yarp::sig::Vector &w0,const yarp::sig::Vector &dw0,const yarp::sig::Vector &ddp0);
980 
987  bool initWrenchEnd(const yarp::sig::Vector &F0,const yarp::sig::Vector &Mu0);
988 
995  bool initWrenchBase(const yarp::sig::Vector &F0,const yarp::sig::Vector &Mu0);
996 
997 
998  //~~~~~~~~~~~~~~~~~~~~~~
999  // get methods
1000  //~~~~~~~~~~~~~~~~~~~~~~
1001 
1005  std::string getInfo() const;
1006 
1010  NewEulMode getMode() const;
1011 
1012 
1013  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1014  // main computation methods
1015  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1016 
1020  void ForwardKinematicFromBase();
1021 
1025  void ForwardKinematicFromBase(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
1026 
1030  void BackwardKinematicFromEnd();
1031 
1035  void BackwardKinematicFromEnd(const yarp::sig::Vector &we, const yarp::sig::Vector &dwe, const yarp::sig::Vector &ddpe);
1036 
1040  void BackwardWrenchFromEnd();
1041 
1045  void BackwardWrenchFromEnd(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu);
1046 
1051  void computeTorques();
1052 
1057  void ForwardWrenchFromBase();
1058 
1065  void ForwardWrenchFromBase(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu);
1066 
1073  bool ForwardWrenchToEnd(unsigned int lSens);
1074 
1081  bool BackwardWrenchToBase(unsigned int lSens);
1082 
1083  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1084  // computations for contacts
1085  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1086 
1094  bool ForwardWrenchFromAtoB(unsigned int lA, unsigned int lB);
1095 
1103  bool BackwardWrenchFromAtoB(unsigned int lA, unsigned int lB);
1104 
1105 };
1106 
1107 
1108 
1109 
1222 class iDynInvSensor
1223 {
1224  friend class iFTransformation;
1225 protected:
1226 
1228  unsigned int lSens;
1230  SensorLinkNewtonEuler * sens;
1232  iDynChain * chain;
1234  NewEulMode mode;
1236  unsigned int verbose;
1238  std::string info;
1239 
1240 public:
1241 
1249  iDynInvSensor(iDyn::iDynChain *_c, const std::string &_info, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1250 
1263  iDynInvSensor(iDyn::iDynChain *_c, unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I, const std::string &_info, const NewEulMode _mode = DYNAMIC, unsigned int verb = 0);
1264 
1274  bool setSensor(unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I);
1275 
1276  bool setSensor(unsigned int i, SensorLinkNewtonEuler* sensor);
1277 
1284  void computeSensorForceMoment();
1285 
1289  std::string toString() const;
1290 
1295  yarp::sig::Vector getSensorForce() const;
1296 
1301  yarp::sig::Vector getSensorMoment() const;
1302 
1307  yarp::sig::Vector getSensorForceMoment() const;
1308 
1313  yarp::sig::Matrix getH() const;
1314 
1319  double getMass() const;
1320 
1325  yarp::sig::Matrix getCOM() const;
1326 
1331  yarp::sig::Matrix getInertia() const;
1332 
1333  //~~~~~~~~~~~~~~
1334  // set methods
1335  //~~~~~~~~~~~~~~
1336 
1337  void setMode(const NewEulMode _mode = DYNAMIC);
1338  void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE);
1339  void setInfo(const std::string &_info);
1340  void setSensorInfo(const std::string &_info);
1341 
1349  bool setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I);
1350 
1351  //~~~~~~~~~~~~~~
1352  // get methods
1353  //~~~~~~~~~~~~~~
1354 
1355  std::string getInfo() const;
1356  std::string getSensorInfo() const;
1357  unsigned int getSensorLink() const;
1358  yarp::sig::Vector getTorques() const;
1359 
1360  //destructor
1361  virtual ~iDynInvSensor();
1362 
1363 };
1364 
1373 class iCubArmSensorLink : public SensorLinkNewtonEuler
1374 {
1375 protected:
1376 
1378  std::string type;
1379 
1380 public:
1381 
1388  iCubArmSensorLink(const std::string &_type, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1389 
1393  virtual ~iCubArmSensorLink() {};
1394 
1398  std::string getType() const;
1399 
1400 };
1401 
1410 class iCubLegSensorLink : public SensorLinkNewtonEuler
1411 {
1412 protected:
1413 
1415  std::string type;
1416 
1417  public:
1418 
1425  iCubLegSensorLink(const std::string _type, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1426 
1430  virtual ~iCubLegSensorLink() {};
1431 
1435  std::string getType() const;
1436 
1437 };
1438 
1439 
1449 class iDynInvSensorArm : public iDynInvSensor
1450 {
1451 
1452 public:
1453 
1460  iDynInvSensorArm(iDyn::iCubArmDyn *_c, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1461 
1470  iDynInvSensorArm(iDyn::iDynChain *_c, const std::string _type, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1471 
1475  std::string getType() const;
1476 
1477 
1478 };
1479 
1489 class iDynInvSensorArmNoTorso : public iDynInvSensor
1490 {
1491 
1492 public:
1493 
1500  iDynInvSensorArmNoTorso(iDyn::iCubArmNoTorsoDyn *_c, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1501 
1510  iDynInvSensorArmNoTorso(iDyn::iDynChain *_c, const std::string _type, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1511 
1515  std::string getType() const;
1516 
1517 
1518 };
1519 
1529 class iDynInvSensorLeg : public iDynInvSensor
1530 {
1531 
1532 public:
1533 
1540  iDynInvSensorLeg(iDyn::iCubLegDyn *_c, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1541 
1549  iDynInvSensorLeg(iDyn::iDynChain *_c, const std::string _type, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1550 
1554  std::string getType() const;
1555 
1556 
1557 };
1558 
1559 
1560 
1561 
1562 
1563 
1564 
1565 
1577 class iDynSensor: public iDynInvSensor
1578 {
1579 
1580 public:
1581 
1589  iDynSensor(iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1590 
1591 
1604  iDynSensor(iDyn::iDynChain *_c, unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I, std::string _info, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1605 
1612  bool setSensorMeasures(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu);
1613 
1620  bool setSensorMeasures(const yarp::sig::Vector &FM);
1621 
1622  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1623  // main computation methods
1624  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1625 
1636  virtual bool computeFromSensorNewtonEuler(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu);
1637 
1647  virtual bool computeFromSensorNewtonEuler(const yarp::sig::Vector &FMu);
1648 
1658  virtual void computeFromSensorNewtonEuler();
1659 
1670  virtual void computeWrenchFromSensorNewtonEuler();
1671 
1672  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1673  // get methods, overloaded from iDyn
1674  //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1675 
1680  yarp::sig::Matrix getForces() const;
1681 
1686  yarp::sig::Matrix getMoments() const;
1687 
1692  yarp::sig::Vector getTorques() const;
1693 
1698  yarp::sig::Vector getForce(const unsigned int iLink) const;
1699 
1704  yarp::sig::Vector getMoment(const unsigned int iLink) const;
1705 
1710  double getTorque(const unsigned int iLink) const;
1711 
1716  yarp::sig::Matrix getForcesNewtonEuler() const;
1717 
1722  yarp::sig::Matrix getMomentsNewtonEuler() const;
1723 
1728  yarp::sig::Vector getTorquesNewtonEuler() const;
1729 
1734  virtual yarp::sig::Vector getForceMomentEndEff() const;
1735 
1736 
1737 };
1738 
1739 
1740 
1741 
1752 class iDynSensorArm : public iDynSensor
1753 {
1754 
1755 public:
1756 
1763  iDynSensorArm(iDyn::iCubArmDyn *_c, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1764 
1768  std::string getType() const;
1769 
1770 
1771 };
1772 
1773 
1784 class iDynSensorArmNoTorso : public iDynSensor
1785 {
1786 
1787 public:
1788 
1795  iDynSensorArmNoTorso(iDyn::iCubArmNoTorsoDyn *_c, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1796 
1800  std::string getType() const;
1801 
1802 
1803 };
1804 
1815 class iDynSensorLeg : public iDynSensor
1816 {
1817 
1818 public:
1819 
1826  iDynSensorLeg(iDyn::iCubLegDyn *_c, const NewEulMode _mode = DYNAMIC, unsigned int verb = iCub::skinDynLib::NO_VERBOSE);
1827 
1831  std::string getType() const;
1832 
1833 
1834 };
1835 
1836 
1837 
1838 
1839 
1840 
1841 
1842 
1843 }
1844 
1845 }//end namespace
1846 
1847 #endif
1848 
1849 
iCub::iDyn::BaseLinkNewtonEuler
A class for setting a virtual base link: this is useful to initialize the forward phase of Newton-Eul...
Definition: iDynInv.h:492
iCub::iDyn::BaseLinkNewtonEuler::toString
std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:852
iCub::iDyn::BaseLinkNewtonEuler::getLinAccC
const yarp::sig::Vector & getLinAccC() const
Definition: iDynInv.cpp:881
iCub::iDyn::BaseLinkNewtonEuler::dw
yarp::sig::Vector dw
initial angular acceleration
Definition: iDynInv.h:497
iCub::iDyn::BaseLinkNewtonEuler::w
yarp::sig::Vector w
initial angular velocity
Definition: iDynInv.h:495
iCub::iDyn::BaseLinkNewtonEuler::setMoment
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
Definition: iDynInv.cpp:920
iCub::iDyn::BaseLinkNewtonEuler::getR
const yarp::sig::Matrix & getR()
Definition: iDynInv.cpp:891
iCub::iDyn::BaseLinkNewtonEuler::getAngVel
const yarp::sig::Vector & getAngVel() const
Definition: iDynInv.cpp:873
iCub::iDyn::BaseLinkNewtonEuler::zeros3
const yarp::sig::Vector zeros3
Definition: iDynInv.h:514
iCub::iDyn::BaseLinkNewtonEuler::getr
const yarp::sig::Vector & getr(bool proj=false)
Definition: iDynInv.cpp:901
iCub::iDyn::BaseLinkNewtonEuler::setAngAccM
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
Definition: iDynInv.cpp:994
iCub::iDyn::BaseLinkNewtonEuler::Mu0
yarp::sig::Vector Mu0
initial moment
Definition: iDynInv.h:507
iCub::iDyn::BaseLinkNewtonEuler::setLinAcc
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
Definition: iDynInv.cpp:971
iCub::iDyn::BaseLinkNewtonEuler::BaseLinkNewtonEuler
BaseLinkNewtonEuler(const yarp::sig::Matrix &_H0, const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor, initializing the base data.
iCub::iDyn::BaseLinkNewtonEuler::getMoment
const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:884
iCub::iDyn::BaseLinkNewtonEuler::ddp
yarp::sig::Vector ddp
initial linear acceleration
Definition: iDynInv.h:499
iCub::iDyn::BaseLinkNewtonEuler::setForce
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
Definition: iDynInv.cpp:904
iCub::iDyn::BaseLinkNewtonEuler::setAngAcc
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
Definition: iDynInv.cpp:955
iCub::iDyn::BaseLinkNewtonEuler::setAngVel
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
Definition: iDynInv.cpp:939
iCub::iDyn::BaseLinkNewtonEuler::H0
yarp::sig::Matrix H0
base roto-traslation (if necessary)
Definition: iDynInv.h:501
iCub::iDyn::BaseLinkNewtonEuler::setTorque
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
Definition: iDynInv.cpp:937
iCub::iDyn::BaseLinkNewtonEuler::BaseLinkNewtonEuler
BaseLinkNewtonEuler(const yarp::sig::Matrix &_H0, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
iCub::iDyn::BaseLinkNewtonEuler::getInertia
const yarp::sig::Matrix & getInertia() const
Definition: iDynInv.cpp:900
iCub::iDyn::BaseLinkNewtonEuler::zeros3x3
const yarp::sig::Matrix zeros3x3
Definition: iDynInv.h:513
iCub::iDyn::BaseLinkNewtonEuler::Tau
double Tau
corresponding torque
Definition: iDynInv.h:509
iCub::iDyn::BaseLinkNewtonEuler::getRC
const yarp::sig::Matrix & getRC()
Definition: iDynInv.cpp:892
iCub::iDyn::BaseLinkNewtonEuler::setAsBase
bool setAsBase(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Sets the base data.
iCub::iDyn::BaseLinkNewtonEuler::getrC
const yarp::sig::Vector & getrC(bool proj=false)
Definition: iDynInv.cpp:902
iCub::iDyn::BaseLinkNewtonEuler::Mu
yarp::sig::Vector Mu
initial moment
Definition: iDynInv.h:505
iCub::iDyn::BaseLinkNewtonEuler::eye3x3
const yarp::sig::Matrix eye3x3
Definition: iDynInv.h:512
iCub::iDyn::BaseLinkNewtonEuler::getAngAccM
const yarp::sig::Vector & getAngAccM() const
Definition: iDynInv.cpp:877
iCub::iDyn::BaseLinkNewtonEuler::getAngAcc
const yarp::sig::Vector & getAngAcc() const
Definition: iDynInv.cpp:875
iCub::iDyn::BaseLinkNewtonEuler::getLinAcc
const yarp::sig::Vector & getLinAcc() const
Definition: iDynInv.cpp:879
iCub::iDyn::BaseLinkNewtonEuler::setAsBase
bool setAsBase(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Sets the base data.
iCub::iDyn::BaseLinkNewtonEuler::F
yarp::sig::Vector F
initial force
Definition: iDynInv.h:503
iCub::iDyn::BaseLinkNewtonEuler::setLinAccC
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
Definition: iDynInv.cpp:987
iCub::iDyn::BaseLinkNewtonEuler::getForce
const yarp::sig::Vector & getForce() const
Definition: iDynInv.cpp:883
iCub::iDyn::FinalLinkNewtonEuler
A class for setting a virtual final link: this is useful to initialize the backward phase of Newton-E...
Definition: iDynInv.h:611
iCub::iDyn::FinalLinkNewtonEuler::F
yarp::sig::Vector F
final force
Definition: iDynInv.h:620
iCub::iDyn::FinalLinkNewtonEuler::setMoment
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
Definition: iDynInv.cpp:1163
iCub::iDyn::FinalLinkNewtonEuler::zeros3
const yarp::sig::Vector zeros3
Definition: iDynInv.h:630
iCub::iDyn::FinalLinkNewtonEuler::getForce
const yarp::sig::Vector & getForce() const
Definition: iDynInv.cpp:1123
iCub::iDyn::FinalLinkNewtonEuler::setLinAccC
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
Definition: iDynInv.cpp:1206
iCub::iDyn::FinalLinkNewtonEuler::getAngVel
const yarp::sig::Vector & getAngVel() const
Definition: iDynInv.cpp:1127
iCub::iDyn::FinalLinkNewtonEuler::setForce
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
Definition: iDynInv.cpp:1147
iCub::iDyn::FinalLinkNewtonEuler::getr
const yarp::sig::Vector & getr(bool proj=false)
Definition: iDynInv.cpp:1144
iCub::iDyn::FinalLinkNewtonEuler::getLinAccC
const yarp::sig::Vector & getLinAccC() const
Definition: iDynInv.cpp:1131
iCub::iDyn::FinalLinkNewtonEuler::FinalLinkNewtonEuler
FinalLinkNewtonEuler(const yarp::sig::Matrix &_HN, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
Definition: iDynInv.cpp:1011
iCub::iDyn::FinalLinkNewtonEuler::getH
const yarp::sig::Matrix & getH()
Definition: iDynInv.cpp:1133
iCub::iDyn::FinalLinkNewtonEuler::setAngAcc
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
Definition: iDynInv.cpp:1190
iCub::iDyn::FinalLinkNewtonEuler::setAngAccM
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
Definition: iDynInv.cpp:1213
iCub::iDyn::FinalLinkNewtonEuler::getLinAcc
const yarp::sig::Vector & getLinAcc() const
Definition: iDynInv.cpp:1130
iCub::iDyn::FinalLinkNewtonEuler::w
yarp::sig::Vector w
initial angular velocity
Definition: iDynInv.h:614
iCub::iDyn::FinalLinkNewtonEuler::getAngAccM
const yarp::sig::Vector & getAngAccM() const
Definition: iDynInv.cpp:1129
iCub::iDyn::FinalLinkNewtonEuler::getInertia
const yarp::sig::Matrix & getInertia() const
Definition: iDynInv.cpp:1143
iCub::iDyn::FinalLinkNewtonEuler::Mu
yarp::sig::Vector Mu
final moment
Definition: iDynInv.h:622
iCub::iDyn::FinalLinkNewtonEuler::setAsFinal
bool setAsFinal(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Set the final frame data.
iCub::iDyn::FinalLinkNewtonEuler::eye3x3
const yarp::sig::Matrix eye3x3
Definition: iDynInv.h:628
iCub::iDyn::FinalLinkNewtonEuler::setTorque
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
Definition: iDynInv.cpp:1179
iCub::iDyn::FinalLinkNewtonEuler::dw
yarp::sig::Vector dw
initial angular acceleration
Definition: iDynInv.h:616
iCub::iDyn::FinalLinkNewtonEuler::setAngVel
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
Definition: iDynInv.cpp:1181
iCub::iDyn::FinalLinkNewtonEuler::getR
const yarp::sig::Matrix & getR()
Definition: iDynInv.cpp:1134
iCub::iDyn::FinalLinkNewtonEuler::FinalLinkNewtonEuler
FinalLinkNewtonEuler(const yarp::sig::Matrix &_HN, const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor, initializing the final frame data.
iCub::iDyn::FinalLinkNewtonEuler::toString
std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:1103
iCub::iDyn::FinalLinkNewtonEuler::ddp
yarp::sig::Vector ddp
initial linear acceleration
Definition: iDynInv.h:618
iCub::iDyn::FinalLinkNewtonEuler::setLinAcc
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
Definition: iDynInv.cpp:1198
iCub::iDyn::FinalLinkNewtonEuler::getMoment
const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:1125
iCub::iDyn::FinalLinkNewtonEuler::getrC
const yarp::sig::Vector & getrC(bool proj=false)
Definition: iDynInv.cpp:1145
iCub::iDyn::FinalLinkNewtonEuler::getRC
const yarp::sig::Matrix & getRC()
Definition: iDynInv.cpp:1135
iCub::iDyn::FinalLinkNewtonEuler::HN
yarp::sig::Matrix HN
final roto-traslation (if necessary)
Definition: iDynInv.h:624
iCub::iDyn::FinalLinkNewtonEuler::setAsFinal
bool setAsFinal(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the final frame data.
iCub::iDyn::FinalLinkNewtonEuler::zeros3x3
const yarp::sig::Matrix zeros3x3
Definition: iDynInv.h:629
iCub::iDyn::FinalLinkNewtonEuler::eye4x4
const yarp::sig::Matrix eye4x4
Definition: iDynInv.h:627
iCub::iDyn::FinalLinkNewtonEuler::getAngAcc
const yarp::sig::Vector & getAngAcc() const
Definition: iDynInv.cpp:1128
iCub::iDyn::OneChainNewtonEuler
A class for computing forces and torques in a iDynChain.
Definition: iDynInv.h:882
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchFromEnd
void BackwardWrenchFromEnd()
[classic] Base function for backward of classical Newton-Euler.
Definition: iDynInv.cpp:1883
iCub::iDyn::OneChainNewtonEuler::getVelAccAfterForward
bool getVelAccAfterForward(unsigned int i, yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &dwM, yarp::sig::Vector &ddp, yarp::sig::Vector &ddpC) const
Useful to debug, getting the intermediate computations after the forward phase.
Definition: iDynInv.cpp:1732
iCub::iDyn::OneChainNewtonEuler::nEndEff
unsigned int nEndEff
the index of the end-effector in the chain (the last frame)
Definition: iDynInv.h:895
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchFromEnd
void BackwardWrenchFromEnd(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
[classic] Backward of classical Newton-Euler, after initializing the final link
iCub::iDyn::OneChainNewtonEuler::getVelAccEnd
void getVelAccEnd(yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp) const
This method is used by iDynChain to retrieve kinematic information for connection with one or more iD...
Definition: iDynInv.cpp:1774
iCub::iDyn::OneChainNewtonEuler::toString
std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:1720
iCub::iDyn::OneChainNewtonEuler::chain
iDyn::iDynChain * chain
the real kinematic/dynamic chain of the robot
Definition: iDynInv.h:888
iCub::iDyn::OneChainNewtonEuler::ForwardKinematicFromBase
void ForwardKinematicFromBase()
[classic/inverse] Base function for forward of classical Newton-Euler.
Definition: iDynInv.cpp:1851
iCub::iDyn::OneChainNewtonEuler::initKinematicBase
bool initKinematicBase(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
[classic] Initialize the base with measured or known kinematics variables
Definition: iDynInv.cpp:1818
iCub::iDyn::OneChainNewtonEuler::initWrenchEnd
bool initWrenchEnd(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
[classic] Initialize the end-effector finalLink with measured or known wrench
Definition: iDynInv.cpp:1828
iCub::iDyn::OneChainNewtonEuler::setMode
void setMode(const NewEulMode _mode)
Definition: iDynInv.cpp:1806
iCub::iDyn::OneChainNewtonEuler::getMode
NewEulMode getMode() const
Definition: iDynInv.cpp:1844
iCub::iDyn::OneChainNewtonEuler::setInfo
void setInfo(const std::string _info)
Definition: iDynInv.cpp:1813
iCub::iDyn::OneChainNewtonEuler::BackwardKinematicFromEnd
void BackwardKinematicFromEnd(const yarp::sig::Vector &we, const yarp::sig::Vector &dwe, const yarp::sig::Vector &ddpe)
[inverse] Forward of classical Newton-Euler, after initializing the base link
iCub::iDyn::OneChainNewtonEuler::ForwardKinematicFromBase
void ForwardKinematicFromBase(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
[classic/inverse] Forward of classical Newton-Euler, after initializing the base link
iCub::iDyn::OneChainNewtonEuler::mode
NewEulMode mode
static/dynamic/dynamicWrotor
Definition: iDynInv.h:898
iCub::iDyn::OneChainNewtonEuler::getWrenchEnd
void getWrenchEnd(yarp::sig::Vector &F, yarp::sig::Vector &Mu) const
This method is used by iDynChain to retrieve wrench information for connection with one or more iDynL...
Definition: iDynInv.cpp:1787
iCub::iDyn::OneChainNewtonEuler::setVerbose
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
Definition: iDynInv.cpp:1799
iCub::iDyn::OneChainNewtonEuler::ForwardWrenchToEnd
bool ForwardWrenchToEnd(unsigned int lSens)
[inverse] Base function for inverse Newton-Euler: from the i-th link to the end, forward of forces an...
Definition: iDynInv.cpp:1926
iCub::iDyn::OneChainNewtonEuler::~OneChainNewtonEuler
~OneChainNewtonEuler()
Standard destructor.
Definition: iDynInv.cpp:1713
iCub::iDyn::OneChainNewtonEuler::OneChainNewtonEuler
OneChainNewtonEuler(iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor (note: without FT sensor)
Definition: iDynInv.cpp:1673
iCub::iDyn::OneChainNewtonEuler::neChain
OneLinkNewtonEuler ** neChain
the chain of links/frames for Newton-Euler computations
Definition: iDynInv.h:890
iCub::iDyn::OneChainNewtonEuler::BackwardKinematicFromEnd
void BackwardKinematicFromEnd()
[inverse] Base function for forward of classical Newton-Euler.
Definition: iDynInv.cpp:1867
iCub::iDyn::OneChainNewtonEuler::ForwardWrenchFromBase
void ForwardWrenchFromBase(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
[inverse] Base function for inverse Newton-Euler: from the i-th link to the base, backward of forces ...
iCub::iDyn::OneChainNewtonEuler::initWrenchBase
bool initWrenchBase(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
[inverse] Initialize the base with measured or known wrench
Definition: iDynInv.cpp:1833
iCub::iDyn::OneChainNewtonEuler::verbose
unsigned int verbose
verbosity flag
Definition: iDynInv.h:902
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchFromAtoB
bool BackwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
Base function for inverse Newton-Euler: from the lA-th link to the lB-th, backward of forces and mome...
Definition: iDynInv.cpp:1995
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchToBase
bool BackwardWrenchToBase(unsigned int lSens)
[classic/inverse] Base function for inverse Newton-Euler: from the i-th link to the base,...
Definition: iDynInv.cpp:1947
iCub::iDyn::OneChainNewtonEuler::getWrenchBase
void getWrenchBase(yarp::sig::Vector &F, yarp::sig::Vector &Mu) const
This method is used by iDynChain to retrieve wrench information for connection with one or more iDynL...
Definition: iDynInv.cpp:1781
iCub::iDyn::OneChainNewtonEuler::getVelAccBase
void getVelAccBase(yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp) const
This method is used by iDynChain to retrieve kinematic information for connection with one or more iD...
Definition: iDynInv.cpp:1767
iCub::iDyn::OneChainNewtonEuler::ForwardWrenchFromBase
void ForwardWrenchFromBase()
[inverse] Base function for inverse Newton-Euler: from the i-th link to the end, forward of forces an...
Definition: iDynInv.cpp:1908
iCub::iDyn::OneChainNewtonEuler::computeTorques
void computeTorques()
Computes all the torques in the chain, assuming that all the internal wrenches have been already comp...
Definition: iDynInv.cpp:1892
iCub::iDyn::OneChainNewtonEuler::info
std::string info
info or useful notes
Definition: iDynInv.h:900
iCub::iDyn::OneChainNewtonEuler::nLinks
unsigned int nLinks
number of links
Definition: iDynInv.h:893
iCub::iDyn::OneChainNewtonEuler::getInfo
std::string getInfo() const
Definition: iDynInv.cpp:1843
iCub::iDyn::OneChainNewtonEuler::getWrenchAfterForward
bool getWrenchAfterForward(unsigned int i, yarp::sig::Vector &F, yarp::sig::Vector &Mu) const
Useful to debug, getting the intermediate computations after the forward phase.
Definition: iDynInv.cpp:1751
iCub::iDyn::OneChainNewtonEuler::initKinematicEnd
bool initKinematicEnd(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
[inverse] Initialize the end-effector finalLink with measured or known kinematics variables
Definition: iDynInv.cpp:1823
iCub::iDyn::OneChainNewtonEuler::ForwardWrenchFromAtoB
bool ForwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
Base function for inverse Newton-Euler: from the lA-th link to the lB-th, forward of forces and momen...
Definition: iDynInv.cpp:1976
iCub::iDyn::OneLinkNewtonEuler
A base class for computing forces and torques in a serial link chain.
Definition: iDynInv.h:96
iCub::iDyn::OneLinkNewtonEuler::computeLinAcc
void computeLinAcc(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] compute linear acceleration of the reference frame of the link
Definition: iDynInv.cpp:492
iCub::iDyn::OneLinkNewtonEuler::mode
NewEulMode mode
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
Definition: iDynInv.h:100
iCub::iDyn::OneLinkNewtonEuler::getrC
virtual const yarp::sig::Vector & getrC(bool proj=false)
Definition: iDynInv.cpp:380
iCub::iDyn::OneLinkNewtonEuler::setLinAccC
virtual bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
Definition: iDynInv.cpp:226
iCub::iDyn::OneLinkNewtonEuler::getForce
virtual const yarp::sig::Vector & getForce() const
Definition: iDynInv.cpp:348
iCub::iDyn::OneLinkNewtonEuler::setVerbose
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
Set the verbosity level of comments during operations.
Definition: iDynInv.cpp:117
iCub::iDyn::OneLinkNewtonEuler::setInfo
void setInfo(const std::string &_info)
Set some information about this OneLink class.
Definition: iDynInv.cpp:256
iCub::iDyn::OneLinkNewtonEuler::OneLinkNewtonEuler
OneLinkNewtonEuler(iDyn::iDynLink *dlink=NULL)
Default constructor.
Definition: iDynInv.cpp:46
iCub::iDyn::OneLinkNewtonEuler::setAsFinal
virtual bool setAsFinal(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the frame as the final one: this is useful to initialize the backward phase of Newton-Euler's met...
iCub::iDyn::OneLinkNewtonEuler::getR
virtual const yarp::sig::Matrix & getR()
Definition: iDynInv.cpp:358
iCub::iDyn::OneLinkNewtonEuler::getAngVel
virtual const yarp::sig::Vector & getAngVel() const
Definition: iDynInv.cpp:338
iCub::iDyn::OneLinkNewtonEuler::computeForceBackward
void computeForceBackward(OneLinkNewtonEuler *next)
[Backward Newton-Euler] compute force from the following link
Definition: iDynInv.cpp:581
iCub::iDyn::OneLinkNewtonEuler::setMoment
virtual bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
Definition: iDynInv.cpp:161
iCub::iDyn::OneLinkNewtonEuler::setAngAcc
virtual bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
Definition: iDynInv.cpp:196
iCub::iDyn::OneLinkNewtonEuler::BackwardWrench
void BackwardWrench(OneLinkNewtonEuler *next)
[Backward Newton-Euler] Compute F, Mu, Tau
Definition: iDynInv.cpp:737
iCub::iDyn::OneLinkNewtonEuler::getInertia
virtual const yarp::sig::Matrix & getInertia() const
Definition: iDynInv.cpp:376
iCub::iDyn::OneLinkNewtonEuler::getDq
virtual double getDq() const
Definition: iDynInv.cpp:366
iCub::iDyn::OneLinkNewtonEuler::getMoment
virtual const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:350
iCub::iDyn::OneLinkNewtonEuler::computeLinAccBackward
void computeLinAccBackward(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] compute linear acceleration of the reference frame of the previous link
Definition: iDynInv.cpp:517
iCub::iDyn::OneLinkNewtonEuler::computeTorque
virtual void computeTorque(OneLinkNewtonEuler *prev)
[all] Compute joint torque; moment must be pre-computed
Definition: iDynInv.cpp:698
iCub::iDyn::OneLinkNewtonEuler::getTorque
virtual double getTorque() const
Definition: iDynInv.cpp:352
iCub::iDyn::OneLinkNewtonEuler::getH
virtual const yarp::sig::Matrix & getH()
Definition: iDynInv.cpp:356
iCub::iDyn::OneLinkNewtonEuler::setZM
bool setZM(const yarp::sig::Vector &_zm)
Set the zM vector.
Definition: iDynInv.cpp:127
iCub::iDyn::OneLinkNewtonEuler::setMeasuredTorque
virtual bool setMeasuredTorque(const double _Tau)
Set measured torque in a joint torque sensor frame
Definition: iDynInv.cpp:286
iCub::iDyn::OneLinkNewtonEuler::setMode
void setMode(const NewEulMode _mode)
Set the operation mode (static,dynamic etc)
Definition: iDynInv.cpp:122
iCub::iDyn::OneLinkNewtonEuler::link
iDyn::iDynLink * link
the corresponding iDynLink
Definition: iDynInv.h:110
iCub::iDyn::OneLinkNewtonEuler::getLinAccC
virtual const yarp::sig::Vector & getLinAccC() const
Definition: iDynInv.cpp:346
iCub::iDyn::OneLinkNewtonEuler::getRC
virtual const yarp::sig::Matrix & getRC()
Definition: iDynInv.cpp:360
iCub::iDyn::OneLinkNewtonEuler::getD2q
virtual double getD2q() const
Definition: iDynInv.cpp:364
iCub::iDyn::OneLinkNewtonEuler::zero
void zero()
Set everything to zero; R is set to an identity matrix.
Definition: iDynInv.cpp:66
iCub::iDyn::OneLinkNewtonEuler::getIm
virtual double getIm() const
Definition: iDynInv.cpp:362
iCub::iDyn::OneLinkNewtonEuler::getInfo
std::string getInfo() const
Definition: iDynInv.cpp:354
iCub::iDyn::OneLinkNewtonEuler::setAsFinal
virtual bool setAsFinal(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Set the frame as the final one: this is useful to initialize the backward phase of Euler's method,...
iCub::iDyn::OneLinkNewtonEuler::setAsBase
virtual bool setAsBase(const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Virtual method to set the frame as the base one: this is useful to initialize the forward phase of Ne...
iCub::iDyn::OneLinkNewtonEuler::setMeasuredFMu
virtual bool setMeasuredFMu(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set measured force and moment in a 'sensor' frame: this is useful to initialize the forward phase of ...
Definition: iDynInv.cpp:261
iCub::iDyn::OneLinkNewtonEuler::info
std::string info
info or useful notes
Definition: iDynInv.h:102
iCub::iDyn::OneLinkNewtonEuler::setLinAcc
virtual bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
Definition: iDynInv.cpp:211
iCub::iDyn::OneLinkNewtonEuler::computeAngVelBackward
void computeAngVelBackward(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] compute angular velocity of the previous link frame
Definition: iDynInv.cpp:408
iCub::iDyn::OneLinkNewtonEuler::computeMomentForward
void computeMomentForward(OneLinkNewtonEuler *prev)
[Inverse Newton-Euler] compute moment from the previous link
Definition: iDynInv.cpp:642
iCub::iDyn::OneLinkNewtonEuler::getAngAccM
virtual const yarp::sig::Vector & getAngAccM() const
Definition: iDynInv.cpp:342
iCub::iDyn::OneLinkNewtonEuler::computeAngAccM
void computeAngAccM(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] compute angular acceleration of the rotor
Definition: iDynInv.cpp:563
iCub::iDyn::OneLinkNewtonEuler::getr
virtual const yarp::sig::Vector & getr(bool proj=false)
Definition: iDynInv.cpp:378
iCub::iDyn::OneLinkNewtonEuler::setAsBase
virtual bool setAsBase(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
iCub::iDyn::OneLinkNewtonEuler::getFv
virtual double getFv() const
Definition: iDynInv.cpp:372
iCub::iDyn::OneLinkNewtonEuler::setAngAccM
virtual bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
Definition: iDynInv.cpp:241
iCub::iDyn::OneLinkNewtonEuler::ForwardWrench
void ForwardWrench(OneLinkNewtonEuler *prev)
[Inverse Newton-Euler] Compute F, Mu, Tau
Definition: iDynInv.cpp:743
iCub::iDyn::OneLinkNewtonEuler::computeLinAccC
void computeLinAccC()
[Forward Newton-Euler] compute linear acceleration of the center of mass
Definition: iDynInv.cpp:542
iCub::iDyn::OneLinkNewtonEuler::computeAngAcc
void computeAngAcc(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] compute angular acceleration of the link
Definition: iDynInv.cpp:428
iCub::iDyn::OneLinkNewtonEuler::getZM
yarp::sig::Vector getZM() const
Definition: iDynInv.cpp:336
iCub::iDyn::OneLinkNewtonEuler::setAngVel
virtual bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
Definition: iDynInv.cpp:181
iCub::iDyn::OneLinkNewtonEuler::ForwardKinematics
void ForwardKinematics(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] Compute w, dw, ddp, ddpC, dwM
Definition: iDynInv.cpp:720
iCub::iDyn::OneLinkNewtonEuler::computeAngVel
void computeAngVel(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] compute angular velocity of the link
Definition: iDynInv.cpp:388
iCub::iDyn::OneLinkNewtonEuler::computeForceForward
void computeForceForward(OneLinkNewtonEuler *prev)
[Inverse Newton-Euler] compute force from the previous link
Definition: iDynInv.cpp:588
iCub::iDyn::OneLinkNewtonEuler::~OneLinkNewtonEuler
virtual ~OneLinkNewtonEuler()
Destructor.
Definition: iDynInv.h:267
iCub::iDyn::OneLinkNewtonEuler::getKr
virtual double getKr() const
Definition: iDynInv.cpp:368
iCub::iDyn::OneLinkNewtonEuler::BackwardKinematics
void BackwardKinematics(OneLinkNewtonEuler *prev)
[Backward Kinematic computation] Compute w, dw, ddp, ddpC, dwM
Definition: iDynInv.cpp:729
iCub::iDyn::OneLinkNewtonEuler::z0
yarp::sig::Vector z0
z0=[0 0 1]'
Definition: iDynInv.h:106
iCub::iDyn::OneLinkNewtonEuler::getAngAcc
virtual const yarp::sig::Vector & getAngAcc() const
Definition: iDynInv.cpp:340
iCub::iDyn::OneLinkNewtonEuler::computeMomentBackward
void computeMomentBackward(OneLinkNewtonEuler *next)
[Backward Newton-Euler] compute moment from the following link
Definition: iDynInv.cpp:596
iCub::iDyn::OneLinkNewtonEuler::zm
yarp::sig::Vector zm
z^{i-1}_{m_{i}} versor rotating solidally with link i, projected in frame i ==>> constant
Definition: iDynInv.h:108
iCub::iDyn::OneLinkNewtonEuler::verbose
unsigned int verbose
verbosity flag
Definition: iDynInv.h:104
iCub::iDyn::OneLinkNewtonEuler::setTorque
virtual void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
Definition: iDynInv.cpp:176
iCub::iDyn::OneLinkNewtonEuler::toString
virtual std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:301
iCub::iDyn::OneLinkNewtonEuler::getLinAcc
virtual const yarp::sig::Vector & getLinAcc() const
Definition: iDynInv.cpp:344
iCub::iDyn::OneLinkNewtonEuler::setForce
virtual bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
Definition: iDynInv.cpp:146
iCub::iDyn::OneLinkNewtonEuler::getFs
virtual double getFs() const
Definition: iDynInv.cpp:370
iCub::iDyn::OneLinkNewtonEuler::getMass
virtual double getMass() const
Definition: iDynInv.cpp:374
iCub::iDyn::OneLinkNewtonEuler::getMode
NewEulMode getMode() const
Definition: iDynInv.cpp:334
iCub::iDyn::OneLinkNewtonEuler::computeAngAccBackward
void computeAngAccBackward(OneLinkNewtonEuler *prev)
[Forward Newton-Euler] compute angular acceleration of the previous link frame
Definition: iDynInv.cpp:460
iCub::iDyn::SensorLinkNewtonEuler
A class for setting a virtual sensor link.
Definition: iDynInv.h:719
iCub::iDyn::SensorLinkNewtonEuler::setAngAcc
bool setAngAcc(const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (i...
Definition: iDynInv.cpp:1446
iCub::iDyn::SensorLinkNewtonEuler::H
yarp::sig::Matrix H
the roto-translational matrix from the i-th link to the sensor: it's the matrix describing the sensor...
Definition: iDynInv.h:735
iCub::iDyn::SensorLinkNewtonEuler::I
yarp::sig::Matrix I
the semi-link inertia
Definition: iDynInv.h:739
iCub::iDyn::SensorLinkNewtonEuler::COM
yarp::sig::Matrix COM
the roto-translational matrix of the COM of the semi-link (bewteen sensor and ith link frame)
Definition: iDynInv.h:737
iCub::iDyn::SensorLinkNewtonEuler::setAngVel
bool setAngVel(const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the chil...
Definition: iDynInv.cpp:1430
iCub::iDyn::SensorLinkNewtonEuler::setMeasuredFMu
bool setMeasuredFMu(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the sensor measured force/moment - if measured by a FT sensor.
Definition: iDynInv.cpp:1260
iCub::iDyn::SensorLinkNewtonEuler::getRC
const yarp::sig::Matrix & getRC()
Definition: iDynInv.cpp:1370
iCub::iDyn::SensorLinkNewtonEuler::computeMoment
void computeMoment(iDynLink *link)
Definition: iDynInv.cpp:1612
iCub::iDyn::SensorLinkNewtonEuler::getrC
const yarp::sig::Vector & getrC(bool proj=false)
Definition: iDynInv.cpp:1379
iCub::iDyn::SensorLinkNewtonEuler::setAngAccM
bool setAngAccM(const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular accele...
Definition: iDynInv.cpp:1494
iCub::iDyn::SensorLinkNewtonEuler::getForce
const yarp::sig::Vector & getForce() const
Definition: iDynInv.cpp:1352
iCub::iDyn::SensorLinkNewtonEuler::zeros0
const yarp::sig::Vector zeros0
Definition: iDynInv.h:749
iCub::iDyn::SensorLinkNewtonEuler::computeForceToLink
void computeForceToLink(iDynLink *link)
Definition: iDynInv.cpp:1562
iCub::iDyn::SensorLinkNewtonEuler::computeMomentToLink
void computeMomentToLink(iDynLink *link)
Definition: iDynInv.cpp:1570
iCub::iDyn::SensorLinkNewtonEuler::BackwardAttachToLink
void BackwardAttachToLink(iDynLink *link)
Compute F,Mu given the reference frame of the link where the sensor is.
Definition: iDynInv.cpp:1334
iCub::iDyn::SensorLinkNewtonEuler::computeAngVel
void computeAngVel(iDynLink *link)
Definition: iDynInv.cpp:1507
iCub::iDyn::SensorLinkNewtonEuler::ForwardForcesMomentsToLink
void ForwardForcesMomentsToLink(iDynLink *link)
Forward the sensor forces and moments, measured by the sensor, to the reference frame of the link whe...
Definition: iDynInv.cpp:1340
iCub::iDyn::SensorLinkNewtonEuler::rc_proj
yarp::sig::Vector rc_proj
Definition: iDynInv.h:748
iCub::iDyn::SensorLinkNewtonEuler::~SensorLinkNewtonEuler
virtual ~SensorLinkNewtonEuler()
Destructor.
Definition: iDynInv.h:770
iCub::iDyn::SensorLinkNewtonEuler::getR
const yarp::sig::Matrix & getR()
Definition: iDynInv.cpp:1368
iCub::iDyn::SensorLinkNewtonEuler::getForceMoment
yarp::sig::Vector getForceMoment() const
Get the sensor force and moment in a single (6x1) vector.
Definition: iDynInv.cpp:1650
iCub::iDyn::SensorLinkNewtonEuler::getType
virtual std::string getType() const
Definition: iDynInv.cpp:1658
iCub::iDyn::SensorLinkNewtonEuler::getAngVel
const yarp::sig::Vector & getAngVel() const
Definition: iDynInv.cpp:1356
iCub::iDyn::SensorLinkNewtonEuler::getH
const yarp::sig::Matrix & getH() const
Definition: iDynInv.cpp:1394
iCub::iDyn::SensorLinkNewtonEuler::SensorLinkNewtonEuler
SensorLinkNewtonEuler(const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_COM, const double _m, const yarp::sig::Matrix &_I, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor.
iCub::iDyn::SensorLinkNewtonEuler::computeAngAcc
void computeAngAcc(iDynLink *link)
Definition: iDynInv.cpp:1512
iCub::iDyn::SensorLinkNewtonEuler::F
yarp::sig::Vector F
measured or estimated force
Definition: iDynInv.h:722
iCub::iDyn::SensorLinkNewtonEuler::setForce
bool setForce(const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the ...
Definition: iDynInv.cpp:1396
iCub::iDyn::SensorLinkNewtonEuler::ForwardAttachToLink
void ForwardAttachToLink(iDynLink *link)
Compute w,dw,ddp,dppC given the reference frame of the link where the sensor is.
Definition: iDynInv.cpp:1326
iCub::iDyn::SensorLinkNewtonEuler::getAngAcc
const yarp::sig::Vector & getAngAcc() const
Definition: iDynInv.cpp:1358
iCub::iDyn::SensorLinkNewtonEuler::setMoment
bool setMoment(const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in th...
Definition: iDynInv.cpp:1412
iCub::iDyn::SensorLinkNewtonEuler::computeLinAcc
void computeLinAcc(iDynLink *link)
Definition: iDynInv.cpp:1517
iCub::iDyn::SensorLinkNewtonEuler::getAngAccM
const yarp::sig::Vector & getAngAccM() const
Definition: iDynInv.cpp:1392
iCub::iDyn::SensorLinkNewtonEuler::toString
std::string toString() const
Useful to print some information.
Definition: iDynInv.cpp:1310
iCub::iDyn::SensorLinkNewtonEuler::ddpC
yarp::sig::Vector ddpC
linear acceleration of the COM
Definition: iDynInv.h:733
iCub::iDyn::SensorLinkNewtonEuler::getr
const yarp::sig::Vector & getr(bool proj=false)
Definition: iDynInv.cpp:1372
iCub::iDyn::SensorLinkNewtonEuler::getLinAcc
const yarp::sig::Vector & getLinAcc() const
Definition: iDynInv.cpp:1360
iCub::iDyn::SensorLinkNewtonEuler::setLinAccC
bool setLinAccC(const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear accelerat...
Definition: iDynInv.cpp:1478
iCub::iDyn::SensorLinkNewtonEuler::dw
yarp::sig::Vector dw
angular acceleration
Definition: iDynInv.h:729
iCub::iDyn::SensorLinkNewtonEuler::computeForce
void computeForce(iDynLink *link)
Definition: iDynInv.cpp:1555
iCub::iDyn::SensorLinkNewtonEuler::getMoment
const yarp::sig::Vector & getMoment(bool isBase=false) const
Definition: iDynInv.cpp:1354
iCub::iDyn::SensorLinkNewtonEuler::ddp
yarp::sig::Vector ddp
linear acceleration
Definition: iDynInv.h:731
iCub::iDyn::SensorLinkNewtonEuler::Mu
yarp::sig::Vector Mu
measured or estimated moment
Definition: iDynInv.h:724
iCub::iDyn::SensorLinkNewtonEuler::setTorque
void setTorque(const double _Tau)
Set the OneLink torque, ie the corresponding iDynLink joint torque (nothing in the child classes deri...
Definition: iDynInv.cpp:1428
iCub::iDyn::SensorLinkNewtonEuler::SensorLinkNewtonEuler
SensorLinkNewtonEuler(const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor.
Definition: iDynInv.cpp:1229
iCub::iDyn::SensorLinkNewtonEuler::r_proj
yarp::sig::Vector r_proj
Definition: iDynInv.h:746
iCub::iDyn::SensorLinkNewtonEuler::setSensor
bool setSensor(const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I)
Set a new sensor or new sensor properties.
Definition: iDynInv.cpp:1280
iCub::iDyn::SensorLinkNewtonEuler::m
double m
the semi-link mass (the portion of link defined by the sensor)
Definition: iDynInv.h:741
iCub::iDyn::SensorLinkNewtonEuler::w
yarp::sig::Vector w
angular velocity
Definition: iDynInv.h:727
iCub::iDyn::SensorLinkNewtonEuler::getInertia
const yarp::sig::Matrix & getInertia() const
Definition: iDynInv.cpp:1366
iCub::iDyn::SensorLinkNewtonEuler::setLinAcc
bool setLinAcc(const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (i...
Definition: iDynInv.cpp:1462
iCub::iDyn::SensorLinkNewtonEuler::getLinAccC
const yarp::sig::Vector & getLinAccC() const
Definition: iDynInv.cpp:1362
iCub::iDyn::iCubArmDyn
A class for defining the 7-DOF iCub Arm in the iDyn framework.
Definition: iDyn.h:1328
iCub::iDyn::iCubArmNoTorsoDyn
A class for defining the 7-DOF iCub Arm in the iDyn framework.
Definition: iDyn.h:1369
iCub::iDyn::iCubLegDyn
A class for defining the 6-DOF iCub Leg.
Definition: iDyn.h:1452
iCub::iDyn::iDynChain
A Base class for defining a Serial Link Chain, using dynamics and kinematics.
Definition: iDyn.h:533
iCub::iDyn::iDynInvSensorArmNoTorso
A class for computing force/moment of the FT sensor placed in the middle of the iCub's left or right ...
Definition: iDynInv.h:1490
iCub::iDyn::iDynInvSensorArmNoTorso::iDynInvSensorArmNoTorso
iDynInvSensorArmNoTorso(iDyn::iCubArmNoTorsoDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2399
iCub::iDyn::iDynInvSensorArm
A class for computing force/moment of the FT sensor placed in the middle of the iCub's left or right ...
Definition: iDynInv.h:1450
iCub::iDyn::iDynInvSensorArm::iDynInvSensorArm
iDynInvSensorArm(iDyn::iCubArmDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2338
iCub::iDyn::iDynInvSensorArm::getType
std::string getType() const
Definition: iDynInv.cpp:2384
iCub::iDyn::iDynInvSensorLeg
A class for computing force/moment of the FT sensor placed in the middle of the iCub's left or right ...
Definition: iDynInv.h:1530
iCub::iDyn::iDynInvSensorLeg::getType
std::string getType() const
Definition: iDynInv.cpp:2558
iCub::iDyn::iDynInvSensorLeg::iDynInvSensorLeg
iDynInvSensorLeg(iDyn::iCubLegDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2512
iCub::iDyn::iDynInvSensor
A class for computing force/moment of a sensor placed anywhere in a kinematic chain; its position in ...
Definition: iDynInv.h:1223
iCub::iDyn::iDynInvSensor::getCOM
yarp::sig::Matrix getCOM() const
Get the sensor roto-traslational matrix of the center of mass of the semi-link defined by the sensor ...
Definition: iDynInv.cpp:2223
iCub::iDyn::iDynInvSensor::setSensorInfo
void setSensorInfo(const std::string &_info)
Definition: iDynInv.cpp:2170
iCub::iDyn::iDynInvSensor::getH
yarp::sig::Matrix getH() const
Get the sensor roto-translational matrix defining its position/orientation wrt the link.
Definition: iDynInv.cpp:2197
iCub::iDyn::iDynInvSensor::setMode
void setMode(const NewEulMode _mode=DYNAMIC)
Definition: iDynInv.cpp:2153
iCub::iDyn::iDynInvSensor::iDynInvSensor
iDynInvSensor(iDyn::iDynChain *_c, unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I, const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=0)
Constructor with FT sensor.
iCub::iDyn::iDynInvSensor::setInfo
void setInfo(const std::string &_info)
Definition: iDynInv.cpp:2165
iCub::iDyn::iDynInvSensor::mode
NewEulMode mode
static/dynamic/etc..
Definition: iDynInv.h:1234
iCub::iDyn::iDynInvSensor::setVerbose
void setVerbose(unsigned int verb=iCub::skinDynLib::VERBOSE)
Definition: iDynInv.cpp:2159
iCub::iDyn::iDynInvSensor::getInertia
yarp::sig::Matrix getInertia() const
Get the inertia of the portion of link defined between sensor and i-th frame.
Definition: iDynInv.cpp:2240
iCub::iDyn::iDynInvSensor::info
std::string info
a string with useful information if needed
Definition: iDynInv.h:1238
iCub::iDyn::iDynInvSensor::getSensorMoment
yarp::sig::Vector getSensorMoment() const
Returns the sensor estimated moment.
Definition: iDynInv.cpp:2130
iCub::iDyn::iDynInvSensor::setSensor
bool setSensor(unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I)
Set a new sensor or new sensor properties.
iCub::iDyn::iDynInvSensor::getSensorInfo
std::string getSensorInfo() const
Definition: iDynInv.cpp:2253
iCub::iDyn::iDynInvSensor::getInfo
std::string getInfo() const
Definition: iDynInv.cpp:2195
iCub::iDyn::iDynInvSensor::getSensorForceMoment
yarp::sig::Vector getSensorForceMoment() const
Get the sensor force and moment in a single (6x1) vector.
Definition: iDynInv.cpp:2265
iCub::iDyn::iDynInvSensor::setDynamicParameters
bool setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I)
Set the dynamic parameters of the the portion of link defined between sensor and i-th frame.
Definition: iDynInv.cpp:2175
iCub::iDyn::iDynInvSensor::getSensorForce
yarp::sig::Vector getSensorForce() const
Returns the sensor estimated force.
Definition: iDynInv.cpp:2118
iCub::iDyn::iDynInvSensor::iDynInvSensor
iDynInvSensor(iDyn::iDynChain *_c, const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor without FT sensor: the sensor must be set with setSensor()
Definition: iDynInv.cpp:2025
iCub::iDyn::iDynInvSensor::sens
SensorLinkNewtonEuler * sens
the sensor
Definition: iDynInv.h:1230
iCub::iDyn::iDynInvSensor::getMass
double getMass() const
Get the mass of the portion of link defined between sensor and i-th frame.
Definition: iDynInv.cpp:2210
iCub::iDyn::iDynInvSensor::computeSensorForceMoment
void computeSensorForceMoment()
Compute forces and moments at the sensor frame; this method calls special Forward and Backward method...
Definition: iDynInv.cpp:2102
iCub::iDyn::iDynInvSensor::getSensorLink
unsigned int getSensorLink() const
Definition: iDynInv.cpp:2277
iCub::iDyn::iDynInvSensor::lSens
unsigned int lSens
the link where the sensor is attached to
Definition: iDynInv.h:1228
iCub::iDyn::iDynInvSensor::verbose
unsigned int verbose
verbosity flag
Definition: iDynInv.h:1236
iCub::iDyn::iDynInvSensor::toString
std::string toString() const
Print some information.
Definition: iDynInv.cpp:2057
iCub::iDyn::iDynInvSensor::getTorques
yarp::sig::Vector getTorques() const
Definition: iDynInv.cpp:2142
iCub::iDyn::iDynInvSensor::chain
iDynChain * chain
the iDynChain describing the robotic chain
Definition: iDynInv.h:1232
iCub::iDyn::iDynSensorArmNoTorso
A class for computing joint force/moment/torque of an iCub arm (left/right) given the FT measurements...
Definition: iDynInv.h:1785
iCub::iDyn::iDynSensorArmNoTorso::getType
std::string getType() const
Definition: iDynInv.cpp:2770
iCub::iDyn::iDynSensorArmNoTorso::iDynSensorArmNoTorso
iDynSensorArmNoTorso(iDyn::iCubArmNoTorsoDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2746
iCub::iDyn::iDynSensorArm
A class for computing joint force/moment/torque of an iCub arm (left/right) given the FT measurements...
Definition: iDynInv.h:1753
iCub::iDyn::iDynSensorArm::iDynSensorArm
iDynSensorArm(iDyn::iCubArmDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2707
iCub::iDyn::iDynSensorArm::getType
std::string getType() const
Definition: iDynInv.cpp:2730
iCub::iDyn::iDynSensorLeg
A class for computing joint force/moment/torque of an iCub leg (left/right) given the FT measurements...
Definition: iDynInv.h:1816
iCub::iDyn::iDynSensorLeg::iDynSensorLeg
iDynSensorLeg(iDyn::iCubLegDyn *_c, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor: the sensor is automatically set with "right" or "left" choice.
Definition: iDynInv.cpp:2787
iCub::iDyn::iDynSensorLeg::getType
std::string getType() const
Definition: iDynInv.cpp:2810
iCub::iDyn::iDynSensor
A class for computing forces and torques in a iDynChain, when a force/torque sensor is placed in the ...
Definition: iDynInv.h:1578
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::iDynSensor::setSensorMeasures
bool setSensorMeasures(const yarp::sig::Vector &FM)
Set the sensor measured force and moment at once.
iCub::iDyn::iDynSensor::computeFromSensorNewtonEuler
virtual void computeFromSensorNewtonEuler()
The main computation method: given the FT sensor measurements, compute forces moments and torques in ...
Definition: iDynInv.cpp:2611
iCub::iDyn::iDynSensor::computeWrenchFromSensorNewtonEuler
virtual void computeWrenchFromSensorNewtonEuler()
The main computation method: given the FT sensor measurements, compute forces moments and torques in ...
Definition: iDynInv.cpp:2634
iCub::iDyn::iDynSensor::getForce
yarp::sig::Vector getForce(const unsigned int iLink) const
Returns the i-th link force.
Definition: iDynInv.cpp:2684
iCub::iDyn::iDynSensor::getMomentsNewtonEuler
yarp::sig::Matrix getMomentsNewtonEuler() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
Definition: iDynInv.cpp:2692
iCub::iDyn::iDynSensor::getTorque
double getTorque(const unsigned int iLink) const
Returns the i-th link torque.
Definition: iDynInv.cpp:2688
iCub::iDyn::iDynSensor::getTorquesNewtonEuler
yarp::sig::Vector getTorquesNewtonEuler() const
Returns the links torque as a vector.
Definition: iDynInv.cpp:2694
iCub::iDyn::iDynSensor::iDynSensor
iDynSensor(iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor without FT sensor: the sensor must be set with setSensor()
Definition: iDynInv.cpp:2574
iCub::iDyn::iDynSensor::getForcesNewtonEuler
yarp::sig::Matrix getForcesNewtonEuler() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
Definition: iDynInv.cpp:2690
iCub::iDyn::iDynSensor::iDynSensor
iDynSensor(iDyn::iDynChain *_c, unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor with FT sensor.
iCub::iDyn::iDynSensor::getMoment
yarp::sig::Vector getMoment(const unsigned int iLink) const
Returns the i-th link moment.
Definition: iDynInv.cpp:2686
iCub::iDyn::iDynSensor::getForces
yarp::sig::Matrix getForces() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
Definition: iDynInv.cpp:2678
iCub::iDyn::iDynSensor::computeFromSensorNewtonEuler
virtual bool computeFromSensorNewtonEuler(const yarp::sig::Vector &FMu)
The main computation method: given the FT sensor measurements, compute forces moments and torques in ...
iCub::iDyn::iDynSensor::computeFromSensorNewtonEuler
virtual bool computeFromSensorNewtonEuler(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
The main computation method: given the FT sensor measurements, compute forces moments and torques in ...
iCub::iDyn::iDynSensor::getMoments
yarp::sig::Matrix getMoments() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
Definition: iDynInv.cpp:2680
iCub::iDyn::iDynSensor::getForceMomentEndEff
virtual yarp::sig::Vector getForceMomentEndEff() const
Returns the end-effector force-moment as a single (6x1) vector.
Definition: iDynInv.cpp:2696
iCub::iDyn::iDynSensor::getTorques
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDynInv.cpp:2682
F
F
Definition: compute_ekf_fast.m:22
iCub::iDyn::ChainIterationMode_s
const std::string ChainIterationMode_s[2]
Definition: iDynInv.h:70
iCub::iDyn::ChainComputationMode
ChainComputationMode
Definition: iDynInv.h:69
iCub::iDyn::KINBWD_WREFWD
@ KINBWD_WREFWD
Definition: iDynInv.h:69
iCub::iDyn::KINFWD_WREBWD
@ KINFWD_WREBWD
Definition: iDynInv.h:69
iCub::iDyn::KINBWD_WREBWD
@ KINBWD_WREBWD
Definition: iDynInv.h:69
iCub::iDyn::KINFWD_WREFWD
@ KINFWD_WREFWD
Definition: iDynInv.h:69
iCub::iDyn::DYNAMIC_W_ROTOR
@ DYNAMIC_W_ROTOR
Definition: iDynInv.h:64
iCub::iDyn::DYNAMIC_CORIOLIS_GRAVITY
@ DYNAMIC_CORIOLIS_GRAVITY
Definition: iDynInv.h:64
iCub::iDyn::DYNAMIC
@ DYNAMIC
Definition: iDynInv.h:64
iCub::iDyn::STATIC
@ STATIC
Definition: iDynInv.h:64
iCub::iDyn::ChainIterationMode
ChainIterationMode
Definition: iDynInv.h:68
iCub::iDyn::BACKWARD
@ BACKWARD
Definition: iDynInv.h:68
iCub::iDyn::FORWARD
@ FORWARD
Definition: iDynInv.h:68
iCub::iDyn::NewEulMode_s
const std::string NewEulMode_s[4]
Definition: iDynInv.h:65
iCub::iDyn::ChainComputationMode_s
const std::string ChainComputationMode_s[4]
Definition: iDynInv.h:71
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
Generated on Wed Nov 13 2024 21:11:41 for iCub-main by doxygen 1.9.1