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;
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:
1128 
1129  virtual void allocate(const std::string &_type);
1130  virtual void clone(const iCubFinger &finger);
1131 
1132 public:
1136  iCubFinger();
1137 
1154  iCubFinger(const std::string &_type);
1155 
1160  iCubFinger(const iCubFinger &finger);
1161 
1168 
1176  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1177 
1197  virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
1198  yarp::sig::Vector &chainJoints);
1199 
1224  virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
1225  const yarp::sig::Vector &jointEncoders,
1226  yarp::sig::Vector &chainJoints,
1227  const yarp::sig::Matrix &jointEncodersBounds=yarp::math::zeros(1,2));
1228 };
1229 
1230 
1236 class iCubLeg : public iKinLimb
1237 {
1238 protected:
1239  double version;
1240 
1241  virtual void allocate(const std::string &_type);
1242 
1243 public:
1247  iCubLeg();
1248 
1255  iCubLeg(const std::string &_type);
1256 
1264  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1265 };
1266 
1267 
1273 class iCubEye : public iKinLimb
1274 {
1275 protected:
1276  double version;
1277 
1278  virtual void allocate(const std::string &_type);
1279 
1280 public:
1284  iCubEye();
1285 
1292  iCubEye(const std::string &_type);
1293 
1301  virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1302 };
1303 
1304 
1311 class iCubEyeNeckRef : public iCubEye
1312 {
1313 protected:
1314  virtual void allocate(const std::string &_type);
1315 
1316 public:
1320  iCubEyeNeckRef();
1321 
1328  iCubEyeNeckRef(const std::string &_type);
1329 };
1330 
1331 
1338 class iCubHeadCenter : public iCubEye
1339 {
1340 protected:
1341  virtual void allocate(const std::string &_type);
1342 
1343 public:
1347  iCubHeadCenter();
1348 
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 
1402 {
1403 protected:
1404  double version;
1405 
1406  virtual void allocate(const std::string &_type);
1407 
1408 public:
1413 
1419  iCubInertialSensorWaist(const std::string &_type);
1420 };
1421 
1422 }
1423 
1424 }
1425 
1426 #endif
1427 
1428 
iCub::iKin::iCubArm::alignJointsBounds
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:1766
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH()
Returns the rigid roto-translation matrix from the root reference frame to the end-effector frame in ...
Definition: iKinFwd.cpp:786
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::iKin::iCubInertialSensor::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2667
iCub::iKin::iKinChain::dRotAng
yarp::sig::Vector dRotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dR)
Definition: iKinFwd.cpp:681
H
H
Definition: compute_ekf_fast.m:27
iCub::iKin::iCubLeg::version
double version
Definition: iKinFwd.h:1239
iCub::iKin::iKinChain::AnaJacobian
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
Definition: iKinFwd.cpp:919
iCub::iKin::iKinChain::RotAng
yarp::sig::Vector RotAng(const yarp::sig::Matrix &R)
Definition: iKinFwd.cpp:667
iCub::iKin::iCubFinger::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1850
iCub::iKin::iKinChain::~iKinChain
virtual ~iKinChain()
Destructor.
Definition: iKinFwd.cpp:1308
iCub::iKin::iKinChain::isLinkBlocked
bool isLinkBlocked(const unsigned int i)
Queries whether the ith Link is blocked.
Definition: iKinFwd.cpp:491
iCub::iKin::iKinChain::Position
yarp::sig::Vector Position(const unsigned int i)
Returns the 3D position coordinates of ith Link.
Definition: iKinFwd.cpp:850
iCub::iKin::iKinChain::getVerbosity
unsigned int getVerbosity() const
Returns the current Chain verbosity level.
Definition: iKinFwd.h:544
iCub::iKin::iCubFinger::clone
virtual void clone(const iCubFinger &finger)
Definition: iKinFwd.cpp:1830
iCub::iKin::iKinChain::N
unsigned int N
Definition: iKinFwd.h:357
iCub::iKin::notImplemented
void notImplemented(const unsigned int verbose)
Definition: iKinFwd.cpp:39
iCub::iKin::iCubLeg::iCubLeg
iCubLeg()
Default constructor.
Definition: iKinFwd.cpp:2264
iCub::iKin::iKinChain::EndEffPosition
yarp::sig::Vector EndEffPosition()
Returns the 3D coordinates of end-effector position.
Definition: iKinFwd.cpp:902
iCub::iKin::iKinLimb::iKinLimb
iKinLimb()
Default constructor.
Definition: iKinFwd.cpp:1323
iCub::iKin::iKinChain::operator=
iKinChain & operator=(const iKinChain &c)
Copies a Chain object into the current one.
Definition: iKinFwd.cpp:298
iCub::iKin::iCubTorso::alignJointsBounds
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:1605
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iCub::iKin::iKinChain::Hessian_ij
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:1102
iCub::iKin::iKinLimb::alignJointsBounds
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
iCub::iKin::iCubLeg::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2278
iCub::iKin::iKinChain::hash
std::deque< unsigned int > hash
Definition: iKinFwd.h:367
iCub::iKin::iCubTorso
Definition: iKinFwd.h:1045
iCub::iKin::iKinChain::operator--
iKinChain & operator--(int)
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:393
iCub::iKin::iKinLimb::operator[]
iKinLink & operator[](const unsigned int i)
Definition: iKinFwd.h:893
iCub::iKin::iKinChain::releaseLink
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:471
iCub::iKin::iKinChain::setH0
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:570
iCub::iKin::iKinChain::popLink
void popLink()
Removes a Link from the bottom of the Chain.
Definition: iKinFwd.cpp:383
iCub::iKin::iCubInertialSensorWaist::version
double version
Definition: iKinFwd.h:1404
iCub::iKin::iKinChain::operator[]
iKinLink & operator[](const unsigned int i)
Returns a reference to the ith Link of the Chain.
Definition: iKinFwd.h:419
iCub::iKin::iCubHeadCenter
Definition: iKinFwd.h:1338
iCub::iKin::iCubFinger::hand
std::string hand
Definition: iKinFwd.h:1124
iCub::iKin::iKinChain::clear
void clear()
Removes all Links.
Definition: iKinFwd.cpp:361
iCub::iKin::iKinChain::d2RotAng
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:694
iCub::iKin::iCubArm::iCubArm
iCubArm()
Default constructor.
Definition: iKinFwd.cpp:1637
iCub::iKin::iKinLimb::clear
void clear()
Definition: iKinFwd.h:898
iCub::iKin::iKinChain::Pose
yarp::sig::Vector Pose(const unsigned int i, const bool axisRep=true)
Returns the coordinates of ith Link.
Definition: iKinFwd.cpp:811
iCub::iKin::iCubFinger
Definition: iKinFwd.h:1121
iCub::iKin::iKinLimb::linkList
std::deque< iKinLink * > linkList
Definition: iKinFwd.h:876
iCub::iKin::iKinChain::getDOF
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
iCub::iKin::iKinLimb::fromLinksProperties
bool fromLinksProperties(const yarp::os::Property &options)
Initializes the Limb from a list of properties wherein links parameters are specified.
Definition: iKinFwd.cpp:1395
iCub::iKin::iCubLeg::alignJointsBounds
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:2393
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iKin::iCubEyeNeckRef::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2612
iCub::iKin::iCubEye
Definition: iKinFwd.h:1273
iCub::iKin::iCubArm
Definition: iKinFwd.h:1081
iCub::iKin::iCubArm::version
double version
Definition: iKinFwd.h:1084
iCub::iKin::iKinChain::setConstraint
void setConstraint(unsigned int i, bool _constrained)
Sets the constraint status of ith link.
Definition: iKinFwd.h:517
iCub::iKin::iCubEye::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2431
math.h
iCub::iKin::iKinChain::clone
virtual void clone(const iKinChain &c)
Definition: iKinFwd.cpp:272
iCub::iKin::iKinChain::curr_q
yarp::sig::Vector curr_q
Definition: iKinFwd.h:362
iCub::iKin::iKinLimb::operator()
iKinLink & operator()(const unsigned int i)
Definition: iKinFwd.h:894
iCub::iKin::iKinLimb::pushLink
void pushLink(iKinLink &l)
Definition: iKinFwd.h:897
iCub::iKin::iCubEyeNeckRef::iCubEyeNeckRef
iCubEyeNeckRef()
Default constructor.
Definition: iKinFwd.cpp:2598
iCub::iKin::iKinLimb
Definition: iKinFwd.h:873
iCub::iKin::iKinChain::iKinChain
iKinChain()
Default constructor.
Definition: iKinFwd.cpp:264
iCub::iKin::iKinLimb::rmLink
bool rmLink(const unsigned int i)
Definition: iKinFwd.h:896
iCub::iKin::iKinChain::getAng
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:619
iCub::iKin::iKinLimb::toLinksProperties
bool toLinksProperties(yarp::os::Property &options)
Provides the links attributes listed in a property object.
Definition: iKinFwd.cpp:1451
iCub::iKin::iKinChain::DJacobian
yarp::sig::Matrix DJacobian(const yarp::sig::Vector &dq)
Compute the time derivative of the geometric Jacobian.
iCub::iKin::iCubInertialSensor::alignJointsBounds
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:2757
iCub::iKin::iKinChain::setHN
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:588
iCub::iKin::iKinLimb::operator=
iKinChain & operator=(const iKinChain &c)
Definition: iKinFwd.h:890
iCub::iKin::iKinChain::fastHessian_ij
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:1125
iCub::iKin::iKinLimb::setMatrixToProperties
virtual void setMatrixToProperties(yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
Definition: iKinFwd.cpp:1382
iCub::iKin::iKinLimb::addLink
bool addLink(const unsigned int i, iKinLink &l)
Definition: iKinFwd.h:895
iCub::iKin::iKinChain::build
virtual void build()
Definition: iKinFwd.cpp:522
iCub::iKin::iKinChain::GeoJacobian
yarp::sig::Matrix GeoJacobian()
Returns the geometric Jacobian of the end-effector.
Definition: iKinFwd.cpp:1056
iCub::iKin::iKinChain::H0
yarp::sig::Matrix H0
Definition: iKinFwd.h:360
iCub::iKin::iKinChain::getConstraint
bool getConstraint(unsigned int i)
Returns the constraint status of ith link.
Definition: iKinFwd.h:523
iCub::iKin::iCubHeadCenter::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2641
iCub::iKin::iKinLimb::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1523
iCub::iKin::iKinChain::addLink
bool addLink(const unsigned int i, iKinLink &l)
Adds a Link at the position ith within the Chain.
Definition: iKinFwd.cpp:307
iCub::iKin::iKinChain::pushLink
void pushLink(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:351
iCub::iKin::iCubFinger::fingers_abduction_max
double fingers_abduction_max
Definition: iKinFwd.h:1127
iCub::iKin::iKinChain::dispose
virtual void dispose()
Definition: iKinFwd.cpp:1315
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::iKin::iKinLimb::clone
virtual void clone(const iKinLimb &limb)
Definition: iKinFwd.cpp:1530
iCub::iKin::iKinLimb::dispose
virtual void dispose()
Definition: iKinFwd.cpp:1542
iCub::iKin::iKinChain::hash_dof
std::deque< unsigned int > hash_dof
Definition: iKinFwd.h:368
n
int n
Definition: debugFunctions.cpp:58
iCub::iKin::iKinChain::prepareForHessian
void prepareForHessian()
Prepares computation for a successive call to fastHessian_ij().
Definition: iKinFwd.cpp:1110
iCub::iKin::iCubInertialSensorWaist::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:2815
iCub::iKin::iKinChain::HN
yarp::sig::Matrix HN
Definition: iKinFwd.h:361
iCub::iKin::iCubTorso::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1568
iCub::iKin::iKinChain::quickList
std::deque< iKinLink * > quickList
Definition: iKinFwd.h:365
iCub::iKin::iKinLimb::getMatrixFromProperties
virtual void getMatrixFromProperties(const yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
Definition: iKinFwd.cpp:1359
iCub::iKin::iKinLimb::type
std::string type
Definition: iKinFwd.h:877
iCub::iKin::iKinChain::getH0
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
iCub::iKin::iCubFinger::alignJointsBounds
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:2021
iCub::iKin::iCubFinger::iCubFinger
iCubFinger()
Default constructor.
Definition: iKinFwd.cpp:1809
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::ctrl::CTRL_PI
constexpr double CTRL_PI
The PI constant.
Definition: math.h:56
iCub::iKin::iCubEye::alignJointsBounds
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:2555
iCub::iKin::iCubInertialSensorWaist::iCubInertialSensorWaist
iCubInertialSensorWaist()
Default constructor.
Definition: iKinFwd.cpp:2801
iCub::iKin::iCubInertialSensor::iCubInertialSensor
iCubInertialSensor()
Default constructor.
Definition: iKinFwd.cpp:2653
iCub::iKin::iKinChain::verbose
unsigned int verbose
Definition: iKinFwd.h:359
iCub::iKin::iKinChain::getHN
yarp::sig::Matrix getHN() const
Returns HN, the rigid roto-translation matrix from the Nth frame to the end-effector.
Definition: iKinFwd.h:579
iCub::iKin::iKinChain::DOF
unsigned int DOF
Definition: iKinFwd.h:358
iCub::iKin::iKinChain::hess_Jlnk
yarp::sig::Matrix hess_Jlnk
Definition: iKinFwd.h:371
iCub::iKin::iKinLimb::~iKinLimb
virtual ~iKinLimb()
Destructor.
Definition: iKinFwd.cpp:1516
iCub::iKin::iKinChain::rmLink
bool rmLink(const unsigned int i)
Removes the ith Link from the Chain.
Definition: iKinFwd.cpp:329
iCub::iKin::iCubInertialSensor::version
double version
Definition: iKinFwd.h:1367
iCub::iKin::iCubFinger::version
std::string version
Definition: iKinFwd.h:1126
iCub::iKin::iKinLimb::popLink
void popLink()
Definition: iKinFwd.h:899
iCub::iKin::iKinChain::setBlockingValue
bool setBlockingValue(const unsigned int i, double Ang)
Changes the value of the ith blocked Link.
Definition: iKinFwd.cpp:422
iCub::iKin::iCubInertialSensorWaist
Definition: iKinFwd.h:1401
iCub::iKin::iKinChain::setVerbosity
void setVerbosity(unsigned int _verbose)
Sets the verbosity level of the Chain.
Definition: iKinFwd.h:538
iCub::iKin::iKinChain::setAllLinkVerbosity
void setAllLinkVerbosity(unsigned int _verbose)
Sets the verbosity level of all Links belonging to the Chain.
Definition: iKinFwd.cpp:514
iCub::iKin::iKinChain::operator()
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
iCub::iKin::iCubFinger::finger
std::string finger
Definition: iKinFwd.h:1125
iCub::iKin::iKinLimb::operator--
iKinChain & operator--(int)
Definition: iKinFwd.h:892
iCub::iKin::iKinChain::allList
std::deque< iKinLink * > allList
Definition: iKinFwd.h:364
iCub::iKin::iCubHeadCenter::iCubHeadCenter
iCubHeadCenter()
Default constructor.
Definition: iKinFwd.cpp:2627
iCub::iKin::iKinChain::EndEffPose
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:858
iCub::iKin::iKinChain::blockLink
bool blockLink(const unsigned int i)
Blocks the ith Link at the current value of its joint angle.
Definition: iKinFwd.h:479
iCub::iKin::iCubEyeNeckRef
Definition: iKinFwd.h:1311
iCub::iKin::iKinChain::blockLink
bool blockLink(const unsigned int i, double Ang)
Blocks the ith Link at the a certain value of its joint angle.
Definition: iKinFwd.cpp:402
iCub::iKin::iCubEye::version
double version
Definition: iKinFwd.h:1276
iCub::iKin::iKinLimb::isValid
bool isValid() const
Checks if the limb has been properly configured.
Definition: iKinFwd.h:998
iCub::iKin::iCubArm::allocate
virtual void allocate(const std::string &_type)
Definition: iKinFwd.cpp:1651
iCub::iKin::iCubFinger::getChainJoints
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...
iCub::iKin::iCubTorso::iCubTorso
iCubTorso()
Default constructor.
Definition: iKinFwd.cpp:1554
iCub::iKin::iKinChain::operator<<
iKinChain & operator<<(iKinLink &l)
Adds a Link at the bottom of the Chain.
Definition: iKinFwd.cpp:374
zeros
zeros(2, 2) eye(2
iCub::iKin::iKinChain::setAllConstraints
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:506
iCub::iKin::iCubLeg
Definition: iKinFwd.h:1236
iCub::iKin::iKinLimb::asChain
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
iCub::iKin::iKinLimb::getType
std::string getType() const
Returns the Limb type as a string.
Definition: iKinFwd.h:1019
iCub::iKin::iCubTorso::version
double version
Definition: iKinFwd.h:1048
iCub::iKin::iCubFinger::operator=
iCubFinger & operator=(const iCubFinger &finger)
Copies a Finger object into the current one.
Definition: iKinFwd.cpp:1839
iCub::iKin::iKinLimb::operator<<
iKinChain & operator<<(iKinLink &l)
Definition: iKinFwd.h:891
iCub::iKin::iCubInertialSensor
Definition: iKinFwd.h:1364
iCub::iKin::iKinChain::hess_J
yarp::sig::Matrix hess_J
Definition: iKinFwd.h:370
iCub::iKin::iCubEye::iCubEye
iCubEye()
Default constructor.
Definition: iKinFwd.cpp:2417