58#include <yarp/os/Property.h>
59#include <yarp/dev/ControlBoardInterfaces.h>
60#include <yarp/sig/Vector.h>
61#include <yarp/sig/Matrix.h>
62#include <yarp/math/Math.h>
119 void addCumH(
const yarp::sig::Matrix &_cumH);
133 iKinLink(
double _A,
double _D,
double _Alpha,
double _Offset,
193 void setA(
const double _A) {
A=_A; }
205 void setD(
const double _D);
241 void setMin(
const double _Min);
253 void setMax(
const double _Max);
266 double setAng(
double _Ang);
275 yarp::sig::Matrix
getH(
bool c_override=
false);
284 yarp::sig::Matrix
getH(
double _Ang,
bool c_override=
false);
295 yarp::sig::Matrix
getDnH(
unsigned int n=1,
bool c_override=
false);
308 const yarp::sig::Matrix&,
const double,
359 yarp::sig::Matrix
H0;
360 yarp::sig::Matrix
HN;
373 virtual void build();
376 yarp::sig::Vector
RotAng(
const yarp::sig::Matrix &R);
377 yarp::sig::Vector
dRotAng(
const yarp::sig::Matrix &R,
const yarp::sig::Matrix &dR);
378 yarp::sig::Vector
d2RotAng(
const yarp::sig::Matrix &R,
const yarp::sig::Matrix &dRi,
379 const yarp::sig::Matrix &dRj,
const yarp::sig::Matrix &d2R);
442 bool rmLink(
const unsigned int i);
470 bool blockLink(
const unsigned int i,
double Ang);
549 unsigned int getN()
const {
return N; }
563 yarp::sig::Matrix
getH0()
const {
return H0; }
571 bool setH0(
const yarp::sig::Matrix &_H0);
578 yarp::sig::Matrix
getHN()
const {
return HN; }
586 bool setHN(
const yarp::sig::Matrix &_HN);
594 yarp::sig::Vector
setAng(
const yarp::sig::Vector &q);
600 yarp::sig::Vector
getAng();
609 double setAng(
const unsigned int i,
double _Ang);
616 double getAng(
const unsigned int i);
628 yarp::sig::Matrix
getH(
const unsigned int i,
const bool allLink=
false);
636 yarp::sig::Matrix
getH();
645 yarp::sig::Matrix
getH(
const yarp::sig::Vector &q);
656 yarp::sig::Vector
Pose(
const unsigned int i,
const bool axisRep=
true);
663 yarp::sig::Vector
Position(
const unsigned int i);
673 yarp::sig::Vector
EndEffPose(
const bool axisRep=
true);
684 yarp::sig::Vector
EndEffPose(
const yarp::sig::Vector &q,
const bool axisRep=
true);
707 yarp::sig::Matrix
AnaJacobian(
const unsigned int i,
unsigned int col);
727 yarp::sig::Matrix
AnaJacobian(
const yarp::sig::Vector &q,
unsigned int col=3);
735 yarp::sig::Matrix
GeoJacobian(
const unsigned int i);
764 yarp::sig::Vector
Hessian_ij(
const unsigned int i,
const unsigned int j);
789 yarp::sig::Vector
fastHessian_ij(
const unsigned int i,
const unsigned int j);
805 yarp::sig::Vector
Hessian_ij(
const unsigned int lnk,
const unsigned int i,
806 const unsigned int j);
836 yarp::sig::Vector
fastHessian_ij(
const unsigned int lnk,
const unsigned int i,
837 const unsigned int j);
846 yarp::sig::Matrix
DJacobian(
const yarp::sig::Vector &dq);
858 yarp::sig::Matrix
DJacobian(
const unsigned int lnk,
const yarp::sig::Vector &dq);
879 const std::string &tag, yarp::sig::Matrix &
H);
881 const std::string &tag, yarp::sig::Matrix &
H);
882 virtual void allocate(
const std::string &_type);
1161 virtual void allocate(
const std::string &_type);
1183 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1197 virtual void allocate(
const std::string &_type);
1211 iCubArm(
const std::string &_type);
1220 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1240 virtual void allocate(
const std::string &_type);
1287 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1309 yarp::sig::Vector &chainJoints);
1336 const yarp::sig::Vector &jointEncoders,
1337 yarp::sig::Vector &chainJoints,
1338 const yarp::sig::Matrix &jointEncodersBounds=yarp::math::zeros(1,2));
1352 virtual void allocate(
const std::string &_type);
1366 iCubLeg(
const std::string &_type);
1375 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1389 virtual void allocate(
const std::string &_type);
1403 iCubEye(
const std::string &_type);
1412 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1425 virtual void allocate(
const std::string &_type);
1452 virtual void allocate(
const std::string &_type);
1480 virtual void allocate(
const std::string &_type);
1502 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1517 virtual void allocate(
const std::string &_type);
A class for defining the iCub Arm.
virtual void allocate(const std::string &_type)
iCubArm()
Default constructor.
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Arm joints bounds with current values set aboard the iCub.
A class for defining the iCub Eye with the root reference frame attached to the neck.
virtual void allocate(const std::string &_type)
iCubEyeNeckRef()
Default constructor.
A class for defining the iCub Eye.
virtual void allocate(const std::string &_type)
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Eye joints bounds with current values set aboard the iCub.
iCubEye()
Default constructor.
A class for defining the iCub Finger.
virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders, const yarp::sig::Vector &jointEncoders, yarp::sig::Vector &chainJoints, const yarp::sig::Matrix &jointEncodersBounds=yarp::math::zeros(1, 2))
Retrieves the vector of actual finger's joint values (to be used in conjuction with the iKinLimb meth...
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the finger joints bounds with current values set aboard the iCub.
virtual void clone(const iCubFinger &finger)
iCubFinger()
Default constructor.
virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders, yarp::sig::Vector &chainJoints)
Retrieves the vector of actual finger's joint values (to be used in conjuction with the iKinLimb meth...
iCubFinger & operator=(const iCubFinger &finger)
Copies a Finger object into the current one.
double fingers_abduction_max
virtual void allocate(const std::string &_type)
A class for describing the kinematic of the straight line coming out from the point located between t...
virtual void allocate(const std::string &_type)
iCubHeadCenter()
Default constructor.
A class for defining the Kinematics of the Inertial Sensor mounted in the iCub's Waist.
virtual void allocate(const std::string &_type)
iCubInertialSensorWaist()
Default constructor.
A class for defining the Inertia Sensor Kinematics of the iCub.
iCubInertialSensor()
Default constructor.
virtual void allocate(const std::string &_type)
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Inertial Sensor joints bounds with current values set aboard the iCub.
A class for defining the iCub Leg.
iCubLeg()
Default constructor.
virtual void allocate(const std::string &_type)
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Leg joints bounds with current values set aboard the iCub.
A class for defining the iCub Torso.
virtual void allocate(const std::string &_type)
iCubTorso()
Default constructor.
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Torso joints bounds with current values set aboard the iCub.
A Base class for defining a Serial Link Chain.
yarp::sig::Vector EndEffPosition(const yarp::sig::Vector &q)
Returns the 3D coordinates of end-effector position computed in q.
void popLink()
Removes a Link from the bottom of the Chain.
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
yarp::sig::Vector Position(const unsigned int i)
Returns the 3D position coordinates of ith Link.
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
void prepareForHessian()
Prepares computation for a successive call to fastHessian_ij().
yarp::sig::Matrix GeoJacobian()
Returns the geometric Jacobian of the end-effector.
yarp::sig::Vector EndEffPosition()
Returns the 3D coordinates of end-effector position.
yarp::sig::Vector d2RotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dRi, const yarp::sig::Matrix &dRj, const yarp::sig::Matrix &d2R)
yarp::sig::Vector Hessian_ij(const unsigned int i, const unsigned int j)
Returns the 6x1 vector where is the forward kinematic function and is the DOF couple.
bool blockLink(const unsigned int i)
Blocks the ith Link at the current value of its joint angle.
unsigned int getVerbosity() const
Returns the current Chain verbosity level.
void pushLink(iKinLink &l)
Adds a Link at the bottom of the Chain.
bool isLinkBlocked(const unsigned int i)
Queries whether the ith Link is blocked.
yarp::sig::Matrix AnaJacobian(const yarp::sig::Vector &q, unsigned int col=3)
Returns the analitical Jacobian of the end-effector computed in q.
iKinChain & operator--(int)
Removes a Link from the bottom of the Chain.
yarp::sig::Matrix DJacobian(const unsigned int lnk, const yarp::sig::Vector &dq)
Compute the time derivative of the geometric Jacobian (link version).
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
bool releaseLink(const unsigned int i)
Releases the ith Link.
virtual void clone(const iKinChain &c)
void clear()
Removes all Links.
void setAllLinkVerbosity(unsigned int _verbose)
Sets the verbosity level of all Links belonging to the Chain.
yarp::sig::Vector fastHessian_ij(const unsigned int i, const unsigned int j)
Returns the 6x1 vector where is the forward kinematic function and is the DOF couple.
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
void setVerbosity(unsigned int _verbose)
Sets the verbosity level of the Chain.
void setConstraint(unsigned int i, bool _constrained)
Sets the constraint status of ith link.
iKinChain & operator=(const iKinChain &c)
Copies a Chain object into the current one.
bool rmLink(const unsigned int i)
Removes the ith Link from the Chain.
std::deque< unsigned int > hash_dof
yarp::sig::Vector EndEffPose(const yarp::sig::Vector &q, const bool axisRep=true)
Returns the coordinates of end-effector computed in q.
iKinChain()
Default constructor.
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
yarp::sig::Vector Pose(const unsigned int i, const bool axisRep=true)
Returns the coordinates of ith Link.
bool addLink(const unsigned int i, iKinLink &l)
Adds a Link at the position ith within the Chain.
yarp::sig::Vector getAng()
Returns the current free joint angles values.
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
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 ...
std::deque< unsigned int > hash
yarp::sig::Matrix getH()
Returns the rigid roto-translation matrix from the root reference frame to the end-effector frame in ...
iKinLink & operator()(const unsigned int i)
Returns a reference to the ith Link of the Chain considering only those Links related to DOF.
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
yarp::sig::Vector dRotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dR)
virtual ~iKinChain()
Destructor.
std::deque< iKinLink * > quickList
yarp::sig::Matrix DJacobian(const yarp::sig::Vector &dq)
Compute the time derivative of the geometric Jacobian.
bool getConstraint(unsigned int i)
Returns the constraint status of ith link.
yarp::sig::Vector RotAng(const yarp::sig::Matrix &R)
iKinLink & operator[](const unsigned int i)
Returns a reference to the ith Link of the Chain.
unsigned int getN() const
Returns the number of Links belonging to the Chain.
yarp::sig::Matrix hess_Jlnk
unsigned int getDOF() const
Returns the current number of Chain's DOF.
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
bool setH0(const yarp::sig::Matrix &_H0)
Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
iKinChain & operator<<(iKinLink &l)
Adds a Link at the bottom of the Chain.
yarp::sig::Matrix GeoJacobian(const yarp::sig::Vector &q)
Returns the geometric Jacobian of the end-effector computed in q.
std::deque< iKinLink * > allList
A class for defining the versions of the iCub limbs.
iKinLimbVersion operator-(const iKinLimbVersion &v) const
Overloaded - operator.
iKinLimbVersion & operator=(const iKinLimbVersion &v)
Overloaded assignment operator.
unsigned long int v_minor
bool operator==(const iKinLimbVersion &v) const
Overloaded == operator.
bool operator<=(const iKinLimbVersion &v) const
Overloaded <= operator.
bool operator>=(const iKinLimbVersion &v) const
Overloaded >=</=> operator.
bool operator!=(const iKinLimbVersion &v) const
Overloaded != operator.
unsigned long int get_minor() const
Return the minor version.
unsigned long int get_major() const
Return the major version.
bool operator>(const iKinLimbVersion &v) const
Overloaded > operator.
bool operator<(const iKinLimbVersion &v) const
Overloaded < operator.
unsigned long int v_major
iKinLimbVersion()
Default Constructor.
std::string get_version() const
Return the version string.
A class for defining generic Limb.
std::deque< iKinLink * > linkList
virtual void getMatrixFromProperties(const yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
iKinChain & operator<<(iKinLink &l)
iKinLink & operator[](const unsigned int i)
virtual ~iKinLimb()
Destructor.
iKinLink & operator()(const unsigned int i)
bool rmLink(const unsigned int i)
iKinChain & operator--(int)
bool fromLinksProperties(const yarp::os::Property &options)
Initializes the Limb from a list of properties wherein links parameters are specified.
std::string getType() const
Returns the Limb type as a string.
iKinChain & operator=(const iKinChain &c)
virtual void allocate(const std::string &_type)
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
void pushLink(iKinLink &l)
bool addLink(const unsigned int i, iKinLink &l)
virtual void setMatrixToProperties(yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &)
Alignes the Limb joints bounds with current values set aboard the robot.
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
iKinLimb(const yarp::os::Property &options)
Creates a new Limb from a list of properties wherein links parameters are specified.
iKinLimb()
Default constructor.
virtual void clone(const iKinLimb &limb)
bool isValid() const
Checks if the limb has been properly configured.
A Base class for defining a Link with standard Denavit-Hartenberg convention.
double setAng(double _Ang)
Sets the joint angle value.
virtual double getTorque() const
void setAlpha(const double _Alpha)
Sets the Link twist Alpha.
void setOffset(const double _Offset)
Sets the joint angle offset.
double getMax() const
Returns the joint angle upper bound.
virtual bool setCOM(const yarp::sig::Matrix &)
virtual double getFs() const
virtual double getIm() const
virtual bool setForce(const yarp::sig::Vector &, const yarp::sig::Vector &)
bool isBlocked() const
Returns the Link blocking status.
virtual void setPosVelAcc(const double, const double, const double)
virtual const yarp::sig::Matrix & getRC()
bool getConstraint() const
Returns the constraint status.
virtual const yarp::sig::Vector & getLinVel() const
virtual void setMass(const double)
void setVerbosity(unsigned int _verbose)
Sets Link verbosity level.
double getMin() const
Returns the joint angle lower bound.
virtual const yarp::sig::Vector & getdW() const
virtual ~iKinLink()
Default destructor.
virtual double getFv() const
virtual double getMass() const
void setConstraint(bool _constrained)
Sets the constraint status.
virtual bool setDynamicParameters(const double, const yarp::sig::Matrix &, const yarp::sig::Matrix &, const double, const double, const double, const double)
virtual const yarp::sig::Vector & getW() const
void setMax(const double _Max)
Sets the joint angle higher bound.
virtual bool setMoment(const yarp::sig::Vector &)
virtual void setTorque(const double)
yarp::sig::Matrix getDnH(unsigned int n=1, bool c_override=false)
Computes the derivative of order n of the homogeneous transformation matrix H with respect to the joi...
double getD() const
Returns the Link offset D.
virtual double setDAng(const double)
virtual bool setCOM(const yarp::sig::Vector &)
virtual bool setDynamicParameters(const double, const yarp::sig::Matrix &, const yarp::sig::Matrix &)
void addCumH(const yarp::sig::Matrix &_cumH)
virtual double getKr() const
double getA() const
Returns the Link length A.
void setA(const double _A)
Sets the Link length A.
iKinLink & operator=(const iKinLink &l)
Copies a Link object into the current one.
double getAlpha() const
Returns the Link twist Alpha.
void setMin(const double _Min)
Sets the joint angle lower bound.
virtual const yarp::sig::Vector & getLinAcc() const
yarp::sig::Matrix getH(bool c_override=false)
Computes the homogeneous transformation matrix H of the Link.
virtual const yarp::sig::Vector & getLinVelC() const
const yarp::sig::Vector zeros1
virtual double getD2Ang() const
virtual bool setStaticParameters(const double, const yarp::sig::Matrix &)
virtual const yarp::sig::Matrix & getCOM() const
virtual const yarp::sig::Vector & getLinAccC() const
virtual const yarp::sig::Vector & getMoment() const
virtual const yarp::sig::Matrix & getInertia() const
double getOffset() const
Returns the joint angle offset.
double getAng() const
Returns the current joint angle value.
virtual void clone(const iKinLink &l)
void setD(const double _D)
Sets the Link offset D.
unsigned int getVerbosity() const
Returns the current Link verbosity level.
virtual double setD2Ang(const double)
virtual const yarp::sig::Vector & getForce() const
virtual const yarp::sig::Vector & getr()
const yarp::sig::Matrix zeros1x1
virtual bool setInertia(const yarp::sig::Matrix &)
virtual const yarp::sig::Vector & getdWM() const
virtual double getDAng() const
virtual const yarp::sig::Vector & getrC()
virtual const yarp::sig::Matrix & getR()
constexpr double CTRL_PI
The PI constant.
void notImplemented(const unsigned int verbose)
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.