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

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. More...
 
 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. More...
 
 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. More...
 
 ~iDynContactSolver ()
 Default destructor. More...
 
bool addContact (const iCub::skinDynLib::dynContact &contact)
 Add a new element to the contact list. More...
 
bool addContacts (const iCub::skinDynLib::dynContactList &contacts)
 Add the specified elements to the contact list. More...
 
void clearContactList ()
 Clear the contact list. More...
 
const iCub::skinDynLib::dynContactListcomputeExternalContacts (const yarp::sig::Vector &FMsens)
 Compute an estimate of the external contact wrenches. More...
 
const iCub::skinDynLib::dynContactListcomputeExternalContacts ()
 Compute an estimate of the external contact wrenches (assuming the F/T sensor measure have already been set) More...
 
void computeWrenchFromSensorNewtonEuler ()
 Compute an estimate of the external and internal contact wrenches (joint torques included). More...
 
const iCub::skinDynLib::dynContactListgetContactList () const
 
yarp::sig::Vector getForceMomentEndEff () const
 Returns the end effector force-moment as a single (6x1) vector. More...
 
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() More...
 
 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. More...
 
bool setSensorMeasures (const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
 Set the sensor measured force and moment. More...
 
bool setSensorMeasures (const yarp::sig::Vector &FM)
 Set the sensor measured force and moment at once. More...
 
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. More...
 
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. More...
 
virtual void computeFromSensorNewtonEuler ()
 The main computation method: given the FT sensor measurements, compute forces moments and torques in the iDynChain. More...
 
yarp::sig::Matrix getForces () const
 Returns the links forces as a matrix, where the i-th col is the i-th force. More...
 
yarp::sig::Matrix getMoments () const
 Returns the links moments as a matrix, where the i-th col is the i-th moment. More...
 
yarp::sig::Vector getTorques () const
 Returns the links torque as a vector. More...
 
yarp::sig::Vector getForce (const unsigned int iLink) const
 Returns the i-th link force. More...
 
yarp::sig::Vector getMoment (const unsigned int iLink) const
 Returns the i-th link moment. More...
 
double getTorque (const unsigned int iLink) const
 Returns the i-th link torque. More...
 
yarp::sig::Matrix getForcesNewtonEuler () const
 Returns the links forces as a matrix, where the i-th col is the i-th force. More...
 
yarp::sig::Matrix getMomentsNewtonEuler () const
 Returns the links moments as a matrix, where the i-th col is the i-th moment. More...
 
yarp::sig::Vector getTorquesNewtonEuler () const
 Returns the links torque as a vector. More...
 
- 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() More...
 
 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. More...
 
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. More...
 
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. More...
 
std::string toString () const
 Print some information. More...
 
yarp::sig::Vector getSensorForce () const
 Returns the sensor estimated force. More...
 
yarp::sig::Vector getSensorMoment () const
 Returns the sensor estimated moment. More...
 
yarp::sig::Vector getSensorForceMoment () const
 Get the sensor force and moment in a single (6x1) vector. More...
 
yarp::sig::Matrix getH () const
 Get the sensor roto-translational matrix defining its position/orientation wrt the link. More...
 
double getMass () const
 Get the mass of the portion of link defined between sensor and i-th frame. More...
 
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. More...
 
yarp::sig::Matrix getInertia () const
 Get the inertia of the portion of link defined between sensor and i-th frame. More...
 
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. More...
 
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. More...
 
yarp::sig::Vector projectContact2Root (const iCub::skinDynLib::dynContact &c)
 Compute the wrench of the specified contact expressed w.r.t. More...
 

Protected Attributes

iCub::skinDynLib::dynContactList contactList
 list of contacts acting on the link chain More...
 
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 More...
 
SensorLinkNewtonEulersens
 the sensor More...
 
iDynChainchain
 the iDynChain describing the robotic chain More...
 
NewEulMode mode
 static/dynamic/etc.. More...
 
unsigned int verbose
 verbosity flag More...
 
std::string info
 a string with useful information if needed More...
 

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
_cthe 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
_cthe robotic chain
sensLinkthe index of the link containing the F/T sensor
sensorthe 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
_cthe robotic chain
sensLinkthe index of the link containing the F/T sensor
_Hthe homogeneous transformation matrix from the F/T sensor link to the F/T sensor sub-link
_Hthe homogeneous transformation matrix from the F/T sensor to the F/T sensor sub-link center of mass
_mmass of the F/T sensor sub-link
_Iinertia 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
contactthe 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
contactsthe 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
FMsensthe 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: