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

A Base class for defining a Serial Link Chain. More...

#include <iKinFwd.h>

+ Inheritance diagram for iCub::iKin::iKinChain:

Public Member Functions

 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<< (iKinLink &l)
 Adds a Link at the bottom of the Chain. More...
 
iKinChainoperator-- (int)
 Removes a Link from the bottom of the Chain. More...
 
iKinLinkoperator[] (const unsigned int i)
 Returns a reference to the ith Link of the Chain. More...
 
iKinLinkoperator() (const unsigned int i)
 Returns a reference to the ith Link of the Chain considering only those Links related to DOF. More...
 
bool addLink (const unsigned int i, iKinLink &l)
 Adds a Link at the position ith within the Chain. More...
 
bool rmLink (const unsigned int i)
 Removes the ith Link from the Chain. More...
 
void pushLink (iKinLink &l)
 Adds a Link at the bottom of the Chain. More...
 
void clear ()
 Removes all Links. More...
 
void popLink ()
 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)
 Releases the ith Link. More...
 
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...
 
void setAllLinkVerbosity (unsigned int _verbose)
 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 iKinChain &c)
 
virtual void build ()
 
virtual void dispose ()
 
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

unsigned int N
 
unsigned int DOF
 
unsigned int verbose
 
yarp::sig::Matrix H0
 
yarp::sig::Matrix HN
 
yarp::sig::Vector curr_q
 
std::deque< iKinLink * > allList
 
std::deque< iKinLink * > quickList
 
std::deque< unsigned int > hash
 
std::deque< unsigned int > hash_dof
 
yarp::sig::Matrix hess_J
 
yarp::sig::Matrix hess_Jlnk
 

Detailed Description

A Base class for defining a Serial Link Chain.

Definition at line 354 of file iKinFwd.h.

Constructor & Destructor Documentation

◆ iKinChain() [1/2]

iKinChain::iKinChain ( )

Default constructor.

Definition at line 256 of file iKinFwd.cpp.

◆ iKinChain() [2/2]

iKinChain::iKinChain ( const iKinChain c)

Creates a new Chain from an already existing Chain object.

Parameters
cis the Chain to be copied.

Definition at line 283 of file iKinFwd.cpp.

◆ ~iKinChain()

iKinChain::~iKinChain ( )
virtual

Destructor.

Definition at line 1300 of file iKinFwd.cpp.

Member Function Documentation

◆ addLink()

bool iKinChain::addLink ( const unsigned int  i,
iKinLink l 
)

Adds a Link at the position ith within the Chain.

Parameters
iis the ith position where the Link is to be added.
lis the Link to be added.
Returns
true if successful (e.g. param i is in range).

Definition at line 299 of file iKinFwd.cpp.

◆ AnaJacobian() [1/3]

Matrix iKinChain::AnaJacobian ( const unsigned int  i,
unsigned int  col 
)

Returns the analitical Jacobian of the ith link.

Parameters
iis the Link number.
colselects the part of the derived homogeneous matrix to be put in the upper side of the Jacobian matrix: 0 => x, 1 => y, 2 => z, 3 => p
Returns
the analitical Jacobian.

Definition at line 911 of file iKinFwd.cpp.

◆ AnaJacobian() [2/3]

yarp::sig::Matrix iCub::iKin::iKinChain::AnaJacobian ( const yarp::sig::Vector &  q,
unsigned int  col = 3 
)

Returns the analitical Jacobian of the end-effector computed in q.

Parameters
qis the vector of new DOF values.
colselects the part of the derived homogeneous matrix to be put in the upper side of the Jacobian matrix: 0 => x, 1 => y, 2 => z, 3 => p (default)
Returns
the analitical Jacobian.

◆ AnaJacobian() [3/3]

Matrix iKinChain::AnaJacobian ( unsigned int  col = 3)

Returns the analitical Jacobian of the end-effector.

Parameters
colselects the part of the derived homogeneous matrix to be put in the upper side of the Jacobian matrix: 0 => x, 1 => y, 2 => z, 3 => p (default)
Returns
the analitical Jacobian.

Definition at line 957 of file iKinFwd.cpp.

◆ blockLink() [1/2]

bool iCub::iKin::iKinChain::blockLink ( const unsigned int  i)
inline

Blocks the ith Link at the current value of its joint angle.

Chain DOF reduced by one.

Parameters
iis the Link number.
Returns
true if successful (e.g. param i is in range).

Definition at line 479 of file iKinFwd.h.

◆ blockLink() [2/2]

bool iKinChain::blockLink ( const unsigned int  i,
double  Ang 
)

Blocks the ith Link at the a certain value of its joint angle.

Chain DOF reduced by one.

Parameters
iis the Link number.
Angis the value of joint angle to which the Link is blocked.
Returns
true if successful (e.g. param i is in range).

Definition at line 394 of file iKinFwd.cpp.

◆ build()

void iKinChain::build ( )
protectedvirtual

Reimplemented in iCub::iDyn::iDynChain.

Definition at line 514 of file iKinFwd.cpp.

◆ clear()

void iKinChain::clear ( )

Removes all Links.

Definition at line 353 of file iKinFwd.cpp.

◆ clone()

void iKinChain::clone ( const iKinChain c)
protectedvirtual

Definition at line 264 of file iKinFwd.cpp.

◆ d2RotAng()

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

Definition at line 686 of file iKinFwd.cpp.

◆ dispose()

void iKinChain::dispose ( )
protectedvirtual

Reimplemented in iCub::iKin::iKinLimb, iCub::iDyn::iDynLimb, and iCub::iDyn::iDynChain.

Definition at line 1307 of file iKinFwd.cpp.

◆ DJacobian() [1/2]

yarp::sig::Matrix iCub::iKin::iKinChain::DJacobian ( const unsigned int  lnk,
const yarp::sig::Vector &  dq 
)

Compute the time derivative of the geometric Jacobian (link version).

Parameters
lnkis the Link number up to which consider the computation.
dqthe (lnk-1)x1 joint velocity vector.
Returns
the 6x(lnk-1) matrix \( \partial{^2}F\left(q\right)/\partial t \partial q. \)

◆ DJacobian() [2/2]

yarp::sig::Matrix iCub::iKin::iKinChain::DJacobian ( const yarp::sig::Vector &  dq)

Compute the time derivative of the geometric Jacobian.

Parameters
dqthe joint velocities.
Returns
the 6xDOF matrix \( \partial{^2}F\left(q\right)/\partial t \partial q. \)

◆ dRotAng()

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

Definition at line 673 of file iKinFwd.cpp.

◆ EndEffPose() [1/2]

Vector iKinChain::EndEffPose ( const bool  axisRep = true)

Returns the coordinates of end-effector.

Two notations are provided: the first with Euler Angles (XYZ form=>6x1 output vector) and second with axis/angle representation (default=>7x1 output vector).

Parameters
axisRepif true returns the axis/angle notation.
Returns
the end-effector pose.

Definition at line 850 of file iKinFwd.cpp.

◆ EndEffPose() [2/2]

yarp::sig::Vector iCub::iKin::iKinChain::EndEffPose ( const yarp::sig::Vector &  q,
const bool  axisRep = true 
)

Returns the coordinates of end-effector computed in q.

Two notations are provided: the first with Euler Angles (XYZ form=>6x1 output vector) and second with axis/angle representation (default=>7x1 output vector).

Parameters
qis the vector of new DOF values.
axisRepif true returns the axis/angle notation.
Returns
the end-effector pose.

◆ EndEffPosition() [1/2]

Vector iKinChain::EndEffPosition ( )

Returns the 3D coordinates of end-effector position.

Returns
the end-effector position.

Definition at line 894 of file iKinFwd.cpp.

◆ EndEffPosition() [2/2]

yarp::sig::Vector iCub::iKin::iKinChain::EndEffPosition ( const yarp::sig::Vector &  q)

Returns the 3D coordinates of end-effector position computed in q.

Parameters
qis the vector of new DOF values.
Returns
the end-effector position.

◆ fastHessian_ij() [1/2]

Vector iKinChain::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.

Fast Version: to be used in conjunction with prepareForHessian().

Parameters
iis the index of the first DOF.
jis the index of the second DOF.
Returns
the 6x1 vector \( \partial{^2}F\left(q\right)/\partial q_i \partial q_j. \)
Note
It is advisable to use this version when successive computations with different indexes values are needed.
See also
prepareForHessian

Definition at line 1117 of file iKinFwd.cpp.

◆ fastHessian_ij() [2/2]

Vector iKinChain::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.

Fast Version: to be used in conjunction with prepareForHessian(lnk).

Parameters
lnkis the Link number up to which consider the computation.
iis the index of the first link.
jis the index of the second link.
Returns
the 6x1 vector \( \partial{^2}F\left(q\right)/\partial q_i \partial q_j. \)
Note
It is advisable to use this version when successive computations with different indexes values are needed.
See also
prepareForHessian

Definition at line 1173 of file iKinFwd.cpp.

◆ GeoJacobian() [1/3]

Matrix iKinChain::GeoJacobian ( )

Returns the geometric Jacobian of the end-effector.

Returns
the 6xDOF geometric Jacobian matrix.
Note
The blocked links are not considered.

Definition at line 1048 of file iKinFwd.cpp.

◆ GeoJacobian() [2/3]

Matrix iKinChain::GeoJacobian ( const unsigned int  i)

Returns the geometric Jacobian of the ith link.

Parameters
iis the Link number.
Returns
the 6x(i-1) geometric Jacobian matrix.
Note
All the links are considered.

Definition at line 1012 of file iKinFwd.cpp.

◆ GeoJacobian() [3/3]

yarp::sig::Matrix iCub::iKin::iKinChain::GeoJacobian ( const yarp::sig::Vector &  q)

Returns the geometric Jacobian of the end-effector computed in q.

Parameters
qis the vector of new DOF values.
Returns
the geometric Jacobian.
Note
The blocked links are not considered.

◆ getAng() [1/2]

Vector iKinChain::getAng ( )

Returns the current free joint angles values.

Returns
the actual DOF values.

Definition at line 611 of file iKinFwd.cpp.

◆ getAng() [2/2]

double iKinChain::getAng ( const unsigned int  i)

Returns the current angle of ith joint.

Parameters
iis the Link number.
Returns
current ith joint angle.

Definition at line 645 of file iKinFwd.cpp.

◆ getConstraint()

bool iCub::iKin::iKinChain::getConstraint ( unsigned int  i)
inline

Returns the constraint status of ith link.

Returns
current constraint status.

Definition at line 523 of file iKinFwd.h.

◆ getDOF()

unsigned int iCub::iKin::iKinChain::getDOF ( ) const
inline

Returns the current number of Chain's DOF.

DOF=N-M, where N=# of Links, M=# of blocked Links.

Returns
number of DOF.

Definition at line 557 of file iKinFwd.h.

◆ getH() [1/3]

Matrix iKinChain::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).

Returns
H(N-1)*HN

Definition at line 778 of file iKinFwd.cpp.

◆ getH() [2/3]

Matrix iKinChain::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.

The second parameter if true enables the spannig over the full set of links, i.e. the blocked links as well.

Parameters
iis the Link number.
allLinkif true enables the spanning over the full set of links (false by default).
Returns
Hi

Definition at line 732 of file iKinFwd.cpp.

◆ getH() [3/3]

yarp::sig::Matrix iCub::iKin::iKinChain::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).

Parameters
qis the vector of new DOF values.
Returns
H(N-1)*HN

◆ getH0()

yarp::sig::Matrix iCub::iKin::iKinChain::getH0 ( ) const
inline

Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.

Returns
H0

Definition at line 564 of file iKinFwd.h.

◆ getHN()

yarp::sig::Matrix iCub::iKin::iKinChain::getHN ( ) const
inline

Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.

Returns
HN

Definition at line 579 of file iKinFwd.h.

◆ getN()

unsigned int iCub::iKin::iKinChain::getN ( ) const
inline

Returns the number of Links belonging to the Chain.

Returns
number of Links.

Definition at line 550 of file iKinFwd.h.

◆ getVerbosity()

unsigned int iCub::iKin::iKinChain::getVerbosity ( ) const
inline

Returns the current Chain verbosity level.

Returns
Link verbosity level.

Definition at line 544 of file iKinFwd.h.

◆ Hessian_ij() [1/2]

Vector iKinChain::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.

Parameters
iis the index of the first DOF.
jis the index of the second DOF.
Returns
the 6x1 vector \( \partial{^2}F\left(q\right)/\partial q_i \partial q_j. \)

Definition at line 1094 of file iKinFwd.cpp.

◆ Hessian_ij() [2/2]

Vector iKinChain::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.

Parameters
lnkis the Link number up to which consider the computation.
iis the index of the first link.
jis the index of the second link.
Returns
the 6x1 vector \( \partial{^2}F\left(q\right)/\partial q_i \partial q_j. \)

Definition at line 1149 of file iKinFwd.cpp.

◆ isLinkBlocked()

bool iKinChain::isLinkBlocked ( const unsigned int  i)

Queries whether the ith Link is blocked.

Parameters
iis the Link number.
Returns
true if blocked && (param i is in range).

Definition at line 483 of file iKinFwd.cpp.

◆ operator()()

iKinLink& iCub::iKin::iKinChain::operator() ( const unsigned int  i)
inline

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

Parameters
iis the Link number within the Chain (in DOF order)
Returns
a reference to the ith Link object.

Definition at line 427 of file iKinFwd.h.

◆ operator--()

iKinChain & iKinChain::operator-- ( int  )

Removes a Link from the bottom of the Chain.

Returns
a reference to the current object.

Definition at line 385 of file iKinFwd.cpp.

◆ operator<<()

iKinChain & iKinChain::operator<< ( iKinLink l)

Adds a Link at the bottom of the Chain.

Parameters
lis the Link to be added.
Returns
a reference to the current object.

Definition at line 366 of file iKinFwd.cpp.

◆ operator=()

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

Copies a Chain object into the current one.

Parameters
cis a reference to an object of type iKinChain.
Returns
a reference to the current object.

Definition at line 290 of file iKinFwd.cpp.

◆ operator[]()

iKinLink& iCub::iKin::iKinChain::operator[] ( const unsigned int  i)
inline

Returns a reference to the ith Link of the Chain.

Parameters
iis the Link number within the Chain.
Returns
a reference to the ith Link object.

Definition at line 419 of file iKinFwd.h.

◆ popLink()

void iKinChain::popLink ( )

Removes a Link from the bottom of the Chain.

See also
operator--()

Definition at line 375 of file iKinFwd.cpp.

◆ Pose()

Vector iKinChain::Pose ( const unsigned int  i,
const bool  axisRep = true 
)

Returns the coordinates of ith Link.

Two notations are provided: the first with Euler Angles (XYZ form=>6x1 output vector) and second with axis/angle representation (default=>7x1 output vector).

Parameters
iis the Link number.
axisRepif true returns the axis/angle notation.
Returns
the ith Link Pose.

Definition at line 803 of file iKinFwd.cpp.

◆ Position()

Vector iKinChain::Position ( const unsigned int  i)

Returns the 3D position coordinates of ith Link.

Parameters
iis the Link number.
Returns
the ith Link Position.

Definition at line 842 of file iKinFwd.cpp.

◆ prepareForHessian() [1/2]

void iKinChain::prepareForHessian ( )

Prepares computation for a successive call to fastHessian_ij().

See also
fastHessian_ij

Definition at line 1102 of file iKinFwd.cpp.

◆ prepareForHessian() [2/2]

void iKinChain::prepareForHessian ( const unsigned int  lnk)

Prepares computation for a successive call to fastHessian_ij() (link version).

Parameters
lnkis the Link number up to which consider the computation.
See also
fastHessian_ij

Definition at line 1158 of file iKinFwd.cpp.

◆ pushLink()

void iKinChain::pushLink ( iKinLink l)

Adds a Link at the bottom of the Chain.

Parameters
lis the Link to be added.
See also
operator<<

Definition at line 343 of file iKinFwd.cpp.

◆ releaseLink()

bool iKinChain::releaseLink ( const unsigned int  i)

Releases the ith Link.

Chain DOF augmented by one.

Parameters
iis the Link number.
Returns
true if successful (e.g. param i is in range).

Definition at line 463 of file iKinFwd.cpp.

◆ rmLink()

bool iKinChain::rmLink ( const unsigned int  i)

Removes the ith Link from the Chain.

Parameters
iis the ith position from which the Link is to be removed.
Returns
true if successful (e.g. param i is in range).

Definition at line 321 of file iKinFwd.cpp.

◆ RotAng()

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

Definition at line 659 of file iKinFwd.cpp.

◆ setAllConstraints()

void iKinChain::setAllConstraints ( bool  _constrained)

Sets the constraint status of all chain links.

Parameters
_constrainedis the new constraint status.

Definition at line 498 of file iKinFwd.cpp.

◆ setAllLinkVerbosity()

void iKinChain::setAllLinkVerbosity ( unsigned int  _verbose)

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

Parameters
_verboseis the verbosity level.

Definition at line 506 of file iKinFwd.cpp.

◆ setAng() [1/2]

double iKinChain::setAng ( const unsigned int  i,
double  _Ang 
)

Sets the ith joint angle.

Parameters
iis the Link number.
Angthe new angle's value.
Returns
current ith joint angle (angle constraint is evaluated).

Definition at line 623 of file iKinFwd.cpp.

◆ setAng() [2/2]

yarp::sig::Vector iCub::iKin::iKinChain::setAng ( const yarp::sig::Vector &  q)

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

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

◆ setBlockingValue()

bool iKinChain::setBlockingValue ( const unsigned int  i,
double  Ang 
)

Changes the value of the ith blocked Link.

Avoid the overhead required for DOFs handling.

Parameters
iis the Link number.
Angis the new value of joint angle to which the Link is blocked.
Returns
true if successful (e.g. param i is in range and the ith Link was already blocked).

Definition at line 414 of file iKinFwd.cpp.

◆ setConstraint()

void iCub::iKin::iKinChain::setConstraint ( unsigned int  i,
bool  _constrained 
)
inline

Sets the constraint status of ith link.

Parameters
_constrainedis the new constraint status.

Definition at line 517 of file iKinFwd.h.

◆ setH0()

bool iKinChain::setH0 ( const yarp::sig::Matrix &  _H0)

Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.

Parameters
H0
Returns
true if succeed, false otherwise.

Definition at line 562 of file iKinFwd.cpp.

◆ setHN()

bool iKinChain::setHN ( const yarp::sig::Matrix &  _HN)

Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.

Parameters
HN
Returns
true if succeed, false otherwise.

Definition at line 580 of file iKinFwd.cpp.

◆ setVerbosity()

void iCub::iKin::iKinChain::setVerbosity ( unsigned int  _verbose)
inline

Sets the verbosity level of the Chain.

Parameters
_verboseis a integer number which progressively enables different levels of warning messages. The larger this value the more detailed is the output.

Definition at line 538 of file iKinFwd.h.

Member Data Documentation

◆ allList

std::deque<iKinLink*> iCub::iKin::iKinChain::allList
protected

Definition at line 364 of file iKinFwd.h.

◆ curr_q

yarp::sig::Vector iCub::iKin::iKinChain::curr_q
protected

Definition at line 362 of file iKinFwd.h.

◆ DOF

unsigned int iCub::iKin::iKinChain::DOF
protected

Definition at line 358 of file iKinFwd.h.

◆ H0

yarp::sig::Matrix iCub::iKin::iKinChain::H0
protected

Definition at line 360 of file iKinFwd.h.

◆ hash

std::deque<unsigned int> iCub::iKin::iKinChain::hash
protected

Definition at line 367 of file iKinFwd.h.

◆ hash_dof

std::deque<unsigned int> iCub::iKin::iKinChain::hash_dof
protected

Definition at line 368 of file iKinFwd.h.

◆ hess_J

yarp::sig::Matrix iCub::iKin::iKinChain::hess_J
protected

Definition at line 370 of file iKinFwd.h.

◆ hess_Jlnk

yarp::sig::Matrix iCub::iKin::iKinChain::hess_Jlnk
protected

Definition at line 371 of file iKinFwd.h.

◆ HN

yarp::sig::Matrix iCub::iKin::iKinChain::HN
protected

Definition at line 361 of file iKinFwd.h.

◆ N

unsigned int iCub::iKin::iKinChain::N
protected

Definition at line 357 of file iKinFwd.h.

◆ quickList

std::deque<iKinLink*> iCub::iKin::iKinChain::quickList
protected

Definition at line 365 of file iKinFwd.h.

◆ verbose

unsigned int iCub::iKin::iKinChain::verbose
protected

Definition at line 359 of file iKinFwd.h.


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