iCub-main
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
iCub::iDyn::iCubUpperTorso Class Reference

A class for connecting head, left and right arm of the iCub, and exchanging kinematic and wrench information between limbs, when both arms have FT sensors and the head use the inertial sensor. More...

#include <iDynBody.h>

+ Inheritance diagram for iCub::iDyn::iCubUpperTorso:

Public Member Functions

 iCubUpperTorso (version_tag tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
 Constructor. More...
 
 ~iCubUpperTorso ()
 Destructor. More...
 
- Public Member Functions inherited from iCub::iDyn::iDynSensorTorsoNode
 iDynSensorTorsoNode (const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
 Constructor. More...
 
 iDynSensorTorsoNode (const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
 Constructor. More...
 
 ~iDynSensorTorsoNode ()
 Destructor. More...
 
bool update ()
 Main method for solving kinematics and wrench among limbs, where information are shared. More...
 
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. More...
 
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. More...
 
yarp::sig::Matrix getHLeft ()
 Return HLeft, i.e. More...
 
yarp::sig::Matrix getHRight ()
 Return HRight, i.e. More...
 
yarp::sig::Matrix getHUp ()
 Return HUp, i.e. More...
 
yarp::sig::Matrix getForces (const std::string &limbType)
 Return the chosen limb forces, as a 6xN matrix. More...
 
yarp::sig::Vector getForce (const std::string &limbType, const unsigned int iLink) const
 Return the chosen limb-link force, as a 3x1 vector. More...
 
yarp::sig::Matrix getMoments (const std::string &limbType)
 Return the chosen limb moments, as a 6xN matrix. More...
 
yarp::sig::Vector getMoment (const std::string &limbType, const unsigned int iLink) const
 Return the chosen limb-link moment, as a 3x1 vector. More...
 
yarp::sig::Vector getTorques (const std::string &limbType)
 Return the chosen limb torques, as a Nx1 vector. More...
 
double getTorque (const std::string &limbType, const unsigned int iLink) const
 Return the chosen limb-link torque, as a real value. More...
 
bool computeCOM ()
 Performs the computation of the center of mass (COM) of the node. More...
 
bool EXPERIMENTAL_computeCOMjacobian ()
 Performs the computation of the center of mass jacobian of the node. More...
 
yarp::sig::Vector getTorsoForce () const
 Return the torso force. More...
 
yarp::sig::Vector getTorsoMoment () const
 Return the torso moment. More...
 
yarp::sig::Vector getTorsoAngVel () const
 Return the torso angular velocity. More...
 
yarp::sig::Vector getTorsoAngAcc () const
 Return the torso angular acceleration. More...
 
yarp::sig::Vector getTorsoLinAcc () const
 Return the torso linear acceleration. More...
 
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. More...
 
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. More...
 
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. More...
 
yarp::sig::Vector setAng (const std::string &limbType, const yarp::sig::Vector &_q)
 Set joints angle position in the chosen limb. More...
 
yarp::sig::Vector getAng (const std::string &limbType)
 Get joints angle position in the chosen limb. More...
 
double setAng (const std::string &limbType, const unsigned int i, double _q)
 Set the i-th joint angle position in the chosen limb. More...
 
double getAng (const std::string &limbType, const unsigned int i)
 Get a joint angle position in the chosen limb. More...
 
yarp::sig::Vector setDAng (const std::string &limbType, const yarp::sig::Vector &_dq)
 Set joints angle velocity in the chosen limb. More...
 
yarp::sig::Vector getDAng (const std::string &limbType)
 Get joints angle velocity in the chosen limb. More...
 
double setDAng (const std::string &limbType, const unsigned int i, double _dq)
 Set the i-th joint angle velocity in the chosen limb. More...
 
double getDAng (const std::string &limbType, const unsigned int i)
 Get a joint angle velocity in the chosen limb. More...
 
yarp::sig::Vector setD2Ang (const std::string &limbType, const yarp::sig::Vector &_ddq)
 Set joints angle acceleration in the chosen limb. More...
 
yarp::sig::Vector getD2Ang (const std::string &limbType)
 Get joints angle acceleration in the chosen limb. More...
 
double setD2Ang (const std::string &limbType, const unsigned int i, double _ddq)
 Set the i-th joint angle acceleration in the chosen limb. More...
 
double getD2Ang (const std::string &limbType, const unsigned int i)
 Get a joint angle acceleration in the chosen limb. More...
 
unsigned int getNLinks (const std::string &limbType) const
 
yarp::sig::Matrix estimateSensorsWrench (const yarp::sig::Matrix &FM, bool afterAttach=false)
 Redefinition from iDynSensorNode. More...
 
void clearContactList ()
 
- Public Member Functions inherited from iCub::iDyn::iDynSensorNode
 iDynSensorNode (const NewEulMode _mode=DYNAMIC)
 Default constructor. More...
 
 iDynSensorNode (const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
 Constructor. More...
 
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. More...
 
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 after FT sensor measurements. More...
 
virtual bool solveWrench ()
 Main function 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, bool afterAttach=false)
 Set the Wrench measures on the limbs attached to the node. More...
 
virtual bool setWrenchMeasure (const yarp::sig::Matrix &FM, bool afterAttach=false)
 Set the Wrench measures on the limbs attached to the node. More...
 
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. More...
 
- Public Member Functions inherited from iCub::iDyn::iDynNode
 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...
 
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 i-th 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 end-effector, 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 end-effector, 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 i-th 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 build ()
 
- Protected Member Functions inherited from iCub::iDyn::iDynSensorNode
unsigned int howManySensors () const
 
- Protected Member Functions inherited from iCub::iDyn::iDynNode
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

version_tag tag
 Build the node. More...
 
- Protected Attributes inherited from iCub::iDyn::iDynSensorNode
std::deque< iDyn::iDynSensor * > sensorList
 the list of iDynSensors used to solve each limb after FT sensor measurements More...
 
- Protected Attributes inherited from iCub::iDyn::iDynNode
std::deque< RigidBodyTransformationrbtList
 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...
 

Additional Inherited Members

- Public Attributes inherited from iCub::iDyn::iDynSensorTorsoNode
yarp::sig::Matrix HUp
 roto-translational matrix defining the central-up base frame with respect to the torso node More...
 
yarp::sig::Matrix HLeft
 roto-translational matrix defining the left limb base frame with respect to the torso node More...
 
yarp::sig::Matrix HRight
 roto-translational matrix defining the right limb base frame with respect to the torso node More...
 
std::string left_name
 name of left limb More...
 
std::string right_name
 name of right limb More...
 
std::string up_name
 name of central-up limb More...
 
std::string name
 the torso node name More...
 
iDyn::iDynContactSolverleftSensor
 Build the node. More...
 
iDyn::iDynContactSolverrightSensor
 right FT sensor and solver More...
 
iDyn::iDynLimbleft
 left limb More...
 
iDyn::iDynLimbright
 right limb More...
 
iDyn::iDynLimbup
 central-up limb More...
 
yarp::sig::Vector total_COM_UP
 COMs and masses of the limbs. More...
 
yarp::sig::Vector total_COM_LF
 
yarp::sig::Vector total_COM_RT
 
double total_mass_UP
 
double total_mass_LF
 
double total_mass_RT
 
yarp::sig::Matrix COM_jacob_UP
 
yarp::sig::Matrix COM_jacob_LF
 
yarp::sig::Matrix COM_jacob_RT
 

Detailed Description

A class for connecting head, left and right arm of the iCub, and exchanging kinematic and wrench information between limbs, when both arms have FT sensors and the head use the inertial sensor.

Definition at line 1401 of file iDynBody.h.

Constructor & Destructor Documentation

◆ iCubUpperTorso()

iCubUpperTorso::iCubUpperTorso ( version_tag  tag,
const NewEulMode  _mode = DYNAMIC,
unsigned int  verb = iCub::skinDynLib::VERBOSE 
)

Constructor.

Parameters
_infosome information, ie the node name
_modethe computation mode for kinematic/wrench using Newton-Euler's formula
verbverbosity flag

Definition at line 2236 of file iDynBody.cpp.

◆ ~iCubUpperTorso()

iCubUpperTorso::~iCubUpperTorso ( )

Destructor.

Definition at line 2303 of file iDynBody.cpp.

Member Function Documentation

◆ build()

void iCubUpperTorso::build ( )
protected

Definition at line 2243 of file iDynBody.cpp.

Member Data Documentation

◆ tag

version_tag iCub::iDyn::iCubUpperTorso::tag
protected

Build the node.

Definition at line 1407 of file iDynBody.h.


The documentation for this class was generated from the following files: