A class for computing forces and torques in a iDynChain, when a force/torque sensor is placed in the middle of the kinematic chain and it is the only available sensor for measuring forces and moments; the sensor position in the chain must be set; the computation of joint forces, moments and torques is performed by an Inverse Newton-Euler method. More...

#include <iDynInv.h>

Inheritance diagram for iCub::iDyn::iDynSensor:

Public Member Functions
	iDynSensor (iDyn::iDynChain *_c, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
	Constructor without FT sensor: the sensor must be set with setSensor()

	iDynSensor (iDyn::iDynChain *_c, unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I, std::string _info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
	Constructor with FT sensor.

bool	setSensorMeasures (const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
	Set the sensor measured force and moment.

bool	setSensorMeasures (const yarp::sig::Vector &FM)
	Set the sensor measured force and moment at once.

virtual bool	computeFromSensorNewtonEuler (const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
	The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain.

virtual bool	computeFromSensorNewtonEuler (const yarp::sig::Vector &FMu)
	The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain.

virtual void	computeFromSensorNewtonEuler ()
	The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain.

virtual void	computeWrenchFromSensorNewtonEuler ()
	The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain.

yarp::sig::Matrix	getForces () const
	Returns the links forces as a matrix, where the i-th col is the i-th force.

yarp::sig::Matrix	getMoments () const
	Returns the links moments as a matrix, where the i-th col is the i-th moment.

yarp::sig::Vector	getTorques () const
	Returns the links torque as a vector.

yarp::sig::Vector	getForce (const unsigned int iLink) const
	Returns the i-th link force.

yarp::sig::Vector	getMoment (const unsigned int iLink) const
	Returns the i-th link moment.

double	getTorque (const unsigned int iLink) const
	Returns the i-th link torque.

yarp::sig::Matrix	getForcesNewtonEuler () const
	Returns the links forces as a matrix, where the i-th col is the i-th force.

yarp::sig::Matrix	getMomentsNewtonEuler () const
	Returns the links moments as a matrix, where the i-th col is the i-th moment.

yarp::sig::Vector	getTorquesNewtonEuler () const
	Returns the links torque as a vector.

virtual yarp::sig::Vector	getForceMomentEndEff () const
	Returns the end-effector force-moment as a single (6x1) vector.

Public Member Functions inherited from iCub::iDyn::iDynInvSensor
	iDynInvSensor (iDyn::iDynChain *_c, const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
	Constructor without FT sensor: the sensor must be set with setSensor()

	iDynInvSensor (iDyn::iDynChain *_c, unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I, const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=0)
	Constructor with FT sensor.

bool	setSensor (unsigned int i, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, const double _m, const yarp::sig::Matrix &_I)
	Set a new sensor or new sensor properties.

bool	setSensor (unsigned int i, SensorLinkNewtonEuler *sensor)

void	computeSensorForceMoment ()
	Compute forces and moments at the sensor frame; this method calls special Forward and Backward methods of SensorLink, using Newton-Euler's formula applied in the link where the sensor is placed on; the link is automatically found, being specified by the index in the chain and the chain itself; The case of a contact (ie external force) acting in the host link is not currently implemented.

std::string	toString () const
	Print some information.

yarp::sig::Vector	getSensorForce () const
	Returns the sensor estimated force.

yarp::sig::Vector	getSensorMoment () const
	Returns the sensor estimated moment.

yarp::sig::Vector	getSensorForceMoment () const
	Get the sensor force and moment in a single (6x1) vector.

yarp::sig::Matrix	getH () const
	Get the sensor roto-translational matrix defining its position/orientation wrt the link.

double	getMass () const
	Get the mass of the portion of link defined between sensor and i-th frame.

yarp::sig::Matrix	getCOM () const
	Get the sensor roto-traslational matrix of the center of mass of the semi-link defined by the sensor in the i-th link.

yarp::sig::Matrix	getInertia () const
	Get the inertia of the portion of link defined between sensor and i-th frame.

void	setMode (const NewEulMode _mode=DYNAMIC)

void	setVerbose (unsigned int verb=iCub::skinDynLib::VERBOSE)

void	setInfo (const std::string &_info)

void	setSensorInfo (const std::string &_info)

bool	setDynamicParameters (const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I)
	Set the dynamic parameters of the the portion of link defined between sensor and i-th frame.

std::string	getInfo () const

std::string	getSensorInfo () const

unsigned int	getSensorLink () const

yarp::sig::Vector	getTorques () const

virtual	~iDynInvSensor ()

Additional Inherited Members
Protected Attributes inherited from iCub::iDyn::iDynInvSensor
unsigned int	lSens
	the link where the sensor is attached to

SensorLinkNewtonEuler *	sens
	the sensor

iDynChain *	chain
	the iDynChain describing the robotic chain

NewEulMode	mode
	static/dynamic/etc..

unsigned int	verbose
	verbosity flag

std::string	info
	a string with useful information if needed

Detailed Description

A class for computing forces and torques in a iDynChain, when a force/torque sensor is placed in the middle of the kinematic chain and it is the only available sensor for measuring forces and moments; the sensor position in the chain must be set; the computation of joint forces, moments and torques is performed by an Inverse Newton-Euler method.

Definition at line 1577 of file iDynInv.h.

Constructor & Destructor Documentation

◆ iDynSensor() [1/2]

iDynSensor::iDynSensor	(	iDyn::iDynChain *	_c,
		std::string	_info,
		const NewEulMode	_mode = `DYNAMIC`,
		unsigned int	verb = `iCub::skinDynLib::NO_VERBOSE`
	)

Constructor without FT sensor: the sensor must be set with setSensor()

Parameters

_c	a pointer to the iDynChain where the sensor is placed on
_info	a string with information
_mode	the analysis mode (static/dynamic)
verb	flag for verbosity

Definition at line 2574 of file iDynInv.cpp.

◆ iDynSensor() [2/2]

iCub::iDyn::iDynSensor::iDynSensor	(	iDyn::iDynChain *	_c,
		unsigned int	i,
		const yarp::sig::Matrix &	_H,
		const yarp::sig::Matrix &	_HC,
		const double	_m,
		const yarp::sig::Matrix &	_I,
		std::string	_info,
		const NewEulMode	_mode = `DYNAMIC`,
		unsigned int	verb = `iCub::skinDynLib::NO_VERBOSE`
	)

Constructor with FT sensor.

Parameters

_c	a pointer to the iDynChain where the sensor is placed on
i	the i-th link to whom the sensor is attached
_H	the roto-traslational matrix from the reference frame of the i-th link to the sensor
_HC	the roto-traslational matrix of the center of mass of the semi-link defined by the sensor in the i-th link
_m	the mass of the semi-link
_I	the inertia of the semi-link
_info	a string with information
_mode	the analysis mode (static/dynamic)
verb	flag for verbosity

Member Function Documentation

◆ computeFromSensorNewtonEuler() [1/3]

void iDynSensor::computeFromSensorNewtonEuler ( )

virtual

The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain.

A forward pass of the classical Newton-Euler method is run, to retrieve angular and linear accelerations. Then, from sensor to end-effector the inverse Newton-Euler formula is applied to retrieve joint forces and torques, while from sensor to base the classical backward pass is run. This method only perform the computations: the force and moment measured on the sensor must be set before calling this method using setSensorMeasures()

Definition at line 2611 of file iDynInv.cpp.

◆ computeFromSensorNewtonEuler() [2/3]

virtual bool iCub::iDyn::iDynSensor::computeFromSensorNewtonEuler	(	const yarp::sig::Vector &	F,
		const yarp::sig::Vector &	Mu
	)

virtual

The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain.

A forward pass of the classical Newton-Euler method is run, to retrieve angular and linear accelerations. Then, from sensor to end-effector the inverse Newton-Euler formula is applied to retrieve joint forces and torques, while from sensor to base the classical backward pass is run.

Parameters

F	the sensor force (3x1)
Mu	the sensor moment (3x1)

Returns: true if the operation is successful, false otherwise (ie wrong vector size)

◆ computeFromSensorNewtonEuler() [3/3]

virtual bool iCub::iDyn::iDynSensor::computeFromSensorNewtonEuler ( const yarp::sig::Vector & FMu )

virtual

The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain.

A forward pass of the classical Newton-Euler method is run, to retrieve angular and linear accelerations. Then, from sensor to end-effector the inverse Newton-Euler formula is applied to retrieve joint forces and torques, while from sensor to base the classical backward pass is run.

Parameters

FMu	the sensor force and moment (6x1)

Returns: true if the operation is successful, false otherwise (ie wrong vector size)

◆ computeWrenchFromSensorNewtonEuler()

void iDynSensor::computeWrenchFromSensorNewtonEuler ( )

virtual

The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain.

The kinematic pass is already performed. Only the wrench computation are performed here: from sensor to end-effector the inverse Newton-Euler formula is applied to retrieve joint forces and torques, while from sensor to base the classical backward pass is run. This method only perform the computations: the force and moment measured on the sensor must be set before calling this method using setSensorMeasures() This method is called by iDynSensorNode.

Reimplemented in iCub::iDyn::iDynContactSolver.

Definition at line 2634 of file iDynInv.cpp.

◆ getForce()

Vector iDynSensor::getForce ( const unsigned int iLink ) const

Returns the i-th link force.

Returns: the i-th link force

Definition at line 2684 of file iDynInv.cpp.

◆ getForceMomentEndEff()

Vector iDynSensor::getForceMomentEndEff ( ) const

virtual

Returns the end-effector force-moment as a single (6x1) vector.

Returns: a 6x1 vector with the the end-effector force-moment

Reimplemented in iCub::iDyn::iDynContactSolver.

Definition at line 2696 of file iDynInv.cpp.

◆ getForces()

Matrix iDynSensor::getForces ( ) const

Returns the links forces as a matrix, where the i-th col is the i-th force.

Returns: a 3xN matrix with forces, in the form: i-th col = F_i

Definition at line 2678 of file iDynInv.cpp.

◆ getForcesNewtonEuler()

Matrix iDynSensor::getForcesNewtonEuler ( ) const

Returns the links forces as a matrix, where the i-th col is the i-th force.

Returns: a 3x(N+2) matrix with forces, in the form: i-th col = F_i

Definition at line 2690 of file iDynInv.cpp.

◆ getMoment()

Vector iDynSensor::getMoment ( const unsigned int iLink ) const

Returns the i-th link moment.

Returns: the i-th link moment

Definition at line 2686 of file iDynInv.cpp.

◆ getMoments()

Matrix iDynSensor::getMoments ( ) const

Returns the links moments as a matrix, where the i-th col is the i-th moment.

Returns: a 3xN matrix with moments, in the form: i-th col = Mu_i

Definition at line 2680 of file iDynInv.cpp.

◆ getMomentsNewtonEuler()

Matrix iDynSensor::getMomentsNewtonEuler ( ) const

Returns the links moments as a matrix, where the i-th col is the i-th moment.

Returns: a 3x(N+2) matrix with moments, in the form: i-th col = Mu_i

Definition at line 2692 of file iDynInv.cpp.

◆ getTorque()

double iDynSensor::getTorque ( const unsigned int iLink ) const

Returns the i-th link torque.

Returns: the i-th link torque

Definition at line 2688 of file iDynInv.cpp.

◆ getTorques()

Vector iDynSensor::getTorques ( ) const

Returns the links torque as a vector.

Returns: a Nx1 vector with the torques

Definition at line 2682 of file iDynInv.cpp.

◆ getTorquesNewtonEuler()

Vector iDynSensor::getTorquesNewtonEuler ( ) const

Returns the links torque as a vector.

Returns: a Nx1 vector with the torques

Definition at line 2694 of file iDynInv.cpp.

◆ setSensorMeasures() [1/2]

bool iCub::iDyn::iDynSensor::setSensorMeasures	(	const yarp::sig::Vector &	F,
		const yarp::sig::Vector &	Mu
	)

Set the sensor measured force and moment.

Parameters

F	the sensor force (3x1)
Mu	the sensor moment (3x1)

Returns: true if the operation is successful, false otherwise (ie wrong vector size)

◆ setSensorMeasures() [2/2]

bool iCub::iDyn::iDynSensor::setSensorMeasures ( const yarp::sig::Vector & FM )

Set the sensor measured force and moment at once.

The measure vector (6x1) is made of 0:2=force 3:5=moment

Parameters

FM	the sensor force and moment (6x1)

Returns: true if the operation is successful, false otherwise (ie wrong vector size)

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

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

Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ iDynSensor() [1/2]

◆ iDynSensor() [2/2]

Member Function Documentation

◆ computeFromSensorNewtonEuler() [1/3]

◆ computeFromSensorNewtonEuler() [2/3]

◆ computeFromSensorNewtonEuler() [3/3]

◆ computeWrenchFromSensorNewtonEuler()

◆ getForce()

◆ getForceMomentEndEff()

◆ getForces()

◆ getForcesNewtonEuler()

◆ getMoment()

◆ getMoments()

◆ getMomentsNewtonEuler()

◆ getTorque()

◆ getTorques()

◆ getTorquesNewtonEuler()

◆ setSensorMeasures() [1/2]

◆ setSensorMeasures() [2/2]