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.
112  iKinLink();
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 
1045 class iCubTorso : public iKinLimb
1046 {
1047 protected:
1048  double version;
1049 
1050  virtual void allocate(const std::string &_type);
1051 
1052 public:
1056  iCubTorso();
1057 
1063  iCubTorso(const std::string &_type);
1064 
1072  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1073 };
1074 
1075 
1081 class iCubArm : public iKinLimb
1082 {
1083 protected:
1084  double version;
1085 
1086  virtual void allocate(const std::string &_type);
1087 
1088 public:
1092  iCubArm();
1093 
1100  iCubArm(const std::string &_type);
1101 
1109  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1110 };
1111 
1112 
1121 class iCubFinger : public iKinLimb
1122 {
1123 protected:
1124  std::string hand;
1125  std::string finger;
1126  std::string version;
1127 
1128  virtual void allocate(const std::string &_type);
1129  virtual void clone(const iCubFinger &finger);
1130 
1131 public:
1135  iCubFinger();
1136 
1153  iCubFinger(const std::string &_type);
1154 
1159  iCubFinger(const iCubFinger &finger);
1160 
1166  iCubFinger &operator=(const iCubFinger &finger);
1167 
1175  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1176 
1196  virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
1197  yarp::sig::Vector &chainJoints);
1198 
1223  virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
1224  const yarp::sig::Vector &jointEncoders,
1225  yarp::sig::Vector &chainJoints,
1226  const yarp::sig::Matrix &jointEncodersBounds=yarp::math::zeros(1,2));
1227 };
1228 
1229 
1235 class iCubLeg : public iKinLimb
1236 {
1237 protected:
1238  double version;
1239 
1240  virtual void allocate(const std::string &_type);
1241 
1242 public:
1246  iCubLeg();
1247 
1254  iCubLeg(const std::string &_type);
1255 
1263  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1264 };
1265 
1266 
1272 class iCubEye : public iKinLimb
1273 {
1274 protected:
1275  double version;
1276 
1277  virtual void allocate(const std::string &_type);
1278 
1279 public:
1283  iCubEye();
1284 
1291  iCubEye(const std::string &_type);
1292 
1300  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1301 };
1302 
1303 
1310 class iCubEyeNeckRef : public iCubEye
1311 {
1312 protected:
1313  virtual void allocate(const std::string &_type);
1314 
1315 public:
1319  iCubEyeNeckRef();
1320 
1327  iCubEyeNeckRef(const std::string &_type);
1328 };
1329 
1330 
1337 class iCubHeadCenter : public iCubEye
1338 {
1339 protected:
1340  virtual void allocate(const std::string &_type);
1341 
1342 public:
1346  iCubHeadCenter();
1347 
1354  iCubHeadCenter(const std::string &_type);
1355 };
1356 
1357 
1365 {
1366 protected:
1367  double version;
1368 
1369  virtual void allocate(const std::string &_type);
1370 
1371 public:
1376 
1382  iCubInertialSensor(const std::string &_type);
1383 
1391  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1392 };
1393 
1394 }
1395 
1396 }
1397 
1398 #endif
1399 
1400 
1401 
std::string getType() const
Returns the Limb type as a string.
Definition: iKinFwd.h:1019
void setVerbosity(unsigned int _verbose)
Sets the verbosity level of the Chain.
Definition: iKinFwd.h:538
bool rmLink(const unsigned int i)
Removes the ith Link from the Chain.
Definition: iKinFwd.cpp:320
iKinLink & operator[](const unsigned int i)
Returns a reference to the ith Link of the Chain.
Definition: iKinFwd.h:419
const double CTRL_PI
The PI constant.
Definition: math.cpp:27
void pushLink(iKinLink &l)
Definition: iKinFwd.h:897
std::deque< unsigned int > hash
Definition: iKinFwd.h:367
void popLink()
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:374
yarp::sig::Matrix HN
Definition: iKinFwd.h:361
zeros(2, 2) eye(2
iKinLink & operator[](const unsigned int i)
Definition: iKinFwd.h:893
yarp::sig::Matrix H0
Definition: iKinFwd.h:360
bool getConstraint(unsigned int i)
Returns the constraint status of ith link.
Definition: iKinFwd.h:523
A class for defining the iCub Leg.
Definition: iKinFwd.h:1235
A class for defining the iCub Eye with the root reference frame attached to the neck.
Definition: iKinFwd.h:1310
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
iKinChain & operator=(const iKinChain &c)
Definition: iKinFwd.h:890
unsigned int getDOF() const
Returns the current number of Chain&#39;s DOF.
Definition: iKinFwd.h:557
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
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
A class for defining the iCub Eye.
Definition: iKinFwd.h:1272
int n
bool addLink(const unsigned int i, iKinLink &l)
Adds a Link at the position ith within the Chain.
Definition: iKinFwd.cpp:298
Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData)
Definition: utils.cpp:548
A class for defining the iCub Torso.
Definition: iKinFwd.h:1045
std::deque< iKinLink * > linkList
Definition: iKinFwd.h:876
yarp::os::Bottle & operator<<(yarp::os::Bottle &out, int val)
Helper functions for serialization in bottles for use in the learningMachine library.
bool rmLink(const unsigned int i)
Definition: iKinFwd.h:896
yarp::sig::Matrix hess_J
Definition: iKinFwd.h:370
unsigned int getVerbosity() const
Returns the current Chain verbosity level.
Definition: iKinFwd.h:544
iKinLink & operator()(const unsigned int i)
Definition: iKinFwd.h:894
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
std::deque< unsigned int > hash_dof
Definition: iKinFwd.h:368
iKinChain & operator<<(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:365
bool addLink(const unsigned int i, iKinLink &l)
Definition: iKinFwd.h:895
bool blockLink(const unsigned int i)
Blocks the ith Link at the current value of its joint angle.
Definition: iKinFwd.h:479
A class for defining the iCub Finger.
Definition: iKinFwd.h:1121
std::string finger
Definition: iKinFwd.h:1125
A class for defining generic Limb.
Definition: iKinFwd.h:873
A class for defining the Inertia Sensor Kinematics of the iCub.
Definition: iKinFwd.h:1364
unsigned int N
Definition: iKinFwd.h:357
iKinChain & operator--(int)
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:384
yarp::sig::Matrix hess_Jlnk
Definition: iKinFwd.h:371
void setConstraint(unsigned int i, bool _constrained)
Sets the constraint status of ith link.
Definition: iKinFwd.h:517
std::string version
Definition: iKinFwd.h:1126
std::string type
Definition: iKinFwd.h:877
iKinChain & operator--(int)
Definition: iKinFwd.h:892
iKinChain & operator<<(iKinLink &l)
Definition: iKinFwd.h:891
unsigned int verbose
Definition: iKinFwd.h:359
void notImplemented(const unsigned int verbose)
Definition: iKinFwd.cpp:30
void pushLink(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:342
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
bool isValid() const
Checks if the limb has been properly configured.
Definition: iKinFwd.h:998
A class for describing the kinematic of the straight line coming out from the point located between t...
Definition: iKinFwd.h:1337
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
iKinChain & operator=(const iKinChain &c)
Copies a Chain object into the current one.
Definition: iKinFwd.cpp:289
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
A class for defining the iCub Arm.
Definition: iKinFwd.h:1081
std::string hand
Definition: iKinFwd.h:1124
yarp::sig::Vector curr_q
Definition: iKinFwd.h:362
std::deque< iKinLink * > allList
Definition: iKinFwd.h:364
std::deque< iKinLink * > quickList
Definition: iKinFwd.h:365
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.h:579
void clear()
Removes all Links.
Definition: iKinFwd.cpp:352