A class for connecting two or mutiple limbs and exchanging kinematic and wrench information between limbs, when one or multiple limbs have FT sensors. More...

#include <iDynBody.h>

Inheritance diagram for iCub::iDyn::iDynSensorNode:

Public Member Functions
	iDynSensorNode (const NewEulMode _mode=DYNAMIC)
	Default constructor.

	iDynSensorNode (const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
	Constructor.

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.

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.

virtual bool	solveWrench ()
	Main function to manage the exchange of wrench information among the limbs attached to the node.

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.

virtual bool	setWrenchMeasure (const yarp::sig::Matrix &FM, 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.

Public Member Functions inherited from iCub::iDyn::iDynNode
	iDynNode (const NewEulMode _mode=DYNAMIC)
	Default constructor.

	iDynNode (const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE)
	Constructor with parameters.

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.

yarp::sig::Matrix	getRBT (unsigned int iLimb) const
	Return the RBT matrix of a certain limb attached to the node.

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.

bool	solveKinematics ()
	Main function to manage the exchange of kinematic information among the limbs attached to the node.

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.

bool	solveWrench (const yarp::sig::Matrix &FM)
	This is to manage the exchange of wrench information among the limbs attached to the node.

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.

virtual bool	setWrenchMeasure (const yarp::sig::Matrix &F, const yarp::sig::Matrix &M)
	Set the wrench measure on the limbs with input wrench.

virtual bool	setWrenchMeasure (const yarp::sig::Matrix &FM)
	Set the wrench measure on the limbs with input wrench.

yarp::sig::Vector	getForce () const
	Return the node force.

yarp::sig::Vector	getMoment () const
	Return the node moment.

yarp::sig::Vector	getAngVel () const
	Return the node angular velocity.

yarp::sig::Vector	getAngAcc () const
	Return the node angular acceleration.

yarp::sig::Vector	getLinAcc () const
	Return the node linear acceleration.

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).

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).

yarp::sig::Matrix	computeJacobian (unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB)
	Compute the Jacobian between two links in two different chains.

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.

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.

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.

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.

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.

Protected Member Functions
unsigned int	howManySensors () const

Protected Member Functions inherited from iCub::iDyn::iDynNode
void	zero ()
	Reset all data to zero.

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.

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.

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.

unsigned int	howManyWrenchInputs (bool afterAttach=false) const
	Return the number of limbs with wrench input, i.e.

unsigned int	howManyKinematicInputs (bool afterAttach=false) const
	Return the number of limbs with kinematic input, i.e.

Protected Attributes
std::deque< iDyn::iDynSensor * >	sensorList
	the list of iDynSensors used to solve each limb after FT sensor measurements

Protected Attributes inherited from iCub::iDyn::iDynNode
std::deque< RigidBodyTransformation >	rbtList
	the list of RBT

NewEulMode	mode
	STATIC/DYNAMIC/DYNAMIC_W_ROTOR/DYNAMIC_CORIOLIS_GRAVITY.

std::string	info
	info or useful notes

unsigned int	verbose
	verbosity flag

yarp::sig::Vector	w
	angular velocity

yarp::sig::Vector	dw
	angular acceleration

yarp::sig::Vector	ddp
	linear acceleration

yarp::sig::Vector	F
	force

yarp::sig::Vector	Mu
	moment

yarp::sig::Vector	COM
	COM position of the node.

double	mass
	total mass of the node

Detailed Description

A class for connecting two or mutiple limbs and exchanging kinematic and wrench information between limbs, when one or multiple limbs have FT sensors.

Definition at line 916 of file iDynBody.h.

Constructor & Destructor Documentation

◆ iDynSensorNode() [1/2]

iDynSensorNode::iDynSensorNode ( const NewEulMode _mode = DYNAMIC )

Default constructor.

Parameters

_mode the computation mode for kinematic/wrench using Newton-Euler's formula

Definition at line 1327 of file iDynBody.cpp.

◆ iDynSensorNode() [2/2]

iDynSensorNode::iDynSensorNode	(	const std::string &	_info,
		const NewEulMode	_mode = `DYNAMIC`,
		unsigned int	verb = `iCub::skinDynLib::VERBOSE`
	)

Constructor.

Parameters

_info	some information, ie the node name
_mode	the computation mode for kinematic/wrench using Newton-Euler's formula
verb	verbosity flag

Definition at line 1333 of file iDynBody.cpp.

Member Function Documentation

◆ addLimb() [1/2]

virtual void iCub::iDyn::iDynSensorNode::addLimb	(	iDyn::iDynLimb *	limb,
		const yarp::sig::Matrix &	H,
		const FlowType	kinFlow = `RBT_NODE_OUT`,
		const FlowType	wreFlow = `RBT_NODE_IN`
	)

virtual

Add one limb to the node, defining its RigidBodyTransformation.

A new RigidBodyTransformation is added to the RBT list. Since there's not an iDynSensor for this limb, it is set to NULL in the sensorList.

Parameters

limb	pointer to generic limb
H	a (4x4) roto-translational 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

◆ addLimb() [2/2]

void iCub::iDyn::iDynSensorNode::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.

A new RigidBodyTransformation is added to the RBT list. The iDynSensor is added to the sensorList.

Parameters

limb	pointer to generic limb
H	a (4x4) roto-translational matrix defining the transformation between node and limb base/end
sensor	pointer to iDynSensor of the limb
kinFlow	the type of information flow of kinematics variables
wreFlow	the type of information flow of wrench variables

◆ estimateSensorsWrench()

Matrix iDynSensorNode::estimateSensorsWrench	(	const yarp::sig::Matrix &	FM,
		bool	afterAttach = `false`
	)

Exploit iDynInvSensor methods to retrieve FT sensor measurements after solving wrenches in the limbs.

The parameter FM is a (6xN), where each column is an external wrench to be used for initializing the wrench phase; N is the number of limbs Nlimbs attached to the node. The boolean flag is used in case the external wrench on the first limb has already been set; this is useful whenever two different nodes are connected and share information: the first node sends kinematic and wrench information to the 'first' limb of the second node (eg the torso). In that case the FM matrix should only contain external wrench for the other limbs, so it should be a (6x(Nlimbs-1)).

Parameters

FM	a (6xN) matrix with the external wrenches,where N is the number of limbs if afterAttach=false, number of limbs -1 if afterAttach=true
afterAttach	a flag for specifying if the external wrench of the first limb has been already set or not

Definition at line 1562 of file iDynBody.cpp.

◆ howManySensors()

unsigned int iDynSensorNode::howManySensors ( ) const

protected

Returns: the number of limbs with sensor

Definition at line 1707 of file iDynBody.cpp.

◆ setWrenchMeasure() [1/2]

virtual bool iCub::iDyn::iDynSensorNode::setWrenchMeasure	(	const yarp::sig::Matrix &	F,
		const yarp::sig::Matrix &	M,
		bool	afterAttach = `false`
	)

virtual

Set the Wrench measures on the limbs attached to the node.

The parameters F and M are (3xN) matrices, where each column is an external wrench to be used for initializing the wrench phase; N is the number of limbs Nlimbs attached to the node. The boolean flag is used in case the external wrench on the first limb has already been set; this is useful whenever two different nodes are connected and share information: the first node sends kinematic and wrench information to the 'first' limb of the second node (eg the torso). In that case the F and M matrices should only contain external wrench for the other limbs, so it should be a (6x(Nlimbs-1)).

Parameters

F	a (3xN) matrix with forces, where N is the number of limbs if afterAttach=false, number of limbs -1 if afterAttach=true
M	a (3xN) matrix with moments , where N is the number of limbs if afterAttach=false, number of limbs -1 if afterAttach=true
afterAttach	a flag for specifying if the external wrench of the first limb has been already set or not

Returns: true if succeeds, false otherwise

◆ setWrenchMeasure() [2/2]

virtual bool iCub::iDyn::iDynSensorNode::setWrenchMeasure	(	const yarp::sig::Matrix &	FM,
		bool	afterAttach = `false`
	)

virtual

Set the Wrench measures on the limbs attached to the node.

The parameter FM is a (6xN), where each column is an external wrench to be used for initializing the wrench phase; N is the number of limbs Nlimbs attached to the node. The boolean flag is used in case the external wrench on the first limb has already been set; this is useful whenever two different nodes are connected and share information: the first node sends kinematic and wrench information to the 'first' limb of the second node (eg the torso). In that case the FM matrix should only contain external wrench for the other limbs, so it should be a (6x(Nlimbs-1)).

Parameters

FM	a (6xN) matrix with the external wrenches, where N is the number of limbs if afterAttach=false, number of limbs -1 if afterAttach=true
afterAttach	a flag for specifying if the external wrench of the first limb has been already set or not

Returns: true if succeeds, false otherwise

◆ solveWrench()

bool iDynSensorNode::solveWrench ( )

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 end-effector or at the base, and calling recursive wrench computation.

Returns: true if succeeds, false otherwise

Reimplemented from iCub::iDyn::iDynNode.

Definition at line 1352 of file iDynBody.cpp.

Member Data Documentation

◆ sensorList

std::deque<iDyn::iDynSensor *> iCub::iDyn::iDynSensorNode::sensorList

protected

the list of iDynSensors used to solve each limb after FT sensor measurements

Definition at line 922 of file iDynBody.h.

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

icub-main/src/libraries/iDyn/include/iCub/iDyn/iDynBody.h
icub-main/src/libraries/iDyn/src/iDynBody.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ iDynSensorNode() [1/2]

◆ iDynSensorNode() [2/2]

Member Function Documentation

◆ addLimb() [1/2]

◆ addLimb() [2/2]

◆ estimateSensorsWrench()

◆ howManySensors()

◆ setWrenchMeasure() [1/2]

◆ setWrenchMeasure() [2/2]

◆ solveWrench()

Member Data Documentation

◆ sensorList