iCub-main
iCub::iDyn::iDynChain Class Reference

#include <iDyn.h>

Inheritance diagram for iCub::iDyn::iDynChain:

## Public Member Functions

iDynChain ()
Default constructor. More...

iDynChain (const iDynChain &c)
Creates a new Chain from an already existing Chain object. More...

iDynChainoperator= (const iDynChain &c)
Copies a Chain object into the current one. More...

yarp::sig::Vector setAng (const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i]. More...

yarp::sig::Vector setDAng (const yarp::sig::Vector &dq)
Sets the free joint angles velocity to values of dq[i]. More...

yarp::sig::Vector setD2Ang (const yarp::sig::Vector &ddq)
Sets the free joint angles acceleration to values of ddq[i]. More...

yarp::sig::Vector getDAng ()
Returns the current free joint angles velocity. More...

yarp::sig::Vector getD2Ang ()
Returns the current free joint angles acceleration. More...

yarp::sig::Vector getJointBoundMin ()
Returns a list containing the min value for each joint. More...

yarp::sig::Vector getJointBoundMax ()
Returns a list containing the max value for each joint. More...

double setAng (const unsigned int i, double q)
Sets the ith joint angle. More...

double setDAng (const unsigned int i, double dq)
Sets the ith joint angle velocity. More...

double setD2Ang (const unsigned int i, double ddq)
Sets the ith joint angle acceleration. More...

double getDAng (const unsigned int i)
Returns the current angle velocity of ith joint. More...

double getD2Ang (const unsigned int i)
Returns the current angle acceleration of ith joint. More...

yarp::sig::Vector getMasses () const
Returns the link masses as a vector. More...

bool setMasses (yarp::sig::Vector _m)
Set the link masses at once. More...

double getMass (const unsigned int i) const
Returns the i-th link mass. More...

bool setMass (const unsigned int i, const double _m)
Set the i-th link mass. More...

yarp::sig::Matrix getInertia (const unsigned int i) const
Returns the i-th link inertia matrix. 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...

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

const 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::Vector getLinVel (const unsigned int i) const
Returns the i-th link linear velocity. More...

yarp::sig::Vector getLinVelCOM (const unsigned int i) const
Returns the i-th link linear velocity of the COM. More...

yarp::sig::Vector getLinAcc (const unsigned int i) const
Returns the i-th link linear acceleration. More...

const yarp::sig::Vector & getLinAccCOM (const unsigned int i) const
Returns the i-th link linear acceleration of the COM. More...

yarp::sig::Vector getAngVel (const unsigned int i) const
Returns the i-th link angular velocity. More...

yarp::sig::Vector getAngAcc (const unsigned int i) const
Returns the i-th link angular acceleration. More...

bool setDynamicParameters (const unsigned int i, const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im)
Set the dynamic parameters of the i-th Link with motor. More...

bool setDynamicParameters (const unsigned int i, const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I)
Set the dynamic parameters of the i-th Link. More...

bool setStaticParameters (const unsigned int i, const double _m, const yarp::sig::Matrix &_HC)
Set the dynamic parameters of the i-th Link if the chain is in a static situation (inertia is null). More...

void prepareNewtonEuler (const NewEulMode NewEulMode_s=DYNAMIC)
Prepare for the Newton-Euler recursive computation of forces and torques. More...

bool computeNewtonEuler (const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Compute forces and torques with the Newton-Euler recursive algorithm: forward and backward phase are performed, and results are stored in the links; to get resulting forces and torques, one can call getForces() and getMoments() methods. More...

bool computeNewtonEuler ()
Compute forces and torques with the Newton-Euler recursive algorithm: forward and backward phase are performed, and results are stored in the links; to get resulting forces and torques, one can call getForces() and getMoments() methods; before calling this method, initNewtonEuler() must be called. More...

bool initNewtonEuler (const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0, dw0 and ddp0 ) and the final (virtual link) forces and moments Fend and Muend. More...

bool initNewtonEuler ()
Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0, dw0 and ddp0 ) and the final (virtual link) forces and moments Fend and Muend all to zero. More...

void setModeNewtonEuler (const NewEulMode NewEulMode_s=DYNAMIC)
Set the computation mode for Newton-Euler (static/dynamic/etc) More...

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

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

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

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

void setIterModeKinematic (const ChainIterationMode _iterateMode_kinematics=FORWARD)
Set the iteration direction during recursive computation of kinematics variables (w,dw,d2p,d2pC). More...

void setIterModeWrench (const ChainIterationMode _iterateMode_wrench=BACKWARD)
Set the iteration direction during recursive computation of wrench variables (F,Mu,Tau). More...

void setIterMode (const ChainComputationMode mode=KINFWD_WREBWD)
Set the computation mode during recursive computation of kinematics (w,dw,d2p,d2pC) and wrench variables(F,Mu,Tau). More...

ChainIterationMode getIterModeKinematic () const
Get the iteration direction during recursive computation of kinematics variables (w,dw,d2p,d2pC). More...

ChainIterationMode getIterModeWrench () const
Get the iteration direction during recursive computation of wrench variables (F,Mu,Tau). More...

void computeKinematicNewtonEuler ()
Calls the proper method to compute kinematics variables: either ForwardKinematicFromBase() or BackwardKinematicFromEnd(). More...

void computeWrenchNewtonEuler ()
Calls the proper method to compute wrench variables: either BackwardWrenchFromEnd() or ForwardWrenchFromBase(). More...

bool initKinematicNewtonEuler (const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0)
Calls the proper method to set kinematics variables in OneChainNewtonEuler: either initKinematicBase() or initKinematicEnd(). More...

bool initWrenchNewtonEuler (const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Calls the proper method to set wrench variables in OneChainNewtonEuler: either initKinematicBase() or initKinematicEnd(). More...

void getKinematicNewtonEuler (yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp)
Calls the proper method to get kinematics variables in OneChainNewtonEuler either in the base or in the final link. More...

void getFrameKinematic (unsigned int i, yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp)
Get the kinematic information of the i-th frame in the OneChainNewtonEuler associated to the current iDynChain, i.e. More...

void getFrameWrench (unsigned int i, yarp::sig::Vector &F, yarp::sig::Vector &Mu)
Get the wrench information of the i-th frame in the OneChainNewtonEuler associated to the current iDynChain, i.e. More...

void getWrenchNewtonEuler (yarp::sig::Vector &F, yarp::sig::Vector &Mu)
Calls the proper method to get wrench variables in OneChainNewtonEuler either in the base or in the final link. More...

virtual ~iDynChain ()
Destructor. More...

yarp::sig::Matrix computeGeoJacobian (const unsigned int iLinkN, const yarp::sig::Matrix &Pn)

yarp::sig::Matrix computeGeoJacobian (const unsigned int iLinkN, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0)

yarp::sig::Matrix computeGeoJacobian (const yarp::sig::Matrix &Pn)
Compute the Jacobian of the chain, from link 0 to N. More...

yarp::sig::Matrix computeGeoJacobian (const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0)
Compute the Jacobian of the chain, from link 0 to N. More...

yarp::sig::Matrix getDenHart (unsigned int i)
Return the Denavit-Hartenberg matrix of the i-th link in the chain. More...

yarp::sig::Matrix TESTING_computeCOMJacobian (const unsigned int iLink)

yarp::sig::Matrix TESTING_computeCOMJacobian (const unsigned int iLink, const yarp::sig::Matrix &Pn)

yarp::sig::Matrix TESTING_computeCOMJacobian (const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0)

Return the COM matrix of the i-th link. More...

Return the H matrix until the COM of the i-th link. More...

yarp::sig::Matrix computeMassMatrix ()
Compute the joint space mass matrix considering only the active joints. More...

yarp::sig::Matrix computeMassMatrix (const yarp::sig::Vector &q)
Compute the joint space mass matrix considering only the active joints. More...

yarp::sig::Vector computeCcTorques ()
Compute the torques due to centrifugal and coriolis effects considering only the active joints. More...

yarp::sig::Vector computeCcTorques (const yarp::sig::Vector &q, const yarp::sig::Vector &dq)
Compute the torques due to centrifugal and coriolis effects considering only the active joints. More...

yarp::sig::Vector computeGravityTorques (const yarp::sig::Vector &ddp0)
Compute the torques generated by gravity considering only the active joints. More...

yarp::sig::Vector computeGravityTorques (const yarp::sig::Vector &ddp0, const yarp::sig::Vector &q)
Compute the torques generated by gravity considering only the active joints. More...

yarp::sig::Vector computeCcGravityTorques (const yarp::sig::Vector &ddp0)
Compute the torques generated by gravity and centrifugal and coriolis forces, considering only the active joints. More...

yarp::sig::Vector computeCcGravityTorques (const yarp::sig::Vector &ddp0, const yarp::sig::Vector &q, const yarp::sig::Vector &dq)
Compute the torques generated by gravity and centrifugal and coriolis forces, considering only the active joints. More...

Public Member Functions inherited from iCub::iKin::iKinChain
iKinChain ()
Default constructor. More...

iKinChain (const iKinChain &c)
Creates a new Chain from an already existing Chain object. More...

iKinChainoperator= (const iKinChain &c)
Copies a Chain object into the current one. More...

iKinChainoperator-- (int)
Removes a Link from the bottom of the Chain. More...

Returns a reference to the ith Link of the Chain. More...

Returns a reference to the ith Link of the Chain considering only those Links related to DOF. More...

bool rmLink (const unsigned int i)
Removes the ith Link from the Chain. More...

void clear ()

Removes a Link from the bottom of the Chain. More...

bool blockLink (const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle. More...

bool blockLink (const unsigned int i)
Blocks the ith Link at the current value of its joint angle. More...

bool setBlockingValue (const unsigned int i, double Ang)
Changes the value of the ith blocked Link. More...

bool releaseLink (const unsigned int i)

bool isLinkBlocked (const unsigned int i)
Queries whether the ith Link is blocked. More...

void setAllConstraints (bool _constrained)
Sets the constraint status of all chain links. More...

void setConstraint (unsigned int i, bool _constrained)
Sets the constraint status of ith link. More...

bool getConstraint (unsigned int i)
Returns the constraint status of ith link. More...

Sets the verbosity level of all Links belonging to the Chain. More...

void setVerbosity (unsigned int _verbose)
Sets the verbosity level of the Chain. More...

unsigned int getVerbosity () const
Returns the current Chain verbosity level. More...

unsigned int getN () const
Returns the number of Links belonging to the Chain. More...

unsigned int getDOF () const
Returns the current number of Chain's DOF. More...

yarp::sig::Matrix getH0 () const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame. More...

bool setH0 (const yarp::sig::Matrix &_H0)
Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame. More...

yarp::sig::Matrix getHN () const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector. More...

bool setHN (const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector. More...

yarp::sig::Vector setAng (const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i]. More...

yarp::sig::Vector getAng ()
Returns the current free joint angles values. More...

double setAng (const unsigned int i, double _Ang)
Sets the ith joint angle. More...

double getAng (const unsigned int i)
Returns the current angle of ith joint. More...

yarp::sig::Matrix getH (const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-Hartenberg notation. More...

yarp::sig::Matrix getH ()
Returns the rigid roto-translation matrix from the root reference frame to the end-effector frame in Denavit-Hartenberg notation (HN is taken into account). More...

yarp::sig::Matrix getH (const yarp::sig::Vector &q)
Returns the rigid roto-translation matrix from the root reference frame to the end-effector frame in Denavit-Hartenberg notation (HN is taken into account). More...

yarp::sig::Vector Pose (const unsigned int i, const bool axisRep=true)
Returns the coordinates of ith Link. More...

yarp::sig::Vector Position (const unsigned int i)
Returns the 3D position coordinates of ith Link. More...

yarp::sig::Vector EndEffPose (const bool axisRep=true)
Returns the coordinates of end-effector. More...

yarp::sig::Vector EndEffPose (const yarp::sig::Vector &q, const bool axisRep=true)
Returns the coordinates of end-effector computed in q. More...

yarp::sig::Vector EndEffPosition ()
Returns the 3D coordinates of end-effector position. More...

yarp::sig::Vector EndEffPosition (const yarp::sig::Vector &q)
Returns the 3D coordinates of end-effector position computed in q. More...

yarp::sig::Matrix AnaJacobian (const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link. More...

yarp::sig::Matrix AnaJacobian (unsigned int col=3)
Returns the analitical Jacobian of the end-effector. More...

yarp::sig::Matrix AnaJacobian (const yarp::sig::Vector &q, unsigned int col=3)
Returns the analitical Jacobian of the end-effector computed in q. More...

yarp::sig::Matrix GeoJacobian (const unsigned int i)
Returns the geometric Jacobian of the ith link. More...

yarp::sig::Matrix GeoJacobian ()
Returns the geometric Jacobian of the end-effector. More...

yarp::sig::Matrix GeoJacobian (const yarp::sig::Vector &q)
Returns the geometric Jacobian of the end-effector computed in q. More...

yarp::sig::Vector Hessian_ij (const unsigned int i, const unsigned int j)
Returns the 6x1 vector $$\partial{^2}F\left(q\right)/\partial q_i \partial q_j,$$ where $$F\left(q\right)$$ is the forward kinematic function and $$\left(q_i,q_j\right)$$ is the DOF couple. More...

void prepareForHessian ()
Prepares computation for a successive call to fastHessian_ij(). More...

yarp::sig::Vector fastHessian_ij (const unsigned int i, const unsigned int j)
Returns the 6x1 vector $$\partial{^2}F\left(q\right)/\partial q_i \partial q_j,$$ where $$F\left(q\right)$$ is the forward kinematic function and $$\left(q_i,q_j\right)$$ is the DOF couple. More...

yarp::sig::Vector Hessian_ij (const unsigned int lnk, const unsigned int i, const unsigned int j)
Returns the 6x1 vector $$\partial{^2}F\left(q\right)/\partial q_i \partial q_j,$$ where $$F\left(q\right)$$ is the forward kinematic function and $$\left(q_i,q_j\right)$$ is the couple of links. More...

void prepareForHessian (const unsigned int lnk)
Prepares computation for a successive call to fastHessian_ij() (link version). More...

yarp::sig::Vector fastHessian_ij (const unsigned int lnk, const unsigned int i, const unsigned int j)
Returns the 6x1 vector $$\partial{^2}F\left(q\right)/\partial q_i \partial q_j,$$ where $$F\left(q\right)$$ is the forward kinematic function and $$\left(q_i,q_j\right)$$ is the couple of links. More...

yarp::sig::Matrix DJacobian (const yarp::sig::Vector &dq)
Compute the time derivative of the geometric Jacobian. More...

yarp::sig::Matrix DJacobian (const unsigned int lnk, const yarp::sig::Vector &dq)
Compute the time derivative of the geometric Jacobian (link version). More...

virtual ~iKinChain ()
Destructor. More...

## Protected Member Functions

virtual void clone (const iDynChain &c)
Clone function. More...

virtual void build ()
Build chains and lists. More...

virtual void dispose ()
Dispose. More...

Returns a pointer to the ith iDynLink. More...

Protected Member Functions inherited from iCub::iKin::iKinChain
virtual void clone (const iKinChain &c)

yarp::sig::Vector RotAng (const yarp::sig::Matrix &R)

yarp::sig::Vector dRotAng (const yarp::sig::Matrix &R, const yarp::sig::Matrix &dR)

yarp::sig::Vector d2RotAng (const yarp::sig::Matrix &R, const yarp::sig::Matrix &dRi, const yarp::sig::Matrix &dRj, const yarp::sig::Matrix &d2R)

## Protected Attributes

ChainIterationMode iterateMode_kinematics
specifies the 'direction' of recursive computation of kinematics variables (w,dw,d2p): FORWARD, BACKWARD More...

ChainIterationMode iterateMode_wrench
specifies the 'direction' of recursive computation of wrenches (F,Mu): FORWARD, BACKWARD More...

yarp::sig::Vector curr_dq
q vel More...

yarp::sig::Vector curr_ddq
q acc More...

OneChainNewtonEulerNE
pointer to OneChainNewtonEuler class, to be used for computing forces and torques More...

const yarp::sig::Vector zero0

Protected Attributes inherited from iCub::iKin::iKinChain
unsigned int N

unsigned int DOF

unsigned int verbose

yarp::sig::Matrix H0

yarp::sig::Matrix HN

yarp::sig::Vector curr_q

std::deque< unsigned int > hash

std::deque< unsigned int > hash_dof

yarp::sig::Matrix hess_J

yarp::sig::Matrix hess_Jlnk

## Friends

class OneChainNewtonEuler

class iDynInvSensor

class iDynSensor

class RigidBodyTransformation

class iDynContactSolver

## Detailed Description

A Base class for defining a Serial Link Chain, using dynamics and kinematics.

Definition at line 532 of file iDyn.h.

## ◆ iDynChain() [1/2]

 iDynChain::iDynChain ( )

Default constructor.

Definition at line 476 of file iDyn.cpp.

## ◆ iDynChain() [2/2]

 iDynChain::iDynChain ( const iDynChain & c )

Creates a new Chain from an already existing Chain object.

Parameters
 c is the Chain to be copied

Definition at line 503 of file iDyn.cpp.

## ◆ ~iDynChain()

 iDynChain::~iDynChain ( )
virtual

Destructor.

Definition at line 518 of file iDyn.cpp.

## ◆ build()

 void iDynChain::build ( )
protectedvirtual

Build chains and lists.

Reimplemented from iCub::iKin::iKinChain.

Definition at line 493 of file iDyn.cpp.

## ◆ clone()

 void iDynChain::clone ( const iDynChain & c )
protectedvirtual

Clone function.

Definition at line 483 of file iDyn.cpp.

## ◆ computeCcGravityTorques() [1/2]

 yarp::sig::Vector iCub::iDyn::iDynChain::computeCcGravityTorques ( const yarp::sig::Vector & ddp0 )

Compute the torques generated by gravity and centrifugal and coriolis forces, considering only the active joints.

Parameters
 ddp0 a vector that is equal and opposite to gravity expressed in the base reference frame (not the 0th frame)
Returns
a DOF-dim vector
Note
Calling this method is faster than calling computeGravityTorques and computeCcTorques subsequently.

## ◆ computeCcGravityTorques() [2/2]

 yarp::sig::Vector iCub::iDyn::iDynChain::computeCcGravityTorques ( const yarp::sig::Vector & ddp0, const yarp::sig::Vector & q, const yarp::sig::Vector & dq )

Compute the torques generated by gravity and centrifugal and coriolis forces, considering only the active joints.

Parameters
 ddp0 a vector that is equal and opposite to gravity expressed in the base reference frame (not the 0th frame) q vector of the active joint positions dq vector of the active joint velocities
Returns
a DOF-dim vector
Note
Calling this method is faster than calling computeGravityTorques and computeCcTorques subsequently.

## ◆ computeCcTorques() [1/2]

 Vector iDynChain::computeCcTorques ( )

Compute the torques due to centrifugal and coriolis effects considering only the active joints.

Returns
a DOF-dim vector

Definition at line 1673 of file iDyn.cpp.

## ◆ computeCcTorques() [2/2]

 yarp::sig::Vector iCub::iDyn::iDynChain::computeCcTorques ( const yarp::sig::Vector & q, const yarp::sig::Vector & dq )

Compute the torques due to centrifugal and coriolis effects considering only the active joints.

Parameters
 q vector of the active joint positions dq vector of the active joint velocities
Returns
a DOF-dim vector

## ◆ computeGeoJacobian() [1/4]

 yarp::sig::Matrix iCub::iDyn::iDynChain::computeGeoJacobian ( const unsigned int iLinkN, const yarp::sig::Matrix & Pn )

This method is used to compute the Jacobian between two links in two different chains.

Parameters
 iLinkN the index of the link, in the chain, being the final () frame for the Jacobian computation Pn the matrix describing the roto-translational matrix between base and end-effector (in two different limbs)
Returns
the Jacobian matrix from the iLink of the chain until the base of the chain (ie from link 4 to 0)

## ◆ computeGeoJacobian() [2/4]

 yarp::sig::Matrix iCub::iDyn::iDynChain::computeGeoJacobian ( const unsigned int iLinkN, const yarp::sig::Matrix & Pn, const yarp::sig::Matrix & _H0 )

This method is used to compute the Jacobian between two links in two different chains.

Parameters
 iLinkN the index of the link, in the chain, being the final () frame for the Jacobian computation Pn the matrix describing the roto-translational matrix between base and end-effector (in two different limbs) _H0 the matrix to initialize the jacobian computation, usually taking into account the previous limb
Returns
the Jacobian matrix from the iLink of the chain until the base of the chain (ie from link 4 to 0)

## ◆ computeGeoJacobian() [3/4]

 yarp::sig::Matrix iCub::iDyn::iDynChain::computeGeoJacobian ( const yarp::sig::Matrix & Pn )

Compute the Jacobian of the chain, from link 0 to N.

This method is used to compute the Jacobian between two links in two different chains.

Parameters
 Pn the matrix describing the roto-translational matrix between base and end-effector (in two different limbs)
Returns
the Jacobian matrix from the iLink of the chain until the base of the chain (ie from link 4 to 0)

## ◆ computeGeoJacobian() [4/4]

 yarp::sig::Matrix iCub::iDyn::iDynChain::computeGeoJacobian ( const yarp::sig::Matrix & Pn, const yarp::sig::Matrix & _H0 )

Compute the Jacobian of the chain, from link 0 to N.

This method is used to compute the Jacobian between two links in two different chains.

Parameters
 Pn the matrix describing the roto-translational matrix between base and end-effector (in two different limbs) _H0 the matrix to initialize the jacobian computation, usually taking into account the previous limb
Returns
the Jacobian matrix from the iLink of the chain until the base of the chain (ie from link 4 to 0)

## ◆ computeGravityTorques() [1/2]

 yarp::sig::Vector iCub::iDyn::iDynChain::computeGravityTorques ( const yarp::sig::Vector & ddp0 )

Compute the torques generated by gravity considering only the active joints.

Parameters
 ddp0 a vector that is equal and opposite to gravity expressed in the base reference frame (not the 0th frame)
Returns
a DOF-dim vector
Note
After calling this method the NewtonEulerMode is set to STATIC

## ◆ computeGravityTorques() [2/2]

 yarp::sig::Vector iCub::iDyn::iDynChain::computeGravityTorques ( const yarp::sig::Vector & ddp0, const yarp::sig::Vector & q )

Compute the torques generated by gravity considering only the active joints.

Parameters
 ddp0 a vector that is equal and opposite to gravity expressed in the base reference frame (not the 0th frame) q vector of the active joint positions
Returns
a DOF-dim vector
Note
After calling this method the NewtonEulerMode is set to STATIC

## ◆ computeKinematicNewtonEuler()

 void iDynChain::computeKinematicNewtonEuler ( )

Calls the proper method to compute kinematics variables: either ForwardKinematicFromBase() or BackwardKinematicFromEnd().

This method is protected and it is used by iDynSensor and iDynNode for the Kinematics computations.

Definition at line 992 of file iDyn.cpp.

## ◆ computeMassMatrix() [1/2]

 Matrix iDynChain::computeMassMatrix ( )

Compute the joint space mass matrix considering only the active joints.

Returns
a DOF-by-DOF symmetric positive-definite matrix

Definition at line 1605 of file iDyn.cpp.

## ◆ computeMassMatrix() [2/2]

 yarp::sig::Matrix iCub::iDyn::iDynChain::computeMassMatrix ( const yarp::sig::Vector & q )

Compute the joint space mass matrix considering only the active joints.

Parameters
 q vector of the active joint positions
Returns
a DOF-by-DOF symmetric positive-definite matrix

## ◆ computeNewtonEuler() [1/2]

 bool iDynChain::computeNewtonEuler ( )

Compute forces and torques with the Newton-Euler recursive algorithm: forward and backward phase are performed, and results are stored in the links; to get resulting forces and torques, one can call getForces() and getMoments() methods; before calling this method, initNewtonEuler() must be called.

Definition at line 965 of file iDyn.cpp.

## ◆ computeNewtonEuler() [2/2]

 bool iCub::iDyn::iDynChain::computeNewtonEuler ( const yarp::sig::Vector & w0, const yarp::sig::Vector & dw0, const yarp::sig::Vector & ddp0, const yarp::sig::Vector & Fend, const yarp::sig::Vector & Muend )

Compute forces and torques with the Newton-Euler recursive algorithm: forward and backward phase are performed, and results are stored in the links; to get resulting forces and torques, one can call getForces() and getMoments() methods.

The function parameters contain the information for initializing the kinematic and wrench phase.

## ◆ computeWrenchNewtonEuler()

 void iDynChain::computeWrenchNewtonEuler ( )

Calls the proper method to compute wrench variables: either BackwardWrenchFromEnd() or ForwardWrenchFromBase().

This method is protected and it is used by iDynSensor and iDynNode for the Wrench computations.

Definition at line 1000 of file iDyn.cpp.

## ◆ dispose()

 void iDynChain::dispose ( )
protectedvirtual

Dispose.

Reimplemented from iCub::iKin::iKinChain.

Reimplemented in iCub::iDyn::iDynLimb.

Definition at line 508 of file iDyn.cpp.

## ◆ getAngAcc()

 Vector iDynChain::getAngAcc ( const unsigned int i ) const

Returns the i-th link angular acceleration.

Returns

Definition at line 744 of file iDyn.cpp.

## ◆ getAngVel()

 Vector iDynChain::getAngVel ( const unsigned int i ) const

Returns the i-th link angular velocity.

Returns

Definition at line 733 of file iDyn.cpp.

## ◆ getCOM()

 Matrix iDynChain::getCOM ( unsigned int iLink )

Return the COM matrix of the i-th link.

Returns
the COM matrix of the i-th link

Definition at line 1585 of file iDyn.cpp.

## ◆ getD2Ang() [1/2]

 Vector iDynChain::getD2Ang ( )

Returns the current free joint angles acceleration.

Returns
the actual DOF values

Definition at line 597 of file iDyn.cpp.

## ◆ getD2Ang() [2/2]

 double iDynChain::getD2Ang ( const unsigned int i )

Returns the current angle acceleration of ith joint.

Parameters
Returns
current ith joint angle acceleration

Definition at line 678 of file iDyn.cpp.

## ◆ getDAng() [1/2]

 Vector iDynChain::getDAng ( )

Returns the current free joint angles velocity.

Returns
the actual DOF values

Definition at line 584 of file iDyn.cpp.

## ◆ getDAng() [2/2]

 double iDynChain::getDAng ( const unsigned int i )

Returns the current angle velocity of ith joint.

Parameters
Returns
current ith joint angle velocity

Definition at line 667 of file iDyn.cpp.

## ◆ getDenHart()

 Matrix iDynChain::getDenHart ( unsigned int i )

Return the Denavit-Hartenberg matrix of the i-th link in the chain.

Note that all the links are considered (0<=i<N)

Parameters
 i the i-th link in the chain
Returns
the Denavit-Hartenberg matrix of the i-th link

Definition at line 1467 of file iDyn.cpp.

## ◆ getForce()

 const Vector & iDynChain::getForce ( const unsigned int iLink ) const

Returns

Definition at line 766 of file iDyn.cpp.

## ◆ getForceMomentEndEff()

 Vector iDynChain::getForceMomentEndEff ( ) const

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

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

Definition at line 1272 of file iDyn.cpp.

## ◆ getForces()

 Matrix iDynChain::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 859 of file iDyn.cpp.

## ◆ getForcesNewtonEuler()

 Matrix iDynChain::getForcesNewtonEuler ( ) const

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

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

Definition at line 1226 of file iDyn.cpp.

## ◆ getFrameKinematic()

 void iDynChain::getFrameKinematic ( unsigned int i, yarp::sig::Vector & w, yarp::sig::Vector & dw, yarp::sig::Vector & ddp )

Get the kinematic information of the i-th frame in the OneChainNewtonEuler associated to the current iDynChain, i.e.

the virtual links (Base, Final) are also considered.

Parameters
 i the i-th frame in the OneChainNE w the vector which will contain the angular velocity dw the vector which will contain the angular acceleration ddp the vector which will contain the linear acceleration

Definition at line 1036 of file iDyn.cpp.

## ◆ getFrameWrench()

 void iDynChain::getFrameWrench ( unsigned int i, yarp::sig::Vector & F, yarp::sig::Vector & Mu )

Get the wrench information of the i-th frame in the OneChainNewtonEuler associated to the current iDynChain, i.e.

the virtual links (Base, Final) are also considered.

Parameters
 i the i-th frame in the OneChainNE F the vector which will contain the force Mu the vector which will contain the moment

Definition at line 1055 of file iDyn.cpp.

## ◆ getHCOM()

 Matrix iDynChain::getHCOM ( unsigned int iLink )

Return the H matrix until the COM of the i-th link.

Returns
the H matrix until the COM of the i-th link

Definition at line 1595 of file iDyn.cpp.

## ◆ getInertia()

 Matrix iDynChain::getInertia ( const unsigned int i ) const

Returns the i-th link inertia matrix.

Returns

Definition at line 847 of file iDyn.cpp.

## ◆ getIterModeKinematic()

 ChainIterationMode iDynChain::getIterModeKinematic ( ) const

Get the iteration direction during recursive computation of kinematics variables (w,dw,d2p,d2pC).

Returns
iterateMode_kinematics

Definition at line 1292 of file iDyn.cpp.

## ◆ getIterModeWrench()

 ChainIterationMode iDynChain::getIterModeWrench ( ) const

Get the iteration direction during recursive computation of wrench variables (F,Mu,Tau).

Returns
iterateMode_wrench

Definition at line 1297 of file iDyn.cpp.

## ◆ getJointBoundMax()

 Vector iDynChain::getJointBoundMax ( )

Returns a list containing the max value for each joint.

Returns
the max joint bounds

Definition at line 621 of file iDyn.cpp.

## ◆ getJointBoundMin()

 Vector iDynChain::getJointBoundMin ( )

Returns a list containing the min value for each joint.

Returns
the min joint bounds

Definition at line 610 of file iDyn.cpp.

## ◆ getKinematicNewtonEuler()

 void iDynChain::getKinematicNewtonEuler ( yarp::sig::Vector & w, yarp::sig::Vector & dw, yarp::sig::Vector & ddp )

Calls the proper method to get kinematics variables in OneChainNewtonEuler either in the base or in the final link.

This method is used by RigidBodyTransformation for setting the kinematics variables.

Parameters
 w the vector which will contain the angular velocity dw the vector which will contain the angular acceleration ddp the vector which will contain the linear acceleration

Definition at line 1008 of file iDyn.cpp.

## ◆ getLinAcc()

 Vector iDynChain::getLinAcc ( const unsigned int i ) const

Returns the i-th link linear acceleration.

Returns

Definition at line 711 of file iDyn.cpp.

## ◆ getLinAccCOM()

 const Vector & iDynChain::getLinAccCOM ( const unsigned int i ) const

Returns the i-th link linear acceleration of the COM.

Returns
the i-th link linear acceleration of the COM

Definition at line 722 of file iDyn.cpp.

## ◆ getLinVel()

 Vector iDynChain::getLinVel ( const unsigned int i ) const

Returns the i-th link linear velocity.

Returns

Definition at line 689 of file iDyn.cpp.

## ◆ getLinVelCOM()

 Vector iDynChain::getLinVelCOM ( const unsigned int i ) const

Returns the i-th link linear velocity of the COM.

Returns
the i-th link linear velocity of the COM

Definition at line 700 of file iDyn.cpp.

## ◆ getMass()

 double iDynChain::getMass ( const unsigned int i ) const

Returns

Definition at line 822 of file iDyn.cpp.

## ◆ getMasses()

 Vector iDynChain::getMasses ( ) const

Returns the link masses as a vector.

Returns
a vector with all the link masses

Definition at line 799 of file iDyn.cpp.

## ◆ getMoment()

 const Vector & iDynChain::getMoment ( const unsigned int iLink ) const

Returns

Definition at line 777 of file iDyn.cpp.

## ◆ getMoments()

 Matrix iDynChain::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 867 of file iDyn.cpp.

## ◆ getMomentsNewtonEuler()

 Matrix iDynChain::getMomentsNewtonEuler ( ) const

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

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

Definition at line 1242 of file iDyn.cpp.

## ◆ getTorque()

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

Returns

Definition at line 788 of file iDyn.cpp.

## ◆ getTorques()

 Vector iDynChain::getTorques ( ) const

Returns the links torque as a vector.

Returns
a vector with the torques

Definition at line 875 of file iDyn.cpp.

## ◆ getTorquesNewtonEuler()

 Vector iDynChain::getTorquesNewtonEuler ( ) const

Returns the links torque as a vector.

Returns
a Nx1 vector with the torques

Definition at line 1257 of file iDyn.cpp.

## ◆ getWrenchNewtonEuler()

 void iDynChain::getWrenchNewtonEuler ( yarp::sig::Vector & F, yarp::sig::Vector & Mu )

Calls the proper method to get wrench variables in OneChainNewtonEuler either in the base or in the final link.

This method is used by RigidBodyTransformation for setting the wrench variables.

Definition at line 1072 of file iDyn.cpp.

## ◆ initKinematicNewtonEuler()

 bool iDynChain::initKinematicNewtonEuler ( const yarp::sig::Vector & w0, const yarp::sig::Vector & dw0, const yarp::sig::Vector & ddp0 )

Calls the proper method to set kinematics variables in OneChainNewtonEuler: either initKinematicBase() or initKinematicEnd().

This method is protected and it is used by RigidBodyTransformation for setting the Kinematics variables.

Parameters
 w0 angular velocity dw0 angular acceleration ddp0 linear acceleration
Returns
true if succeed, false otherwise

Definition at line 1144 of file iDyn.cpp.

## ◆ initNewtonEuler() [1/2]

 bool iDynChain::initNewtonEuler ( )

Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0, dw0 and ddp0 ) and the final (virtual link) forces and moments Fend and Muend all to zero.

Definition at line 1099 of file iDyn.cpp.

## ◆ initNewtonEuler() [2/2]

 bool iCub::iDyn::iDynChain::initNewtonEuler ( const yarp::sig::Vector & w0, const yarp::sig::Vector & dw0, const yarp::sig::Vector & ddp0, const yarp::sig::Vector & Fend, const yarp::sig::Vector & Muend )

Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0, dw0 and ddp0 ) and the final (virtual link) forces and moments Fend and Muend.

## ◆ initWrenchNewtonEuler()

 bool iDynChain::initWrenchNewtonEuler ( const yarp::sig::Vector & Fend, const yarp::sig::Vector & Muend )

Calls the proper method to set wrench variables in OneChainNewtonEuler: either initKinematicBase() or initKinematicEnd().

This method is protected and it is used by RigidBodyTransformation for setting the Kinematics variables.

Parameters
 Fend external force Muend external moment
Returns
true if succeed, false otherwise

Definition at line 1174 of file iDyn.cpp.

## ◆ operator=()

 iDynChain & iDynChain::operator= ( const iDynChain & c )

Copies a Chain object into the current one.

Parameters
 c is a reference to an object of type iKinChain
Returns
a reference to the current object

Definition at line 523 of file iDyn.cpp.

## ◆ prepareNewtonEuler()

 void iDynChain::prepareNewtonEuler ( const NewEulMode NewEulMode_s = DYNAMIC )

Prepare for the Newton-Euler recursive computation of forces and torques.

Definition at line 916 of file iDyn.cpp.

protected

Returns a pointer to the ith iDynLink.

Parameters
Returns

Definition at line 755 of file iDyn.cpp.

## ◆ setAng() [1/2]

 double iDynChain::setAng ( const unsigned int i, double q )

Sets the ith joint angle.

Parameters
 i is the Link position. Ang the new angle's value.
Returns
current ith joint angle (angle constraint is evaluated).

Definition at line 632 of file iDyn.cpp.

## ◆ setAng() [2/2]

 yarp::sig::Vector iCub::iDyn::iDynChain::setAng ( const yarp::sig::Vector & q )

Sets the free joint angles to values of q[i].

Parameters
 q is a vector containing values for DOF.
Returns
the actual DOF values (angles constraints are evaluated).

## ◆ setD2Ang() [1/2]

 double iDynChain::setD2Ang ( const unsigned int i, double ddq )

Sets the ith joint angle acceleration.

Parameters
 i is the Link position Ang the new angle's acceleration value
Returns
current ith joint angle (acceleration constraints can be evaluated - not done at the moment)

Definition at line 656 of file iDyn.cpp.

## ◆ setD2Ang() [2/2]

 yarp::sig::Vector iCub::iDyn::iDynChain::setD2Ang ( const yarp::sig::Vector & ddq )

Sets the free joint angles acceleration to values of ddq[i].

Parameters
 ddq is a vector containing values for each DOF's acceleration
Returns
the actual DOF acceleration values (acceleration constraints can be evaluated - not done at the moment)

## ◆ setDAng() [1/2]

 double iDynChain::setDAng ( const unsigned int i, double dq )

Sets the ith joint angle velocity.

Parameters
 i is the Link position Ang the new angle's velocity value
Returns
current ith joint angle (velocity constraints can be evaluated - not done at the moment)

Definition at line 644 of file iDyn.cpp.

## ◆ setDAng() [2/2]

 yarp::sig::Vector iCub::iDyn::iDynChain::setDAng ( const yarp::sig::Vector & dq )

Sets the free joint angles velocity to values of dq[i].

Parameters
 dq is a vector containing values for each DOF's velocity
Returns
the actual DOF velocity vector (velocity constraints can be evaluated - not done at the moment)

## ◆ setDynamicParameters() [1/2]

 bool iCub::iDyn::iDynChain::setDynamicParameters ( const unsigned int i, const double _m, const yarp::sig::Matrix & _HC, const yarp::sig::Matrix & _I )

Set the dynamic parameters of the i-th Link.

Parameters
 i the i-th Link _m is the Link mass _HC is the rototranslation matrix from the link frame to the center of mass _I is the Inertia matrix

## ◆ setDynamicParameters() [2/2]

 bool iCub::iDyn::iDynChain::setDynamicParameters ( const unsigned int i, const double _m, const yarp::sig::Matrix & _HC, const yarp::sig::Matrix & _I, const double _kr, const double _Fv, const double _Fs, const double _Im )

Set the dynamic parameters of the i-th Link with motor.

Parameters
 i the i-th Link _m is the Link mass _HC is the rototranslation matrix from the link frame to the center of mass _I is the Inertia matrix _kr is the rotor constant _Fv is the viscous friction constant _Fs is the static friction constant _Im is the rotor inertia

## ◆ setIterMode()

 void iDynChain::setIterMode ( const ChainComputationMode mode = KINFWD_WREBWD )

Set the computation mode during recursive computation of kinematics (w,dw,d2p,d2pC) and wrench variables(F,Mu,Tau).

The mode is NE_KIN_WRE_kw, where the suffix 'kw' identifies the modes for the kinematics ('k') and wrench ('w') computations: {FF,FB,BF,BB} where the F stands for FORWARD and B for BACKWARD. Default mode is KINFWD_WREBWD, which sets Kinematics=FORWARD and Wrench=BACKWARD.

Parameters
 mode NE_KIN_WRE_{FF,FB,BF,BB}

Definition at line 1302 of file iDyn.cpp.

## ◆ setIterModeKinematic()

 void iDynChain::setIterModeKinematic ( const ChainIterationMode _iterateMode_kinematics = FORWARD )

Set the iteration direction during recursive computation of kinematics variables (w,dw,d2p,d2pC).

Default is FORWARD, which is also set in the constructor.

Parameters
 _iterateMode_kinematics FORWARD/BACKWARD

Definition at line 1282 of file iDyn.cpp.

## ◆ setIterModeWrench()

 void iDynChain::setIterModeWrench ( const ChainIterationMode _iterateMode_wrench = BACKWARD )

Set the iteration direction during recursive computation of wrench variables (F,Mu,Tau).

Default is BACKWARD, which is also set in the constructor.

Parameters
 _iterateMode_wrench FORWARD/BACKWARD

Definition at line 1287 of file iDyn.cpp.

## ◆ setMass()

 bool iDynChain::setMass ( const unsigned int i, const double _m )

Returns
true if operation is successful, false otherwise

Definition at line 833 of file iDyn.cpp.

## ◆ setMasses()

 bool iDynChain::setMasses ( yarp::sig::Vector _m )

Set the link masses at once.

Returns
true if operation is successful, false otherwise

Definition at line 807 of file iDyn.cpp.

## ◆ setModeNewtonEuler()

 void iDynChain::setModeNewtonEuler ( const NewEulMode NewEulMode_s = DYNAMIC )

Set the computation mode for Newton-Euler (static/dynamic/etc)

Definition at line 1204 of file iDyn.cpp.

## ◆ setStaticParameters()

 bool iDynChain::setStaticParameters ( const unsigned int i, const double _m, const yarp::sig::Matrix & _HC )

Set the dynamic parameters of the i-th Link if the chain is in a static situation (inertia is null).

Parameters
 i the i-th Link _m is the Link mass _HC is the rototranslation matrix from the link frame to the center of mass

Definition at line 905 of file iDyn.cpp.

## ◆ TESTING_computeCOMJacobian() [1/3]

 Matrix iDynChain::TESTING_computeCOMJacobian ( const unsigned int iLink )

The chain considered for the computation is the entire kinematic chain, between links 0 and N-1. Hence, the Jacobian is 6xN (differently from the Jacobian of the i-th link which assumes the chain to be from 0 to iLink, and differently from the Jacobian of the end-effector which is 6xDOF).

Parameters
Returns
the Jacobian matrix of the COM of the iLink of the chain

Definition at line 1478 of file iDyn.cpp.

## ◆ TESTING_computeCOMJacobian() [2/3]

 yarp::sig::Matrix iCub::iDyn::iDynChain::TESTING_computeCOMJacobian ( const unsigned int iLink, const yarp::sig::Matrix & Pn )

Parameters
 iLinkN the index of the link, in the chain, being the final frame for the Jacobian computation Pn the matrix describing the roto-translational matrix between base and the iLink (in two different limbs)
Returns
the Jacobian matrix of the COM of the iLink of the chain

## ◆ TESTING_computeCOMJacobian() [3/3]

 yarp::sig::Matrix iCub::iDyn::iDynChain::TESTING_computeCOMJacobian ( const unsigned int iLink, const yarp::sig::Matrix & Pn, const yarp::sig::Matrix & _H0 )

Parameters
 iLinkN the index of the link, in the chain, being the final frame for the Jacobian computation Pn the matrix describing the roto-translational matrix between base and the iLink (in two different limbs) _H0 the matrix to initialize the jacobian computation, usually taking into account the previous limb
Returns
the Jacobian matrix of the COM of the iLink of the chain

## ◆ iDynContactSolver

 friend class iDynContactSolver
friend

Definition at line 538 of file iDyn.h.

## ◆ iDynInvSensor

 friend class iDynInvSensor
friend

Definition at line 535 of file iDyn.h.

## ◆ iDynSensor

 friend class iDynSensor
friend

Definition at line 536 of file iDyn.h.

## ◆ OneChainNewtonEuler

 friend class OneChainNewtonEuler
friend

Definition at line 534 of file iDyn.h.

## ◆ RigidBodyTransformation

 friend class RigidBodyTransformation
friend

Definition at line 537 of file iDyn.h.

## ◆ curr_ddq

 yarp::sig::Vector iCub::iDyn::iDynChain::curr_ddq
protected

q acc

Definition at line 551 of file iDyn.h.

## ◆ curr_dq

 yarp::sig::Vector iCub::iDyn::iDynChain::curr_dq
protected

q vel

Definition at line 549 of file iDyn.h.

## ◆ iterateMode_kinematics

 ChainIterationMode iCub::iDyn::iDynChain::iterateMode_kinematics
protected

specifies the 'direction' of recursive computation of kinematics variables (w,dw,d2p): FORWARD, BACKWARD

Definition at line 543 of file iDyn.h.

## ◆ iterateMode_wrench

 ChainIterationMode iCub::iDyn::iDynChain::iterateMode_wrench
protected

specifies the 'direction' of recursive computation of wrenches (F,Mu): FORWARD, BACKWARD

Definition at line 545 of file iDyn.h.

## ◆ NE

 OneChainNewtonEuler* iCub::iDyn::iDynChain::NE
protected

pointer to OneChainNewtonEuler class, to be used for computing forces and torques

Definition at line 554 of file iDyn.h.

## ◆ zero0

 const yarp::sig::Vector iCub::iDyn::iDynChain::zero0
protected

Definition at line 556 of file iDyn.h.

The documentation for this class was generated from the following files:
• icub-main/src/libraries/iDyn/include/iCub/iDyn/iDyn.h
• icub-main/src/libraries/iDyn/src/iDyn.cpp