iCub-main
iDynBody.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 
46 #ifndef __IDYNBODY_H__
47 #define __IDYNBODY_H__
48 
49 #include <yarp/sig/Vector.h>
50 #include <yarp/sig/Matrix.h>
51 #include <iCub/ctrl/math.h>
52 #include <iCub/iKin/iKinFwd.h>
53 #include <iCub/iDyn/iDynInv.h>
54 #include <iCub/iDyn/iDynContact.h>
55 #include <deque>
56 #include <string>
57 
58 
59 namespace iCub
60 {
61 
62 namespace iDyn
63 {
64 
65  class OneLinkNewtonEuler;
66  class BaseLinkNewtonEuler;
67  class FinalLinkNewtonEuler;
68  class SensorLinkNewtonEuler;
69  class ContactNewtonEuler;
70  class OneChainNewtonEuler;
71  class OneChainSensorNewtonEuler;
72  class iDynSensor;
73  class iFTransform;
74  class iSFrame;
75  class iFB;
76  class iDynSensorLeg;
77  class iDynSensorArm;
78 
79 
80 
81 // Interaction type
82 // used to define what side of the chain (limb) is attached to the node
83 // the base or the end-effector
84 // note: up, arm and legs are attached with the base
85 // torso is attached with the end-effector to the upper arm, and with the
87 
88 // Flow Type
89 // define the flow of kinematic/wrench information for each RBT in a node
90 // RBT_NODE_IN = the variable is set from outside, "propagates" into the limb and then goes into the node
91 // RBT_NODE_OUT = the variable is read from the node and goes into the limb
93 
94 // Jacobian Flow
95 // used to specify if the direction of computation of the Jacobian is the same as
96 // the one of the kinematics of the chain
97 // JAC_KIN = same flow of kinematics
98 // JAC_IKIN = inverse flow wrt kinematics
100 
101 
102 #define RBT_HAS_SENSOR true
103 #define RBT_NO_SENSOR false
104 #define NODE_AFTER_ATTACH true
105 #define NODE_NO_ATTACH false
106 
107 
109 {
113 
115  {
116  head_version=1;
117  head_subversion=0;
118  legs_version=1;
119  }
120 };
121 
122 //enum partEnum{ LEFT_ARM=0, RIGHT_ARM, LEFT_LEG, RIGHT_LEG, TORSO, HEAD, ALL };
123 
132 {
133 protected:
134 
137 
140 
143 
145  yarp::sig::Matrix H;
146 
149 
151  std::string info;
152 
154  unsigned int verbose;
155 
157  bool hasSensor;
158 
159  // these variables are not redundant: because multiple RBT can be attached
160  // to a node, and different policies of information sharing can exist
161 
163  yarp::sig::Vector w;
165  yarp::sig::Vector dw;
167  yarp::sig::Vector ddp;
169  yarp::sig::Vector F;
171  yarp::sig::Vector Mu;
172 
173 
174  // useful methods
175 
180  yarp::sig::Matrix getR();
181 
187  yarp::sig::Vector getr(bool proj=false);
188 
195  void computeKinematic();
196 
203  void computeWrench();
204 
205 
206 public:
207 
218  RigidBodyTransformation(iDyn::iDynLimb *_limb, const yarp::sig::Matrix &_H, const std::string &_info, bool _hasSensor = false, const FlowType kin=RBT_NODE_OUT, const FlowType wre=RBT_NODE_IN, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
219 
224 
231  bool setRBT(const yarp::sig::Matrix &_H);
232 
243  bool setKinematic(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
244 
256  bool setKinematicMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
257 
267  bool setWrench(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0);
268 
278  bool setWrenchMeasure(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0);
279 
287  bool setWrenchMeasure(iDyn::iDynSensor *sensor, const yarp::sig::Vector &Fsens, const yarp::sig::Vector &Musens);
288 
293  yarp::sig::Matrix getRBT() const;
294 
306  void getKinematic( yarp::sig::Vector &wNode, yarp::sig::Vector &dwNode, yarp::sig::Vector &ddpNode);
307 
318  void getWrench( yarp::sig::Vector &FNode, yarp::sig::Vector &MuNode);
319 
325  void setInfoFlow(const FlowType kin, const FlowType wre);
326 
331  FlowType getKinematicFlow() const;
332 
337  FlowType getWrenchFlow() const;
338 
343  std::string toString() const;
344 
350  bool isSensorized() const;
351 
355  void computeLimbKinematic();
356 
360  void computeLimbWrench();
361 
366  unsigned int getNLinks() const;
367 
372  unsigned int getDOF() const;
373 
382  yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false);
383 
388  yarp::sig::Matrix getH();
389 
395  yarp::sig::Vector getEndEffPose(const bool axisRep = true);
396 
405  yarp::sig::Matrix computeGeoJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, bool rbtRoto = false);
406 
415  yarp::sig::Matrix computeGeoJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &H0, bool rbtRoto = false);
416 
424  yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn, bool rbtRoto = false);
425 
435  yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &H0, bool rbtRoto = false);
436 
441  yarp::sig::Matrix computeGeoJacobian(bool rbtRoto = false);
442 
450  yarp::sig::Matrix computeGeoJacobian(const unsigned int iLink, bool rbtRoto = false);
451 
456  yarp::sig::Matrix getR6() const;
457 
462  yarp::sig::Matrix getH0() const;
463 
469  bool setH0(const yarp::sig::Matrix &_H0);
470 
471  //---------------
472  // JAC COM
473  //---------------
474 
481  yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, bool rbtRoto = false);
482 
491  yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, bool rbtRoto = false);
492 
503  yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0, bool rbtRoto = false );
504 
510  yarp::sig::Matrix getHCOM(unsigned int iLink);
511 
512 };
513 
514 
531 class iDynNode
532 {
533 protected:
534 
536  std::deque<RigidBodyTransformation> rbtList;
537 
540 
542  std::string info;
543 
545  unsigned int verbose;
546 
548  yarp::sig::Vector w;
550  yarp::sig::Vector dw;
552  yarp::sig::Vector ddp;
554  yarp::sig::Vector F;
556  yarp::sig::Vector Mu;
558  yarp::sig::Vector COM;
560  double mass;
561 
565  void zero();
566 
571  void compute_Pn_HAN(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node);
572 
577  void compute_Pn_HAN(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node);
578 
583  void compute_Pn_HAN_COM(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node);
584 
593  unsigned int howManyWrenchInputs(bool afterAttach=false) const;
594 
603  unsigned int howManyKinematicInputs(bool afterAttach=false) const;
604 
605 public:
606 
611  iDynNode(const NewEulMode _mode=DYNAMIC);
612 
619  iDynNode(const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
620 
630  virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN, bool hasSensor=false);
631 
637  yarp::sig::Matrix getRBT(unsigned int iLimb) const;
638 
652  bool solveKinematics( const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
653 
658  bool solveKinematics();
659 
663  bool setKinematicMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
664 
677  virtual bool solveWrench();
678 
703  bool solveWrench(const yarp::sig::Matrix &FM);
704 
729  bool solveWrench(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M);
730 
737  virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M);
738 
744  virtual bool setWrenchMeasure(const yarp::sig::Matrix &FM);
745 
750  yarp::sig::Vector getForce() const;
751 
756  yarp::sig::Vector getMoment() const;
757 
762  yarp::sig::Vector getAngVel() const;
763 
768  yarp::sig::Vector getAngAcc() const;
769 
774  yarp::sig::Vector getLinAcc() const;
775 
776  //-----------------
777  // jacobians
778  //-----------------
779 
786  yarp::sig::Matrix computeJacobian(unsigned int iChain);
787 
801  yarp::sig::Matrix computeJacobian(unsigned int iChain, unsigned int iLink);
802 
818  yarp::sig::Matrix computeJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB);
819 
838  yarp::sig::Matrix computeJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB);
839 
857  yarp::sig::Vector computePose(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, const bool axisRep);
858 
877  yarp::sig::Vector computePose(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, const bool axisRep);
878 
879 
880  //---------------
881  // JAC COM
882  //---------------
883 
891  yarp::sig::Matrix TESTING_computeCOMJacobian(unsigned int iChain, unsigned int iLink);
892 
906  yarp::sig::Matrix TESTING_computeCOMJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB);
907 };
908 
909 
916 class iDynSensorNode : public iDynNode
917 {
918 
919 protected:
920 
922  std::deque<iDyn::iDynSensor *> sensorList;
923 
927  unsigned int howManySensors() const;
928 
929 public:
930 
935  iDynSensorNode(const NewEulMode _mode=DYNAMIC);
936 
943  iDynSensorNode(const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
944 
954  virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN);
955 
966  void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, iDyn::iDynSensor *sensor, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN);
967 
980  virtual bool solveWrench();
981 
998  virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M, bool afterAttach=false);
999 
1014  virtual bool setWrenchMeasure(const yarp::sig::Matrix &FM, bool afterAttach=false);
1015 
1029  yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false);
1030 
1031 
1032 
1033 };
1034 
1035 
1047 {
1048 public:
1049 
1051  yarp::sig::Matrix HUp;
1053  yarp::sig::Matrix HLeft;
1055  yarp::sig::Matrix HRight;
1056 
1058  std::string left_name;
1060  std::string right_name;
1062  std::string up_name;
1064  std::string name;
1065 
1069  //virtual void build();
1070 
1071 public:
1076 
1077 
1084 
1086  yarp::sig::Vector total_COM_UP;
1087  yarp::sig::Vector total_COM_LF;
1088  yarp::sig::Vector total_COM_RT;
1092  yarp::sig::Matrix COM_jacob_UP;
1093  yarp::sig::Matrix COM_jacob_LF;
1094  yarp::sig::Matrix COM_jacob_RT;
1095 
1101  iDynSensorTorsoNode(const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
1102 
1109  iDynSensorTorsoNode(const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
1110 
1115 
1120  bool update();
1121 
1130  bool update(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, bool afterAttach=true);
1131 
1142  bool update(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, const yarp::sig::Vector &FM_up);
1143 
1144 
1145  //----------------
1146  // GET
1147  //----------------
1151  inline yarp::sig::Matrix getHLeft() { return HLeft; }
1155  yarp::sig::Matrix getHRight() { return HRight; }
1159  yarp::sig::Matrix getHUp() { return HUp; }
1160 
1166  yarp::sig::Matrix getForces(const std::string &limbType);
1167 
1174  yarp::sig::Vector getForce(const std::string &limbType, const unsigned int iLink) const ;
1175 
1181  yarp::sig::Matrix getMoments(const std::string &limbType);
1182 
1189  yarp::sig::Vector getMoment(const std::string &limbType, const unsigned int iLink) const;
1190 
1196  yarp::sig::Vector getTorques(const std::string &limbType);
1197 
1204  double getTorque(const std::string &limbType, const unsigned int iLink) const;
1205 
1210  bool computeCOM();
1211 
1217 
1222  yarp::sig::Vector getTorsoForce() const;
1227  yarp::sig::Vector getTorsoMoment() const;
1232  yarp::sig::Vector getTorsoAngVel() const;
1237  yarp::sig::Vector getTorsoAngAcc() const;
1242  yarp::sig::Vector getTorsoLinAcc() const;
1243 
1244 
1245  //----------------
1246  // SET
1247  //----------------
1248 
1256  bool setInertialMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
1257 
1266  bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left);
1267 
1276  bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, const yarp::sig::Vector &FM_up);
1277 
1278 
1279  //------------------
1280  // LIMB CALLS
1281  //------------------
1282 
1289  yarp::sig::Vector setAng(const std::string &limbType, const yarp::sig::Vector &_q);
1295  yarp::sig::Vector getAng(const std::string &limbType);
1303  double setAng(const std::string &limbType, const unsigned int i, double _q);
1310  double getAng(const std::string &limbType, const unsigned int i);
1311 
1318  yarp::sig::Vector setDAng(const std::string &limbType, const yarp::sig::Vector &_dq);
1324  yarp::sig::Vector getDAng(const std::string &limbType);
1332  double setDAng(const std::string &limbType, const unsigned int i, double _dq);
1339  double getDAng(const std::string &limbType, const unsigned int i);
1340 
1347  yarp::sig::Vector setD2Ang(const std::string &limbType, const yarp::sig::Vector &_ddq);
1353  yarp::sig::Vector getD2Ang(const std::string &limbType);
1361  double setD2Ang(const std::string &limbType, const unsigned int i, double _ddq);
1368  double getD2Ang(const std::string &limbType, const unsigned int i);
1369 
1374  unsigned int getNLinks(const std::string &limbType) const;
1375 
1383  yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
1384  { return iDynSensorNode::estimateSensorsWrench(FM,afterAttach); }
1385 
1386 
1387  void clearContactList();
1388 
1389 
1390 };
1391 
1392 
1393 
1402 {
1403 protected:
1408  void build();
1409 
1410 public:
1411 
1418  iCubUpperTorso(version_tag tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
1419 
1423  ~iCubUpperTorso();
1424 
1425 };
1426 
1427 
1438 {
1439 protected:
1444  void build();
1445 
1446 public:
1447 
1454  iCubLowerTorso(version_tag _tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
1455 
1459  ~iCubLowerTorso();
1460 
1461 };
1462 
1463 
1472 {
1473 protected:
1478 
1479 public:
1480 
1485 
1487  double whole_mass;
1488  double lower_mass;
1489  double upper_mass;
1490  yarp::sig::Vector whole_COM;
1491  yarp::sig::Vector upper_COM;
1492  yarp::sig::Vector lower_COM;
1493  yarp::sig::Matrix COM_Jacob;
1494  double sw_getcom;
1500  iCubWholeBody(version_tag _tag, const NewEulMode mode=DYNAMIC, unsigned int verbose=iCub::skinDynLib::VERBOSE);
1501 
1505  ~iCubWholeBody();
1506 
1511  void attachLowerTorso(const yarp::sig::Vector &FM_right_leg, const yarp::sig::Vector &FM_left_leg);
1512 
1517  bool computeCOM();
1518 
1526  bool getCOM(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &COM, double & mass);
1527 
1534  bool getAllVelocities(yarp::sig::Vector &vel);
1535 
1542  bool getAllPositions(yarp::sig::Vector &pos);
1543 
1549  bool EXPERIMENTAL_getCOMjacobian(iCub::skinDynLib::BodyPart which_part, yarp::sig::Matrix &jac);
1550 
1556 
1562  bool EXPERIMENTAL_getCOMvelocity(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &vel, yarp::sig::Vector &dq);
1563 };
1564 
1565 
1566 }// end of namespace iDyn
1567 }//end of namespace iCub
1568 
1569 #endif
1570 
1571 
1572 
A class for setting a rigid body transformation between iDynLimb and iDynNode.
Definition: iDynBody.h:132
FlowType getWrenchFlow() const
return the wrench flow type
Definition: iDynBody.cpp:190
NewEulMode mode
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
Definition: iDynBody.h:148
yarp::sig::Vector ddp
linear acceleration
Definition: iDynBody.h:167
bool setWrenchMeasure(iDyn::iDynSensor *sensor, const yarp::sig::Vector &Fsens, const yarp::sig::Vector &Musens)
Set the wrench variables in the sensor.
yarp::sig::Vector getEndEffPose(const bool axisRep=true)
Return the end-effector pose: x-y-z Cartesian position and 3/4 angles of orientation.
Definition: iDynBody.cpp:308
yarp::sig::Matrix getRBT() const
Return the the (4x4) roto-translational matrix defining the rigid body transformation.
Definition: iDynBody.cpp:121
bool setRBT(const yarp::sig::Matrix &_H)
Set the roto-translational matrix between the limb and the node, defining the rigid body transformati...
Definition: iDynBody.cpp:67
yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, bool rbtRoto=false)
Returns the Jacobian matrix of the COM of the selected link (index = iLink) in the chain: in this cas...
FlowType kinFlow
kinematic flow: in-to/out-from node (RBT_NODE_IN/RBT_NODE_OUT)
Definition: iDynBody.h:139
yarp::sig::Matrix H
the roto-translation between the limb and the node
Definition: iDynBody.h:145
iDyn::iDynLimb * limb
the limb attached to the RigidBodyTransformation
Definition: iDynBody.h:136
void getWrench(yarp::sig::Vector &FNode, yarp::sig::Vector &MuNode)
Get the wrench variables (F,Mu) of the limb, transform it according to the RBT and add it to the node...
Definition: iDynBody.cpp:166
yarp::sig::Matrix getH()
Return the end-effector roto-translational matrix of the end-effector H of the end-effector.
Definition: iDynBody.cpp:303
yarp::sig::Vector F
force
Definition: iDynBody.h:169
bool setWrench(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
Set the wrench variables (F,Mu) of the limb.
Definition: iDynBody.cpp:100
yarp::sig::Matrix computeGeoJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, bool rbtRoto=false)
This method is used to compute the Jacobian between two links in two different chains (eg from link 4...
yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn, bool rbtRoto=false)
This method is used to compute the Jacobian between two links in two different chains: in this case i...
void computeLimbWrench()
Calls the compute wrench of the limb.
Definition: iDynBody.cpp:283
yarp::sig::Matrix getHCOM(unsigned int iLink)
Returns the COM matrix of the selected link (index = iLink) in the chain.
Definition: iDynBody.cpp:401
yarp::sig::Matrix getR6() const
Return a 6x6 diagonal matrix with the rotational matrix of the RBT.
Definition: iDynBody.cpp:131
void computeLimbKinematic()
Calls the compute kinematic of the limb.
Definition: iDynBody.cpp:278
yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &H0, bool rbtRoto=false)
This method is used to compute the Jacobian between two links in two different chains: in this case i...
yarp::sig::Vector getr(bool proj=false)
Return the translational part of the RBT matrix.
Definition: iDynBody.cpp:145
FlowType wreFlow
wrench flow: in-to/out-from node (RBT_NODE_IN/RBT_NODE_OUT)
Definition: iDynBody.h:142
yarp::sig::Matrix computeGeoJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &H0, bool rbtRoto=false)
This method is used to compute the Jacobian between two links in two different chains.
bool setKinematic(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the kinematic variables (w,dw,ddp) of the limb.
Definition: iDynBody.cpp:83
void computeWrench()
Basic computations for applying RBT on wrench variables.
Definition: iDynBody.cpp:246
std::string toString() const
Return some information.
unsigned int verbose
verbosity flag
Definition: iDynBody.h:154
yarp::sig::Vector w
angular velocity
Definition: iDynBody.h:163
yarp::sig::Matrix getR()
Return the rotational 3x3 matrix of the RBT.
Definition: iDynBody.cpp:126
yarp::sig::Vector Mu
moment
Definition: iDynBody.h:171
RigidBodyTransformation(iDyn::iDynLimb *_limb, const yarp::sig::Matrix &_H, const std::string &_info, bool _hasSensor=false, const FlowType kin=RBT_NODE_OUT, const FlowType wre=RBT_NODE_IN, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor, defining the limb attached to the node.
Definition: iDynBody.cpp:44
yarp::sig::Matrix getH0() const
Return the H0 matrix of the limb attached to the RBT.
Definition: iDynBody.cpp:361
bool setKinematicMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the kinematic variables (w,dw,ddp) of the limb, coming from external measurements (ie a sensor).
Definition: iDynBody.cpp:95
unsigned int getDOF() const
Return the number of DOF of the limb (DOF <= N)
Definition: iDynBody.cpp:293
yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, bool rbtRoto=false)
Returns the Jacobian matrix of the COM of the selected link (index = iLink) in the chain.
Definition: iDynBody.cpp:377
void getKinematic(yarp::sig::Vector &wNode, yarp::sig::Vector &dwNode, yarp::sig::Vector &ddpNode)
Get the kinematic variables (w,dw,ddp) of the limb, applies the RBT transformation and compute the ki...
Definition: iDynBody.cpp:152
std::string info
info or useful notes
Definition: iDynBody.h:151
yarp::sig::Vector dw
angular acceleration
Definition: iDynBody.h:165
bool setWrenchMeasure(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
Set the wrench variables (F,Mu) of the limb.
bool isSensorized() const
Return a boolean, depending if the limb attached to the RBT has a FT sensor or not.
Definition: iDynBody.cpp:273
void setInfoFlow(const FlowType kin, const FlowType wre)
Set the flow of kinematic/wrench information: input to node or output from node.
Definition: iDynBody.cpp:179
unsigned int getNLinks() const
Return the number of links of the limb (N)
Definition: iDynBody.cpp:288
yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0, bool rbtRoto=false)
Returns the Jacobian matrix of the COM of the selected link (index = iLink) in the chain: in this cas...
void computeKinematic()
Basic computations for applying RBT on kinematic variables.
Definition: iDynBody.cpp:195
bool setH0(const yarp::sig::Matrix &_H0)
Set a new H0 matrix in the limb attached to the RBT.
Definition: iDynBody.cpp:366
FlowType getKinematicFlow() const
Return the kinematic flow type.
Definition: iDynBody.cpp:185
bool hasSensor
flag for sensor or not - only used for setWrenchMeasures()
Definition: iDynBody.h:157
A class for connecting torso, left and right leg of the iCub, and exchanging kinematic and wrench inf...
Definition: iDynBody.h:1438
iCubLowerTorso(version_tag _tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:2317
version_tag tag
Build the node.
Definition: iDynBody.h:1443
~iCubLowerTorso()
Destructor.
Definition: iDynBody.cpp:2387
A class for connecting head, left and right arm of the iCub, and exchanging kinematic and wrench info...
Definition: iDynBody.h:1402
version_tag tag
Build the node.
Definition: iDynBody.h:1407
iCubUpperTorso(version_tag tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:2236
~iCubUpperTorso()
Destructor.
Definition: iDynBody.cpp:2303
A class for connecting UpperTorso and LowerTorso of the iCub, then getting the WholeBody in the dynam...
Definition: iDynBody.h:1472
yarp::sig::Vector upper_COM
Definition: iDynBody.h:1491
bool getAllVelocities(yarp::sig::Vector &vel)
Retrieves a vector containing the velocities of all the iCub joints, ordered in this way: left leg (6...
Definition: iDynBody.cpp:2629
RigidBodyTransformation * rbt
the rigid body transformation linking the UpperTorso node with the final link of the iCubTorsoDyn cha...
Definition: iDynBody.h:1476
bool EXPERIMENTAL_getCOMvelocity(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &vel, yarp::sig::Vector &dq)
Retrieves a 3x1 vector containing the velocity of the robot COM.
Definition: iDynBody.cpp:3016
yarp::sig::Matrix COM_Jacob
Definition: iDynBody.h:1493
yarp::sig::Vector lower_COM
Definition: iDynBody.h:1492
iCubUpperTorso * upperTorso
pointer to UpperTorso = head + right arm + left arm
Definition: iDynBody.h:1482
iCubWholeBody(version_tag _tag, const NewEulMode mode=DYNAMIC, unsigned int verbose=iCub::skinDynLib::VERBOSE)
Constructor: build the nodes and creates the whole body.
Definition: iDynBody.cpp:2400
bool getAllPositions(yarp::sig::Vector &pos)
Retrieves a vector containing the anglular position of all the iCub joints, ordered in this way: left...
Definition: iDynBody.cpp:2607
~iCubWholeBody()
Standard destructor.
Definition: iDynBody.cpp:2414
bool EXPERIMENTAL_computeCOMjacobian()
Performs the computation of the center of mass jacobian of the whole iCub.
Definition: iDynBody.cpp:2675
bool computeCOM()
Performs the computation of the center of mass (COM) of the whole iCub.
Definition: iDynBody.cpp:2473
iCubLowerTorso * lowerTorso
pointer to LowerTorso = torso + right leg + left leg
Definition: iDynBody.h:1484
yarp::sig::Vector whole_COM
Definition: iDynBody.h:1490
double whole_mass
masses and position of the center of mass of the iCub sub-parts
Definition: iDynBody.h:1487
bool getCOM(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &COM, double &mass)
Retrieves the result of the last COM computation.
Definition: iDynBody.cpp:2511
void attachLowerTorso(const yarp::sig::Vector &FM_right_leg, const yarp::sig::Vector &FM_left_leg)
Connect upper and lower torso: this procedure handles the exchange of kinematic and wrench variables ...
Definition: iDynBody.cpp:2421
bool EXPERIMENTAL_getCOMjacobian(iCub::skinDynLib::BodyPart which_part, yarp::sig::Matrix &jac)
Retrieves the result of the last COM jacobian computation.
Definition: iDynBody.cpp:2746
This solver uses a modified version of the Newton-Euler algorithm to estimate both the external and i...
Definition: iDynContact.h:53
A class for defining a generic Limb within the iDyn framework.
Definition: iDyn.h:1177
A class for connecting two or mutiple limbs and exchanging kinematic and wrench information between l...
Definition: iDynBody.h:532
yarp::sig::Vector ddp
linear acceleration
Definition: iDynBody.h:552
virtual bool setWrenchMeasure(const yarp::sig::Matrix &FM)
Set the wrench measure on the limbs with input wrench.
yarp::sig::Vector getAngAcc() const
Return the node angular acceleration.
Definition: iDynBody.cpp:823
unsigned int howManyKinematicInputs(bool afterAttach=false) const
Return the number of limbs with kinematic input, i.e.
Definition: iDynBody.cpp:801
yarp::sig::Vector getMoment() const
Return the node moment.
Definition: iDynBody.cpp:819
yarp::sig::Vector getForce() const
Return the node force.
Definition: iDynBody.cpp:817
yarp::sig::Matrix getRBT(unsigned int iLimb) const
Return the RBT matrix of a certain limb attached to the node.
Definition: iDynBody.cpp:449
unsigned int verbose
verbosity flag
Definition: iDynBody.h:545
unsigned int howManyWrenchInputs(bool afterAttach=false) const
Return the number of limbs with wrench input, i.e.
Definition: iDynBody.cpp:785
void compute_Pn_HAN(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node)
Compute Pn and H_A_Node matrices given two chains.
NewEulMode mode
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
Definition: iDynBody.h:539
yarp::sig::Vector w
angular velocity
Definition: iDynBody.h:548
virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M)
Set the wrench measure on the limbs with input wrench.
yarp::sig::Vector computePose(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, const bool axisRep)
Compute the Pose of the end-effector, given a "virtual" chain connecting two limbs.
Definition: iDynBody.cpp:1062
bool solveKinematics(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Main function to manage the exchange of kinematic information among the limbs attached to the node.
yarp::sig::Vector Mu
moment
Definition: iDynBody.h:556
yarp::sig::Vector getAngVel() const
Return the node angular velocity.
Definition: iDynBody.cpp:821
iDynNode(const NewEulMode _mode=DYNAMIC)
Default constructor.
Definition: iDynBody.cpp:416
std::string info
info or useful notes
Definition: iDynBody.h:542
yarp::sig::Matrix TESTING_computeCOMJacobian(unsigned int iChain, unsigned int iLink)
Compute the Jacobian of the COM of the i-th link of the limb with index iChain in the node.
Definition: iDynBody.cpp:1194
std::deque< RigidBodyTransformation > rbtList
the list of RBT
Definition: iDynBody.h:536
bool solveWrench(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M)
This is to manage the exchange of wrench information among the limbs attached to the node.
bool solveKinematics()
Main function to manage the exchange of kinematic information among the limbs attached to the node.
Definition: iDynBody.cpp:462
yarp::sig::Vector getLinAcc() const
Return the node linear acceleration.
Definition: iDynBody.cpp:825
bool setKinematicMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the kinematic measurement (w,dw,ddp) on the limb where the kinematic flow is of type RBT_NODE_IN.
Definition: iDynBody.cpp:561
yarp::sig::Matrix computeJacobian(unsigned int iChain)
Compute the Jacobian of the limb with index iChain in the node, in its default direction (as it would...
Definition: iDynBody.cpp:834
void compute_Pn_HAN(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node)
Compute Pn and H_A_Node matrices given two chains.
yarp::sig::Vector dw
angular acceleration
Definition: iDynBody.h:550
virtual bool solveWrench()
Main function to manage the exchange of wrench information among the limbs attached to the node.
Definition: iDynBody.cpp:590
double mass
total mass of the node
Definition: iDynBody.h:560
void compute_Pn_HAN_COM(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node)
Compute Pn and H_A_Node matrices given two chains.
Definition: iDynBody.cpp:1281
void zero()
Reset all data to zero.
Definition: iDynBody.cpp:433
bool solveWrench(const yarp::sig::Matrix &FM)
This is to manage the exchange of wrench information among the limbs attached to the node.
virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN, bool hasSensor=false)
Add one limb to the node, defining its RigidBodyTransformation.
Definition: iDynBody.cpp:442
yarp::sig::Vector COM
COM position of the node.
Definition: iDynBody.h:558
yarp::sig::Vector F
force
Definition: iDynBody.h:554
A class for connecting two or mutiple limbs and exchanging kinematic and wrench information between l...
Definition: iDynBody.h:917
void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, iDyn::iDynSensor *sensor, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN)
Add one limb to the node, defining its RigidBodyTransformation and the iDynSensor used to solve it af...
virtual bool setWrenchMeasure(const yarp::sig::Matrix &FM, bool afterAttach=false)
Set the Wrench measures on the limbs attached to the node.
virtual bool solveWrench()
Main function to manage the exchange of wrench information among the limbs attached to the node.
Definition: iDynBody.cpp:1352
unsigned int howManySensors() const
Definition: iDynBody.cpp:1707
virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M, bool afterAttach=false)
Set the Wrench measures on the limbs attached to the node.
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Exploit iDynInvSensor methods to retrieve FT sensor measurements after solving wrenches in the limbs.
Definition: iDynBody.cpp:1562
virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN)
Add one limb to the node, defining its RigidBodyTransformation.
std::deque< iDyn::iDynSensor * > sensorList
the list of iDynSensors used to solve each limb after FT sensor measurements
Definition: iDynBody.h:922
iDynSensorNode(const NewEulMode _mode=DYNAMIC)
Default constructor.
Definition: iDynBody.cpp:1327
A class for connecting a central-up limb, a left and right limb of the iCub, and exchanging kinematic...
Definition: iDynBody.h:1047
yarp::sig::Vector getD2Ang(const std::string &limbType)
Get joints angle acceleration in the chosen limb.
Definition: iDynBody.cpp:2172
yarp::sig::Matrix HRight
roto-translational matrix defining the right limb base frame with respect to the torso node
Definition: iDynBody.h:1055
bool computeCOM()
Performs the computation of the center of mass (COM) of the node.
Definition: iDynBody.cpp:1912
bool update(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, bool afterAttach=true)
Main method for solving kinematics and wrench among limbs, where information are shared.
yarp::sig::Matrix COM_jacob_LF
Definition: iDynBody.h:1093
yarp::sig::Matrix COM_jacob_UP
Definition: iDynBody.h:1092
iDynSensorTorsoNode(const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:1729
std::string name
the torso node name
Definition: iDynBody.h:1064
yarp::sig::Vector getTorques(const std::string &limbType)
Return the chosen limb torques, as a Nx1 vector.
Definition: iDynBody.cpp:1890
bool update(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, const yarp::sig::Vector &FM_up)
Main method for solving kinematics and wrench among limbs, where information are shared.
yarp::sig::Vector getAng(const std::string &limbType)
Get joints angle position in the chosen limb.
Definition: iDynBody.cpp:2076
double getTorque(const std::string &limbType, const unsigned int iLink) const
Return the chosen limb-link torque, as a real value.
iDyn::iDynContactSolver * leftSensor
Build the node.
Definition: iDynBody.h:1073
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Redefinition from iDynSensorNode.
Definition: iDynBody.h:1383
yarp::sig::Matrix HLeft
roto-translational matrix defining the left limb base frame with respect to the torso node
Definition: iDynBody.h:1053
iDyn::iDynLimb * left
left limb
Definition: iDynBody.h:1079
yarp::sig::Vector setDAng(const std::string &limbType, const yarp::sig::Vector &_dq)
Set joints angle velocity in the chosen limb.
yarp::sig::Vector getTorsoAngAcc() const
Return the torso angular acceleration.
Definition: iDynBody.cpp:1908
yarp::sig::Matrix getMoments(const std::string &limbType)
Return the chosen limb moments, as a 6xN matrix.
Definition: iDynBody.cpp:1878
std::string up_name
name of central-up limb
Definition: iDynBody.h:1062
bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left)
Set the FT sensor measurements on the sensor in right and left limb.
yarp::sig::Vector setD2Ang(const std::string &limbType, const yarp::sig::Vector &_ddq)
Set joints angle acceleration in the chosen limb.
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
Definition: iDynBody.cpp:1910
yarp::sig::Vector getMoment(const std::string &limbType, const unsigned int iLink) const
Return the chosen limb-link moment, as a 3x1 vector.
yarp::sig::Matrix COM_jacob_RT
Definition: iDynBody.h:1094
yarp::sig::Matrix getHLeft()
Return HLeft, i.e.
Definition: iDynBody.h:1151
yarp::sig::Vector getTorsoForce() const
Return the torso force.
Definition: iDynBody.cpp:1902
iDyn::iDynLimb * right
right limb
Definition: iDynBody.h:1081
std::string right_name
name of right limb
Definition: iDynBody.h:1060
iDyn::iDynContactSolver * rightSensor
right FT sensor and solver
Definition: iDynBody.h:1075
bool EXPERIMENTAL_computeCOMjacobian()
Performs the computation of the center of mass jacobian of the node.
Definition: iDynBody.cpp:1970
yarp::sig::Vector getForce(const std::string &limbType, const unsigned int iLink) const
Return the chosen limb-link force, as a 3x1 vector.
yarp::sig::Vector total_COM_RT
Definition: iDynBody.h:1088
unsigned int getNLinks(const std::string &limbType) const
Definition: iDynBody.cpp:2208
yarp::sig::Matrix getForces(const std::string &limbType)
Return the chosen limb forces, as a 6xN matrix.
Definition: iDynBody.cpp:1866
yarp::sig::Vector getDAng(const std::string &limbType)
Get joints angle velocity in the chosen limb.
Definition: iDynBody.cpp:2124
std::string left_name
name of left limb
Definition: iDynBody.h:1058
iDyn::iDynLimb * up
central-up limb
Definition: iDynBody.h:1083
yarp::sig::Vector setAng(const std::string &limbType, const yarp::sig::Vector &_q)
Set joints angle position in the chosen limb.
bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, const yarp::sig::Vector &FM_up)
Set the FT sensor measurements on the sensor in right and left limb.
yarp::sig::Matrix getHUp()
Return HUp, i.e.
Definition: iDynBody.h:1159
yarp::sig::Vector getTorsoMoment() const
Return the torso moment.
Definition: iDynBody.cpp:1904
bool setInertialMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the inertial sensor measurements on the central-up limb.
Definition: iDynBody.cpp:1753
yarp::sig::Vector total_COM_UP
COMs and masses of the limbs.
Definition: iDynBody.h:1086
yarp::sig::Vector getTorsoAngVel() const
Return the torso angular velocity.
Definition: iDynBody.cpp:1906
bool update()
Main method for solving kinematics and wrench among limbs, where information are shared.
Definition: iDynBody.cpp:1794
yarp::sig::Vector total_COM_LF
Definition: iDynBody.h:1087
yarp::sig::Matrix HUp
roto-translational matrix defining the central-up base frame with respect to the torso node
Definition: iDynBody.h:1051
yarp::sig::Matrix getHRight()
Return HRight, i.e.
Definition: iDynBody.h:1155
A class for computing forces and torques in a iDynChain, when a force/torque sensor is placed in the ...
Definition: iDynInv.h:1578
@ RBT_NODE_OUT
Definition: iDynBody.h:92
@ RBT_NODE_IN
Definition: iDynBody.h:92
@ DYNAMIC
Definition: iDynInv.h:64
InteractionType
Definition: iDynBody.h:86
@ RBT_BASE
Definition: iDynBody.h:86
@ RBT_ENDEFF
Definition: iDynBody.h:86
@ JAC_IKIN
Definition: iDynBody.h:99
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.