iCub-main
Loading...
Searching...
No Matches
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
52#ifndef __IKINFWD_H__
53#define __IKINFWD_H__
54
55#include <string>
56#include <deque>
57
58#include <yarp/os/Property.h>
59#include <yarp/dev/ControlBoardInterfaces.h>
60#include <yarp/sig/Vector.h>
61#include <yarp/sig/Matrix.h>
62#include <yarp/math/Math.h>
63
64#include <iCub/ctrl/math.h>
65
66
67namespace iCub
68{
69
70namespace iKin
71{
72
73void notImplemented(const unsigned int verbose);
74
85{
86protected:
87 double A;
88 double D;
89 double Alpha;
90 double Offset;
91 double c_alpha;
92 double s_alpha;
93 double Min;
94 double Max;
95 double Ang;
96 bool blocked;
99 unsigned int verbose;
100
101 yarp::sig::Matrix H;
102 yarp::sig::Matrix cumH;
103 yarp::sig::Matrix DnH;
104
105 const yarp::sig::Matrix zeros1x1;
106 const yarp::sig::Vector zeros1;
107
108 friend class iKinChain;
109
110 // Default constructor: not implemented.
112
113 virtual void clone(const iKinLink &l);
114 bool isCumulative() { return cumulative; }
115 void block() { blocked=true; }
116 void block(double _Ang) { setAng(_Ang); blocked=true; }
117 void release() { blocked=false; }
118 void rmCumH() { cumulative=false; }
119 void addCumH(const yarp::sig::Matrix &_cumH);
120
121public:
133 iKinLink(double _A, double _D, double _Alpha, double _Offset,
134 double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
135
140 iKinLink(const iKinLink &l);
141
147 iKinLink &operator=(const iKinLink &l);
148
154 void setConstraint(bool _constrained) { constrained=_constrained; }
155
160 bool getConstraint() const { return constrained; }
161
169 void setVerbosity(unsigned int _verbose) { verbose=_verbose; }
170
175 unsigned int getVerbosity() const { return verbose; }
176
181 bool isBlocked() const { return blocked; }
182
187 double getA() const { return A; }
188
193 void setA(const double _A) { A=_A; }
194
199 double getD() const { return D; }
200
205 void setD(const double _D);
206
211 double getAlpha() const { return Alpha; }
212
217 void setAlpha(const double _Alpha);
218
223 double getOffset() const { return Offset; }
224
229 void setOffset(const double _Offset) { Offset=_Offset; }
230
235 double getMin() const { return Min; }
236
241 void setMin(const double _Min);
242
247 double getMax() const { return Max; }
248
253 void setMax(const double _Max);
254
259 double getAng() const { return Ang; }
260
266 double setAng(double _Ang);
267
275 yarp::sig::Matrix getH(bool c_override=false);
276
284 yarp::sig::Matrix getH(double _Ang, bool c_override=false);
285
295 yarp::sig::Matrix getDnH(unsigned int n=1, bool c_override=false);
296
300 virtual ~iKinLink() { }
301
302 // void methods for iDyn
303 // set
304 virtual double setDAng(const double) { notImplemented(verbose); return 0.0; }
305 virtual double setD2Ang(const double) { notImplemented(verbose); return 0.0; }
306 virtual void setPosVelAcc(const double, const double, const double) { notImplemented(verbose); }
307 virtual bool setDynamicParameters(const double, const yarp::sig::Matrix&,
308 const yarp::sig::Matrix&, const double,
309 const double, const double, const double) { notImplemented(verbose); return false; }
310 virtual bool setDynamicParameters(const double, const yarp::sig::Matrix&,
311 const yarp::sig::Matrix&) { notImplemented(verbose); return false; }
312 virtual bool setStaticParameters(const double, const yarp::sig::Matrix&) { notImplemented(verbose); return false; }
313 virtual bool setInertia(const yarp::sig::Matrix &) { notImplemented(verbose); return false; }
314 virtual void setMass(const double) { notImplemented(verbose); }
315 virtual bool setCOM(const yarp::sig::Matrix&) { notImplemented(verbose); return false; }
316 virtual bool setCOM(const yarp::sig::Vector&) { notImplemented(verbose); return false; }
317 virtual bool setForce(const yarp::sig::Vector&, const yarp::sig::Vector&) { notImplemented(verbose); return false; }
318 virtual bool setMoment(const yarp::sig::Vector&) { notImplemented(verbose); return false; }
319 virtual void setTorque(const double) { notImplemented(verbose); }
320
321 // get
322 virtual const yarp::sig::Matrix &getInertia() const { notImplemented(verbose); return zeros1x1; }
323 virtual double getMass() const { notImplemented(verbose); return 0.0; }
324 virtual double getIm() const { notImplemented(verbose); return 0.0; }
325 virtual double getKr() const { notImplemented(verbose); return 0.0; }
326 virtual double getFs() const { notImplemented(verbose); return 0.0; }
327 virtual double getFv() const { notImplemented(verbose); return 0.0; }
328 virtual const yarp::sig::Matrix &getCOM() const { notImplemented(verbose); return zeros1x1; }
329 virtual double getDAng() const { notImplemented(verbose); return 0.0; }
330 virtual double getD2Ang() const { notImplemented(verbose); return 0.0; }
331 virtual const yarp::sig::Matrix &getR() { notImplemented(verbose); return zeros1x1; }
332 virtual const yarp::sig::Matrix &getRC() { notImplemented(verbose); return zeros1x1; }
333 virtual const yarp::sig::Vector &getr() { notImplemented(verbose); return zeros1; }
334 virtual const yarp::sig::Vector &getrC() { notImplemented(verbose); return zeros1; }
335 virtual const yarp::sig::Vector &getW() const { notImplemented(verbose); return zeros1; }
336 virtual const yarp::sig::Vector &getdW() const { notImplemented(verbose); return zeros1; }
337 virtual const yarp::sig::Vector &getdWM() const { notImplemented(verbose); return zeros1; }
338 virtual const yarp::sig::Vector &getLinAcc() const { notImplemented(verbose); return zeros1; }
339 virtual const yarp::sig::Vector &getLinAccC() const { notImplemented(verbose); return zeros1; }
340 virtual const yarp::sig::Vector &getLinVel() const { notImplemented(verbose); return zeros1; }
341 virtual const yarp::sig::Vector &getLinVelC() const { notImplemented(verbose); return zeros1; }
342 virtual const yarp::sig::Vector &getForce() const { notImplemented(verbose); return zeros1; }
343 virtual const yarp::sig::Vector &getMoment() const { notImplemented(verbose); return zeros1; }
344 virtual double getTorque() const { notImplemented(verbose); return 0.0; }
345};
346
347
354{
355protected:
356 unsigned int N;
357 unsigned int DOF;
358 unsigned int verbose;
359 yarp::sig::Matrix H0;
360 yarp::sig::Matrix HN;
361 yarp::sig::Vector curr_q;
362
363 std::deque<iKinLink*> allList;
364 std::deque<iKinLink*> quickList;
365
366 std::deque<unsigned int> hash;
367 std::deque<unsigned int> hash_dof;
368
369 yarp::sig::Matrix hess_J;
370 yarp::sig::Matrix hess_Jlnk;
371
372 virtual void clone(const iKinChain &c);
373 virtual void build();
374 virtual void dispose();
375
376 yarp::sig::Vector RotAng(const yarp::sig::Matrix &R);
377 yarp::sig::Vector dRotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dR);
378 yarp::sig::Vector d2RotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dRi,
379 const yarp::sig::Matrix &dRj, const yarp::sig::Matrix &d2R);
380
381public:
385 iKinChain();
386
391 iKinChain(const iKinChain &c);
392
398 iKinChain &operator=(const iKinChain &c);
399
406
411 iKinChain &operator--(int);
412
418 iKinLink &operator[](const unsigned int i) { return *allList[i]; }
419
426 iKinLink &operator()(const unsigned int i) { return *quickList[hash_dof[i]]; }
427
434 bool addLink(const unsigned int i, iKinLink &l);
435
442 bool rmLink(const unsigned int i);
443
449 void pushLink(iKinLink &l);
450
454 void clear();
455
460 void popLink();
461
470 bool blockLink(const unsigned int i, double Ang);
471
478 bool blockLink(const unsigned int i) { return blockLink(i,getAng(i)); }
479
489 bool setBlockingValue(const unsigned int i, double Ang);
490
497 bool releaseLink(const unsigned int i);
498
504 bool isLinkBlocked(const unsigned int i);
505
510 void setAllConstraints(bool _constrained);
511
516 void setConstraint(unsigned int i, bool _constrained) { allList[i]->setConstraint(_constrained); }
517
522 bool getConstraint(unsigned int i) { return allList[i]->getConstraint(); }
523
528 void setAllLinkVerbosity(unsigned int _verbose);
529
537 void setVerbosity(unsigned int _verbose) { verbose=_verbose; }
538
543 unsigned int getVerbosity() const { return verbose; }
544
549 unsigned int getN() const { return N; }
550
556 unsigned int getDOF() const { return DOF; }
557
563 yarp::sig::Matrix getH0() const { return H0; }
564
571 bool setH0(const yarp::sig::Matrix &_H0);
572
578 yarp::sig::Matrix getHN() const { return HN; }
579
586 bool setHN(const yarp::sig::Matrix &_HN);
587
594 yarp::sig::Vector setAng(const yarp::sig::Vector &q);
595
600 yarp::sig::Vector getAng();
601
609 double setAng(const unsigned int i, double _Ang);
610
616 double getAng(const unsigned int i);
617
628 yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false);
629
636 yarp::sig::Matrix getH();
637
645 yarp::sig::Matrix getH(const yarp::sig::Vector &q);
646
656 yarp::sig::Vector Pose(const unsigned int i, const bool axisRep=true);
657
663 yarp::sig::Vector Position(const unsigned int i);
664
673 yarp::sig::Vector EndEffPose(const bool axisRep=true);
674
684 yarp::sig::Vector EndEffPose(const yarp::sig::Vector &q, const bool axisRep=true);
685
690 yarp::sig::Vector EndEffPosition();
691
697 yarp::sig::Vector EndEffPosition(const yarp::sig::Vector &q);
698
707 yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col);
708
716 yarp::sig::Matrix AnaJacobian(unsigned int col=3);
717
727 yarp::sig::Matrix AnaJacobian(const yarp::sig::Vector &q, unsigned int col=3);
728
735 yarp::sig::Matrix GeoJacobian(const unsigned int i);
736
742 yarp::sig::Matrix GeoJacobian();
743
751 yarp::sig::Matrix GeoJacobian(const yarp::sig::Vector &q);
752
764 yarp::sig::Vector Hessian_ij(const unsigned int i, const unsigned int j);
765
771 void prepareForHessian();
772
789 yarp::sig::Vector fastHessian_ij(const unsigned int i, const unsigned int j);
790
805 yarp::sig::Vector Hessian_ij(const unsigned int lnk, const unsigned int i,
806 const unsigned int j);
807
815 void prepareForHessian(const unsigned int lnk);
816
836 yarp::sig::Vector fastHessian_ij(const unsigned int lnk, const unsigned int i,
837 const unsigned int j);
838
846 yarp::sig::Matrix DJacobian(const yarp::sig::Vector &dq);
847
858 yarp::sig::Matrix DJacobian(const unsigned int lnk, const yarp::sig::Vector &dq);
859
863 virtual ~iKinChain();
864};
865
866
872class iKinLimb : public iKinChain
873{
874protected:
875 std::deque<iKinLink*> linkList;
876 std::string type;
877
878 virtual void getMatrixFromProperties(const yarp::os::Property &options,
879 const std::string &tag, yarp::sig::Matrix &H);
880 virtual void setMatrixToProperties(yarp::os::Property &options,
881 const std::string &tag, yarp::sig::Matrix &H);
882 virtual void allocate(const std::string &_type);
883 virtual void clone(const iKinLimb &limb);
884 virtual void dispose();
885
886 // make the following methods protected in order to prevent user from changing
887 // too easily the internal structure of the chain;
888 // to get access anyway to these hidden methods, user can rely on asChain()
892 iKinLink &operator[](const unsigned int i) { return iKinChain::operator[](i); }
893 iKinLink &operator()(const unsigned int i) { return iKinChain::operator()(i); }
894 bool addLink(const unsigned int i, iKinLink &l) { return iKinChain::addLink(i,l); }
895 bool rmLink(const unsigned int i) { return iKinChain::rmLink(i); }
899 void pushLink(iKinLink *pl);
900
901public:
905 iKinLimb();
906
912 iKinLimb(const std::string &_type);
913
918 iKinLimb(const iKinLimb &limb);
919
926 iKinLimb(const yarp::os::Property &options);
927
984 bool fromLinksProperties(const yarp::os::Property &options);
985
991 bool toLinksProperties(yarp::os::Property &options);
992
997 bool isValid() const { return (N>0); }
998
1004 iKinLimb &operator=(const iKinLimb &limb);
1005
1012 iKinChain *asChain() { return static_cast<iKinChain*>(this); }
1013
1018 std::string getType() const { return type; }
1019
1030 virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*>&) { notImplemented(verbose); return true; }
1031
1035 virtual ~iKinLimb();
1036};
1037
1038
1045{
1046protected:
1047 unsigned long int v_major;
1048 unsigned long int v_minor;
1049
1050public:
1055
1061 iKinLimbVersion(const std::string &version);
1062
1068 iKinLimbVersion(const unsigned long int v_major, const unsigned long int v_minor);
1069
1074
1079 unsigned long int get_major() const;
1080
1085 unsigned long int get_minor() const;
1086
1091 std::string get_version() const;
1092
1099
1105 bool operator<(const iKinLimbVersion& v) const;
1106
1112 bool operator<=(const iKinLimbVersion& v) const;
1113
1119 bool operator==(const iKinLimbVersion& v) const;
1120
1126 bool operator!=(const iKinLimbVersion& v) const;
1127
1133 bool operator>=(const iKinLimbVersion& v) const;
1134
1140 bool operator>(const iKinLimbVersion& v) const;
1141
1148};
1149
1150
1156class iCubTorso : public iKinLimb
1157{
1158protected:
1160
1161 virtual void allocate(const std::string &_type);
1162
1163public:
1167 iCubTorso();
1168
1174 iCubTorso(const std::string &_type);
1175
1183 virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1184};
1185
1186
1192class iCubArm : public iKinLimb
1193{
1194protected:
1196
1197 virtual void allocate(const std::string &_type);
1198
1199public:
1203 iCubArm();
1204
1211 iCubArm(const std::string &_type);
1212
1220 virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1221};
1222
1223
1232class iCubFinger : public iKinLimb
1233{
1234protected:
1235 std::string hand;
1236 std::string finger;
1237 std::string version;
1239
1240 virtual void allocate(const std::string &_type);
1241 virtual void clone(const iCubFinger &finger);
1242
1243public:
1247 iCubFinger();
1248
1265 iCubFinger(const std::string &_type);
1266
1272
1279
1287 virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1288
1308 virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
1309 yarp::sig::Vector &chainJoints);
1310
1335 virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
1336 const yarp::sig::Vector &jointEncoders,
1337 yarp::sig::Vector &chainJoints,
1338 const yarp::sig::Matrix &jointEncodersBounds=yarp::math::zeros(1,2));
1339};
1340
1341
1347class iCubLeg : public iKinLimb
1348{
1349protected:
1351
1352 virtual void allocate(const std::string &_type);
1353
1354public:
1358 iCubLeg();
1359
1366 iCubLeg(const std::string &_type);
1367
1375 virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1376};
1377
1378
1384class iCubEye : public iKinLimb
1385{
1386protected:
1388
1389 virtual void allocate(const std::string &_type);
1390
1391public:
1395 iCubEye();
1396
1403 iCubEye(const std::string &_type);
1404
1412 virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1413};
1414
1415
1423{
1424protected:
1425 virtual void allocate(const std::string &_type);
1426
1427public:
1432
1439 iCubEyeNeckRef(const std::string &_type);
1440};
1441
1442
1450{
1451protected:
1452 virtual void allocate(const std::string &_type);
1453
1454public:
1459
1465 iCubHeadCenter(const std::string &_type);
1466};
1467
1468
1476{
1477protected:
1479
1480 virtual void allocate(const std::string &_type);
1481
1482public:
1487
1493 iCubInertialSensor(const std::string &_type);
1494
1502 virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
1503};
1504
1505
1513{
1514protected:
1516
1517 virtual void allocate(const std::string &_type);
1518
1519public:
1524
1530 iCubInertialSensorWaist(const std::string &_type);
1531};
1532
1533}
1534
1535}
1536
1537#endif
1538
1539
A class for defining the iCub Arm.
Definition iKinFwd.h:1193
virtual void allocate(const std::string &_type)
Definition iKinFwd.cpp:1773
iCubArm()
Default constructor.
Definition iKinFwd.cpp:1759
iKinLimbVersion version
Definition iKinFwd.h:1195
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:1423
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:1385
virtual void allocate(const std::string &_type)
Definition iKinFwd.cpp:2553
iKinLimbVersion version
Definition iKinFwd.h:1387
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:1233
virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders, const yarp::sig::Vector &jointEncoders, yarp::sig::Vector &chainJoints, const yarp::sig::Matrix &jointEncodersBounds=yarp::math::zeros(1, 2))
Retrieves the vector of actual finger's joint values (to be used in conjuction with the iKinLimb meth...
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &lim)
Alignes the finger joints bounds with current values set aboard the iCub.
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 version
Definition iKinFwd.h:1237
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:1450
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:1513
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:1476
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:1348
iCubLeg()
Default constructor.
Definition iKinFwd.cpp:2386
virtual void allocate(const std::string &_type)
Definition iKinFwd.cpp:2400
iKinLimbVersion version
Definition iKinFwd.h:1350
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:1157
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:1159
A Base class for defining a Serial Link Chain.
Definition iKinFwd.h:354
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:361
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:578
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:369
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:478
unsigned int getVerbosity() const
Returns the current Chain verbosity level.
Definition iKinFwd.h:543
yarp::sig::Matrix H0
Definition iKinFwd.h:359
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:358
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:537
void setConstraint(unsigned int i, bool _constrained)
Sets the constraint status of ith link.
Definition iKinFwd.h:516
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:367
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:366
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
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:426
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
Definition iKinFwd.h:563
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:364
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:522
unsigned int N
Definition iKinFwd.h:356
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:418
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition iKinFwd.h:549
yarp::sig::Matrix hess_Jlnk
Definition iKinFwd.h:370
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition iKinFwd.h:556
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:357
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition iKinFwd.cpp:498
yarp::sig::Matrix HN
Definition iKinFwd.h:360
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:363
A class for defining the versions of the iCub limbs.
Definition iKinFwd.h:1045
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:1048
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:1047
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:873
std::deque< iKinLink * > linkList
Definition iKinFwd.h:875
virtual void getMatrixFromProperties(const yarp::os::Property &options, const std::string &tag, yarp::sig::Matrix &H)
Definition iKinFwd.cpp:1351
iKinChain & operator<<(iKinLink &l)
Definition iKinFwd.h:890
virtual void dispose()
Definition iKinFwd.cpp:1534
iKinLink & operator[](const unsigned int i)
Definition iKinFwd.h:892
virtual ~iKinLimb()
Destructor.
Definition iKinFwd.cpp:1508
iKinLink & operator()(const unsigned int i)
Definition iKinFwd.h:893
bool rmLink(const unsigned int i)
Definition iKinFwd.h:895
iKinChain & operator--(int)
Definition iKinFwd.h:891
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:1018
iKinChain & operator=(const iKinChain &c)
Definition iKinFwd.h:889
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:896
bool addLink(const unsigned int i, iKinLink &l)
Definition iKinFwd.h:894
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:876
virtual bool alignJointsBounds(const std::deque< yarp::dev::IControlLimits * > &)
Alignes the Limb joints bounds with current values set aboard the robot.
Definition iKinFwd.h:1030
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition iKinFwd.h:1012
iKinLimb(const yarp::os::Property &options)
Creates a new Limb from a list of properties wherein links parameters are specified.
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:997
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.