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 
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 
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 
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 
iCub::iDyn::iDynNode::dw
yarp::sig::Vector dw
angular acceleration
Definition: iDynBody.h:550
iCub::iDyn::iCubWholeBody::~iCubWholeBody
~iCubWholeBody()
Standard destructor.
Definition: iDynBody.cpp:2414
iCub::iDyn::RigidBodyTransformation::getR
yarp::sig::Matrix getR()
Return the rotational 3x3 matrix of the RBT.
Definition: iDynBody.cpp:126
iCub::iDyn::iDynNode::getForce
yarp::sig::Vector getForce() const
Return the node force.
Definition: iDynBody.cpp:817
iCub::iDyn::iDynSensorTorsoNode::getTorsoAngAcc
yarp::sig::Vector getTorsoAngAcc() const
Return the torso angular acceleration.
Definition: iDynBody.cpp:1908
iCub::iDyn::JacobType
JacobType
Definition: iDynBody.h:99
iCub::iDyn::iDynSensorTorsoNode::clearContactList
void clearContactList()
Definition: iDynBody.cpp:2220
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::version_tag::head_subversion
int head_subversion
Definition: iDynBody.h:111
iCub::iDyn::iDynSensorNode::addLimb
virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN)
Add one limb to the node, defining its RigidBodyTransformation.
H
H
Definition: compute_ekf_fast.m:27
iCub::iDyn::iDynSensorTorsoNode::total_COM_LF
yarp::sig::Vector total_COM_LF
Definition: iDynBody.h:1087
iCub::iDyn::InteractionType
InteractionType
Definition: iDynBody.h:86
iCub::iDyn::iDynNode::solveWrench
virtual bool solveWrench()
Main function to manage the exchange of wrench information among the limbs attached to the node.
Definition: iDynBody.cpp:590
iCub::iDyn::iDynNode::w
yarp::sig::Vector w
angular velocity
Definition: iDynBody.h:548
iCub::iDyn::iDynNode::iDynNode
iDynNode(const NewEulMode _mode=DYNAMIC)
Default constructor.
Definition: iDynBody.cpp:416
iCub::iDyn::iDynNode::computeJacobian
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
iCub::iDyn::iDynNode::F
yarp::sig::Vector F
force
Definition: iDynBody.h:554
iCub::iDyn::iCubWholeBody::getAllPositions
bool getAllPositions(yarp::sig::Vector &pos)
Retrieves a vector containing the anglular position of all the iCub joints, ordered in this way: left...
Definition: iDynBody.cpp:2607
iCub::iDyn::RigidBodyTransformation::TESTING_computeCOMJacobian
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
iCub::iDyn::iCubWholeBody::COM_Jacob
yarp::sig::Matrix COM_Jacob
Definition: iDynBody.h:1493
iCub::iDyn::RigidBodyTransformation::hasSensor
bool hasSensor
flag for sensor or not - only used for setWrenchMeasures()
Definition: iDynBody.h:157
iCub::iDyn::iDynNode::ddp
yarp::sig::Vector ddp
linear acceleration
Definition: iDynBody.h:552
iCub::iDyn::RigidBodyTransformation::getNLinks
unsigned int getNLinks() const
Return the number of links of the limb (N)
Definition: iDynBody.cpp:288
iCub::iDyn::iCubLowerTorso::build
void build()
Definition: iDynBody.cpp:2324
iCub::iDyn::RigidBodyTransformation::~RigidBodyTransformation
~RigidBodyTransformation()
Destructor.
Definition: iDynBody.cpp:61
iCub::iDyn::RigidBodyTransformation::toString
std::string toString() const
Return some information.
iCub::iDyn::RBT_NODE_OUT
@ RBT_NODE_OUT
Definition: iDynBody.h:92
pos
const dReal * pos
Definition: iCub_Sim.cpp:62
iCub::iDyn::RigidBodyTransformation::computeGeoJacobian
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...
iDynInv.h
iCub::iDyn::RigidBodyTransformation::mode
NewEulMode mode
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
Definition: iDynBody.h:148
iCub::iDyn::RigidBodyTransformation::getEndEffPose
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
iCub::iDyn::RBT_ENDEFF
@ RBT_ENDEFF
Definition: iDynBody.h:86
iCub::iDyn::iCubWholeBody::EXPERIMENTAL_computeCOMjacobian
bool EXPERIMENTAL_computeCOMjacobian()
Performs the computation of the center of mass jacobian of the whole iCub.
Definition: iDynBody.cpp:2675
iCub::iDyn::iDynNode::verbose
unsigned int verbose
verbosity flag
Definition: iDynBody.h:545
iCub::iDyn::version_tag::legs_version
int legs_version
Definition: iDynBody.h:112
iCub::iDyn::RigidBodyTransformation::H
yarp::sig::Matrix H
the roto-translation between the limb and the node
Definition: iDynBody.h:145
iCub::iDyn::NewEulMode
NewEulMode
Definition: iDynInv.h:64
iCub::iDyn::iDynNode::getRBT
yarp::sig::Matrix getRBT(unsigned int iLimb) const
Return the RBT matrix of a certain limb attached to the node.
Definition: iDynBody.cpp:449
iCub::iDyn::iDynSensorNode::setWrenchMeasure
virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M, bool afterAttach=false)
Set the Wrench measures on the limbs attached to the node.
iCub::iDyn::iDynSensorTorsoNode::right
iDyn::iDynLimb * right
right limb
Definition: iDynBody.h:1081
iCub::iDyn::iDynSensorTorsoNode::HRight
yarp::sig::Matrix HRight
roto-translational matrix defining the right limb base frame with respect to the torso node
Definition: iDynBody.h:1055
iCub::iDyn::RigidBodyTransformation::getWrench
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
iCub::iDyn::iDynSensorTorsoNode::setSensorMeasurement
bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left)
Set the FT sensor measurements on the sensor in right and left limb.
iCub::iDyn::RigidBodyTransformation::w
yarp::sig::Vector w
angular velocity
Definition: iDynBody.h:163
iCub::iDyn::iDynSensorNode::sensorList
std::deque< iDyn::iDynSensor * > sensorList
the list of iDynSensors used to solve each limb after FT sensor measurements
Definition: iDynBody.h:922
iCub::iDyn::iDynSensorTorsoNode::computeCOM
bool computeCOM()
Performs the computation of the center of mass (COM) of the node.
Definition: iDynBody.cpp:1912
iCub::iDyn::RigidBodyTransformation::setWrench
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
iCub::iDyn::RigidBodyTransformation::getRBT
yarp::sig::Matrix getRBT() const
Return the the (4x4) roto-translational matrix defining the rigid body transformation.
Definition: iDynBody.cpp:121
iCub::iDyn::iDynSensorTorsoNode::up
iDyn::iDynLimb * up
central-up limb
Definition: iDynBody.h:1083
iCub::iDyn::RBT_BASE
@ RBT_BASE
Definition: iDynBody.h:86
iCub::iDyn::iDynNode::computePose
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
iCub::iDyn::iCubWholeBody::iCubWholeBody
iCubWholeBody(version_tag _tag, const NewEulMode mode=DYNAMIC, unsigned int verbose=iCub::skinDynLib::VERBOSE)
Constructor: build the nodes and creates the whole body.
Definition: iDynBody.cpp:2400
iCub::iDyn::iDynNode::getAngVel
yarp::sig::Vector getAngVel() const
Return the node angular velocity.
Definition: iDynBody.cpp:821
iCub::iDyn::iDynNode
Definition: iDynBody.h:531
iCub::iDyn::iCubWholeBody::EXPERIMENTAL_getCOMvelocity
bool EXPERIMENTAL_getCOMvelocity(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &vel, yarp::sig::Vector &dq)
Retrieves a 3x1 vector containing the velocity of the robot COM.
Definition: iDynBody.cpp:3016
iDynContact.h
iCub::iDyn::iDynSensorTorsoNode
Definition: iDynBody.h:1046
iCub::iDyn::iDynSensorTorsoNode::estimateSensorsWrench
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Redefinition from iDynSensorNode.
Definition: iDynBody.h:1383
iCub::iDyn::iDynSensorTorsoNode::getTorques
yarp::sig::Vector getTorques(const std::string &limbType)
Return the chosen limb torques, as a Nx1 vector.
Definition: iDynBody.cpp:1890
iCub::iDyn::iDynNode::solveKinematics
bool solveKinematics()
Main function to manage the exchange of kinematic information among the limbs attached to the node.
Definition: iDynBody.cpp:462
iCub::iDyn::RigidBodyTransformation::getKinematicFlow
FlowType getKinematicFlow() const
Return the kinematic flow type.
Definition: iDynBody.cpp:185
iCub::iDyn::iCubUpperTorso::build
void build()
Definition: iDynBody.cpp:2243
iCub::iDyn::iCubUpperTorso::~iCubUpperTorso
~iCubUpperTorso()
Destructor.
Definition: iDynBody.cpp:2303
iCub::iDyn::iDynNode::setKinematicMeasure
bool setKinematicMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the kinematic measurement (w,dw,ddp) on the limb where the kinematic flow is of type RBT_NODE_IN.
Definition: iDynBody.cpp:561
iCub::iDyn::iDynSensorTorsoNode::total_mass_UP
double total_mass_UP
Definition: iDynBody.h:1089
iCub::iDyn::iDynSensorNode
Definition: iDynBody.h:916
iCub::iDyn::iCubWholeBody::computeCOM
bool computeCOM()
Performs the computation of the center of mass (COM) of the whole iCub.
Definition: iDynBody.cpp:2473
iCub::iDyn::iDynSensorNode::solveWrench
virtual bool solveWrench()
Main function to manage the exchange of wrench information among the limbs attached to the node.
Definition: iDynBody.cpp:1352
math.h
iCub::iDyn::iCubLowerTorso::iCubLowerTorso
iCubLowerTorso(version_tag _tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:2317
iCub::iDyn::iCubWholeBody::attachLowerTorso
void attachLowerTorso(const yarp::sig::Vector &FM_right_leg, const yarp::sig::Vector &FM_left_leg)
Connect upper and lower torso: this procedure handles the exchange of kinematic and wrench variables ...
Definition: iDynBody.cpp:2421
iCub::iDyn::iDynSensorTorsoNode::getMoments
yarp::sig::Matrix getMoments(const std::string &limbType)
Return the chosen limb moments, as a 6xN matrix.
Definition: iDynBody.cpp:1878
iCub::iDyn::RigidBodyTransformation::verbose
unsigned int verbose
verbosity flag
Definition: iDynBody.h:154
iCub::iDyn::version_tag::head_version
int head_version
Definition: iDynBody.h:110
iCub::iDyn::iDynSensorTorsoNode::getTorsoMoment
yarp::sig::Vector getTorsoMoment() const
Return the torso moment.
Definition: iDynBody.cpp:1904
iCub::iDyn::iDynSensorTorsoNode::COM_jacob_LF
yarp::sig::Matrix COM_jacob_LF
Definition: iDynBody.h:1093
iCub::iDyn::DYNAMIC
@ DYNAMIC
Definition: iDynInv.h:64
iCub::iDyn::iDynSensorTorsoNode::HUp
yarp::sig::Matrix HUp
roto-translational matrix defining the central-up base frame with respect to the torso node
Definition: iDynBody.h:1051
iCub::iDyn::iDynSensorTorsoNode::getTorsoAngVel
yarp::sig::Vector getTorsoAngVel() const
Return the torso angular velocity.
Definition: iDynBody.cpp:1906
iCub::iDyn::iCubWholeBody::upper_mass
double upper_mass
Definition: iDynBody.h:1489
iCub::iDyn::iCubWholeBody::lowerTorso
iCubLowerTorso * lowerTorso
pointer to LowerTorso = torso + right leg + left leg
Definition: iDynBody.h:1484
iCub::iDyn::RigidBodyTransformation::getHCOM
yarp::sig::Matrix getHCOM(unsigned int iLink)
Returns the COM matrix of the selected link (index = iLink) in the chain.
Definition: iDynBody.cpp:401
iCub::iDyn::iDynSensorTorsoNode::right_name
std::string right_name
name of right limb
Definition: iDynBody.h:1060
mode
GLenum mode
Definition: rendering.cpp:48
iCub::iDyn::iCubUpperTorso
Definition: iDynBody.h:1401
iCub::iDyn::iDynNode::COM
yarp::sig::Vector COM
COM position of the node.
Definition: iDynBody.h:558
iCub::iDyn::iDynSensorTorsoNode::total_COM_UP
yarp::sig::Vector total_COM_UP
COMs and masses of the limbs.
Definition: iDynBody.h:1086
iCub::iDyn::iDynSensorTorsoNode::getTorsoLinAcc
yarp::sig::Vector getTorsoLinAcc() const
Return the torso linear acceleration.
Definition: iDynBody.cpp:1910
iCub::iDyn::RigidBodyTransformation::computeLimbKinematic
void computeLimbKinematic()
Calls the compute kinematic of the limb.
Definition: iDynBody.cpp:278
iCub::iDyn::iDynNode::mass
double mass
total mass of the node
Definition: iDynBody.h:560
iCub::iDyn::iDynSensorNode::iDynSensorNode
iDynSensorNode(const NewEulMode _mode=DYNAMIC)
Default constructor.
Definition: iDynBody.cpp:1327
iCub::iDyn::iDynSensorTorsoNode::iDynSensorTorsoNode
iDynSensorTorsoNode(const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:1729
iCub::iDyn::RigidBodyTransformation::computeKinematic
void computeKinematic()
Basic computations for applying RBT on kinematic variables.
Definition: iDynBody.cpp:195
iCub::iDyn::iDynSensorTorsoNode::getTorque
double getTorque(const std::string &limbType, const unsigned int iLink) const
Return the chosen limb-link torque, as a real value.
iCub::iDyn::iDynSensorTorsoNode::getTorsoForce
yarp::sig::Vector getTorsoForce() const
Return the torso force.
Definition: iDynBody.cpp:1902
iCub::iDyn::iDynSensorTorsoNode::setAng
yarp::sig::Vector setAng(const std::string &limbType, const yarp::sig::Vector &_q)
Set joints angle position in the chosen limb.
iCub::iDyn::iDynSensorTorsoNode::getHRight
yarp::sig::Matrix getHRight()
Return HRight, i.e.
Definition: iDynBody.h:1155
iCub::iDyn::RigidBodyTransformation::setH0
bool setH0(const yarp::sig::Matrix &_H0)
Set a new H0 matrix in the limb attached to the RBT.
Definition: iDynBody.cpp:366
iCub::iDyn::RigidBodyTransformation::RigidBodyTransformation
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
iCub::iDyn::iCubLowerTorso
Definition: iDynBody.h:1437
iCub::iDyn::iDynLimb
Definition: iDyn.h:1176
iCub::iDyn::iDynSensorTorsoNode::left
iDyn::iDynLimb * left
left limb
Definition: iDynBody.h:1079
iCub::iDyn::RigidBodyTransformation::limb
iDyn::iDynLimb * limb
the limb attached to the RigidBodyTransformation
Definition: iDynBody.h:136
iCub::iDyn::iCubWholeBody::whole_COM
yarp::sig::Vector whole_COM
Definition: iDynBody.h:1490
iCub::iDyn::RigidBodyTransformation
Definition: iDynBody.h:131
iCub::iDyn::RigidBodyTransformation::ddp
yarp::sig::Vector ddp
linear acceleration
Definition: iDynBody.h:167
iCub::iDyn::iDynSensorTorsoNode::getNLinks
unsigned int getNLinks(const std::string &limbType) const
Definition: iDynBody.cpp:2208
iCub::iDyn::iCubWholeBody::EXPERIMENTAL_getCOMjacobian
bool EXPERIMENTAL_getCOMjacobian(iCub::skinDynLib::BodyPart which_part, yarp::sig::Matrix &jac)
Retrieves the result of the last COM jacobian computation.
Definition: iDynBody.cpp:2746
iCub::iDyn::RigidBodyTransformation::dw
yarp::sig::Vector dw
angular acceleration
Definition: iDynBody.h:165
iCub::iDyn::iDynSensorTorsoNode::~iDynSensorTorsoNode
~iDynSensorTorsoNode()
Destructor.
Definition: iDynBody.cpp:1741
iCub::iDyn::iDynNode::info
std::string info
info or useful notes
Definition: iDynBody.h:542
iCub::iDyn::RigidBodyTransformation::wreFlow
FlowType wreFlow
wrench flow: in-to/out-from node (RBT_NODE_IN/RBT_NODE_OUT)
Definition: iDynBody.h:142
iCub::iDyn::iDynSensorTorsoNode::name
std::string name
the torso node name
Definition: iDynBody.h:1064
iCub::iDyn::iDynSensorTorsoNode::COM_jacob_UP
yarp::sig::Matrix COM_jacob_UP
Definition: iDynBody.h:1092
iCub::iDyn::iDynSensor
Definition: iDynInv.h:1577
iCub::iDyn::RigidBodyTransformation::computeWrench
void computeWrench()
Basic computations for applying RBT on wrench variables.
Definition: iDynBody.cpp:246
iCub::iDyn::iCubWholeBody::getAllVelocities
bool getAllVelocities(yarp::sig::Vector &vel)
Retrieves a vector containing the velocities of all the iCub joints, ordered in this way: left leg (6...
Definition: iDynBody.cpp:2629
iCub::iDyn::iCubWholeBody::sw_getcom
double sw_getcom
Definition: iDynBody.h:1494
iCub::iDyn::iDynNode::rbtList
std::deque< RigidBodyTransformation > rbtList
the list of RBT
Definition: iDynBody.h:536
iCub::iDyn::iDynSensorTorsoNode::left_name
std::string left_name
name of left limb
Definition: iDynBody.h:1058
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::skinDynLib::VERBOSE
@ VERBOSE
Definition: common.h:38
iCub::iDyn::iDynNode::mode
NewEulMode mode
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
Definition: iDynBody.h:539
iCub::iDyn::iDynSensorTorsoNode::EXPERIMENTAL_computeCOMjacobian
bool EXPERIMENTAL_computeCOMjacobian()
Performs the computation of the center of mass jacobian of the node.
Definition: iDynBody.cpp:1970
iCub::iDyn::RigidBodyTransformation::setKinematicMeasure
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
iCub::iDyn::iDynNode::compute_Pn_HAN
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.
iCub::iDyn::iDynSensorTorsoNode::getHUp
yarp::sig::Matrix getHUp()
Return HUp, i.e.
Definition: iDynBody.h:1159
iCub::iDyn::RigidBodyTransformation::getr
yarp::sig::Vector getr(bool proj=false)
Return the translational part of the RBT matrix.
Definition: iDynBody.cpp:145
iCub::iDyn::RigidBodyTransformation::getR6
yarp::sig::Matrix getR6() const
Return a 6x6 diagonal matrix with the rotational matrix of the RBT.
Definition: iDynBody.cpp:131
iCub::iDyn::iDynSensorTorsoNode::update
bool update()
Main method for solving kinematics and wrench among limbs, where information are shared.
Definition: iDynBody.cpp:1794
iCub::iDyn::iCubLowerTorso::~iCubLowerTorso
~iCubLowerTorso()
Destructor.
Definition: iDynBody.cpp:2387
iCub::iDyn::iCubWholeBody::whole_mass
double whole_mass
masses and position of the center of mass of the iCub sub-parts
Definition: iDynBody.h:1487
iCub::iDyn::RigidBodyTransformation::info
std::string info
info or useful notes
Definition: iDynBody.h:151
iCub::iDyn::iDynNode::getLinAcc
yarp::sig::Vector getLinAcc() const
Return the node linear acceleration.
Definition: iDynBody.cpp:825
iCub::iDyn::iDynSensorTorsoNode::total_mass_LF
double total_mass_LF
Definition: iDynBody.h:1090
iCub::iDyn::iDynNode::addLimb
virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN, bool hasSensor=false)
Add one limb to the node, defining its RigidBodyTransformation.
Definition: iDynBody.cpp:442
iCub::iDyn::RigidBodyTransformation::setRBT
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
iCub::iDyn::version_tag
Definition: iDynBody.h:108
iCub::iDyn::RigidBodyTransformation::getKinematic
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
iCub::iDyn::iDynSensorNode::estimateSensorsWrench
yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false)
Exploit iDynInvSensor methods to retrieve FT sensor measurements after solving wrenches in the limbs.
Definition: iDynBody.cpp:1562
iCub::iDyn::iDynNode::getAngAcc
yarp::sig::Vector getAngAcc() const
Return the node angular acceleration.
Definition: iDynBody.cpp:823
iCub::iDyn::RigidBodyTransformation::setWrenchMeasure
bool setWrenchMeasure(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
Set the wrench variables (F,Mu) of the limb.
iCub::iDyn::iDynSensorTorsoNode::total_mass_RT
double total_mass_RT
Definition: iDynBody.h:1091
iCub::iDyn::iCubWholeBody::upperTorso
iCubUpperTorso * upperTorso
pointer to UpperTorso = head + right arm + left arm
Definition: iDynBody.h:1482
iCub::skinDynLib::BodyPart
BodyPart
Definition: common.h:44
iCub::iDyn::iDynNode::howManyKinematicInputs
unsigned int howManyKinematicInputs(bool afterAttach=false) const
Return the number of limbs with kinematic input, i.e.
Definition: iDynBody.cpp:801
iCub::iDyn::iDynNode::Mu
yarp::sig::Vector Mu
moment
Definition: iDynBody.h:556
iCub::iDyn::iCubWholeBody::lower_mass
double lower_mass
Definition: iDynBody.h:1488
iCub::iDyn::iDynSensorNode::howManySensors
unsigned int howManySensors() const
Definition: iDynBody.cpp:1707
iCub::iDyn::iDynContactSolver
Definition: iDynContact.h:52
iCub::iDyn::iCubUpperTorso::iCubUpperTorso
iCubUpperTorso(version_tag tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
Constructor.
Definition: iDynBody.cpp:2236
iCub::iDyn::iCubWholeBody::rbt
RigidBodyTransformation * rbt
the rigid body transformation linking the UpperTorso node with the final link of the iCubTorsoDyn cha...
Definition: iDynBody.h:1476
iCub::iDyn::iCubWholeBody::tag
version_tag tag
Definition: iDynBody.h:1477
iCub::iDyn::FlowType
FlowType
Definition: iDynBody.h:92
iCub::iDyn::iCubWholeBody::upper_COM
yarp::sig::Vector upper_COM
Definition: iDynBody.h:1491
iCub::iDyn::iCubWholeBody
Definition: iDynBody.h:1471
iCub::iDyn::iDynSensorTorsoNode::up_name
std::string up_name
name of central-up limb
Definition: iDynBody.h:1062
iCub::iDyn::iDynNode::compute_Pn_HAN_COM
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
iCub::iDyn::iCubWholeBody::lower_COM
yarp::sig::Vector lower_COM
Definition: iDynBody.h:1492
iCub::iDyn::RigidBodyTransformation::setInfoFlow
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
iCub::iDyn::RigidBodyTransformation::getH0
yarp::sig::Matrix getH0() const
Return the H0 matrix of the limb attached to the RBT.
Definition: iDynBody.cpp:361
iCub::iDyn::iDynNode::zero
void zero()
Reset all data to zero.
Definition: iDynBody.cpp:433
iCub::iDyn::iDynSensorTorsoNode::HLeft
yarp::sig::Matrix HLeft
roto-translational matrix defining the left limb base frame with respect to the torso node
Definition: iDynBody.h:1053
iCub::iDyn::iDynSensorTorsoNode::setInertialMeasure
bool setInertialMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Set the inertial sensor measurements on the central-up limb.
Definition: iDynBody.cpp:1753
iCub::iDyn::iDynSensorTorsoNode::getHLeft
yarp::sig::Matrix getHLeft()
Return HLeft, i.e.
Definition: iDynBody.h:1151
iCub::iDyn::iCubLowerTorso::tag
version_tag tag
Build the node.
Definition: iDynBody.h:1443
iCub::iDyn::iDynSensorTorsoNode::setDAng
yarp::sig::Vector setDAng(const std::string &limbType, const yarp::sig::Vector &_dq)
Set joints angle velocity in the chosen limb.
iCub::iDyn::iDynSensorTorsoNode::total_COM_RT
yarp::sig::Vector total_COM_RT
Definition: iDynBody.h:1088
iCub::iDyn::RigidBodyTransformation::setKinematic
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
iCub::iDyn::iDynSensorTorsoNode::getForces
yarp::sig::Matrix getForces(const std::string &limbType)
Return the chosen limb forces, as a 6xN matrix.
Definition: iDynBody.cpp:1866
iCub::iDyn::iDynNode::getMoment
yarp::sig::Vector getMoment() const
Return the node moment.
Definition: iDynBody.cpp:819
iCub::iDyn::iDynNode::setWrenchMeasure
virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M)
Set the wrench measure on the limbs with input wrench.
iCub::iDyn::JAC_KIN
@ JAC_KIN
Definition: iDynBody.h:99
iCub::iDyn::version_tag::version_tag
version_tag()
Definition: iDynBody.h:114
iCub::iDyn::RigidBodyTransformation::Mu
yarp::sig::Vector Mu
moment
Definition: iDynBody.h:171
iCub::iDyn::iCubWholeBody::getCOM
bool getCOM(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &COM, double &mass)
Retrieves the result of the last COM computation.
Definition: iDynBody.cpp:2511
iCub::iDyn::RigidBodyTransformation::isSensorized
bool isSensorized() const
Return a boolean, depending if the limb attached to the RBT has a FT sensor or not.
Definition: iDynBody.cpp:273
iCub::iDyn::iDynSensorTorsoNode::COM_jacob_RT
yarp::sig::Matrix COM_jacob_RT
Definition: iDynBody.h:1094
iCub::iDyn::iDynSensorTorsoNode::setD2Ang
yarp::sig::Vector setD2Ang(const std::string &limbType, const yarp::sig::Vector &_ddq)
Set joints angle acceleration in the chosen limb.
iCub::iDyn::RigidBodyTransformation::kinFlow
FlowType kinFlow
kinematic flow: in-to/out-from node (RBT_NODE_IN/RBT_NODE_OUT)
Definition: iDynBody.h:139
iCub::iDyn::RigidBodyTransformation::F
yarp::sig::Vector F
force
Definition: iDynBody.h:169
iCub::iDyn::RigidBodyTransformation::computeLimbWrench
void computeLimbWrench()
Calls the compute wrench of the limb.
Definition: iDynBody.cpp:283
iCub::iDyn::iDynSensorTorsoNode::rightSensor
iDyn::iDynContactSolver * rightSensor
right FT sensor and solver
Definition: iDynBody.h:1075
iCub::iDyn::iDynNode::TESTING_computeCOMJacobian
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
iCub::iDyn::iDynSensorTorsoNode::getD2Ang
yarp::sig::Vector getD2Ang(const std::string &limbType)
Get joints angle acceleration in the chosen limb.
Definition: iDynBody.cpp:2172
iCub::iDyn::iDynNode::howManyWrenchInputs
unsigned int howManyWrenchInputs(bool afterAttach=false) const
Return the number of limbs with wrench input, i.e.
Definition: iDynBody.cpp:785
iCub::iDyn::JAC_IKIN
@ JAC_IKIN
Definition: iDynBody.h:99
iCub::iDyn::RBT_NODE_IN
@ RBT_NODE_IN
Definition: iDynBody.h:92
iCub::iDyn::RigidBodyTransformation::getDOF
unsigned int getDOF() const
Return the number of DOF of the limb (DOF <= N)
Definition: iDynBody.cpp:293
iCub::iDyn::iDynSensorTorsoNode::getDAng
yarp::sig::Vector getDAng(const std::string &limbType)
Get joints angle velocity in the chosen limb.
Definition: iDynBody.cpp:2124
iCub::iDyn::iDynSensorTorsoNode::getAng
yarp::sig::Vector getAng(const std::string &limbType)
Get joints angle position in the chosen limb.
Definition: iDynBody.cpp:2076
iCub::iDyn::RigidBodyTransformation::getWrenchFlow
FlowType getWrenchFlow() const
return the wrench flow type
Definition: iDynBody.cpp:190
iCub::iDyn::RigidBodyTransformation::getH
yarp::sig::Matrix getH()
Return the end-effector roto-translational matrix of the end-effector H of the end-effector.
Definition: iDynBody.cpp:303
iCub::iDyn::iCubUpperTorso::tag
version_tag tag
Build the node.
Definition: iDynBody.h:1407
iCub::iDyn::iDynSensorTorsoNode::leftSensor
iDyn::iDynContactSolver * leftSensor
Build the node.
Definition: iDynBody.h:1073