iCub-main
iDyn.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 
61 #ifndef __IDYN_H__
62 #define __IDYN_H__
63 
64 #include <yarp/os/Property.h>
65 #include <yarp/dev/ControlBoardInterfaces.h>
66 #include <yarp/sig/Vector.h>
67 #include <yarp/sig/Matrix.h>
68 
69 #include <iCub/ctrl/math.h>
70 
71 #include <iCub/iKin/iKinFwd.h>
72 #include <iCub/iDyn/iDynInv.h>
73 
74 #include <deque>
75 #include <string>
76 
77 
78 namespace iCub
79 {
80 
81 namespace iDyn
82 {
83  void notImplemented(const unsigned int verbose);
84  void notImplemented(const unsigned int verbose, const std::string &msg);
85  void workInProgress(const unsigned int verbose, const std::string &msg);
86  bool asWrench(yarp::sig::Vector &w, const yarp::sig::Vector &f, const yarp::sig::Vector &m);
87  yarp::sig::Vector asWrench(const yarp::sig::Vector &f, const yarp::sig::Vector &m);
88  bool asForceMoment(const yarp::sig::Vector &w, yarp::sig::Vector &f, yarp::sig::Vector &m);
89 
90  class OneLinkNewtonEuler;
91  class BaseLinkNewtonEuler;
94  class ContactNewtonEuler;
95  class iDynContactSolver;
96  class OneChainNewtonEuler;
97  class OneChainSensorNewtonEuler;
98  class iDynSensor;
99  class iGenericFrame;
100  class iFrameOnLink;
101  class iFTransformation;
102  class iDynSensorLeg;
103  class iDynSensorArm;
104  class iCubWholeBody;
105 
106 
107 
117 class iDynLink : public iKin::iKinLink
118 {
119  friend class iDynChain;
120  friend class OneLinkNewtonEuler;
121 
122 protected:
123  // DH rototranslation matrix (it's the same matrix you get calling iKinLink->getH(true) but it's stored here for performance reason)
124  yarp::sig::Matrix H_store;
125  yarp::sig::Matrix R_store;
126  yarp::sig::Vector r_store;
127  yarp::sig::Vector r_proj_store;
128  // flag that is true is if H_store is valid, false otherwise
130 
132  double m;
134  yarp::sig::Matrix HC;
136  yarp::sig::Matrix RC;
138  yarp::sig::Vector rc, rc_proj;
140  yarp::sig::Matrix I;
142  double dq;
144  double ddq;
146  yarp::sig::Vector w;
148  yarp::sig::Vector dw;
150  yarp::sig::Vector dwM;
151 
153  yarp::sig::Vector dp;
155  yarp::sig::Vector dpC;
157  yarp::sig::Vector ddp;
159  yarp::sig::Vector ddpC;
160 
162  yarp::sig::Vector F;
164  yarp::sig::Vector Mu;
166  double Tau;
167 
168  //only including motor dynamic
169 
171  double Im;
173  double kr;
175  double Fv;
177  double Fs;
178 
182  iDynLink();
183 
187  virtual void clone(const iDynLink &l);
188 
189  virtual void updateHstore();
190 
191 public:
192 
193  //~~~~~~~~~~~~~~~~~~~~~~
194  // set methods
195  //~~~~~~~~~~~~~~~~~~~~~~
196 
208  bool setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im);
209 
217  bool setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I);
218 
225  bool setStaticParameters(const double _m, const yarp::sig::Matrix &_HC);
226 
232  bool setInertia(const yarp::sig::Matrix &_I);
233 
243  void setInertia(const double Ixx, const double Ixy, const double Ixz, const double Iyy, const double Iyz, const double Izz);
244 
249  void setMass(const double _m);
250 
256  double setAng(const double _teta);
257 
263  double setDAng(const double _dteta);
264 
270  double setD2Ang(const double _ddteta);
271 
276  const yarp::sig::Vector& getLinVel() const;
277 
282  const yarp::sig::Vector& getLinVelC() const;
283 
290  void setAngPosVelAcc(const double _teta,const double _dteta,const double _ddteta);
291 
297  bool setCOM(const yarp::sig::Matrix &_HC);
303  bool setCOM(const yarp::sig::Vector &_rC);
304 
311  void setCOM(const double _rCx, const double _rCy, const double _rCz);
312 
318  bool setForce(const yarp::sig::Vector &_F);
319 
325  bool setMoment(const yarp::sig::Vector &_Mu);
326 
333  bool setForceMoment(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
334 
339  void setTorque(const double _Tau);
340 
341 
342  //~~~~~~~~~~~~~~~~~~~~~~
343  // get methods
344  //~~~~~~~~~~~~~~~~~~~~~~
345 
350  const yarp::sig::Matrix& getInertia() const;
355  double getMass() const;
356  double getIm() const;
357  double getKr() const;
358  double getFs() const;
359  double getFv() const;
364  const yarp::sig::Matrix& getCOM() const;
369  double getDAng() const;
374  double getD2Ang() const;
379  const yarp::sig::Vector& getW() const;
384  const yarp::sig::Vector& getdW() const;
389  const yarp::sig::Vector& getdWM() const;
394  const yarp::sig::Vector& getLinAcc() const;
399  const yarp::sig::Vector& getLinAccC() const;
404  const yarp::sig::Vector& getForce() const;
409  const yarp::sig::Vector& getMoment() const;
414  double getTorque() const;
419  const yarp::sig::Matrix& getH();
424  const yarp::sig::Matrix& getR();
429  const yarp::sig::Matrix& getRC() const;
430 
436  const yarp::sig::Vector& getr(bool proj=false);
442  const yarp::sig::Vector& getrC(bool proj=false) const;
443 
444  //~~~~~~~~~~~~~~~~~~~~~~
445  // basic functions
446  //~~~~~~~~~~~~~~~~~~~~~~
447 
457  iDynLink(double _A, double _D, double _Alpha, double _Offset, double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
458 
471  iDynLink(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, double _A, double _D, double _Alpha, double _Offset, double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
472 
485  iDynLink(const double _m, const yarp::sig::Vector &_C, const yarp::sig::Matrix &_I, double _A, double _D, double _Alpha, double _Offset, double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
486 
506  iDynLink(const double _m, const double _rCx, const double _rCy, const double _rCz, const double Ixx, const double Ixy, const double Ixz, const double Iyy, const double Iyz, const double Izz, double _A, double _D, double _Alpha, double _Offset, double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
507 
511  iDynLink(const iDynLink &c);
512 
516  void zero();
517 
521  iDynLink &operator=(const iDynLink &c);
522 
523 };
524 
525 
526 
533 {
534  friend class OneChainNewtonEuler;
535  friend class iDynInvSensor;
536  friend class iDynSensor;
538  friend class iDynContactSolver;
539 
540 protected:
541 
546 
547  //curr_q = q pos is already in iKinChain
549  yarp::sig::Vector curr_dq;
551  yarp::sig::Vector curr_ddq;
552 
555 
556  const yarp::sig::Vector zero0;
557 
561  virtual void clone(const iDynChain &c);
562 
566  virtual void build();
567 
571  virtual void dispose();
572 
578  iDynLink * refLink(const unsigned int i);
579 
580 
581 public:
582 
586  iDynChain();
587 
592  iDynChain(const iDynChain &c);
593 
599  iDynChain &operator=(const iDynChain &c);
600 
607  yarp::sig::Vector setAng(const yarp::sig::Vector &q);
608 
614  yarp::sig::Vector setDAng(const yarp::sig::Vector &dq);
615 
621  yarp::sig::Vector setD2Ang(const yarp::sig::Vector &ddq);
622 
627  yarp::sig::Vector getDAng();
628 
633  yarp::sig::Vector getD2Ang();
634 
639  yarp::sig::Vector getJointBoundMin();
640 
645  yarp::sig::Vector getJointBoundMax();
646 
654  double setAng(const unsigned int i, double q);
655 
662  double setDAng(const unsigned int i, double dq);
663 
670  double setD2Ang(const unsigned int i, double ddq);
671 
677  double getDAng(const unsigned int i);
678 
684  double getD2Ang(const unsigned int i);
685 
690  yarp::sig::Vector getMasses() const;
691 
696  bool setMasses(yarp::sig::Vector _m);
697 
702  double getMass(const unsigned int i) const;
703 
708  bool setMass(const unsigned int i, const double _m);
709 
714  yarp::sig::Matrix getInertia(const unsigned int i) const;
715 
720  yarp::sig::Matrix getForces() const;
721 
726  yarp::sig::Matrix getMoments() const;
727 
732  yarp::sig::Vector getTorques() const;
733 
738  const yarp::sig::Vector& getForce(const unsigned int iLink) const;
739 
744  const yarp::sig::Vector& getMoment(const unsigned int iLink) const;
745 
750  double getTorque(const unsigned int iLink) const;
751 
756  yarp::sig::Vector getLinVel(const unsigned int i) const;
757 
762  yarp::sig::Vector getLinVelCOM(const unsigned int i) const;
763 
768  yarp::sig::Vector getLinAcc(const unsigned int i) const;
769 
774  const yarp::sig::Vector& getLinAccCOM(const unsigned int i) const;
775 
780  yarp::sig::Vector getAngVel(const unsigned int i) const;
781 
786  yarp::sig::Vector getAngAcc (const unsigned int i) const;
787 
799  bool setDynamicParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im);
800 
808  bool setDynamicParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I);
809 
816  bool setStaticParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC);
817 
822 
830  bool computeNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend);
831 
838  bool computeNewtonEuler();
839 
844  bool initNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend);
845 
850  bool initNewtonEuler();
851 
856 
861  yarp::sig::Matrix getForcesNewtonEuler() const;
862 
867  yarp::sig::Matrix getMomentsNewtonEuler() const;
868 
873  yarp::sig::Vector getTorquesNewtonEuler() const;
874 
879  yarp::sig::Vector getForceMomentEndEff() const;
880 
886  void setIterModeKinematic(const ChainIterationMode _iterateMode_kinematics = FORWARD );
887 
893  void setIterModeWrench(const ChainIterationMode _iterateMode_wrench = BACKWARD );
894 
904 
911 
918 
925 
932 
943  bool initKinematicNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
944 
954  bool initWrenchNewtonEuler(const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend);
955 
964  void getKinematicNewtonEuler( yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp);
965 
974  void getFrameKinematic(unsigned int i, yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp);
975 
983  void getFrameWrench(unsigned int i, yarp::sig::Vector &F, yarp::sig::Vector &Mu);
989  void getWrenchNewtonEuler( yarp::sig::Vector &F, yarp::sig::Vector &Mu);
990 
994  virtual ~iDynChain();
995 
996 
997  //------------
998  // jacobians
999  //------------
1000 
1008  yarp::sig::Matrix computeGeoJacobian(const unsigned int iLinkN , const yarp::sig::Matrix &Pn );
1009 
1018  yarp::sig::Matrix computeGeoJacobian(const unsigned int iLinkN, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0 );
1019 
1026  yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn );
1027 
1035  yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0 );
1036 
1043  yarp::sig::Matrix getDenHart(unsigned int i);
1044 
1045  //---------------
1046  // jacobians COM
1047  //---------------
1048 
1058  yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink);
1059 
1066  yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn);
1067 
1075  yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0 );
1076 
1081  yarp::sig::Matrix getCOM(unsigned int iLink);
1082 
1087  yarp::sig::Matrix getHCOM(unsigned int iLink);
1088 
1089 
1090  //---------------------------
1091  // Lagrange Formulation
1092  //---------------------------
1093 
1098  yarp::sig::Matrix computeMassMatrix();
1099 
1105  yarp::sig::Matrix computeMassMatrix(const yarp::sig::Vector& q);
1106 
1111  yarp::sig::Vector computeCcTorques();
1112 
1119  yarp::sig::Vector computeCcTorques(const yarp::sig::Vector& q, const yarp::sig::Vector& dq);
1120 
1127  yarp::sig::Vector computeGravityTorques(const yarp::sig::Vector& ddp0);
1128 
1136  yarp::sig::Vector computeGravityTorques(const yarp::sig::Vector& ddp0, const yarp::sig::Vector& q);
1137 
1144  yarp::sig::Vector computeCcGravityTorques(const yarp::sig::Vector& ddp0);
1145 
1154  yarp::sig::Vector computeCcGravityTorques(const yarp::sig::Vector& ddp0, const yarp::sig::Vector& q, const yarp::sig::Vector& dq);
1155 
1156 
1157 
1158 };
1159 
1160 
1161 
1162 
1163 
1164 
1176 class iDynLimb : public iDynChain
1177 {
1178 protected:
1179  std::deque<iDynLink*> linkList;
1182 
1183  virtual void allocate(const std::string &_type);
1184  virtual void clone(const iDynLimb &limb);
1185  virtual void dispose();
1186 
1187  // make the following methods protected in order to prevent user from changing
1188  // too easily the internal structure of the chain;
1189  // to get access anyway to these hidden methods, user can rely on asChain()
1193  iKin::iKinLink &operator[](const unsigned int i) { return iKin::iKinChain::operator[](i); }
1194  iKin::iKinLink &operator()(const unsigned int i) { return iKin::iKinChain::operator()(i); }
1195  bool addLink(const unsigned int i, iKin::iKinLink &l) { return iKin::iKinChain::addLink(i,l); }
1196  bool rmLink(const unsigned int i) { return iKin::iKinChain::rmLink(i); }
1200  void pushLink(iDynLink *pl);
1201 
1202 public:
1206  iDynLimb();
1207 
1212  iDynLimb(const std::string &_type);
1213 
1218  iDynLimb(const iDynLimb &limb);
1219 
1225  iDynLimb(const yarp::os::Property &option);
1226 
1273  bool fromLinksProperties(const yarp::os::Property &option);
1274 
1279  bool isValid() { return configured; }
1280 
1286  iDynLimb &operator=(const iDynLimb &limb);
1287 
1294  iDynChain *asChain() { return static_cast<iDynChain*>(this); }
1295 
1300  std::string getType() { return type; }
1301 
1305  virtual ~iDynLimb();
1306 
1316  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim)
1317  { notImplemented(verbose); return true; }
1318 
1319 };
1320 
1321 
1327 class iCubArmDyn : public iDynLimb
1328 {
1329 protected:
1330  virtual void allocate(const std::string &_type);
1331 
1332 public:
1336  iCubArmDyn();
1337 
1342  iCubArmDyn(const std::string &_type, const ChainComputationMode _mode=KINFWD_WREBWD);
1343 
1348  iCubArmDyn(const iCubArmDyn &arm);
1349 
1357  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1358 
1359 };
1360 
1361 
1362 
1369 {
1370 protected:
1371  virtual void allocate(const std::string &_type);
1372 
1373 public:
1378 
1385 
1391 
1399  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1400 
1401 };
1402 
1403 
1409 class iCubTorsoDyn : public iDynLimb
1410 {
1411 protected:
1412  virtual void allocate(const std::string &_type);
1413 
1414 public:
1418  iCubTorsoDyn();
1419 
1425  iCubTorsoDyn(const std::string &_type, const ChainComputationMode _mode=KINFWD_WREBWD);
1426 
1431  iCubTorsoDyn(const iCubTorsoDyn &torso);
1432 
1440  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1441 
1442 
1443 };
1444 
1445 
1451 class iCubLegDyn : public iDynLimb
1452 {
1453 protected:
1454  virtual void allocate(const std::string &_type);
1455 
1456 public:
1460  iCubLegDyn();
1461 
1467  iCubLegDyn(const std::string &_type,const ChainComputationMode _mode=KINFWD_WREBWD);
1468 
1473  iCubLegDyn(const iCubLegDyn &leg);
1474 
1482  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1483 
1484 };
1485 
1491 class iCubLegDynV2 : public iDynLimb
1492 {
1493 protected:
1494  virtual void allocate(const std::string &_type);
1495 
1496 public:
1500  iCubLegDynV2();
1501 
1507  iCubLegDynV2(const std::string &_type,const ChainComputationMode _mode=KINFWD_WREBWD);
1508 
1513  iCubLegDynV2(const iCubLegDyn &leg);
1514 
1522  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1523 
1524 };
1525 
1526 
1533 {
1534 protected:
1535  virtual void allocate(const std::string &_type);
1536 
1537 public:
1542 
1548 
1556  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1557 
1558 };
1559 
1566 {
1567 protected:
1568  virtual void allocate(const std::string &_type);
1569 
1570 public:
1574  iCubNeckInertialDynV2(const ChainComputationMode _mode=KINBWD_WREBWD, const std::string &_type ="v2.5");
1575 
1581 
1589  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1590 
1591 };
1592 
1593 }
1594 
1595 }//end namespace
1596 
1597 #endif
1598 
1599 
1600 
iCub::iDyn::iDynChain::NE
OneChainNewtonEuler * NE
pointer to OneChainNewtonEuler class, to be used for computing forces and torques
Definition: iDyn.h:554
iCub::iDyn::iCubArmDyn::iCubArmDyn
iCubArmDyn()
Default constructor.
Definition: iDyn.cpp:1950
iCub::iDyn::iDynLimb::popLink
void popLink()
Definition: iDyn.h:1199
iCub::iDyn::iDynChain::setModeNewtonEuler
void setModeNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC)
Set the computation mode for Newton-Euler (static/dynamic/etc)
Definition: iDyn.cpp:1204
iCub::iDyn::iDynInvSensor
Definition: iDynInv.h:1222
iCub::iDyn::iDynChain::getForces
yarp::sig::Matrix getForces() const
Returns the links forces as a matrix, where the i-th col is the i-th force.
Definition: iDyn.cpp:859
iCub::iDyn::iDynChain::initKinematicNewtonEuler
bool initKinematicNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Calls the proper method to set kinematics variables in OneChainNewtonEuler: either initKinematicBase(...
Definition: iDyn.cpp:1144
iKinFwd.h
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::iDyn::iDynLimb::dispose
virtual void dispose()
Dispose.
Definition: iDyn.cpp:1926
iCub::iDyn::iCubTorsoDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Torso joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2202
iCub::iDyn::iDynLimb::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:1891
iCub::iDyn::iCubTorsoDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2166
iCub::iDyn::iDynChain::computeNewtonEuler
bool computeNewtonEuler()
Compute forces and torques with the Newton-Euler recursive algorithm: forward and backward phase are ...
Definition: iDyn.cpp:965
iCub::iDyn::iDynLimb::pushLink
void pushLink(iKin::iKinLink &l)
Definition: iDyn.h:1197
iCub::iDyn::iDynChain::getForceMomentEndEff
yarp::sig::Vector getForceMomentEndEff() const
Returns the end effector force-moment as a single (6x1) vector.
Definition: iDyn.cpp:1272
iCub::iDyn::iCubNeckInertialDynV2::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Inertial Sensor joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2635
iCub::iDyn::iDynLimb::rmLink
bool rmLink(const unsigned int i)
Definition: iDyn.h:1196
iCub::iDyn::iCubTorsoDyn
Definition: iDyn.h:1409
iCub::iDyn::asForceMoment
bool asForceMoment(const yarp::sig::Vector &w, yarp::sig::Vector &f, yarp::sig::Vector &m)
iCub::iDyn::iDynChain::initWrenchNewtonEuler
bool initWrenchNewtonEuler(const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Calls the proper method to set wrench variables in OneChainNewtonEuler: either initKinematicBase() or...
Definition: iDyn.cpp:1174
iCub::iDyn::notImplemented
void notImplemented(const unsigned int verbose)
Definition: iDyn.cpp:54
iCub::iDyn::iDynLimb::operator[]
iKin::iKinLink & operator[](const unsigned int i)
Definition: iDyn.h:1193
iCub::iDyn::iDynChain
Definition: iDyn.h:532
iCub::iDyn::iDynChain::zero0
const yarp::sig::Vector zero0
Definition: iDyn.h:556
iCub::iDyn::iCubArmNoTorsoDyn::iCubArmNoTorsoDyn
iCubArmNoTorsoDyn()
Default constructor.
Definition: iDyn.cpp:2060
iCub::iDyn::iDynChain::getAngVel
yarp::sig::Vector getAngVel(const unsigned int i) const
Returns the i-th link angular velocity.
Definition: iDyn.cpp:733
iCub::iDyn::iDynChain::computeKinematicNewtonEuler
void computeKinematicNewtonEuler()
Calls the proper method to compute kinematics variables: either ForwardKinematicFromBase() or Backwar...
Definition: iDyn.cpp:992
iDynInv.h
iCub::iDyn::iDynLimb::configured
bool configured
Definition: iDyn.h:1181
iCub::learningmachine::serialization::operator<<
yarp::os::Bottle & operator<<(yarp::os::Bottle &out, int val)
Pushes a serialization of a vector to the end of a Bottle.
Definition: Serialization.cpp:25
iCub::iDyn::iCubNeckInertialDynV2::iCubNeckInertialDynV2
iCubNeckInertialDynV2(const ChainComputationMode _mode=KINBWD_WREBWD, const std::string &_type="v2.5")
Default constructor.
Definition: iDyn.cpp:2586
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iCub::iDyn::NewEulMode
NewEulMode
Definition: iDynInv.h:64
iCub::iDyn::workInProgress
void workInProgress(const unsigned int verbose, const std::string &msg)
Definition: iDyn.cpp:64
iCub::iDyn::iDynChain::setIterModeWrench
void setIterModeWrench(const ChainIterationMode _iterateMode_wrench=BACKWARD)
Set the iteration direction during recursive computation of wrench variables (F,Mu,...
Definition: iDyn.cpp:1287
iCub::iDyn::iCubNeckInertialDyn::iCubNeckInertialDyn
iCubNeckInertialDyn(const ChainComputationMode _mode=KINBWD_WREBWD)
Default constructor.
Definition: iDyn.cpp:2521
iCub::iDyn::iDynChain::iterateMode_wrench
ChainIterationMode iterateMode_wrench
specifies the 'direction' of recursive computation of wrenches (F,Mu): FORWARD, BACKWARD
Definition: iDyn.h:545
iCub::iDyn::iDynChain::getIterModeWrench
ChainIterationMode getIterModeWrench() const
Get the iteration direction during recursive computation of wrench variables (F,Mu,...
Definition: iDyn.cpp:1297
iCub::iDyn::iDynLimb::operator<<
iKin::iKinChain & operator<<(iKin::iKinLink &l)
Definition: iDyn.h:1191
iCub::iDyn::iDynChain::computeGravityTorques
yarp::sig::Vector computeGravityTorques(const yarp::sig::Vector &ddp0)
Compute the torques generated by gravity considering only the active joints.
iCub::iDyn::iDynLimb::linkList
std::deque< iDynLink * > linkList
Definition: iDyn.h:1179
iCub::iDyn::iDynChain::getMass
double getMass(const unsigned int i) const
Returns the i-th link mass.
Definition: iDyn.cpp:822
iCub::iDyn::BaseLinkNewtonEuler
Definition: iDynInv.h:491
iCub::iDyn::iCubLegDyn
Definition: iDyn.h:1451
iCub::iKin::iKinChain::operator--
iKinChain & operator--(int)
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:393
iCub::iKin::iKinChain::popLink
void popLink()
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:383
iCub::iDyn::iFTransformation
Definition: iDynTransform.h:298
iCub::iKin::iKinChain::operator[]
iKinLink & operator[](const unsigned int i)
Returns a reference to the ith Link of the Chain.
Definition: iKinFwd.h:419
iCub::iDyn::asWrench
bool asWrench(yarp::sig::Vector &w, const yarp::sig::Vector &f, const yarp::sig::Vector &m)
iCub::iKin::iKinChain::clear
void clear()
Removes all Links.
Definition: iKinFwd.cpp:361
iCub::iDyn::iCubLegDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2255
iCub::iDyn::iCubArmNoTorsoDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2077
iCub::iDyn::iDynChain::getTorquesNewtonEuler
yarp::sig::Vector getTorquesNewtonEuler() const
Returns the links torque as a vector.
Definition: iDyn.cpp:1257
iCub::iDyn::iCubLegDynV2
Definition: iDyn.h:1491
iCub::iDyn::iDynChain::computeGeoJacobian
yarp::sig::Matrix computeGeoJacobian(const unsigned int iLinkN, const yarp::sig::Matrix &Pn)
Compute the Jacobian from link 0 to iLinkN.
iCub::iDyn::iDynChain::computeCcGravityTorques
yarp::sig::Vector computeCcGravityTorques(const yarp::sig::Vector &ddp0)
Compute the torques generated by gravity and centrifugal and coriolis forces, considering only the ac...
iCub::iDyn::iDynLimb::operator--
iKin::iKinChain & operator--(int)
Definition: iDyn.h:1192
iCub::iDyn::iCubNeckInertialDyn
Definition: iDyn.h:1532
iCub::iDyn::iCubLegDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Leg joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2316
iCub::iDyn::iDynChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iDyn::iCubArmDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Arm joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2016
iCub::iDyn::iDynChain::getFrameWrench
void getFrameWrench(unsigned int i, yarp::sig::Vector &F, yarp::sig::Vector &Mu)
Get the wrench information of the i-th frame in the OneChainNewtonEuler associated to the current iDy...
Definition: iDyn.cpp:1055
iCub::iDyn::iDynChain::iDynChain
iDynChain()
Default constructor.
Definition: iDyn.cpp:476
iCub::iDyn::iDynLimb::clear
void clear()
Definition: iDyn.h:1198
iCub::iDyn::iDynLimb::clone
virtual void clone(const iDynLimb &limb)
Definition: iDyn.cpp:1901
iCub::iDyn::iDynChain::prepareNewtonEuler
void prepareNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC)
Prepare for the Newton-Euler recursive computation of forces and torques.
Definition: iDyn.cpp:916
iCub::iDyn::iDynChain::setIterMode
void setIterMode(const ChainComputationMode mode=KINFWD_WREBWD)
Set the computation mode during recursive computation of kinematics (w,dw,d2p,d2pC) and wrench variab...
Definition: iDyn.cpp:1302
iCub::iDyn::iDynChain::getMasses
yarp::sig::Vector getMasses() const
Returns the link masses as a vector.
Definition: iDyn.cpp:799
iCub::iDyn::iDynChain::getLinVelCOM
yarp::sig::Vector getLinVelCOM(const unsigned int i) const
Returns the i-th link linear velocity of the COM.
Definition: iDyn.cpp:700
iCub::iDyn::ChainComputationMode
ChainComputationMode
Definition: iDynInv.h:69
iCub::iDyn::OneChainNewtonEuler
Definition: iDynInv.h:881
iCub::iDyn::iDynChain::initNewtonEuler
bool initNewtonEuler()
Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0...
Definition: iDyn.cpp:1099
iCub::iDyn::iCubLegDyn::iCubLegDyn
iCubLegDyn()
Default constructor.
Definition: iDyn.cpp:2238
F
F
Definition: compute_ekf_fast.m:22
iCub::iDyn::iDynChain::dispose
virtual void dispose()
Dispose.
Definition: iDyn.cpp:508
math.h
iCub::iDyn::iCubArmDyn
Definition: iDyn.h:1327
iCub::iDyn::iDynChain::getForcesNewtonEuler
yarp::sig::Matrix getForcesNewtonEuler() const
Returns the links forces as a matrix, where the (i+1)-th col is the i-th force.
Definition: iDyn.cpp:1226
iCub::iDyn::iDynChain::~iDynChain
virtual ~iDynChain()
Destructor.
Definition: iDyn.cpp:518
iCub::iDyn::iDynLimb::operator=
iDynChain & operator=(const iDynChain &c)
Definition: iDyn.h:1190
iCub::iDyn::iDynLimb::fromLinksProperties
bool fromLinksProperties(const yarp::os::Property &option)
Initializes the Limb from a list of properties wherein links parameters are specified.
Definition: iDyn.cpp:1769
iCub::iDyn::iDynChain::getKinematicNewtonEuler
void getKinematicNewtonEuler(yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp)
Calls the proper method to get kinematics variables in OneChainNewtonEuler either in the base or in t...
Definition: iDyn.cpp:1008
iCub::iDyn::DYNAMIC
@ DYNAMIC
Definition: iDynInv.h:64
iCub::iDyn::iDynLimb::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Limb joints bounds with current values set aboard the robot - see also iKin.
Definition: iDyn.h:1316
iCub::iDyn::SensorLinkNewtonEuler
Definition: iDynInv.h:718
mode
GLenum mode
Definition: rendering.cpp:48
iCub::iDyn::iDynChain::refLink
iDynLink * refLink(const unsigned int i)
Returns a pointer to the ith iDynLink.
Definition: iDyn.cpp:755
iCub::iDyn::iDynChain::setMass
bool setMass(const unsigned int i, const double _m)
Set the i-th link mass.
Definition: iDyn.cpp:833
iCub::iKin::iKinChain::addLink
bool addLink(const unsigned int i, iKinLink &l)
Adds a Link at the position ith within the Chain.
Definition: iKinFwd.cpp:307
iCub::iDyn::iGenericFrame
Definition: iDynTransform.h:121
iCub::iDyn::iDynChain::build
virtual void build()
Build chains and lists.
Definition: iDyn.cpp:493
iCub::iDyn::iDynLimb::operator()
iKin::iKinLink & operator()(const unsigned int i)
Definition: iDyn.h:1194
iCub::iKin::iKinChain::pushLink
void pushLink(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:351
iCub::iDyn::iDynLimb
Definition: iDyn.h:1176
iCub::iDyn::RigidBodyTransformation
Definition: iDynBody.h:131
iCub::iDyn::iDynChain::getMoment
const yarp::sig::Vector & getMoment(const unsigned int iLink) const
Returns the i-th link moment.
Definition: iDyn.cpp:777
iCub::iDyn::iDynChain::operator=
iDynChain & operator=(const iDynChain &c)
Copies a Chain object into the current one.
Definition: iDyn.cpp:523
iCub::iDyn::OneLinkNewtonEuler
Definition: iDynInv.h:95
iCub::iDyn::iDynChain::setMasses
bool setMasses(yarp::sig::Vector _m)
Set the link masses at once.
Definition: iDyn.cpp:807
iCub::iDyn::iDynChain::setD2Ang
yarp::sig::Vector setD2Ang(const yarp::sig::Vector &ddq)
Sets the free joint angles acceleration to values of ddq[i].
iCub::iDyn::iDynChain::setIterModeKinematic
void setIterModeKinematic(const ChainIterationMode _iterateMode_kinematics=FORWARD)
Set the iteration direction during recursive computation of kinematics variables (w,...
Definition: iDyn.cpp:1282
iCub::iDyn::iCubTorsoDyn::iCubTorsoDyn
iCubTorsoDyn()
Default constructor.
Definition: iDyn.cpp:2149
iCub::iDyn::iCubLegDynV2::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Leg joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2493
iCub::iDyn::iDynSensor
Definition: iDynInv.h:1577
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::ctrl::CTRL_PI
constexpr double CTRL_PI
The PI constant.
Definition: math.h:56
iCub::iKin::iKinChain::verbose
unsigned int verbose
Definition: iKinFwd.h:359
iCub::iDyn::iDynChain::computeCcTorques
yarp::sig::Vector computeCcTorques()
Compute the torques due to centrifugal and coriolis effects considering only the active joints.
Definition: iDyn.cpp:1673
iCub::iDyn::iCubLegDynV2::iCubLegDynV2
iCubLegDynV2()
Default constructor.
Definition: iDyn.cpp:2343
iCub::iDyn::iCubArmDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:1967
iCub::iDyn::iDynChain::clone
virtual void clone(const iDynChain &c)
Clone function.
Definition: iDyn.cpp:483
iCub::iDyn::iDynChain::getDAng
yarp::sig::Vector getDAng()
Returns the current free joint angles velocity.
Definition: iDyn.cpp:584
iCub::iDyn::iDynSensorLeg
Definition: iDynInv.h:1815
f
f
Definition: compute_ekf_sym.m:22
iCub::iDyn::iDynChain::setStaticParameters
bool setStaticParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC)
Set the dynamic parameters of the i-th Link if the chain is in a static situation (inertia is null).
Definition: iDyn.cpp:905
iCub::iKin::iKinChain::rmLink
bool rmLink(const unsigned int i)
Removes the ith Link from the Chain.
Definition: iKinFwd.cpp:329
iCub::iDyn::iDynChain::curr_ddq
yarp::sig::Vector curr_ddq
q acc
Definition: iDyn.h:551
iCub::iDyn::NewEulMode_s
const std::string NewEulMode_s[4]
Definition: iDynInv.h:65
iCub::iDyn::iCubNeckInertialDyn::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2532
iCub::iDyn::ChainIterationMode
ChainIterationMode
Definition: iDynInv.h:68
iCub::iDyn::iDynChain::getTorque
double getTorque(const unsigned int iLink) const
Returns the i-th link torque.
Definition: iDyn.cpp:788
iCub::iDyn::iCubArmNoTorsoDyn
Definition: iDyn.h:1368
iCub::iKin::iKinChain::operator()
iKinLink & operator()(const unsigned int i)
Returns a reference to the ith Link of the Chain considering only those Links related to DOF.
Definition: iKinFwd.h:427
iCub::iDyn::iDynChain::getForce
const yarp::sig::Vector & getForce(const unsigned int iLink) const
Returns the i-th link force.
Definition: iDyn.cpp:766
iCub::iDyn::iDynChain::getJointBoundMin
yarp::sig::Vector getJointBoundMin()
Returns a list containing the min value for each joint.
Definition: iDyn.cpp:610
iCub::iDyn::iCubNeckInertialDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Inertial Sensor joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2556
iCub::iDyn::iDynChain::getLinAcc
yarp::sig::Vector getLinAcc(const unsigned int i) const
Returns the i-th link linear acceleration.
Definition: iDyn.cpp:711
iCub::iDyn::iDynContactSolver
Definition: iDynContact.h:52
iCub::iDyn::iCubLegDynV2::allocate
virtual void allocate(const std::string &_type)
commented by Traversaro, 2 sept 2013 void iCubLegDynV2::allocate(const string &_type) { iDynLimb::all...
Definition: iDyn.cpp:2428
iCub::iDyn::KINBWD_WREBWD
@ KINBWD_WREBWD
Definition: iDynInv.h:69
iCub::iDyn::iDynChain::computeMassMatrix
yarp::sig::Matrix computeMassMatrix()
Compute the joint space mass matrix considering only the active joints.
Definition: iDyn.cpp:1605
iCub::iDyn::iDynChain::curr_dq
yarp::sig::Vector curr_dq
q vel
Definition: iDyn.h:549
iCub::iDyn::iDynChain::getWrenchNewtonEuler
void getWrenchNewtonEuler(yarp::sig::Vector &F, yarp::sig::Vector &Mu)
Calls the proper method to get wrench variables in OneChainNewtonEuler either in the base or in the f...
Definition: iDyn.cpp:1072
iCub::iDyn::iCubWholeBody
Definition: iDynBody.h:1471
iCub::iDyn::iDynChain::TESTING_computeCOMJacobian
yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink)
Compute the Jacobian of the COM of link indexed iLink.
Definition: iDyn.cpp:1478
iCub::iDyn::iDynChain::iterateMode_kinematics
ChainIterationMode iterateMode_kinematics
specifies the 'direction' of recursive computation of kinematics variables (w,dw,d2p): FORWARD,...
Definition: iDyn.h:543
iCub::iDyn::iDynLimb::~iDynLimb
virtual ~iDynLimb()
Destructor.
Definition: iDyn.cpp:1886
iCub::iDyn::KINFWD_WREBWD
@ KINFWD_WREBWD
Definition: iDynInv.h:69
iCub::iDyn::iCubNeckInertialDynV2
Definition: iDyn.h:1565
iCub::iDyn::iDynChain::setDynamicParameters
bool setDynamicParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im)
Set the dynamic parameters of the i-th Link with motor.
iCub::iDyn::FORWARD
@ FORWARD
Definition: iDynInv.h:68
iCub::iDyn::iDynChain::getD2Ang
yarp::sig::Vector getD2Ang()
Returns the current free joint angles acceleration.
Definition: iDyn.cpp:597
iCub::iDyn::iDynChain::getLinAccCOM
const yarp::sig::Vector & getLinAccCOM(const unsigned int i) const
Returns the i-th link linear acceleration of the COM.
Definition: iDyn.cpp:722
iCub::iDyn::iDynChain::getIterModeKinematic
ChainIterationMode getIterModeKinematic() const
Get the iteration direction during recursive computation of kinematics variables (w,...
Definition: iDyn.cpp:1292
iCub::iDyn::iDynLimb::getType
std::string getType()
Returns the Limb type as a string.
Definition: iDyn.h:1300
iCub::iDyn::iDynChain::getJointBoundMax
yarp::sig::Vector getJointBoundMax()
Returns a list containing the max value for each joint.
Definition: iDyn.cpp:621
iCub::iDyn::iDynLimb::isValid
bool isValid()
Checks if the limb has been properly configured.
Definition: iDyn.h:1279
iCub::iDyn::iDynChain::getCOM
yarp::sig::Matrix getCOM(unsigned int iLink)
Return the COM matrix of the i-th link.
Definition: iDyn.cpp:1585
iCub::iDyn::iDynLimb::type
std::string type
Definition: iDyn.h:1180
iCub::iDyn::iDynChain::getFrameKinematic
void getFrameKinematic(unsigned int i, yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp)
Get the kinematic information of the i-th frame in the OneChainNewtonEuler associated to the current ...
Definition: iDyn.cpp:1036
iCub::iDyn::iDynChain::computeWrenchNewtonEuler
void computeWrenchNewtonEuler()
Calls the proper method to compute wrench variables: either BackwardWrenchFromEnd() or ForwardWrenchF...
Definition: iDyn.cpp:1000
iCub::iDyn::BACKWARD
@ BACKWARD
Definition: iDynInv.h:68
iCub::iDyn::iCubNeckInertialDynV2::allocate
virtual void allocate(const std::string &_type)
Definition: iDyn.cpp:2597
iCub::iDyn::iDynLimb::iDynLimb
iDynLimb()
Default constructor: right arm is default.
Definition: iDyn.cpp:1743
iCub::iDyn::iDynChain::getMoments
yarp::sig::Matrix getMoments() const
Returns the links moments as a matrix, where the i-th col is the i-th moment.
Definition: iDyn.cpp:867
iCub::iDyn::iDynLimb::addLink
bool addLink(const unsigned int i, iKin::iKinLink &l)
Definition: iDyn.h:1195
iCub::iDyn::iDynChain::getTorques
yarp::sig::Vector getTorques() const
Returns the links torque as a vector.
Definition: iDyn.cpp:875
iCub::iDyn::iDynLimb::asChain
iDynChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iDyn.h:1294
iCub::iDyn::iDynChain::getMomentsNewtonEuler
yarp::sig::Matrix getMomentsNewtonEuler() const
Returns the links moments as a matrix, where the (i+1)-th col is the i-th moment.
Definition: iDyn.cpp:1242
iCub::iDyn::iDynChain::getAngAcc
yarp::sig::Vector getAngAcc(const unsigned int i) const
Returns the i-th link angular acceleration.
Definition: iDyn.cpp:744
iCub::iDyn::iDynChain::getInertia
yarp::sig::Matrix getInertia(const unsigned int i) const
Returns the i-th link inertia matrix.
Definition: iDyn.cpp:847
iCub::iDyn::FinalLinkNewtonEuler
Definition: iDynInv.h:610
iCub::iDyn::iCubArmNoTorsoDyn::alignJointsBounds
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Arm joints bounds with current values set aboard the iCub.
Definition: iDyn.cpp:2117
iCub::iDyn::iDynChain::getDenHart
yarp::sig::Matrix getDenHart(unsigned int i)
Return the Denavit-Hartenberg matrix of the i-th link in the chain.
Definition: iDyn.cpp:1467
iCub::iDyn::iDynChain::setDAng
yarp::sig::Vector setDAng(const yarp::sig::Vector &dq)
Sets the free joint angles velocity to values of dq[i].
iCub::iDyn::iDynChain::getHCOM
yarp::sig::Matrix getHCOM(unsigned int iLink)
Return the H matrix until the COM of the i-th link.
Definition: iDyn.cpp:1595
iCub::iDyn::iDynChain::getLinVel
yarp::sig::Vector getLinVel(const unsigned int i) const
Returns the i-th link linear velocity.
Definition: iDyn.cpp:689
iCub::iDyn::iDynSensorArm
Definition: iDynInv.h:1752