iCub-main

#include <iDynInv.h>

## Public Member Functions

SensorLinkNewtonEuler (const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Default constructor. More...

SensorLinkNewtonEuler (const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_COM, const double _m, const yarp::sig::Matrix &_I, const NewEulMode _mode, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor. More...

Destructor. More...

bool setMeasuredFMu (const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the sensor measured force/moment - if measured by a FT sensor. More...

bool setSensor (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...

Compute w,dw,ddp,dppC given the reference frame of the link where the sensor is. More...

Compute F,Mu given the reference frame of the link where the sensor is. More...

Forward the sensor forces and moments, measured by the sensor, to the reference frame of the link where the sensor is: this method is the base for the inverse Newton-Euler algorithm, since it forwards the sensor measurements to the iDynChain. More...

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

const yarp::sig::Vector & getAngVel () const

const yarp::sig::Vector & getAngAcc () const

const yarp::sig::Vector & getLinAcc () const

const yarp::sig::Vector & getLinAccC () const

const yarp::sig::Vector & getForce () const

const yarp::sig::Vector & getMoment (bool isBase=false) const

double getIm () const

double getFs () const

double getFv () const

double getD2q () const

double getDq () const

double getKr () const

const yarp::sig::Vector & getAngAccM () const

double getTorque () const

const yarp::sig::Matrix & getH () const

double getMass () const

const yarp::sig::Matrix & getInertia () const

const yarp::sig::Matrix & getR ()

const yarp::sig::Matrix & getRC ()

const yarp::sig::Vector & getr (bool proj=false)

const yarp::sig::Vector & getrC (bool proj=false)

bool setForce (const yarp::sig::Vector &_F)
Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the child classes derived from OneLink, such as SensorLink. More...

bool setMoment (const yarp::sig::Vector &_Mu)
Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in the child classes derived from OneLink, such as SensorLink. More...

void setTorque (const double _Tau)

bool setAngVel (const yarp::sig::Vector &_w)
Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the child classes derived from OneLink, it depends) More...

bool setAngAcc (const yarp::sig::Vector &_dw)
Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (in the child classes derived from OneLink, it depends) More...

bool setLinAcc (const yarp::sig::Vector &_ddp)
Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (in the child classes derived from OneLink, it depends) More...

bool setLinAccC (const yarp::sig::Vector &_ddpC)
Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear acceleration of the COM (ddpC) (nothing in the child classes derived from OneLink, except for SensorLink) More...

bool setAngAccM (const yarp::sig::Vector &_dwM)
Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular acceleration of the COM (dwM) (nothing in the child classes derived from OneLink) More...

std::string toString () const
Useful to print some information. More...

void computeLinAccC ()

virtual std::string getType () const

Public Member Functions inherited from iCub::iDyn::OneLinkNewtonEuler
Default constructor. More...

Constructor, with initialization of some data. More...

Destructor. More...

void zero ()
Set everything to zero; R is set to an identity matrix. More...

virtual bool setAsBase (const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Virtual method to set the frame as the base one: this is useful to initialize the forward phase of Newton-Euler's method. More...

virtual bool setAsBase (const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)

virtual bool setAsFinal (const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the frame as the final one: this is useful to initialize the backward phase of Newton-Euler's method, by setting F and Mu; R is an identity matrix. More...

virtual bool setAsFinal (const yarp::sig::Vector &_w, const yarp::sig::Vector &_dw, const yarp::sig::Vector &_ddp)
Set the frame as the final one: this is useful to initialize the backward phase of Euler's method, by setting w, dw and ddp; R is an identity matrix. More...

virtual bool setMeasuredTorque (const double _Tau)
Set measured torque in a joint torque sensor frame
More...

void setVerbose (unsigned int verb=iCub::skinDynLib::VERBOSE)
Set the verbosity level of comments during operations. More...

void setMode (const NewEulMode _mode)
Set the operation mode (static,dynamic etc) More...

bool setZM (const yarp::sig::Vector &_zm)
Set the zM vector. More...

void setInfo (const std::string &_info)

NewEulMode getMode () const

yarp::sig::Vector getZM () const

std::string getInfo () const

virtual const yarp::sig::Matrix & getH ()

[Forward Newton-Euler] Compute w, dw, ddp, ddpC, dwM More...

[Backward Kinematic computation] Compute w, dw, ddp, ddpC, dwM More...

[Backward Newton-Euler] Compute F, Mu, Tau More...

[Inverse Newton-Euler] Compute F, Mu, Tau More...

[all] Compute joint torque; moment must be pre-computed More...

## Protected Attributes

yarp::sig::Vector F
measured or estimated force More...

yarp::sig::Vector Mu
measured or estimated moment 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 ddpC
linear acceleration of the COM More...

yarp::sig::Matrix H
the roto-translational matrix from the i-th link to the sensor: it's the matrix describing the sensor position and orientation with respect to the frame of the link where the sensor is placed on More...

yarp::sig::Matrix COM
the roto-translational matrix of the COM of the semi-link (bewteen sensor and ith link frame) More...

yarp::sig::Matrix I

double m
the semi-link mass (the portion of link defined by the sensor) More...

yarp::sig::Matrix R

yarp::sig::Matrix RC

yarp::sig::Vector r

yarp::sig::Vector r_proj

yarp::sig::Vector rc

yarp::sig::Vector rc_proj

const yarp::sig::Vector zeros0

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 z0
z0=[0 0 1]' More...

yarp::sig::Vector zm
z^{i-1}_{m_{i}} versor rotating solidally with link i, projected in frame i ==>> constant More...

Protected Member Functions inherited from iCub::iDyn::OneLinkNewtonEuler
[Forward Newton-Euler] compute angular velocity of the link More...

[Forward Newton-Euler] compute angular velocity of the previous link frame More...

[Forward Newton-Euler] compute angular acceleration of the link More...

[Forward Newton-Euler] compute angular acceleration of the previous link frame More...

[Forward Newton-Euler] compute linear acceleration of the reference frame of the link More...

[Forward Newton-Euler] compute linear acceleration of the reference frame of the previous link More...

void computeLinAccC ()
[Forward Newton-Euler] compute linear acceleration of the center of mass More...

[Forward Newton-Euler] compute angular acceleration of the rotor More...

[Backward Newton-Euler] compute force from the following link More...

[Inverse Newton-Euler] compute force from the previous link More...

[Backward Newton-Euler] compute moment from the following link More...

[Inverse Newton-Euler] compute moment from the previous link More...

## Detailed Description

A class for setting a virtual sensor link. This class is used to initialize the forward and backward phase of Newton-Euler's method in the Inverse formulation, but also to have estimation of the FT measures by the sensor; the sensor frame is defined with respect to the i-th link, where the sensor is attached. Note that this is a virtual link, since there's no iDynLink attached: it is just necessary to make the recursive Inverse Newton-Euler computations; however, inertia, mass and COM are defined, for the portion of link defined between sensor and i-th frame.

Definition at line 718 of file iDynInv.h.

## Constructor & Destructor Documentation

 SensorLinkNewtonEuler::SensorLinkNewtonEuler ( const NewEulMode _mode, unsigned int verb = iCub::skinDynLib::NO_VERBOSE )

Default constructor.

Parameters
 _mode the analysis mode (static/dynamic) verb flag for verbosity

Definition at line 1229 of file iDynInv.cpp.

 iCub::iDyn::SensorLinkNewtonEuler::SensorLinkNewtonEuler ( const yarp::sig::Matrix & _H, const yarp::sig::Matrix & _COM, const double _m, const yarp::sig::Matrix & _I, const NewEulMode _mode, unsigned int verb = iCub::skinDynLib::NO_VERBOSE )

Constructor.

Parameters
 _mode the analysis mode (static/dynamic) verb flag for verbosity

inlinevirtual

Destructor.

Definition at line 770 of file iDynInv.h.

## Member Function Documentation

Compute F,Mu given the reference frame of the link where the sensor is.

Parameters

Definition at line 1334 of file iDynInv.cpp.

## ◆ computeAngAcc()

Definition at line 1512 of file iDynInv.cpp.

## ◆ computeAngVel()

Definition at line 1507 of file iDynInv.cpp.

## ◆ computeForce()

Definition at line 1555 of file iDynInv.cpp.

Definition at line 1562 of file iDynInv.cpp.

## ◆ computeLinAcc()

Definition at line 1517 of file iDynInv.cpp.

## ◆ computeLinAccC()

Definition at line 1537 of file iDynInv.cpp.

## ◆ computeMoment()

Definition at line 1612 of file iDynInv.cpp.

Definition at line 1570 of file iDynInv.cpp.

Compute w,dw,ddp,dppC given the reference frame of the link where the sensor is.

Parameters

Definition at line 1326 of file iDynInv.cpp.

Forward the sensor forces and moments, measured by the sensor, to the reference frame of the link where the sensor is: this method is the base for the inverse Newton-Euler algorithm, since it forwards the sensor measurements to the iDynChain.

Parameters

Definition at line 1340 of file iDynInv.cpp.

## ◆ getAngAcc()

 const Vector & SensorLinkNewtonEuler::getAngAcc ( ) const
virtual
Returns
dw = angular acceleration (3x1)

Definition at line 1358 of file iDynInv.cpp.

## ◆ getAngAccM()

 const Vector & SensorLinkNewtonEuler::getAngAccM ( ) const
virtual
Returns
dwM = angular acceleration of the rotor (3x1)

Definition at line 1392 of file iDynInv.cpp.

## ◆ getAngVel()

 const Vector & SensorLinkNewtonEuler::getAngVel ( ) const
virtual
Returns
w = angular velocity (3x1)

Definition at line 1356 of file iDynInv.cpp.

## ◆ getD2q()

virtual
Returns
ddq = d2q = joint acceleration

Definition at line 1389 of file iDynInv.cpp.

## ◆ getDq()

virtual
Returns
dq = dq = joint velocity

Definition at line 1390 of file iDynInv.cpp.

## ◆ getForce()

 const Vector & SensorLinkNewtonEuler::getForce ( ) const
virtual
Returns
F = force (3x1)

Definition at line 1352 of file iDynInv.cpp.

## ◆ getForceMoment()

Get the sensor force and moment in a single (6x1) vector.

Returns
a (6x1) vector where 0:2=force 3:5=moment

Definition at line 1650 of file iDynInv.cpp.

## ◆ getFs()

virtual
Returns
Fs = static friction

Definition at line 1387 of file iDynInv.cpp.

## ◆ getFv()

virtual
Returns
Fv = viscous friction

Definition at line 1388 of file iDynInv.cpp.

## ◆ getH()

 const Matrix & SensorLinkNewtonEuler::getH ( ) const

Definition at line 1394 of file iDynInv.cpp.

## ◆ getIm()

virtual
Returns
Im

Definition at line 1386 of file iDynInv.cpp.

## ◆ getInertia()

 const Matrix & SensorLinkNewtonEuler::getInertia ( ) const
virtual
Returns
I = inertia of the link

Definition at line 1366 of file iDynInv.cpp.

## ◆ getKr()

virtual
Returns
Kr

Definition at line 1391 of file iDynInv.cpp.

## ◆ getLinAcc()

 const Vector & SensorLinkNewtonEuler::getLinAcc ( ) const
virtual
Returns
ddp = d2p = linear acceleration (3x1)

Definition at line 1360 of file iDynInv.cpp.

## ◆ getLinAccC()

 const Vector & SensorLinkNewtonEuler::getLinAccC ( ) const
virtual
Returns
ddpC = d2pC = linear acceleration of the center of mass (3x1)

Definition at line 1362 of file iDynInv.cpp.

## ◆ getMass()

virtual
Returns
m = mass of the link

Definition at line 1364 of file iDynInv.cpp.

## ◆ getMoment()

 const Vector & SensorLinkNewtonEuler::getMoment ( bool isBase = false ) const
virtual
Returns
Mu = moment (3x1)

Definition at line 1354 of file iDynInv.cpp.

## ◆ getR()

 const Matrix & SensorLinkNewtonEuler::getR ( )
virtual
Returns
R = (3x3) rotational matrix from the Denavit-Hartenberg roto-translational matrix describing the link

Definition at line 1368 of file iDynInv.cpp.

## ◆ getr()

 const Vector & SensorLinkNewtonEuler::getr ( bool proj = false )
virtual
Returns
r = (3x1) distance vector from the Denavit-Hartenberg roto-translational matrix describing the distance vector between frames and <i-1>

Definition at line 1372 of file iDynInv.cpp.

## ◆ getRC()

 const Matrix & SensorLinkNewtonEuler::getRC ( )
virtual
Returns
RC = (3x3) rotational matrix from the roto-translational matrix of the link COM

Definition at line 1370 of file iDynInv.cpp.

## ◆ getrC()

 const Vector & SensorLinkNewtonEuler::getrC ( bool proj = false )
virtual
Returns
rC = (3x1) distance vector from the roto-translational matrix of the link COM, describing the distance vector between COM and frame

Definition at line 1379 of file iDynInv.cpp.

## ◆ getTorque()

virtual
Returns
Tau = torque (1x1)

Definition at line 1393 of file iDynInv.cpp.

## ◆ getType()

virtual

Definition at line 1658 of file iDynInv.cpp.

## ◆ setAngAcc()

 bool SensorLinkNewtonEuler::setAngAcc ( const yarp::sig::Vector & _dw )
virtual

Set the OneLink angular acceleration (dw), ie the corresponding iDynLink angular acceleration (dw) (in the child classes derived from OneLink, it depends)

Parameters
 _dw angular acceleration

Definition at line 1446 of file iDynInv.cpp.

## ◆ setAngAccM()

 bool SensorLinkNewtonEuler::setAngAccM ( const yarp::sig::Vector & _dwM )
virtual

Set the OneLink angular acceleration of the motor (dwM), ie the corresponding iDynLink angular acceleration of the COM (dwM) (nothing in the child classes derived from OneLink)

Parameters
 _dwM angular acceleration of the motor

Definition at line 1494 of file iDynInv.cpp.

## ◆ setAngVel()

 bool SensorLinkNewtonEuler::setAngVel ( const yarp::sig::Vector & _w )
virtual

Set the OneLink angular velocity (w), ie the corresponding iDynLink angular velocity (w) (in the child classes derived from OneLink, it depends)

Parameters
 _w angular velocity

Definition at line 1430 of file iDynInv.cpp.

## ◆ setForce()

 bool SensorLinkNewtonEuler::setForce ( const yarp::sig::Vector & _F )
virtual

Set the OneLink force: either the corresponding iDynLink force, or the one declared as member in the child classes derived from OneLink, such as SensorLink.

Parameters
 _F a (3x1) vector of forces
Returns
true if size is correct, false otherwise

Definition at line 1396 of file iDynInv.cpp.

## ◆ setLinAcc()

 bool SensorLinkNewtonEuler::setLinAcc ( const yarp::sig::Vector & _ddp )
virtual

Set the OneLink linear acceleration (ddp), ie the corresponding iDynLink linear acceleration (ddp) (in the child classes derived from OneLink, it depends)

Parameters
 _ddp linear acceleration

Definition at line 1462 of file iDynInv.cpp.

## ◆ setLinAccC()

 bool SensorLinkNewtonEuler::setLinAccC ( const yarp::sig::Vector & _ddpC )
virtual

Set the OneLink linear acceleration of the COM (ddpC), ie the corresponding iDynLink linear acceleration of the COM (ddpC) (nothing in the child classes derived from OneLink, except for SensorLink)

Parameters
 _ddpC linear acceleration of the COM

Definition at line 1478 of file iDynInv.cpp.

## ◆ setMeasuredFMu()

 bool SensorLinkNewtonEuler::setMeasuredFMu ( const yarp::sig::Vector & _F, const yarp::sig::Vector & _Mu )
virtual

Set the sensor measured force/moment - if measured by a FT sensor.

Parameters
 _F the final force _Mu the final moment
Returns
true if dimensions are correct, false otherwise

Definition at line 1260 of file iDynInv.cpp.

## ◆ setMoment()

 bool SensorLinkNewtonEuler::setMoment ( const yarp::sig::Vector & _Mu )
virtual

Set the OneLink moment: either the corresponding iDynLink moment, or the one declared as member in the child classes derived from OneLink, such as SensorLink.

Parameters
 _Mu a 3x1 vector of moments
Returns
true if size is correct, false otherwise

Definition at line 1412 of file iDynInv.cpp.

## ◆ setSensor()

 bool SensorLinkNewtonEuler::setSensor ( 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.

Parameters
 _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

Definition at line 1280 of file iDynInv.cpp.

## ◆ setTorque()

 void SensorLinkNewtonEuler::setTorque ( const double _Tau )
virtual

Parameters
 _Tau a real torque value

Definition at line 1428 of file iDynInv.cpp.

## ◆ toString()

virtual

Useful to print some information.

Definition at line 1310 of file iDynInv.cpp.

## ◆ COM

protected

the roto-translational matrix of the COM of the semi-link (bewteen sensor and ith link frame)

Definition at line 737 of file iDynInv.h.

## ◆ ddp

protected

linear acceleration

Definition at line 731 of file iDynInv.h.

## ◆ ddpC

protected

linear acceleration of the COM

Definition at line 733 of file iDynInv.h.

## ◆ dw

protected

angular acceleration

Definition at line 729 of file iDynInv.h.

## ◆ F

protected

measured or estimated force

Definition at line 722 of file iDynInv.h.

## ◆ H

protected

the roto-translational matrix from the i-th link to the sensor: it's the matrix describing the sensor position and orientation with respect to the frame of the link where the sensor is placed on

Definition at line 735 of file iDynInv.h.

## ◆ I

protected

Definition at line 739 of file iDynInv.h.

## ◆ m

protected

Definition at line 741 of file iDynInv.h.

## ◆ Mu

protected

measured or estimated moment

Definition at line 724 of file iDynInv.h.

## ◆ R

protected

Definition at line 743 of file iDynInv.h.

## ◆ r

protected

Definition at line 745 of file iDynInv.h.

## ◆ r_proj

protected

Definition at line 746 of file iDynInv.h.

## ◆ RC

protected

Definition at line 744 of file iDynInv.h.

## ◆ rc

protected

Definition at line 747 of file iDynInv.h.

## ◆ rc_proj

protected

Definition at line 748 of file iDynInv.h.

## ◆ w

protected

angular velocity

Definition at line 727 of file iDynInv.h.