This solver uses a modified version of the Newton-Euler algorithm to estimate both the external and internal forces/moments of a single kinematic chain. More...

#include <iDynContact.h>

Inheritance diagram for iCub::iDyn::iDynContactSolver:

Public Member Functions
	iDynContactSolver (iDynChain *_c, const std::string &_info="", const NewEulMode _mode=DYNAMIC, iCub::skinDynLib::BodyPart bodyPart=iCub::skinDynLib::BODY_PART_UNKNOWN, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
	Constructor.

	iDynContactSolver (iDynChain _c, unsigned int sensLink, SensorLinkNewtonEuler sensor, const std::string &_info="", const NewEulMode _mode=DYNAMIC, iCub::skinDynLib::BodyPart bodyPart=iCub::skinDynLib::BODY_PART_UNKNOWN, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
	Constructor with F/T sensor.

	iDynContactSolver (iDynChain *_c, unsigned int sensLink, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, double _m, const yarp::sig::Matrix &_I, const std::string &_info="", const NewEulMode _mode=DYNAMIC, iCub::skinDynLib::BodyPart bodyPart=iCub::skinDynLib::BODY_PART_UNKNOWN, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
	Constructor with F/T sensor information.

	~iDynContactSolver ()
	Default destructor.

bool	addContact (const iCub::skinDynLib::dynContact &contact)
	Add a new element to the contact list.

bool	addContacts (const iCub::skinDynLib::dynContactList &contacts)
	Add the specified elements to the contact list.

void	clearContactList ()
	Clear the contact list.

const iCub::skinDynLib::dynContactList &	computeExternalContacts (const yarp::sig::Vector &FMsens)
	Compute an estimate of the external contact wrenches.

const iCub::skinDynLib::dynContactList &	computeExternalContacts ()
	Compute an estimate of the external contact wrenches (assuming the F/T sensor measure have already been set)

void	computeWrenchFromSensorNewtonEuler ()
	Compute an estimate of the external and internal contact wrenches (joint torques included).

const iCub::skinDynLib::dynContactList &	getContactList () const

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

unsigned int	getUnknownNumber () const

Public Member Functions inherited from iCub::iDyn::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()

	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.

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.

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

Protected Member Functions
void	findContactSubChain (unsigned int &firstLink, unsigned int &lastLink)

yarp::sig::Matrix	buildA (unsigned int firstContactLink, unsigned int lastContactLink)

yarp::sig::Vector	buildB (unsigned int firstContactLink, unsigned int lastContactLink)

yarp::sig::Matrix	getHFromAtoB (unsigned int a, unsigned int b)
	Compute the rototraslation matrix from frame a to frame b.

yarp::sig::Vector	projectContact2Root (const iCub::skinDynLib::dynContact &c)
	Compute the wrench of the specified contact expressed w.r.t.

Protected Attributes
iCub::skinDynLib::dynContactList	contactList
	list of contacts acting on the link chain

const iCub::skinDynLib::dynContactList	nullList

iCub::skinDynLib::BodyPart	bodyPart

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

This solver uses a modified version of the Newton-Euler algorithm to estimate both the external and internal forces/moments of a single kinematic chain.

The solver assumes the contact locations are known and a F/T sensor measure is available.

Definition at line 52 of file iDynContact.h.

Constructor & Destructor Documentation

◆ iDynContactSolver() [1/3]

iDynContactSolver::iDynContactSolver	(	iDynChain *	_c,
		const std::string &	_info = `""`,
		const NewEulMode	_mode = `DYNAMIC`,
		iCub::skinDynLib::BodyPart	bodyPart = `iCub::skinDynLib::BODY_PART_UNKNOWN`,
		unsigned int	verb = `iCub::skinDynLib::NO_VERBOSE`
	)

Constructor.

Parameters

_c	the robotic chain

Definition at line 39 of file iDynContact.cpp.

◆ iDynContactSolver() [2/3]

iDynContactSolver::iDynContactSolver	(	iDynChain *	_c,
		unsigned int	sensLink,
		SensorLinkNewtonEuler *	sensor,
		const std::string &	_info = `""`,
		const NewEulMode	_mode = `DYNAMIC`,
		iCub::skinDynLib::BodyPart	bodyPart = `iCub::skinDynLib::BODY_PART_UNKNOWN`,
		unsigned int	verb = `iCub::skinDynLib::NO_VERBOSE`
	)

Constructor with F/T sensor.

Parameters

_c	the robotic chain
sensLink	the index of the link containing the F/T sensor
sensor	the F/T sensor

Definition at line 42 of file iDynContact.cpp.

◆ iDynContactSolver() [3/3]

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

Constructor with F/T sensor information.

Parameters

_c	the robotic chain
sensLink	the index of the link containing the F/T sensor
_H	the homogeneous transformation matrix from the F/T sensor link to the F/T sensor sub-link
_H	the homogeneous transformation matrix from the F/T sensor to the F/T sensor sub-link center of mass
_m	mass of the F/T sensor sub-link
_I	inertia of the F/T sensor sub-link

◆ ~iDynContactSolver()

iDynContactSolver::~iDynContactSolver ( )

Default destructor.

Definition at line 54 of file iDynContact.cpp.

Member Function Documentation

◆ addContact()

bool iDynContactSolver::addContact ( const iCub::skinDynLib::dynContact & contact )

Add a new element to the contact list.

The content of this new element is initialized to a copy of "contact".

Parameters

contact the contact to add

Returns: true if the operation is successful, false otherwise (eg index out of range)

Definition at line 56 of file iDynContact.cpp.

◆ addContacts()

bool iDynContactSolver::addContacts ( const iCub::skinDynLib::dynContactList & contacts )

Add the specified elements to the contact list.

Parameters

contacts the elements to add

Returns: true if operation succeeded, false otherwise

Definition at line 69 of file iDynContact.cpp.

◆ buildA()

Matrix iDynContactSolver::buildA	(	unsigned int	firstContactLink,
		unsigned int	lastContactLink
	)

protected

Definition at line 188 of file iDynContact.cpp.

◆ buildB()

Vector iDynContactSolver::buildB	(	unsigned int	firstContactLink,
		unsigned int	lastContactLink
	)

protected

Definition at line 239 of file iDynContact.cpp.

◆ clearContactList()

void iDynContactSolver::clearContactList ( )

Clear the contact list.

Definition at line 78 of file iDynContact.cpp.

◆ computeExternalContacts() [1/2]

const dynContactList & iDynContactSolver::computeExternalContacts ( )

Compute an estimate of the external contact wrenches (assuming the F/T sensor measure have already been set)

Returns: A copy of the external contact list

Definition at line 91 of file iDynContact.cpp.

◆ computeExternalContacts() [2/2]

const iCub::skinDynLib::dynContactList & iCub::iDyn::iDynContactSolver::computeExternalContacts ( const yarp::sig::Vector & FMsens )

Compute an estimate of the external contact wrenches.

Parameters

FMsens the wrench measured by the F/T sensor

Returns: A copy of the external contact list

◆ computeWrenchFromSensorNewtonEuler()

void iDynContactSolver::computeWrenchFromSensorNewtonEuler ( )

virtual

Compute an estimate of the external and internal contact wrenches (joint torques included).

Reimplemented from iCub::iDyn::iDynSensor.

Definition at line 177 of file iDynContact.cpp.

◆ findContactSubChain()

void iDynContactSolver::findContactSubChain	(	unsigned int &	firstLink,
		unsigned int &	lastLink
	)

protected

Definition at line 308 of file iDynContact.cpp.

◆ getContactList()

const dynContactList & iDynContactSolver::getContactList ( ) const

Returns: A copy of the external contact list

Definition at line 293 of file iDynContact.cpp.

◆ getForceMomentEndEff()

Vector iDynContactSolver::getForceMomentEndEff ( ) const

virtual

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

Returns: a (6x1) vector, in the form 0:2=F 3:5=Mu

Reimplemented from iCub::iDyn::iDynSensor.

Definition at line 298 of file iDynContact.cpp.

◆ getHFromAtoB()

Matrix iDynContactSolver::getHFromAtoB	(	unsigned int	a,
		unsigned int	b
	)

protected

Compute the rototraslation matrix from frame a to frame b.

Definition at line 324 of file iDynContact.cpp.

◆ getUnknownNumber()

unsigned int iDynContactSolver::getUnknownNumber ( ) const

Returns: the number of unknowns in the current contact list

Definition at line 350 of file iDynContact.cpp.

◆ projectContact2Root()

Vector iDynContactSolver::projectContact2Root ( const iCub::skinDynLib::dynContact & c )

protected

Compute the wrench of the specified contact expressed w.r.t.

the root reference frame of the chain (not the 0th frame, but the root).

Definition at line 341 of file iDynContact.cpp.

Member Data Documentation

◆ bodyPart

iCub::skinDynLib::BodyPart iCub::iDyn::iDynContactSolver::bodyPart

protected

Definition at line 60 of file iDynContact.h.

◆ contactList

iCub::skinDynLib::dynContactList iCub::iDyn::iDynContactSolver::contactList

protected

list of contacts acting on the link chain

Definition at line 56 of file iDynContact.h.

◆ nullList

const iCub::skinDynLib::dynContactList iCub::iDyn::iDynContactSolver::nullList

protected

Definition at line 58 of file iDynContact.h.

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

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

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ iDynContactSolver() [1/3]

◆ iDynContactSolver() [2/3]

◆ iDynContactSolver() [3/3]

◆ ~iDynContactSolver()

Member Function Documentation

◆ addContact()

◆ addContacts()

◆ buildA()

◆ buildB()

◆ clearContactList()

◆ computeExternalContacts() [1/2]

◆ computeExternalContacts() [2/2]

◆ computeWrenchFromSensorNewtonEuler()

◆ findContactSubChain()

◆ getContactList()

◆ getForceMomentEndEff()

◆ getHFromAtoB()

◆ getUnknownNumber()

◆ projectContact2Root()

Member Data Documentation

◆ bodyPart

◆ contactList

◆ nullList