A class for connecting a central-up limb, a left and right limb of the iCub, and exchanging kinematic and wrench information between limbs, when both left/right limb have FT sensors and the central-up one use the kinematic and wrench information coming from a inertial measurements or another iDynSensorNode. More...

#include <iDynBody.h>

Inheritance diagram for iCub::iDyn::iDynSensorTorsoNode:

Public Member Functions
	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...

Public Attributes
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::iDynContactSolver *	leftSensor
	Build the node. More...

iDyn::iDynContactSolver *	rightSensor
	right FT sensor and solver More...

iDyn::iDynLimb *	left
	left limb More...

iDyn::iDynLimb *	right
	right limb More...

iDyn::iDynLimb *	up
	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

Additional Inherited Members
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 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< 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...

Detailed Description

A class for connecting a central-up limb, a left and right limb of the iCub, and exchanging kinematic and wrench information between limbs, when both left/right limb have FT sensors and the central-up one use the kinematic and wrench information coming from a inertial measurements or another iDynSensorNode.

This is the base class of UpperTorso and LowerTorso. The connection between UpperTorso and LowerTorso is not handled here: it is simply supposed that a Torso Node receives correct kinematic and wrench input from outside.

Definition at line 1046 of file iDynBody.h.

Constructor & Destructor Documentation

◆ iDynSensorTorsoNode() [1/2]

iDynSensorTorsoNode::iDynSensorTorsoNode	(	const NewEulMode	_mode = `DYNAMIC`,
		unsigned int	verb = `iCub::skinDynLib::VERBOSE`
	)

Constructor.

Parameters

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

Definition at line 1729 of file iDynBody.cpp.

◆ iDynSensorTorsoNode() [2/2]

iDynSensorTorsoNode::iDynSensorTorsoNode	(	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 1735 of file iDynBody.cpp.

◆ ~iDynSensorTorsoNode()

iDynSensorTorsoNode::~iDynSensorTorsoNode ( )

Destructor.

Definition at line 1741 of file iDynBody.cpp.

Member Function Documentation

◆ clearContactList()

void iDynSensorTorsoNode::clearContactList ( )

Definition at line 2220 of file iDynBody.cpp.

◆ computeCOM()

bool iDynSensorTorsoNode::computeCOM ( )

Performs the computation of the center of mass (COM) of the node.

Returns: true if succeeds, false otherwise

Definition at line 1912 of file iDynBody.cpp.

◆ estimateSensorsWrench()

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

inline

Redefinition from iDynSensorNode.

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

Parameters

FM	a (6xN) matrix of forces/moments where N is the number of external wrenches for the N limbs of the node

Returns: a (6xM), matrix with estimated wrench for the M sensors

Definition at line 1383 of file iDynBody.h.

◆ EXPERIMENTAL_computeCOMjacobian()

bool iDynSensorTorsoNode::EXPERIMENTAL_computeCOMjacobian ( )

Performs the computation of the center of mass jacobian of the node.

Returns: true if succeeds, false otherwise

Definition at line 1970 of file iDynBody.cpp.

◆ getAng() [1/2]

Vector iDynSensorTorsoNode::getAng ( const std::string & limbType )

Get joints angle position in the chosen limb.

Parameters

limbType a string with limb type

Returns: the joint angles

Definition at line 2076 of file iDynBody.cpp.

◆ getAng() [2/2]

double iDynSensorTorsoNode::getAng	(	const std::string &	limbType,
		const unsigned int	i
	)

Get a joint angle position in the chosen limb.

Parameters

limbType	a string with limb type
i	the link index in the limb

Returns: the joint angle

Definition at line 2100 of file iDynBody.cpp.

◆ getD2Ang() [1/2]

Vector iDynSensorTorsoNode::getD2Ang ( const std::string & limbType )

Get joints angle acceleration in the chosen limb.

Parameters

limbType a string with limb type

Returns: the joint acceleration

Definition at line 2172 of file iDynBody.cpp.

◆ getD2Ang() [2/2]

double iDynSensorTorsoNode::getD2Ang	(	const std::string &	limbType,
		const unsigned int	i
	)

Get a joint angle acceleration in the chosen limb.

Parameters

limbType	a string with limb type
i	the link index in the limb

Returns: the joint acceleration

Definition at line 2196 of file iDynBody.cpp.

◆ getDAng() [1/2]

Vector iDynSensorTorsoNode::getDAng ( const std::string & limbType )

Get joints angle velocity in the chosen limb.

Parameters

limbType a string with limb type

Returns: the joint velocity

Definition at line 2124 of file iDynBody.cpp.

◆ getDAng() [2/2]

double iDynSensorTorsoNode::getDAng	(	const std::string &	limbType,
		const unsigned int	i
	)

Get a joint angle velocity in the chosen limb.

Parameters

limbType	a string with limb type
i	the link index in the limb

Returns: the joint velocity

Definition at line 2148 of file iDynBody.cpp.

◆ getForce()

yarp::sig::Vector iCub::iDyn::iDynSensorTorsoNode::getForce	(	const std::string &	limbType,
		const unsigned int	iLink
	)		const

Return the chosen limb-link force, as a 3x1 vector.

Parameters

limbType	a string with the limb name
iLink	the link index in the limb

Returns: the chosen limb-link force

◆ getForces()

Matrix iDynSensorTorsoNode::getForces ( const std::string & limbType )

Return the chosen limb forces, as a 6xN matrix.

Parameters

limbType a string with the limb name

Returns: the chosen limb forces

Definition at line 1866 of file iDynBody.cpp.

◆ getHLeft()

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::getHLeft ( )

inline

Return HLeft, i.e.

the roto-translation matrix between the 'left' limb and the node.

Definition at line 1151 of file iDynBody.h.

◆ getHRight()

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::getHRight ( )

inline

Return HRight, i.e.

the roto-translation matrix between the 'right' limb and the node.

Definition at line 1155 of file iDynBody.h.

◆ getHUp()

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::getHUp ( )

inline

Return HUp, i.e.

the roto-translation matrix between the 'up' limb and the node.

Definition at line 1159 of file iDynBody.h.

◆ getMoment()

yarp::sig::Vector iCub::iDyn::iDynSensorTorsoNode::getMoment	(	const std::string &	limbType,
		const unsigned int	iLink
	)		const

Return the chosen limb-link moment, as a 3x1 vector.

Parameters

limbType	a string with the limb name
iLink	the link index in the limb

Returns: the chosen limb-link moment

◆ getMoments()

Matrix iDynSensorTorsoNode::getMoments ( const std::string & limbType )

Return the chosen limb moments, as a 6xN matrix.

Parameters

limbType a string with the limb name

Returns: the chosen limb moments

Definition at line 1878 of file iDynBody.cpp.

◆ getNLinks()

unsigned int iDynSensorTorsoNode::getNLinks ( const std::string & limbType ) const

Parameters

limbType a string with the limb name

Returns: the number of links of the chosen limb

Definition at line 2208 of file iDynBody.cpp.

◆ getTorque()

double iCub::iDyn::iDynSensorTorsoNode::getTorque	(	const std::string &	limbType,
		const unsigned int	iLink
	)		const

Return the chosen limb-link torque, as a real value.

Parameters

limbType	a string with the limb name
iLink	the link index in the limb

Returns: the chosen limb-link torque

◆ getTorques()

Vector iDynSensorTorsoNode::getTorques ( const std::string & limbType )

Return the chosen limb torques, as a Nx1 vector.

Parameters

limbType a string with the limb name

Returns: the chosen limb torques

Definition at line 1890 of file iDynBody.cpp.

◆ getTorsoAngAcc()

Vector iDynSensorTorsoNode::getTorsoAngAcc ( ) const

Return the torso angular acceleration.

Returns: the torso angular acceleration

Definition at line 1908 of file iDynBody.cpp.

◆ getTorsoAngVel()

Vector iDynSensorTorsoNode::getTorsoAngVel ( ) const

Return the torso angular velocity.

Returns: the torso angular velocity

Definition at line 1906 of file iDynBody.cpp.

◆ getTorsoForce()

Vector iDynSensorTorsoNode::getTorsoForce ( ) const

Return the torso force.

Returns: the torso force

Definition at line 1902 of file iDynBody.cpp.

◆ getTorsoLinAcc()

Vector iDynSensorTorsoNode::getTorsoLinAcc ( ) const

Return the torso linear acceleration.

Returns: the torso linear acceleration

Definition at line 1910 of file iDynBody.cpp.

◆ getTorsoMoment()

Vector iDynSensorTorsoNode::getTorsoMoment ( ) const

Return the torso moment.

Returns: the torso moment

Definition at line 1904 of file iDynBody.cpp.

◆ setAng() [1/2]

double iDynSensorTorsoNode::setAng	(	const std::string &	limbType,
		const unsigned int	i,
		double	_q
	)

Set the i-th joint angle position in the chosen limb.

Parameters

_q	the joint position
i	the link index in the limb
limbType	a string with limb type

Returns: the effective joint angle, considering min/max values

Definition at line 2088 of file iDynBody.cpp.

◆ setAng() [2/2]

yarp::sig::Vector iCub::iDyn::iDynSensorTorsoNode::setAng	(	const std::string &	limbType,
		const yarp::sig::Vector &	_q
	)

Set joints angle position in the chosen limb.

Parameters

_q	the joints position
limbType	a string with limb type

Returns: the effective joint angles, considering min/max values

◆ setD2Ang() [1/2]

double iDynSensorTorsoNode::setD2Ang	(	const std::string &	limbType,
		const unsigned int	i,
		double	_ddq
	)

Set the i-th joint angle acceleration in the chosen limb.

Parameters

_ddq	the joint acceleration
i	the link index in the limb
limbType	a string with limb type

Returns: the effective joint acceleration

Definition at line 2184 of file iDynBody.cpp.

◆ setD2Ang() [2/2]

yarp::sig::Vector iCub::iDyn::iDynSensorTorsoNode::setD2Ang	(	const std::string &	limbType,
		const yarp::sig::Vector &	_ddq
	)

Set joints angle acceleration in the chosen limb.

Parameters

_dq	the joints acceleration
limbType	a string with limb type

Returns: the effective joint acceleration

◆ setDAng() [1/2]

double iDynSensorTorsoNode::setDAng	(	const std::string &	limbType,
		const unsigned int	i,
		double	_dq
	)

Set the i-th joint angle velocity in the chosen limb.

Parameters

_dq	the joint velocity
i	the link index in the limb
limbType	a string with limb type

Returns: the effective joint velocity

Definition at line 2136 of file iDynBody.cpp.

◆ setDAng() [2/2]

yarp::sig::Vector iCub::iDyn::iDynSensorTorsoNode::setDAng	(	const std::string &	limbType,
		const yarp::sig::Vector &	_dq
	)

Set joints angle velocity in the chosen limb.

Parameters

_dq	the joints velocity
limbType	a string with limb type

Returns: the effective joint velocity

◆ setInertialMeasure()

bool iDynSensorTorsoNode::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.

Parameters

w0	a 3x1 vector with the initial/measured angular velocity
dw0	a 3x1 vector with the initial/measured angular acceleration
ddp0	a 3x1 vector with the initial/measured linear acceleration

Returns: true if succeeds (correct vectors size), false otherwise

Definition at line 1753 of file iDynBody.cpp.

◆ setSensorMeasurement() [1/2]

bool iCub::iDyn::iDynSensorTorsoNode::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.

This operation is necessary to initialize the wrench phase correctly. The central-up limb wrench is assumed to be already initialized: for example after an attachTorso().

Parameters

FM_right	a 6x1 vector with forces and moments measured by the FT sensor in the right limb
FM_left	a 6x1 vector with forces and moments measured by the FT sensor in the left limb

Returns: true if succeeds, false otherwise

◆ setSensorMeasurement() [2/2]

bool iCub::iDyn::iDynSensorTorsoNode::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.

This operation is necessary to initialize the wrench phase correctly. The central-up limb wrench initializing wrench is also specified.

Parameters

FM_right	a 6x1 vector with forces and moments measured by the FT sensor in the right limb
FM_left	a 6x1 vector with forces and moments measured by the FT sensor in the left limb
FM_up	a 6x1 vector with forces and moments initializing the central-up limb

Returns: true if succeeds, false otherwise

◆ update() [1/3]

bool iDynSensorTorsoNode::update ( )

Main method for solving kinematics and wrench among limbs, where information are shared.

Returns: true if kinematics and wrench phases succeed, false otherwise

Definition at line 1794 of file iDynBody.cpp.

◆ update() [2/3]

bool iCub::iDyn::iDynSensorTorsoNode::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.

This method assumes that the initial kinematics informations have already been set, as the external wrench on the central limb: i.e. this method is the one called after attachTorso().

Parameters

FM_right	a 6x1 vector with forces and moments measured by the FT sensor in the right limb
FM_left	a 6x1 vector with forces and moments measured by the FT sensor in the left limb

Returns: true if succeeds, false otherwise

◆ update() [3/3]

bool iCub::iDyn::iDynSensorTorsoNode::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.

Parameters

w0	a 3x1 vector with the initial/measured angular velocity
dw0	a 3x1 vector with the initial/measured angular acceleration
ddp0	a 3x1 vector with the initial/measured linear acceleration
FM_right	a 6x1 vector with forces and moments measured by the FT sensor in the right limb
FM_left	a 6x1 vector with forces and moments measured by the FT sensor in the left limb
FM_up	a 6x1 vector with forces and moments initializing the central limb

Returns: true if succeeds, false otherwise

Member Data Documentation

◆ COM_jacob_LF

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::COM_jacob_LF

Definition at line 1093 of file iDynBody.h.

◆ COM_jacob_RT

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::COM_jacob_RT

Definition at line 1094 of file iDynBody.h.

◆ COM_jacob_UP

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::COM_jacob_UP

Definition at line 1092 of file iDynBody.h.

◆ HLeft

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::HLeft

roto-translational matrix defining the left limb base frame with respect to the torso node

Definition at line 1053 of file iDynBody.h.

◆ HRight

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::HRight

roto-translational matrix defining the right limb base frame with respect to the torso node

Definition at line 1055 of file iDynBody.h.

◆ HUp

yarp::sig::Matrix iCub::iDyn::iDynSensorTorsoNode::HUp

roto-translational matrix defining the central-up base frame with respect to the torso node

Definition at line 1051 of file iDynBody.h.

◆ left

iDyn::iDynLimb* iCub::iDyn::iDynSensorTorsoNode::left

left limb

Definition at line 1079 of file iDynBody.h.

◆ left_name

std::string iCub::iDyn::iDynSensorTorsoNode::left_name

name of left limb

Definition at line 1058 of file iDynBody.h.

◆ leftSensor

iDyn::iDynContactSolver* iCub::iDyn::iDynSensorTorsoNode::leftSensor

Build the node.

left FT sensor and solver

Definition at line 1073 of file iDynBody.h.

◆ name

std::string iCub::iDyn::iDynSensorTorsoNode::name

the torso node name

Definition at line 1064 of file iDynBody.h.

◆ right

iDyn::iDynLimb* iCub::iDyn::iDynSensorTorsoNode::right

right limb

Definition at line 1081 of file iDynBody.h.

◆ right_name

std::string iCub::iDyn::iDynSensorTorsoNode::right_name

name of right limb

Definition at line 1060 of file iDynBody.h.

◆ rightSensor

iDyn::iDynContactSolver* iCub::iDyn::iDynSensorTorsoNode::rightSensor

right FT sensor and solver

Definition at line 1075 of file iDynBody.h.

◆ total_COM_LF

yarp::sig::Vector iCub::iDyn::iDynSensorTorsoNode::total_COM_LF

Definition at line 1087 of file iDynBody.h.

◆ total_COM_RT

yarp::sig::Vector iCub::iDyn::iDynSensorTorsoNode::total_COM_RT

Definition at line 1088 of file iDynBody.h.

◆ total_COM_UP

yarp::sig::Vector iCub::iDyn::iDynSensorTorsoNode::total_COM_UP

COMs and masses of the limbs.

Definition at line 1086 of file iDynBody.h.

◆ total_mass_LF

double iCub::iDyn::iDynSensorTorsoNode::total_mass_LF

Definition at line 1090 of file iDynBody.h.

◆ total_mass_RT

double iCub::iDyn::iDynSensorTorsoNode::total_mass_RT

Definition at line 1091 of file iDynBody.h.

◆ total_mass_UP

double iCub::iDyn::iDynSensorTorsoNode::total_mass_UP

Definition at line 1089 of file iDynBody.h.

◆ up

iDyn::iDynLimb* iCub::iDyn::iDynSensorTorsoNode::up

central-up limb

Definition at line 1083 of file iDynBody.h.

◆ up_name

std::string iCub::iDyn::iDynSensorTorsoNode::up_name

name of central-up limb

Definition at line 1062 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

Public Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ iDynSensorTorsoNode() [1/2]

◆ iDynSensorTorsoNode() [2/2]

◆ ~iDynSensorTorsoNode()

Member Function Documentation

◆ clearContactList()

◆ computeCOM()

◆ estimateSensorsWrench()

◆ EXPERIMENTAL_computeCOMjacobian()

◆ getAng() [1/2]

◆ getAng() [2/2]

◆ getD2Ang() [1/2]

◆ getD2Ang() [2/2]

◆ getDAng() [1/2]

◆ getDAng() [2/2]

◆ getForce()

◆ getForces()

◆ getHLeft()

◆ getHRight()

◆ getHUp()

◆ getMoment()

◆ getMoments()

◆ getNLinks()

◆ getTorque()

◆ getTorques()

◆ getTorsoAngAcc()

◆ getTorsoAngVel()

◆ getTorsoForce()

◆ getTorsoLinAcc()

◆ getTorsoMoment()

◆ setAng() [1/2]

◆ setAng() [2/2]

◆ setD2Ang() [1/2]

◆ setD2Ang() [2/2]

◆ setDAng() [1/2]

◆ setDAng() [2/2]

◆ setInertialMeasure()

◆ setSensorMeasurement() [1/2]

◆ setSensorMeasurement() [2/2]

◆ update() [1/3]

◆ update() [2/3]

◆ update() [3/3]

Member Data Documentation

◆ COM_jacob_LF

◆ COM_jacob_RT

◆ COM_jacob_UP

◆ HLeft

◆ HRight

◆ HUp

◆ left

◆ left_name

◆ leftSensor

◆ name

◆ right

◆ right_name

◆ rightSensor

◆ total_COM_LF

◆ total_COM_RT

◆ total_COM_UP

◆ total_mass_LF

◆ total_mass_RT

◆ total_mass_UP

◆ up

◆ up_name