iCubmain

A class for connecting two or mutiple limbs and exchanging kinematic and wrench information between limbs. More...
#include <iDynBody.h>
Public Member Functions  
iDynNode (const NewEulMode _mode=DYNAMIC)  
Default constructor. More...  
iDynNode (const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)  
Constructor with parameters. More...  
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. More...  
yarp::sig::Matrix  getRBT (unsigned int iLimb) const 
Return the RBT matrix of a certain limb attached to the node. More...  
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. More...  
bool  solveKinematics () 
Main function to manage the exchange of kinematic information among the limbs attached to the node. More...  
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. More...  
virtual bool  solveWrench () 
Main function to manage the exchange of wrench information among the limbs attached to the node. More...  
bool  solveWrench (const yarp::sig::Matrix &FM) 
This is to manage the exchange of wrench information among the limbs attached to the node. More...  
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. More...  
virtual bool  setWrenchMeasure (const yarp::sig::Matrix &F, const yarp::sig::Matrix &M) 
Set the wrench measure on the limbs with input wrench. More...  
virtual bool  setWrenchMeasure (const yarp::sig::Matrix &FM) 
Set the wrench measure on the limbs with input wrench. More...  
yarp::sig::Vector  getForce () const 
Return the node force. More...  
yarp::sig::Vector  getMoment () const 
Return the node moment. More...  
yarp::sig::Vector  getAngVel () const 
Return the node angular velocity. More...  
yarp::sig::Vector  getAngAcc () const 
Return the node angular acceleration. More...  
yarp::sig::Vector  getLinAcc () const 
Return the node linear acceleration. More...  
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 be done by iKin). More...  
yarp::sig::Matrix  computeJacobian (unsigned int iChain, unsigned int iLink) 
Compute the Jacobian of the ith link of the limb with index iChain in the node, in its default direction (as it would be done by iKin). More...  
yarp::sig::Matrix  computeJacobian (unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB) 
Compute the Jacobian between two links in two different chains. More...  
yarp::sig::Matrix  computeJacobian (unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB) 
Compute the Jacobian between two links in two different chains. More...  
yarp::sig::Vector  computePose (unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, const bool axisRep) 
Compute the Pose of the endeffector, given a "virtual" chain connecting two limbs. More...  
yarp::sig::Vector  computePose (unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, const bool axisRep) 
Compute the Pose of the endeffector, given a "virtual" chain connecting two limbs. More...  
yarp::sig::Matrix  TESTING_computeCOMJacobian (unsigned int iChain, unsigned int iLink) 
Compute the Jacobian of the COM of the ith link of the limb with index iChain in the node. More...  
yarp::sig::Matrix  TESTING_computeCOMJacobian (unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB) 
Compute the Jacobian of the COM of link iLinkB, in chainB, when two different chains (A and B) are connected. More...  
Protected Member Functions  
void  zero () 
Reset all data to zero. More...  
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. More...  
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. More...  
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. More...  
unsigned int  howManyWrenchInputs (bool afterAttach=false) const 
Return the number of limbs with wrench input, i.e. More...  
unsigned int  howManyKinematicInputs (bool afterAttach=false) const 
Return the number of limbs with kinematic input, i.e. More...  
Protected Attributes  
std::deque< RigidBodyTransformation >  rbtList 
the list of RBT More...  
NewEulMode  mode 
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY. More...  
std::string  info 
info or useful notes More...  
unsigned int  verbose 
verbosity flag More...  
yarp::sig::Vector  w 
angular velocity More...  
yarp::sig::Vector  dw 
angular acceleration More...  
yarp::sig::Vector  ddp 
linear acceleration More...  
yarp::sig::Vector  F 
force More...  
yarp::sig::Vector  Mu 
moment More...  
yarp::sig::Vector  COM 
COM position of the node. More...  
double  mass 
total mass of the node More...  
A class for connecting two or mutiple limbs and exchanging kinematic and wrench information between limbs.
A virtual node, connecting multiple limbs, is set. The limbs can exchange kinematics and wrench information with the node through a RigidBodyTransfromation. The node only has kinematic (w,dw,ddp) and wrench (F,Mu) information: no mass, length, inertia, COM, or else. Each limb is connected to the node by a rototranslation matrix, which must be set when a limb is attached to the node: a RigidBodyTransfromation object is then created, which allows the proper computation of wrench and kinematic variables. When multiple limbs are attached to a node, the kinematic variables are set by a single limb, having kinematic flow = RBT_NODE_IN, while the wrench variables are found as the sum of the wrench contribution of all the links (inbound and outbound wrenches must balance in the node).
Definition at line 531 of file iDynBody.h.
iDynNode::iDynNode  (  const NewEulMode  _mode = DYNAMIC  ) 
Default constructor.
_mode  the modality for dynamic computation 
Definition at line 416 of file iDynBody.cpp.
iDynNode::iDynNode  (  const std::string &  _info, 
const NewEulMode  _mode = DYNAMIC , 

unsigned int  verb = iCub::skinDynLib::VERBOSE 

) 
Constructor with parameters.
_info  some information on the node, i.e. its description 
_mode  the modality for dynamic computation 
verb  verbosity level 
Definition at line 424 of file iDynBody.cpp.

virtual 
Add one limb to the node, defining its RigidBodyTransformation.
A new RigidBodyTransformation is added to the RBT list.
limb  pointer to generic limb 
H  a (4x4) rototranslational matrix defining the transformation between node and limb base/end 
kinFlow  the type of information flow of kinematics variables 
wreFlow  the type of information flow of wrench variables 
hasSensor  flag for having or not a FT sensor 
Definition at line 442 of file iDynBody.cpp.

protected 
Compute Pn and H_A_Node matrices given two chains.
This function is private, and is used by computeJacobian() and computePose() to merely avoid code duplication.

protected 
Compute Pn and H_A_Node matrices given two chains.
This function is private, and is used by computeJacobian() and computePose() to merely avoid code duplication.

protected 
Compute Pn and H_A_Node matrices given two chains.
This function is private, and is used by computeCOMJacobian() and computeCOMPose() to merely avoid code duplication.
Definition at line 1281 of file iDynBody.cpp.
Matrix iDynNode::computeJacobian  (  unsigned int  iChain  ) 
Compute the Jacobian of the limb with index iChain in the node, in its default direction (as it would be done by iKin).
iChain  the index of the chain (limb) in the node 
Definition at line 834 of file iDynBody.cpp.
Matrix iDynNode::computeJacobian  (  unsigned int  iChain, 
unsigned int  iLink  
) 
Compute the Jacobian of the ith link of the limb with index iChain in the node, in its default direction (as it would be done by iKin).
If the link index is not correct, a null Jacobian is returned.
Important note: since we are specifying the link index in the chain, the Jacobian computation will deal with all the links, even blocked links. The Jacobian size is not 6x(the DOF until iLinkB) but 6xiLinkB, where 0<iLinkB<N
iChain  the index of the chain (limb) in the node 
iLink  the index of the limnk in the limb 
Definition at line 847 of file iDynBody.cpp.
Matrix iDynNode::computeJacobian  (  unsigned int  iChainA, 
JacobType  dirA,  
unsigned int  iChainB,  
JacobType  dirB  
) 
Compute the Jacobian between two links in two different chains.
The chains are specified by their index in the list (the progressive number of insertion). The first limb (limb A  index=iChainA) has the base link of the jacobian while the second limb (limb B  index=iChainB) has the final link of the jacobian. Whether the base/final of the Jacobian coincides with the base/end of the chains, depends on the flags dirA,dirB: if dirA=JAC_DIR, then the beginning is at the base of chain, otherwise it is at the endeffector; if dirB=JAC_DIR, the final link of the jacobian is on the endeffector of the chain, otherwise on its base.
iChainA  the index of the chain (the limb) in the node having the base frame 
dirA  the 'direction' of the chain wrt the jacobian computation 
iChainB  the index of the chain (the limb) in the node having the final frame 
dirB  the 'direction' of the chain wrt the jacobian computation 
Definition at line 862 of file iDynBody.cpp.
Matrix iDynNode::computeJacobian  (  unsigned int  iChainA, 
JacobType  dirA,  
unsigned int  iChainB,  
unsigned int  iLinkB,  
JacobType  dirB  
) 
Compute the Jacobian between two links in two different chains.
The chains are specified by their index in the list (the progressive number of insertion). The first limb has the base of the jacobian (base or endeffector of the limb, depending on the Jacobian direction JacA) while the second limb (limb B  index=iChainB) has the final link of the jacobian (index=iLinkB).
Important note: since we are specifying the link index in chain B, the Jacobian computation on chain B will deal with all the links, even blocked links. The Jacobian size is not 6x(DOF_A + the DOF until iLinkB) but 6x(DOF_A+iLinkB), where 0<iLinkB<N
iChainA  the index of the chain (the limb) in the node having the base (<0>) frame 
dirA  the 'direction' of the chain wrt the jacobian computation 
iChainB  the index of the chain (the limb) in the node having the final (<N>) frame 
iLinkB  the index of the link, in the indexChainN chain, being the final (<N>) frame for the Jacobian computation 
dirB  the 'direction' of the chain wrt the jacobian computation 
Definition at line 927 of file iDynBody.cpp.
Vector iDynNode::computePose  (  unsigned int  iChainA, 
JacobType  dirA,  
unsigned int  iChainB,  
JacobType  dirB,  
const bool  axisRep  
) 
Compute the Pose of the endeffector, given a "virtual" chain connecting two limbs.
The chains are specified by their index in the list (the progressive number of insertion). The first limb (limb A  index=iChainA) has the base link of the augmented chain while the second limb (limb B  index=iChainB) has the final link of the augmented chain . Whether the base/end of the augmented chain coincides with the base/end of the single chains, depends on the flags dirA,dirB: if dirA=JAC_DIR, then the beginning is at the base of chain, otherwise it is at the endeffector; if dirB=JAC_DIR, the final link of the augmented chain is on the endeffector of the chain, otherwise on its base. This method is useful to compute the endeffector pose (i.e. computing the arm pose, when the chain torso + arm is considered) in a multilimb chain.
iChainA  the index of the chain (the limb) in the node having the base frame 
dirA  the 'direction' of visit of the chain 
iChainB  the index of the chain (the limb) in the node having the final frame 
dirB  the 'direction' of visit of the chain 
Definition at line 1062 of file iDynBody.cpp.
Vector iDynNode::computePose  (  unsigned int  iChainA, 
JacobType  dirA,  
unsigned int  iChainB,  
unsigned int  iLinkB,  
JacobType  dirB,  
const bool  axisRep  
) 
Compute the Pose of the endeffector, given a "virtual" chain connecting two limbs.
The chains are specified by their index in the list (the progressive number of insertion). The first limb (limb A  index=iChainA) has the base link of the augmented chain while the second limb (limb B  index=iChainB) has the final link of the augmented chain, ending in the link iLinkB. Whether the base/end of the augmented chain coincides with the base/end of the single chains, depends on the flags dirA,dirB: if dirA=JAC_DIR, then the beginning is at the base of chain, otherwise it is at the endeffector; if dirB=JAC_DIR, the final link of the augmented chain is on the endeffector of the chain, otherwise on its base. This method is useful to compute the endeffector pose (i.e. computing the elbow pose, when the chain torso + arm is considered) in a multilimb chain.
iChainA  the index of the chain (the limb) in the node having the base frame 
dirA  the 'direction' of visit of the chain 
iChainB  the index of the chain (the limb) in the node having the final frame 
iLinkB  the index of the link, in the indexChainN chain, being the final (<N>) frame for the Jacobian computation 
dirB  the 'direction' of visit of the chain 
Definition at line 1122 of file iDynBody.cpp.
Vector iDynNode::getAngAcc  (  )  const 
Return the node angular acceleration.
Definition at line 823 of file iDynBody.cpp.
Vector iDynNode::getAngVel  (  )  const 
Return the node angular velocity.
Definition at line 821 of file iDynBody.cpp.
Vector iDynNode::getForce  (  )  const 
Vector iDynNode::getLinAcc  (  )  const 
Return the node linear acceleration.
Definition at line 825 of file iDynBody.cpp.
Vector iDynNode::getMoment  (  )  const 
Matrix iDynNode::getRBT  (  unsigned int  iLimb  )  const 
Return the RBT matrix of a certain limb attached to the node.
iLimb  the index of the limb  the index is the number of insertion of the limb in the node 
Definition at line 449 of file iDynBody.cpp.

protected 
Return the number of limbs with kinematic input, i.e.
receiving kinematic information from external measurements.
afterAttach  =true only if the limb received kinematic parameters during an attachTorso() procedure 
Definition at line 801 of file iDynBody.cpp.

protected 
Return the number of limbs with wrench input, i.e.
receiving wrench information from external measurements.
afterAttach  =true only if the limb received wrench parameters during an attachTorso() procedure 
Definition at line 785 of file iDynBody.cpp.
bool iDynNode::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 at line 561 of file iDynBody.cpp.

virtual 
Set the wrench measure on the limbs with input wrench.
F  a (3xN) matrix with forces 
M  a (3xN) matrix with moments 

virtual 
Set the wrench measure on the limbs with input wrench.
FM  a (6xN) matrix with forces and moments 
bool iDynNode::solveKinematics  (  ) 
Main function to manage the exchange of kinematic information among the limbs attached to the node.
Definition at line 462 of file iDynBody.cpp.
bool iCub::iDyn::iDynNode::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.
One single limb with kinematic flow of input type must exist: this limb is initilized with the kinematic variables w0,dw0,ddp0 (eg the up receives this information from the inertia sensor). The limb itself knows where to init the chain (base/end) depending on how it is attached to the node. Then the first limb kinematics is solved. The kinematic variables are retrieved from the RBT, which applies its rototranslation. Then the kinematic variables are sent to the other limbs, having kinematic flow of output type: the RBT transformation is applied from node to limb.
w0  the initial/measured angular velocity 
dw0  the initial/measured angular acceleration 
ddp0  the initial/measured linear acceleration 

virtual 
Main function to manage the exchange of wrench information among the limbs attached to the node.
Multiple limbs with wrench flow of input type can exist, but at least one limb with output type must exist, to compute the wrench balance on the node. The measured/input wrenches to the limbs are here assumed to be set elsewhere: eg another class or the main is setting the measured wrenches. Note: RBT calls computeWrenchNewtonEuler in the limb, meaning that performs a "basic" wrench computation without any sensor, just setting wrenches at the endeffector or at the base, and calling recursive wrench computation.
Reimplemented in iCub::iDyn::iDynSensorNode.
Definition at line 590 of file iDynBody.cpp.
bool iCub::iDyn::iDynNode::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.
Multiple limbs with wrench flow of input type can exist, but at least one limb with output type must exist, to compute the wrench balance on the node. The measured/input wrenches to the limbs are here passed as a big matrix. In this function the input wrench is set in the limb calling initWrenchNewtonEuler(), which simply set the measured forces in the base/final link of the limb. Input (eg measured) wrenches are stored in two 3xN matrix: each column is a 3x1 vector with force/moment; N is the number of columns, ie the number of measured/input wrenches to the limb the order is assumed coherent with the one built when adding limbs eg: adding limbs: addLimb(limb1), addLimb(limb2), addLimb(limb3) where limb1, limb3 have wrench flow input passing wrenches: Matrix F(3,2), F.setcol(0,f1), F.setcol(1,f3) and similar for moment
Note: RBT calls computeWrenchNewtonEuler in the limb, meaning that perform a "basic" wrench computation without any sensor, just setting wrenches at the endeffector or at the base, and calling recursive wrench computation.
F  a (3xN) matrix with forces 
M  a (3xN) matrix with moments 
bool iCub::iDyn::iDynNode::solveWrench  (  const yarp::sig::Matrix &  FM  ) 
This is to manage the exchange of wrench information among the limbs attached to the node.
Multiple limbs with wrench flow of input type can exist, but at least one limb with output type must exist, to compute the wrench balance on the node. The measured/input wrenches to the limbs are here passed as a big matrix. In this function the input wrench is set in the limb calling initWrenchNewtonEuler(), which simply set the measured forces in the base/final link of the limb. elsewhere: eg another class or the main is setting the measured wrenches. Input (eg measured) wrenches are stored in a 6xN matrix: each column is a 6x1 vector with force/moment; N is the number of columns, ie the number of measured/input wrenches to the limb the order is assumed coherent with the one built when adding limbs eg: adding limbs: addLimb(limb1), addLimb(limb2), addLimb(limb3) where limb1, limb3 have wrench flow input setting wrenches: Matrix FM(6,2), FM.setcol(0,fm1), FM.setcol(1,fm3)
Note: RBT calls computeWrenchNewtonEuler in the limb, meaning that perform a "basic" wrench computation without any sensor, just setting wrenches at the endeffector or at the base, and calling recursive wrench computation.
FM  a (6xN) matrix with forces and moments 
Matrix iDynNode::TESTING_computeCOMJacobian  (  unsigned int  iChain, 
unsigned int  iLink  
) 
Compute the Jacobian of the COM of the ith link of the limb with index iChain in the node.
If the link index is not correct, a null Jacobian is returned.
iChain  the index of the chain (limb) in the node 
iLink  the index of the limnk in the limb 
Definition at line 1194 of file iDynBody.cpp.
Matrix iDynNode::TESTING_computeCOMJacobian  (  unsigned int  iChainA, 
JacobType  dirA,  
unsigned int  iChainB,  
unsigned int  iLinkB,  
JacobType  dirB  
) 
Compute the Jacobian of the COM of link iLinkB, in chainB, when two different chains (A and B) are connected.
The chains are specified by their index in the list (the progressive number of insertion). The first limb has the base of the jacobian (base or endeffector of the limb, depending on the Jacobian direction JacA) while the second limb (limb B  index=iChainB) has the final link of the jacobian (index=iLinkB).
iChainA  the index of the chain (the limb) in the node having the base (<0>) frame 
dirA  the 'direction' of the chain wrt the jacobian computation 
iChainB  the index of the chain (the limb) in the node having the final (<N>) frame 
iLinkB  the index of the link, in the indexChainN chain, being the final (<N>) frame for the Jacobian computation 
dirB  the 'direction' of the chain wrt the jacobian computation 
Definition at line 1209 of file iDynBody.cpp.

protected 
Reset all data to zero.
The list of limbs is not modified or deleted.
Definition at line 433 of file iDynBody.cpp.

protected 
COM position of the node.
Definition at line 558 of file iDynBody.h.

protected 
linear acceleration
Definition at line 552 of file iDynBody.h.

protected 
angular acceleration
Definition at line 550 of file iDynBody.h.

protected 
force
Definition at line 554 of file iDynBody.h.

protected 
info or useful notes
Definition at line 542 of file iDynBody.h.

protected 
total mass of the node
Definition at line 560 of file iDynBody.h.

protected 
STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.
Definition at line 539 of file iDynBody.h.

protected 
moment
Definition at line 556 of file iDynBody.h.

protected 
the list of RBT
Definition at line 536 of file iDynBody.h.

protected 
verbosity flag
Definition at line 545 of file iDynBody.h.

protected 
angular velocity
Definition at line 548 of file iDynBody.h.