iCub-main
Loading...
Searching...
No Matches
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
56namespace iCub
57{
58
59namespace iDyn
60{
61
62// Newton-Euler types
63// useful for iDyn, iDynInv, iDynBody
64enum NewEulMode {STATIC,DYNAMIC,DYNAMIC_W_ROTOR,DYNAMIC_CORIOLIS_GRAVITY};
65const std::string NewEulMode_s[4] = {"static","dynamic","dynamic with motor/rotor","dynamic with only Coriolis and gravitational terms"};
66
67// Kinematic and Wrench modes
68enum ChainIterationMode { FORWARD, BACKWARD };
69enum ChainComputationMode { KINFWD_WREFWD, KINFWD_WREBWD, KINBWD_WREFWD, KINBWD_WREBWD };
70const std::string ChainIterationMode_s[2] = {"Forward (Base To End)","Backward (End To Base)"};
71const 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
95class OneLinkNewtonEuler
96{
97protected:
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
252public:
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
491class BaseLinkNewtonEuler : public OneLinkNewtonEuler
492{
493protected:
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
516public:
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
610class FinalLinkNewtonEuler : public OneLinkNewtonEuler
611{
612protected:
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
632public:
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
718class SensorLinkNewtonEuler : public OneLinkNewtonEuler
719{
720protected:
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
751public:
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
881class OneChainNewtonEuler
882{
883 friend class iDynChain;
884
885protected:
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
904public:
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
1222class iDynInvSensor
1223{
1224 friend class iFTransformation;
1225protected:
1226
1228 unsigned int lSens;
1230 SensorLinkNewtonEuler * sens;
1232 iDynChain * chain;
1234 NewEulMode mode;
1236 unsigned int verbose;
1238 std::string info;
1239
1240public:
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
1373class iCubArmSensorLink : public SensorLinkNewtonEuler
1374{
1375protected:
1376
1378 std::string type;
1379
1380public:
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
1410class iCubLegSensorLink : public SensorLinkNewtonEuler
1411{
1412protected:
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
1449class iDynInvSensorArm : public iDynInvSensor
1450{
1451
1452public:
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
1489class iDynInvSensorArmNoTorso : public iDynInvSensor
1490{
1491
1492public:
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
1529class iDynInvSensorLeg : public iDynInvSensor
1530{
1531
1532public:
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
1577class iDynSensor: public iDynInvSensor
1578{
1579
1580public:
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
1752class iDynSensorArm : public iDynSensor
1753{
1754
1755public:
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
1784class iDynSensorArmNoTorso : public iDynSensor
1785{
1786
1787public:
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
1815class iDynSensorLeg : public iDynSensor
1816{
1817
1818public:
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::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::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::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::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::~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::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::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::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::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::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::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::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::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::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::ChainIterationMode
ChainIterationMode
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 Dec 18 2024 16:34:47 for iCub-main by doxygen 1.9.8