iCub-main
iKinFwd.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
53 #ifndef __IKINFWD_H__
54 #define __IKINFWD_H__
55 
56 #include <string>
57 #include <deque>
58 
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>
64 
65 #include <iCub/ctrl/math.h>
66 
67 
68 namespace iCub
69 {
70 
71 namespace iKin
72 {
73 
74 void notImplemented(const unsigned int verbose);
75 
85 class iKinLink
86 {
87 protected:
88  double A;
89  double D;
90  double Alpha;
91  double Offset;
92  double c_alpha;
93  double s_alpha;
94  double Min;
95  double Max;
96  double Ang;
97  bool blocked;
98  bool cumulative;
100  unsigned int verbose;
101 
102  yarp::sig::Matrix H;
103  yarp::sig::Matrix cumH;
104  yarp::sig::Matrix DnH;
105 
106  const yarp::sig::Matrix zeros1x1;
107  const yarp::sig::Vector zeros1;
108 
109  friend class iKinChain;
110 
111  // Default constructor: not implemented.
113 
114  virtual void clone(const iKinLink &l);
115  bool isCumulative() { return cumulative; }
116  void block() { blocked=true; }
117  void block(double _Ang) { setAng(_Ang); blocked=true; }
118  void release() { blocked=false; }
119  void rmCumH() { cumulative=false; }
120  void addCumH(const yarp::sig::Matrix &_cumH);
121 
122 public:
134  iKinLink(double _A, double _D, double _Alpha, double _Offset,
135  double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
136 
141  iKinLink(const iKinLink &l);
142 
148  iKinLink &operator=(const iKinLink &l);
149 
155  void setConstraint(bool _constrained) { constrained=_constrained; }
156 
161  bool getConstraint() const { return constrained; }
162 
170  void setVerbosity(unsigned int _verbose) { verbose=_verbose; }
171 
176  unsigned int getVerbosity() const { return verbose; }
177 
182  bool isBlocked() const { return blocked; }
183 
188  double getA() const { return A; }
189 
194  void setA(const double _A) { A=_A; }
195 
200  double getD() const { return D; }
201 
206  void setD(const double _D);
207 
212  double getAlpha() const { return Alpha; }
213 
218  void setAlpha(const double _Alpha);
219 
224  double getOffset() const { return Offset; }
225 
230  void setOffset(const double _Offset) { Offset=_Offset; }
231 
236  double getMin() const { return Min; }
237 
242  void setMin(const double _Min);
243 
248  double getMax() const { return Max; }
249 
254  void setMax(const double _Max);
255 
260  double getAng() const { return Ang; }
261 
267  double setAng(double _Ang);
268 
276  yarp::sig::Matrix getH(bool c_override=false);
277 
285  yarp::sig::Matrix getH(double _Ang, bool c_override=false);
286 
296  yarp::sig::Matrix getDnH(unsigned int n=1, bool c_override=false);
297 
301  virtual ~iKinLink() { }
302 
303  // void methods for iDyn
304  // set
305  virtual double setDAng(const double) { notImplemented(verbose); return 0.0; }
306  virtual double setD2Ang(const double) { notImplemented(verbose); return 0.0; }
307  virtual void setPosVelAcc(const double, const double, const double) { notImplemented(verbose); }
308  virtual bool setDynamicParameters(const double, const yarp::sig::Matrix&,
309  const yarp::sig::Matrix&, const double,
310  const double, const double, const double) { notImplemented(verbose); return false; }
311  virtual bool setDynamicParameters(const double, const yarp::sig::Matrix&,
312  const yarp::sig::Matrix&) { notImplemented(verbose); return false; }
313  virtual bool setStaticParameters(const double, const yarp::sig::Matrix&) { notImplemented(verbose); return false; }
314  virtual bool setInertia(const yarp::sig::Matrix &) { notImplemented(verbose); return false; }
315  virtual void setMass(const double) { notImplemented(verbose); }
316  virtual bool setCOM(const yarp::sig::Matrix&) { notImplemented(verbose); return false; }
317  virtual bool setCOM(const yarp::sig::Vector&) { notImplemented(verbose); return false; }
318  virtual bool setForce(const yarp::sig::Vector&, const yarp::sig::Vector&) { notImplemented(verbose); return false; }
319  virtual bool setMoment(const yarp::sig::Vector&) { notImplemented(verbose); return false; }
320  virtual void setTorque(const double) { notImplemented(verbose); }
321 
322  // get
323  virtual const yarp::sig::Matrix &getInertia() const { notImplemented(verbose); return zeros1x1; }
324  virtual double getMass() const { notImplemented(verbose); return 0.0; }
325  virtual double getIm() const { notImplemented(verbose); return 0.0; }
326  virtual double getKr() const { notImplemented(verbose); return 0.0; }
327  virtual double getFs() const { notImplemented(verbose); return 0.0; }
328  virtual double getFv() const { notImplemented(verbose); return 0.0; }
329  virtual const yarp::sig::Matrix &getCOM() const { notImplemented(verbose); return zeros1x1; }
330  virtual double getDAng() const { notImplemented(verbose); return 0.0; }
331  virtual double getD2Ang() const { notImplemented(verbose); return 0.0; }
332  virtual const yarp::sig::Matrix &getR() { notImplemented(verbose); return zeros1x1; }
333  virtual const yarp::sig::Matrix &getRC() { notImplemented(verbose); return zeros1x1; }
334  virtual const yarp::sig::Vector &getr() { notImplemented(verbose); return zeros1; }
335  virtual const yarp::sig::Vector &getrC() { notImplemented(verbose); return zeros1; }
336  virtual const yarp::sig::Vector &getW() const { notImplemented(verbose); return zeros1; }
337  virtual const yarp::sig::Vector &getdW() const { notImplemented(verbose); return zeros1; }
338  virtual const yarp::sig::Vector &getdWM() const { notImplemented(verbose); return zeros1; }
339  virtual const yarp::sig::Vector &getLinAcc() const { notImplemented(verbose); return zeros1; }
340  virtual const yarp::sig::Vector &getLinAccC() const { notImplemented(verbose); return zeros1; }
341  virtual const yarp::sig::Vector &getLinVel() const { notImplemented(verbose); return zeros1; }
342  virtual const yarp::sig::Vector &getLinVelC() const { notImplemented(verbose); return zeros1; }
343  virtual const yarp::sig::Vector &getForce() const { notImplemented(verbose); return zeros1; }
344  virtual const yarp::sig::Vector &getMoment() const { notImplemented(verbose); return zeros1; }
345  virtual double getTorque() const { notImplemented(verbose); return 0.0; }
346 };
347 
348 
355 {
356 protected:
357  unsigned int N;
358  unsigned int DOF;
359  unsigned int verbose;
360  yarp::sig::Matrix H0;
361  yarp::sig::Matrix HN;
362  yarp::sig::Vector curr_q;
363 
364  std::deque<iKinLink*> allList;
365  std::deque<iKinLink*> quickList;
366 
367  std::deque<unsigned int> hash;
368  std::deque<unsigned int> hash_dof;
369 
370  yarp::sig::Matrix hess_J;
371  yarp::sig::Matrix hess_Jlnk;
372 
373  virtual void clone(const iKinChain &c);
374  virtual void build();
375  virtual void dispose();
376 
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);
381 
382 public:
386  iKinChain();
387 
392  iKinChain(const iKinChain &c);
393 
399  iKinChain &operator=(const iKinChain &c);
400 
407 
412  iKinChain &operator--(int);
413 
419  iKinLink &operator[](const unsigned int i) { return *allList[i]; }
420 
427  iKinLink &operator()(const unsigned int i) { return *quickList[hash_dof[i]]; }
428 
435  bool addLink(const unsigned int i, iKinLink &l);
436 
443  bool rmLink(const unsigned int i);
444 
450  void pushLink(iKinLink &l);
451 
455  void clear();
456 
461  void popLink();
462 
471  bool blockLink(const unsigned int i, double Ang);
472 
479  bool blockLink(const unsigned int i) { return blockLink(i,getAng(i)); }
480 
490  bool setBlockingValue(const unsigned int i, double Ang);
491 
498  bool releaseLink(const unsigned int i);
499 
505  bool isLinkBlocked(const unsigned int i);
506 
511  void setAllConstraints(bool _constrained);
512 
517  void setConstraint(unsigned int i, bool _constrained) { allList[i]->setConstraint(_constrained); }
518 
523  bool getConstraint(unsigned int i) { return allList[i]->getConstraint(); }
524 
529  void setAllLinkVerbosity(unsigned int _verbose);
530 
538  void setVerbosity(unsigned int _verbose) { verbose=_verbose; }
539 
544  unsigned int getVerbosity() const { return verbose; }
545 
550  unsigned int getN() const { return N; }
551 
557  unsigned int getDOF() const { return DOF; }
558 
564  yarp::sig::Matrix getH0() const { return H0; }
565 
572  bool setH0(const yarp::sig::Matrix &_H0);
573 
579  yarp::sig::Matrix getHN() const { return HN; }
580 
587  bool setHN(const yarp::sig::Matrix &_HN);
588 
595  yarp::sig::Vector setAng(const yarp::sig::Vector &q);
596 
601  yarp::sig::Vector getAng();
602 
610  double setAng(const unsigned int i, double _Ang);
611 
617  double getAng(const unsigned int i);
618 
629  yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false);
630 
637  yarp::sig::Matrix getH();
638 
646  yarp::sig::Matrix getH(const yarp::sig::Vector &q);
647 
657  yarp::sig::Vector Pose(const unsigned int i, const bool axisRep=true);
658 
664  yarp::sig::Vector Position(const unsigned int i);
665 
674  yarp::sig::Vector EndEffPose(const bool axisRep=true);
675 
685  yarp::sig::Vector EndEffPose(const yarp::sig::Vector &q, const bool axisRep=true);
686 
691  yarp::sig::Vector EndEffPosition();
692 
698  yarp::sig::Vector EndEffPosition(const yarp::sig::Vector &q);
699 
708  yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col);
709 
717  yarp::sig::Matrix AnaJacobian(unsigned int col=3);
718 
728  yarp::sig::Matrix AnaJacobian(const yarp::sig::Vector &q, unsigned int col=3);
729 
736  yarp::sig::Matrix GeoJacobian(const unsigned int i);
737 
743  yarp::sig::Matrix GeoJacobian();
744 
752  yarp::sig::Matrix GeoJacobian(const yarp::sig::Vector &q);
753 
765  yarp::sig::Vector Hessian_ij(const unsigned int i, const unsigned int j);
766 
772  void prepareForHessian();
773 
790  yarp::sig::Vector fastHessian_ij(const unsigned int i, const unsigned int j);
791 
806  yarp::sig::Vector Hessian_ij(const unsigned int lnk, const unsigned int i,
807  const unsigned int j);
808 
816  void prepareForHessian(const unsigned int lnk);
817 
837  yarp::sig::Vector fastHessian_ij(const unsigned int lnk, const unsigned int i,
838  const unsigned int j);
839 
847  yarp::sig::Matrix DJacobian(const yarp::sig::Vector &dq);
848 
859  yarp::sig::Matrix DJacobian(const unsigned int lnk, const yarp::sig::Vector &dq);
860 
864  virtual ~iKinChain();
865 };
866 
867 
873 class iKinLimb : public iKinChain
874 {
875 protected:
876  std::deque<iKinLink*> linkList;
877  std::string type;
878 
879  virtual void getMatrixFromProperties(const yarp::os::Property &options,
880  const std::string &tag, yarp::sig::Matrix &H);
881  virtual void setMatrixToProperties(yarp::os::Property &options,
882  const std::string &tag, yarp::sig::Matrix &H);
883  virtual void allocate(const std::string &_type);
884  virtual void clone(const iKinLimb &limb);
885  virtual void dispose();
886 
887  // make the following methods protected in order to prevent user from changing
888  // too easily the internal structure of the chain;
889  // to get access anyway to these hidden methods, user can rely on asChain()
893  iKinLink &operator[](const unsigned int i) { return iKinChain::operator[](i); }
894  iKinLink &operator()(const unsigned int i) { return iKinChain::operator()(i); }
895  bool addLink(const unsigned int i, iKinLink &l) { return iKinChain::addLink(i,l); }
896  bool rmLink(const unsigned int i) { return iKinChain::rmLink(i); }
898  void clear() { iKinChain::clear(); }
900  void pushLink(iKinLink *pl);
901 
902 public:
906  iKinLimb();
907 
913  iKinLimb(const std::string &_type);
914 
919  iKinLimb(const iKinLimb &limb);
920 
927  iKinLimb(const yarp::os::Property &options);
928 
985  bool fromLinksProperties(const yarp::os::Property &options);
986 
992  bool toLinksProperties(yarp::os::Property &options);
993 
998  bool isValid() const { return (N>0); }
999 
1005  iKinLimb &operator=(const iKinLimb &limb);
1006 
1013  iKinChain *asChain() { return static_cast<iKinChain*>(this); }
1014 
1019  std::string getType() const { return type; }
1020 
1031  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*>&) { notImplemented(verbose); return true; }
1032 
1036  virtual ~iKinLimb();
1037 };
1038 
1039 
1046 {
1047 protected:
1048  unsigned long int v_major;
1049  unsigned long int v_minor;
1050 
1051 public:
1055  iKinLimbVersion();
1056 
1062  iKinLimbVersion(const std::string &version);
1063 
1069  iKinLimbVersion(const unsigned long int v_major, const unsigned long int v_minor);
1070 
1074  iKinLimbVersion(const iKinLimbVersion& v);
1075 
1080  unsigned long int get_major() const;
1081 
1086  unsigned long int get_minor() const;
1087 
1092  std::string get_version() const;
1093 
1100 
1106  bool operator<(const iKinLimbVersion& v) const;
1107 
1113  bool operator<=(const iKinLimbVersion& v) const;
1114 
1120  bool operator==(const iKinLimbVersion& v) const;
1121 
1127  bool operator!=(const iKinLimbVersion& v) const;
1128 
1134  bool operator>=(const iKinLimbVersion& v) const;
1135 
1141  bool operator>(const iKinLimbVersion& v) const;
1142 
1148  iKinLimbVersion operator-(const iKinLimbVersion& v) const;
1149 };
1150 
1151 
1157 class iCubTorso : public iKinLimb
1158 {
1159 protected:
1161 
1162  virtual void allocate(const std::string &_type);
1163 
1164 public:
1168  iCubTorso();
1169 
1175  iCubTorso(const std::string &_type);
1176 
1184  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1185 };
1186 
1187 
1193 class iCubArm : public iKinLimb
1194 {
1195 protected:
1197 
1198  virtual void allocate(const std::string &_type);
1199 
1200 public:
1204  iCubArm();
1205 
1212  iCubArm(const std::string &_type);
1213 
1221  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1222 };
1223 
1224 
1233 class iCubFinger : public iKinLimb
1234 {
1235 protected:
1236  std::string hand;
1237  std::string finger;
1238  std::string version;
1240 
1241  virtual void allocate(const std::string &_type);
1242  virtual void clone(const iCubFinger &finger);
1243 
1244 public:
1248  iCubFinger();
1249 
1266  iCubFinger(const std::string &_type);
1267 
1272  iCubFinger(const iCubFinger &finger);
1273 
1280 
1288  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1289 
1309  virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
1310  yarp::sig::Vector &chainJoints);
1311 
1336  virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
1337  const yarp::sig::Vector &jointEncoders,
1338  yarp::sig::Vector &chainJoints,
1339  const yarp::sig::Matrix &jointEncodersBounds=yarp::math::zeros(1,2));
1340 };
1341 
1342 
1348 class iCubLeg : public iKinLimb
1349 {
1350 protected:
1352 
1353  virtual void allocate(const std::string &_type);
1354 
1355 public:
1359  iCubLeg();
1360 
1367  iCubLeg(const std::string &_type);
1368 
1376  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1377 };
1378 
1379 
1385 class iCubEye : public iKinLimb
1386 {
1387 protected:
1389 
1390  virtual void allocate(const std::string &_type);
1391 
1392 public:
1396  iCubEye();
1397 
1404  iCubEye(const std::string &_type);
1405 
1413  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1414 };
1415 
1416 
1423 class iCubEyeNeckRef : public iCubEye
1424 {
1425 protected:
1426  virtual void allocate(const std::string &_type);
1427 
1428 public:
1432  iCubEyeNeckRef();
1433 
1440  iCubEyeNeckRef(const std::string &_type);
1441 };
1442 
1443 
1450 class iCubHeadCenter : public iCubEye
1451 {
1452 protected:
1453  virtual void allocate(const std::string &_type);
1454 
1455 public:
1459  iCubHeadCenter();
1460 
1466  iCubHeadCenter(const std::string &_type);
1467 };
1468 
1469 
1477 {
1478 protected:
1480 
1481  virtual void allocate(const std::string &_type);
1482 
1483 public:
1488 
1494  iCubInertialSensor(const std::string &_type);
1495 
1503  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1504 };
1505 
1506 
1514 {
1515 protected:
1517 
1518  virtual void allocate(const std::string &_type);
1519 
1520 public:
1525 
1531  iCubInertialSensorWaist(const std::string &_type);
1532 };
1533 
1534 }
1535 
1536 }
1537 
1538 #endif
1539 
1540 
A class for defining the iCub Arm.
Definition: iKinFwd.h:1194
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1773
iCubArm()
Default constructor.
Definition: iKinFwd.cpp:1759
iKinLimbVersion version
Definition: iKinFwd.h:1196
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Arm joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:1888
A class for defining the iCub Eye with the root reference frame attached to the neck.
Definition: iKinFwd.h:1424
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2743
iCubEyeNeckRef()
Default constructor.
Definition: iKinFwd.cpp:2729
A class for defining the iCub Eye.
Definition: iKinFwd.h:1386
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2553
iKinLimbVersion version
Definition: iKinFwd.h:1388
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Eye joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:2686
iCubEye()
Default constructor.
Definition: iKinFwd.cpp:2539
A class for defining the iCub Finger.
Definition: iKinFwd.h:1234
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...
std::string finger
Definition: iKinFwd.h:1237
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the finger joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:2143
virtual void clone(const iCubFinger &finger)
Definition: iKinFwd.cpp:1952
iCubFinger()
Default constructor.
Definition: iKinFwd.cpp:1931
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.
Definition: iKinFwd.cpp:1961
std::string hand
Definition: iKinFwd.h:1236
std::string version
Definition: iKinFwd.h:1238
double fingers_abduction_max
Definition: iKinFwd.h:1239
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1972
A class for describing the kinematic of the straight line coming out from the point located between t...
Definition: iKinFwd.h:1451
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2772
iCubHeadCenter()
Default constructor.
Definition: iKinFwd.cpp:2758
A class for defining the Kinematics of the Inertial Sensor mounted in the iCub's Waist.
Definition: iKinFwd.h:1514
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2946
iCubInertialSensorWaist()
Default constructor.
Definition: iKinFwd.cpp:2932
A class for defining the Inertia Sensor Kinematics of the iCub.
Definition: iKinFwd.h:1477
iCubInertialSensor()
Default constructor.
Definition: iKinFwd.cpp:2784
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2798
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Inertial Sensor joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:2888
A class for defining the iCub Leg.
Definition: iKinFwd.h:1349
iCubLeg()
Default constructor.
Definition: iKinFwd.cpp:2386
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2400
iKinLimbVersion version
Definition: iKinFwd.h:1351
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Leg joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:2515
A class for defining the iCub Torso.
Definition: iKinFwd.h:1158
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1690
iCubTorso()
Default constructor.
Definition: iKinFwd.cpp:1676
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the Torso joints bounds with current values set aboard the iCub.
Definition: iKinFwd.cpp:1727
iKinLimbVersion version
Definition: iKinFwd.h:1160
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:355
yarp::sig::Vector EndEffPosition(const yarp::sig::Vector &q)
Returns the 3D coordinates of end-effector position computed in q.
yarp::sig::Vector curr_q
Definition: iKinFwd.h:362
void popLink()
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:375
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.h:579
yarp::sig::Vector Position(const unsigned int i)
Returns the 3D position coordinates of ith Link.
Definition: iKinFwd.cpp:842
bool setHN(const yarp::sig::Matrix &_HN)
Sets HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.cpp:580
void prepareForHessian()
Prepares computation for a successive call to fastHessian_ij().
Definition: iKinFwd.cpp:1102
yarp::sig::Matrix GeoJacobian()
Returns the geometric Jacobian of the end-effector.
Definition: iKinFwd.cpp:1048
yarp::sig::Vector EndEffPosition()
Returns the 3D coordinates of end-effector position.
Definition: iKinFwd.cpp:894
yarp::sig::Matrix hess_J
Definition: iKinFwd.h:370
yarp::sig::Vector d2RotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dRi, const yarp::sig::Matrix &dRj, const yarp::sig::Matrix &d2R)
Definition: iKinFwd.cpp:686
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.
Definition: iKinFwd.cpp:1094
bool blockLink(const unsigned int i)
Blocks the ith Link at the current value of its joint angle.
Definition: iKinFwd.h:479
unsigned int getVerbosity() const
Returns the current Chain verbosity level.
Definition: iKinFwd.h:544
yarp::sig::Matrix H0
Definition: iKinFwd.h:360
void pushLink(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:343
bool isLinkBlocked(const unsigned int i)
Queries whether the ith Link is blocked.
Definition: iKinFwd.cpp:483
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.
Definition: iKinFwd.cpp:385
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.
Definition: iKinFwd.cpp:911
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:463
virtual void clone(const iKinChain &c)
Definition: iKinFwd.cpp:264
void clear()
Removes all Links.
Definition: iKinFwd.cpp:353
void setAllLinkVerbosity(unsigned int _verbose)
Sets the verbosity level of all Links belonging to the Chain.
Definition: iKinFwd.cpp:506
virtual void build()
Definition: iKinFwd.cpp:514
unsigned int verbose
Definition: iKinFwd.h:359
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.
Definition: iKinFwd.cpp:1117
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
Definition: iKinFwd.cpp:414
void setVerbosity(unsigned int _verbose)
Sets the verbosity level of the Chain.
Definition: iKinFwd.h:538
void setConstraint(unsigned int i, bool _constrained)
Sets the constraint status of ith link.
Definition: iKinFwd.h:517
iKinChain & operator=(const iKinChain &c)
Copies a Chain object into the current one.
Definition: iKinFwd.cpp:290
bool rmLink(const unsigned int i)
Removes the ith Link from the Chain.
Definition: iKinFwd.cpp:321
std::deque< unsigned int > hash_dof
Definition: iKinFwd.h:368
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.
Definition: iKinFwd.cpp:256
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:850
yarp::sig::Vector Pose(const unsigned int i, const bool axisRep=true)
Returns the coordinates of ith Link.
Definition: iKinFwd.cpp:803
bool addLink(const unsigned int i, iKinLink &l)
Adds a Link at the position ith within the Chain.
Definition: iKinFwd.cpp:299
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:611
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
Definition: iKinFwd.h:367
yarp::sig::Matrix getH()
Returns the rigid roto-translation matrix from the root reference frame to the end-effector frame in ...
Definition: iKinFwd.cpp:778
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
Definition: iKinFwd.h:564
yarp::sig::Vector dRotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dR)
Definition: iKinFwd.cpp:673
virtual ~iKinChain()
Destructor.
Definition: iKinFwd.cpp:1300
std::deque< iKinLink * > quickList
Definition: iKinFwd.h:365
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.
Definition: iKinFwd.h:523
unsigned int N
Definition: iKinFwd.h:357
yarp::sig::Vector RotAng(const yarp::sig::Matrix &R)
Definition: iKinFwd.cpp:659
iKinLink & operator[](const unsigned int i)
Returns a reference to the ith Link of the Chain.
Definition: iKinFwd.h:419
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
yarp::sig::Matrix hess_Jlnk
Definition: iKinFwd.h:371
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:394
bool setH0(const yarp::sig::Matrix &_H0)
Sets H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
Definition: iKinFwd.cpp:562
virtual void dispose()
Definition: iKinFwd.cpp:1307
unsigned int DOF
Definition: iKinFwd.h:358
iKinLink & operator()(const unsigned int i)
Returns a reference to the ith Link of the Chain considering only those Links related to DOF.
Definition: iKinFwd.h:427
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:498
yarp::sig::Matrix HN
Definition: iKinFwd.h:361
iKinChain & operator<<(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:366
yarp::sig::Matrix GeoJacobian(const yarp::sig::Vector &q)
Returns the geometric Jacobian of the end-effector computed in q.
std::deque< iKinLink * > allList
Definition: iKinFwd.h:364
A class for defining the versions of the iCub limbs.
Definition: iKinFwd.h:1046
iKinLimbVersion operator-(const iKinLimbVersion &v) const
Overloaded - operator.
Definition: iKinFwd.cpp:1665
iKinLimbVersion & operator=(const iKinLimbVersion &v)
Overloaded assignment operator.
Definition: iKinFwd.cpp:1609
unsigned long int v_minor
Definition: iKinFwd.h:1049
bool operator==(const iKinLimbVersion &v) const
Overloaded == operator.
Definition: iKinFwd.cpp:1637
bool operator<=(const iKinLimbVersion &v) const
Overloaded <= operator.
Definition: iKinFwd.cpp:1651
bool operator>=(const iKinLimbVersion &v) const
Overloaded >=</=> operator.
Definition: iKinFwd.cpp:1658
bool operator!=(const iKinLimbVersion &v) const
Overloaded != operator.
Definition: iKinFwd.cpp:1644
unsigned long int get_minor() const
Return the minor version.
Definition: iKinFwd.cpp:1593
unsigned long int get_major() const
Return the major version.
Definition: iKinFwd.cpp:1586
bool operator>(const iKinLimbVersion &v) const
Overloaded > operator.
Definition: iKinFwd.cpp:1630
bool operator<(const iKinLimbVersion &v) const
Overloaded < operator.
Definition: iKinFwd.cpp:1618
unsigned long int v_major
Definition: iKinFwd.h:1048
iKinLimbVersion()
Default Constructor.
Definition: iKinFwd.cpp:1546
std::string get_version() const
Return the version string.
Definition: iKinFwd.cpp:1600
A class for defining generic Limb.
Definition: iKinFwd.h:874
std::deque< iKinLink * > linkList
Definition: iKinFwd.h:876
virtual void getMatrixFromProperties(const yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
Definition: iKinFwd.cpp:1351
virtual void dispose()
Definition: iKinFwd.cpp:1534
iKinChain & operator=(const iKinChain &c)
Definition: iKinFwd.h:890
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
virtual ~iKinLimb()
Destructor.
Definition: iKinFwd.cpp:1508
bool rmLink(const unsigned int i)
Definition: iKinFwd.h:896
iKinLink & operator()(const unsigned int i)
Definition: iKinFwd.h:894
bool fromLinksProperties(const yarp::os::Property &options)
Initializes the Limb from a list of properties wherein links parameters are specified.
Definition: iKinFwd.cpp:1387
std::string getType() const
Returns the Limb type as a string.
Definition: iKinFwd.h:1019
iKinLink & operator[](const unsigned int i)
Definition: iKinFwd.h:893
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1515
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
Definition: iKinFwd.cpp:1443
void pushLink(iKinLink &l)
Definition: iKinFwd.h:897
bool addLink(const unsigned int i, iKinLink &l)
Definition: iKinFwd.h:895
virtual void setMatrixToProperties(yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
Definition: iKinFwd.cpp:1374
std::string type
Definition: iKinFwd.h:877
iKinChain & operator<<(iKinLink &l)
Definition: iKinFwd.h:891
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &)
Alignes the Limb joints bounds with current values set aboard the robot.
Definition: iKinFwd.h:1031
iKinLimb(const yarp::os::Property &options)
Creates a new Limb from a list of properties wherein links parameters are specified.
iKinChain & operator--(int)
Definition: iKinFwd.h:892
iKinLimb()
Default constructor.
Definition: iKinFwd.cpp:1315
virtual void clone(const iKinLimb &limb)
Definition: iKinFwd.cpp:1522
bool isValid() const
Checks if the limb has been properly configured.
Definition: iKinFwd.h:998
zeros(2, 2) eye(2
int n
constexpr double CTRL_PI
The PI constant.
Definition: math.h:56
void notImplemented(const unsigned int verbose)
Definition: iKinFwd.cpp:31
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.