Go to the documentation of this file.
59 #include <yarp/os/Property.h>
60 #include <yarp/dev/ControlBoardInterfaces.h>
61 #include <yarp/sig/Vector.h>
62 #include <yarp/sig/Matrix.h>
63 #include <yarp/math/Math.h>
120 void addCumH(
const yarp::sig::Matrix &_cumH);
134 iKinLink(
double _A,
double _D,
double _Alpha,
double _Offset,
194 void setA(
const double _A) {
A=_A; }
206 void setD(
const double _D);
242 void setMin(
const double _Min);
254 void setMax(
const double _Max);
267 double setAng(
double _Ang);
276 yarp::sig::Matrix
getH(
bool c_override=
false);
285 yarp::sig::Matrix
getH(
double _Ang,
bool c_override=
false);
296 yarp::sig::Matrix
getDnH(
unsigned int n=1,
bool c_override=
false);
309 const yarp::sig::Matrix&,
const double,
360 yarp::sig::Matrix
H0;
361 yarp::sig::Matrix
HN;
374 virtual void build();
377 yarp::sig::Vector
RotAng(
const yarp::sig::Matrix &R);
378 yarp::sig::Vector
dRotAng(
const yarp::sig::Matrix &R,
const yarp::sig::Matrix &dR);
379 yarp::sig::Vector
d2RotAng(
const yarp::sig::Matrix &R,
const yarp::sig::Matrix &dRi,
380 const yarp::sig::Matrix &dRj,
const yarp::sig::Matrix &d2R);
443 bool rmLink(
const unsigned int i);
471 bool blockLink(
const unsigned int i,
double Ang);
550 unsigned int getN()
const {
return N; }
564 yarp::sig::Matrix
getH0()
const {
return H0; }
572 bool setH0(
const yarp::sig::Matrix &_H0);
579 yarp::sig::Matrix
getHN()
const {
return HN; }
587 bool setHN(
const yarp::sig::Matrix &_HN);
595 yarp::sig::Vector
setAng(
const yarp::sig::Vector &q);
601 yarp::sig::Vector
getAng();
610 double setAng(
const unsigned int i,
double _Ang);
617 double getAng(
const unsigned int i);
629 yarp::sig::Matrix
getH(
const unsigned int i,
const bool allLink=
false);
637 yarp::sig::Matrix
getH();
646 yarp::sig::Matrix
getH(
const yarp::sig::Vector &q);
657 yarp::sig::Vector
Pose(
const unsigned int i,
const bool axisRep=
true);
664 yarp::sig::Vector
Position(
const unsigned int i);
674 yarp::sig::Vector
EndEffPose(
const bool axisRep=
true);
685 yarp::sig::Vector
EndEffPose(
const yarp::sig::Vector &q,
const bool axisRep=
true);
708 yarp::sig::Matrix
AnaJacobian(
const unsigned int i,
unsigned int col);
728 yarp::sig::Matrix
AnaJacobian(
const yarp::sig::Vector &q,
unsigned int col=3);
736 yarp::sig::Matrix
GeoJacobian(
const unsigned int i);
752 yarp::sig::Matrix
GeoJacobian(
const yarp::sig::Vector &q);
765 yarp::sig::Vector
Hessian_ij(
const unsigned int i,
const unsigned int j);
790 yarp::sig::Vector
fastHessian_ij(
const unsigned int i,
const unsigned int j);
806 yarp::sig::Vector
Hessian_ij(
const unsigned int lnk,
const unsigned int i,
807 const unsigned int j);
837 yarp::sig::Vector
fastHessian_ij(
const unsigned int lnk,
const unsigned int i,
838 const unsigned int j);
847 yarp::sig::Matrix
DJacobian(
const yarp::sig::Vector &dq);
859 yarp::sig::Matrix
DJacobian(
const unsigned int lnk,
const yarp::sig::Vector &dq);
927 iKinLimb(
const yarp::os::Property &options);
1072 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1109 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1176 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1197 virtual bool getChainJoints(
const yarp::sig::Vector &motorEncoders,
1198 yarp::sig::Vector &chainJoints);
1224 virtual bool getChainJoints(
const yarp::sig::Vector &motorEncoders,
1225 const yarp::sig::Vector &jointEncoders,
1226 yarp::sig::Vector &chainJoints,
1264 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1301 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
1391 virtual bool alignJointsBounds(
const std::deque<yarp::dev::IControlLimits*> &lim);
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Arm joints bounds with current values set aboard the iCub.
yarp::sig::Matrix getH()
Returns the rigid roto-translation matrix from the root reference frame to the end-effector frame in ...
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
virtual double getFs() const
virtual void allocate(const std::string &_type)
double getA() const
Returns the Link length A.
yarp::sig::Vector dRotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dR)
virtual double getTorque() const
double getAlpha() const
Returns the Link twist Alpha.
virtual const yarp::sig::Matrix & getRC()
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
yarp::sig::Vector RotAng(const yarp::sig::Matrix &R)
virtual const yarp::sig::Vector & getLinVelC() const
virtual void allocate(const std::string &_type)
double getMin() const
Returns the joint angle lower bound.
virtual ~iKinChain()
Destructor.
bool isLinkBlocked(const unsigned int i)
Queries whether the ith Link is blocked.
yarp::sig::Vector Position(const unsigned int i)
Returns the 3D position coordinates of ith Link.
unsigned int getVerbosity() const
Returns the current Chain verbosity level.
virtual void clone(const iCubFinger &finger)
void notImplemented(const unsigned int verbose)
iCubLeg()
Default constructor.
yarp::sig::Vector EndEffPosition()
Returns the 3D coordinates of end-effector position.
iKinLimb()
Default constructor.
void setMax(const double _Max)
Sets the joint angle higher bound.
iKinChain & operator=(const iKinChain &c)
Copies a Chain object into the current one.
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Torso joints bounds with current values set aboard the iCub.
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.
iKinLink & operator=(const iKinLink &l)
Copies a Link object into the current one.
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &)
Alignes the Limb joints bounds with current values set aboard the robot.
virtual ~iKinLink()
Default destructor.
virtual const yarp::sig::Vector & getMoment() const
unsigned int getVerbosity() const
Returns the current Link verbosity level.
virtual bool setCOM(const yarp::sig::Vector &)
virtual bool setMoment(const yarp::sig::Vector &)
virtual void allocate(const std::string &_type)
std::deque< unsigned int > hash
iKinChain & operator--(int)
Removes a Link from the bottom of the Chain.
iKinLink & operator[](const unsigned int i)
bool releaseLink(const unsigned int i)
Releases the ith Link.
virtual double getIm() const
bool setH0(const yarp::sig::Matrix &_H0)
Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
void popLink()
Removes a Link from the bottom of the Chain.
virtual const yarp::sig::Vector & getdW() const
iKinLink & operator[](const unsigned int i)
Returns a reference to the ith Link of the Chain.
double getOffset() const
Returns the joint angle offset.
void clear()
Removes all Links.
yarp::sig::Vector d2RotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dRi, const yarp::sig::Matrix &dRj, const yarp::sig::Matrix &d2R)
bool isBlocked() const
Returns the Link blocking status.
iCubArm()
Default constructor.
void addCumH(const yarp::sig::Matrix &_cumH)
yarp::sig::Vector Pose(const unsigned int i, const bool axisRep=true)
Returns the coordinates of ith Link.
std::deque< iKinLink * > linkList
virtual const yarp::sig::Vector & getLinAcc() const
unsigned int getDOF() const
Returns the current number of Chain's DOF.
bool fromLinksProperties(const yarp::os::Property &options)
Initializes the Limb from a list of properties wherein links parameters are specified.
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Leg joints bounds with current values set aboard the iCub.
virtual const yarp::sig::Vector & getdWM() const
virtual void clone(const iKinLink &l)
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
virtual const yarp::sig::Matrix & getCOM() const
virtual void allocate(const std::string &_type)
void setConstraint(unsigned int i, bool _constrained)
Sets the constraint status of ith link.
void setD(const double _D)
Sets the Link offset D.
virtual void allocate(const std::string &_type)
virtual bool setStaticParameters(const double, const yarp::sig::Matrix &)
virtual void clone(const iKinChain &c)
iKinLink & operator()(const unsigned int i)
void pushLink(iKinLink &l)
iCubEyeNeckRef()
Default constructor.
iKinChain()
Default constructor.
bool rmLink(const unsigned int i)
yarp::sig::Vector getAng()
Returns the current free joint angles values.
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
yarp::sig::Matrix DJacobian(const yarp::sig::Vector &dq)
Compute the time derivative of the geometric Jacobian.
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Inertial Sensor joints bounds with current values set aboard the iCub.
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
iKinChain & operator=(const iKinChain &c)
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.
double getD() const
Returns the Link offset D.
virtual void setMatrixToProperties(yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
bool addLink(const unsigned int i, iKinLink &l)
virtual const yarp::sig::Vector & getW() const
double setAng(double _Ang)
Sets the joint angle value.
yarp::sig::Matrix GeoJacobian()
Returns the geometric Jacobian of the end-effector.
bool getConstraint(unsigned int i)
Returns the constraint status of ith link.
virtual void allocate(const std::string &_type)
virtual void allocate(const std::string &_type)
bool addLink(const unsigned int i, iKinLink &l)
Adds a Link at the position ith within the Chain.
void pushLink(iKinLink &l)
Adds a Link at the bottom of the Chain.
virtual bool setInertia(const yarp::sig::Matrix &)
double fingers_abduction_max
void setVerbosity(unsigned int _verbose)
Sets Link verbosity level.
unsigned int getN() const
Returns the number of Links belonging to the Chain.
virtual void clone(const iKinLimb &limb)
std::deque< unsigned int > hash_dof
void setOffset(const double _Offset)
Sets the joint angle offset.
void prepareForHessian()
Prepares computation for a successive call to fastHessian_ij().
virtual void allocate(const std::string &_type)
virtual void allocate(const std::string &_type)
std::deque< iKinLink * > quickList
virtual void getMatrixFromProperties(const yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
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...
virtual const yarp::sig::Matrix & getInertia() const
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
void setAlpha(const double _Alpha)
Sets the Link twist Alpha.
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the finger joints bounds with current values set aboard the iCub.
virtual void setPosVelAcc(const double, const double, const double)
virtual const yarp::sig::Matrix & getR()
iCubFinger()
Default constructor.
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
constexpr double CTRL_PI
The PI constant.
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Eye joints bounds with current values set aboard the iCub.
iCubInertialSensorWaist()
Default constructor.
virtual bool setDynamicParameters(const double, const yarp::sig::Matrix &, const yarp::sig::Matrix &, const double, const double, const double, const double)
iCubInertialSensor()
Default constructor.
void setConstraint(bool _constrained)
Sets the constraint status.
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
virtual bool setDynamicParameters(const double, const yarp::sig::Matrix &, const yarp::sig::Matrix &)
virtual const yarp::sig::Vector & getLinVel() const
double getMax() const
Returns the joint angle upper bound.
yarp::sig::Matrix hess_Jlnk
virtual const yarp::sig::Vector & getForce() const
virtual ~iKinLimb()
Destructor.
virtual const yarp::sig::Vector & getrC()
bool rmLink(const unsigned int i)
Removes the ith Link from the Chain.
bool getConstraint() const
Returns the constraint status.
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 setAllLinkVerbosity(unsigned int _verbose)
Sets the verbosity level of all Links belonging to the Chain.
iKinLink & operator()(const unsigned int i)
Returns a reference to the ith Link of the Chain considering only those Links related to DOF.
double getAng() const
Returns the current joint angle value.
iKinChain & operator--(int)
const yarp::sig::Vector zeros1
std::deque< iKinLink * > allList
virtual bool setCOM(const yarp::sig::Matrix &)
virtual double getFv() const
iCubHeadCenter()
Default constructor.
virtual double setDAng(const double)
virtual double getDAng() const
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
bool blockLink(const unsigned int i)
Blocks the ith Link at the current value of its joint angle.
yarp::sig::Matrix getH(bool c_override=false)
Computes the homogeneous transformation matrix H of the Link.
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
bool isValid() const
Checks if the limb has been properly configured.
virtual const yarp::sig::Vector & getLinAccC() const
virtual double getKr() const
const yarp::sig::Matrix zeros1x1
virtual bool setForce(const yarp::sig::Vector &, const yarp::sig::Vector &)
virtual void allocate(const std::string &_type)
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...
virtual double getD2Ang() const
iCubTorso()
Default constructor.
iKinChain & operator<<(iKinLink &l)
Adds a Link at the bottom of the Chain.
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
std::string getType() const
Returns the Limb type as a string.
virtual void setTorque(const double)
iCubFinger & operator=(const iCubFinger &finger)
Copies a Finger object into the current one.
iKinChain & operator<<(iKinLink &l)
virtual void setMass(const double)
virtual const yarp::sig::Vector & getr()
virtual double getMass() const
void setA(const double _A)
Sets the Link length A.
iCubEye()
Default constructor.
virtual double setD2Ang(const double)
void setMin(const double _Min)
Sets the joint angle lower bound.