iCub-main
iKinInv.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 
25 #ifndef __IKININV_H__
26 #define __IKININV_H__
27 
28 #include <yarp/os/Property.h>
29 #include <iCub/ctrl/minJerkCtrl.h>
30 #include <iCub/ctrl/pids.h>
31 #include <iCub/iKin/iKinFwd.h>
32 
33 #define IKINCTRL_STATE_RUNNING 0
34 #define IKINCTRL_STATE_INTARGET 1
35 #define IKINCTRL_STATE_DEADLOCK 2
36 
37 #define IKINCTRL_POSE_FULL 0
38 #define IKINCTRL_POSE_XYZ 1
39 #define IKINCTRL_POSE_ANG 2
40 
41 #define IKINCTRL_STEEP_JT 0
42 #define IKINCTRL_STEEP_PINV 1
43 
44 #define IKINCTRL_RET_TOLX 0
45 #define IKINCTRL_RET_TOLSIZE 1
46 #define IKINCTRL_RET_TOLQ 2
47 #define IKINCTRL_RET_MAXITER 3
48 #define IKINCTRL_RET_EXHALT 4
49 
50 #define IKINCTRL_DISABLED -1
51 
52 
53 namespace iCub
54 {
55 
56 namespace iKin
57 {
58 
64 class iKinCtrl
65 {
66 private:
67  // Default constructor: not implemented.
68  iKinCtrl();
69  // Copy constructor: not implemented.
70  iKinCtrl(const iKinCtrl&);
71  // Assignment operator: not implemented.
72  iKinCtrl &operator=(const iKinCtrl&);
73 
74 protected:
76  unsigned int ctrlPose;
77 
78  yarp::sig::Vector x_set;
79  yarp::sig::Vector x;
80  yarp::sig::Vector e;
81  yarp::sig::Vector q;
82  yarp::sig::Matrix J;
83  yarp::sig::Matrix Jt;
84  yarp::sig::Matrix pinvJ;
85  yarp::sig::Vector grad;
86 
87  yarp::sig::Vector q_old;
88 
89  double inTargetTol;
90  double watchDogTol;
91 
92  unsigned int dim;
93  unsigned int iter;
94 
95  int state;
96 
97  bool watchDogOn;
100 
107  virtual yarp::sig::Vector calc_e();
108 
112  virtual void update_state();
113 
117  virtual void watchDog();
118 
126  virtual yarp::sig::Vector checkVelocity(const yarp::sig::Vector &_qdot, double _Ts);
127 
132  virtual void inTargetFcn() = 0;
133 
138  virtual void deadLockRecoveryFcn() = 0;
139 
158  virtual void printIter(const unsigned int verbose=0) = 0;
159 
165  unsigned int printHandling(const unsigned int verbose=0);
166 
167 public:
177  iKinCtrl(iKinChain &c, unsigned int _ctrlPose);
178 
183  virtual void setChainConstraints(bool _constrained) { chain.setAllConstraints(_constrained); }
184 
204  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0) = 0;
205 
239  virtual yarp::sig::Vector solve(yarp::sig::Vector &xd, const double tol_size=IKINCTRL_DISABLED,
240  const int max_iter=IKINCTRL_DISABLED, const unsigned int verbose=0,
241  int *exit_code=NULL, bool *exhalt=NULL);
242 
250  virtual bool test_convergence(const double tol_size) = 0;
251 
257  virtual void restart(const yarp::sig::Vector &q0);
258 
264  virtual std::string getAlgoName() = 0;
265 
272  void switchWatchDog(bool sw) { watchDogOn=sw; }
273 
278  virtual void setInTargetTol(double tol_x) { inTargetTol=tol_x; }
279 
284  virtual double getInTargetTol() const { return inTargetTol; }
285 
290  virtual void setWatchDogTol(double tol_q) { watchDogTol=tol_q; }
291 
296  virtual double getWatchDogTol() const { return watchDogTol; }
297 
303  virtual void setWatchDogMaxIter(int maxIter) { watchDogMaxIter=maxIter; }
304 
309  virtual int getWatchDogMaxIter() const { return watchDogMaxIter; }
310 
315  virtual bool isInTarget() { return dist()<inTargetTol; }
316 
324  int get_state() const { return state; }
325 
333  void set_ctrlPose(unsigned int _ctrlPose);
334 
339  unsigned int get_ctrlPose() const { return ctrlPose; }
340 
345  unsigned int get_dim() const { return dim; }
346 
351  unsigned int get_iter() const { return iter; }
352 
357  virtual yarp::sig::Vector get_x() const { return x; }
358 
363  virtual yarp::sig::Vector get_e() const { return e; }
364 
369  virtual void set_q(const yarp::sig::Vector &q0);
370 
375  virtual yarp::sig::Vector get_q() const { return q; }
376 
381  virtual yarp::sig::Vector get_grad() const { return grad; }
382 
387  virtual yarp::sig::Matrix get_J() const { return J; }
388 
394  virtual double dist() const { return yarp::math::norm(e); }
395 
399  virtual ~iKinCtrl() { }
400 };
401 
402 
411 class SteepCtrl : public iKinCtrl
412 {
413 private:
414  // Default constructor: not implemented.
415  SteepCtrl();
416  // Copy constructor: not implemented.
417  SteepCtrl(const SteepCtrl&);
418  // Assignment operator: not implemented.
419  SteepCtrl &operator=(const SteepCtrl&);
420 
421 protected:
422  double Ts;
424 
425  unsigned int type;
427 
428  double Kp;
429  yarp::sig::Vector qdot;
430  yarp::sig::Vector gpm;
431 
432  virtual void inTargetFcn() { }
433  virtual void deadLockRecoveryFcn() { }
434  virtual void printIter(const unsigned int verbose);
435  virtual yarp::sig::Vector update_qdot();
436 
437 public:
452  SteepCtrl(iKinChain &c, unsigned int _type, unsigned int _ctrlPose,
453  double _Ts, double _Kp);
454 
464  virtual yarp::sig::Vector computeGPM() { return yarp::sig::Vector(dim,0.0); }
465 
466  virtual void setChainConstraints(bool _constrained);
467  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0);
468  virtual void restart(const yarp::sig::Vector &q0);
469  virtual bool test_convergence(const double tol_size) { return yarp::math::norm(grad)<tol_size; }
470  virtual std::string getAlgoName() { return "steepest-descent"; }
471 
475  void resetInt() { I->reset(q); }
476 
481  yarp::sig::Vector get_qdot() const { return qdot; }
482 
487  yarp::sig::Vector get_gpm() const { return gpm; }
488 
493  double get_Kp() const { return Kp; }
494 
498  virtual ~SteepCtrl();
499 };
500 
501 
512 class VarKpSteepCtrl : public SteepCtrl
513 {
514 private:
515  // Default constructor: not implemented.
516  VarKpSteepCtrl();
517  // Copy constructor: not implemented.
519  // Assignment operator: not implemented.
520  VarKpSteepCtrl &operator=(const VarKpSteepCtrl&);
521 
522 protected:
523  double Kp0;
524  double Kp_inc;
525  double Kp_dec;
526  double Kp_max;
527  double max_perf_inc;
528 
529  double dist_old;
530 
531  void reset_Kp();
532  virtual void inTargetFcn() { reset_Kp(); }
533  virtual yarp::sig::Vector update_qdot();
534 
535 public:
554  VarKpSteepCtrl(iKinChain &c, unsigned int _type, unsigned int _ctrlPose, double _Ts,
555  double _Kp0, double _Kp_inc, double _Kp_dec, double _Kp_max, double _max_perf_inc);
556 
557  virtual void restart(const yarp::sig::Vector &q0) { SteepCtrl::restart(q0); reset_Kp(); }
558  virtual std::string getAlgoName() { return "variable-gain-steepest-descent"; }
559 };
560 
561 
576 class LMCtrl : public iKinCtrl
577 {
578 private:
579  // Default constructor: not implemented.
580  LMCtrl();
581  // Copy constructor: not implemented.
582  LMCtrl(const LMCtrl&);
583  // Assignment operator: not implemented.
584  LMCtrl &operator=(const LMCtrl&);
585 
586 protected:
587  double Ts;
589 
591 
592  yarp::sig::Vector qdot;
593  yarp::sig::Vector gpm;
594  yarp::sig::Matrix pinvLM;
595 
596  double mu;
597  double mu0;
598  double mu_inc;
599  double mu_dec;
600  double mu_min;
601  double mu_max;
602 
603  double dist_old;
604  double svMin;
605  double svThres;
606 
607  virtual double update_mu();
608  virtual void inTargetFcn() { }
609  virtual void deadLockRecoveryFcn() { }
610  virtual void printIter(const unsigned int verbose);
611 
612  virtual yarp::sig::Matrix pinv(const yarp::sig::Matrix &A, const double tol=0.0);
613 
614 public:
632  LMCtrl(iKinChain &c, unsigned int _ctrlPose, double _Ts, double _mu0, double _mu_inc,
633  double _mu_dec, double _mu_min, double _mu_max, double _sv_thres=1e-6);
634 
645  virtual yarp::sig::Vector computeGPM() { return yarp::sig::Vector(dim,0.0); }
646 
647  virtual void setChainConstraints(bool _constrained);
648  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0);
649  virtual void restart(const yarp::sig::Vector &q0);
650  virtual bool test_convergence(const double tol_size) { return yarp::math::norm(grad)<tol_size; }
651  virtual std::string getAlgoName() { return "levenberg-marquardt"; }
652 
656  void resetInt() { I->reset(q); }
657 
662  yarp::sig::Vector get_qdot() const { return qdot; }
663 
668  yarp::sig::Vector get_gpm() const { return gpm; }
669 
674  double get_mu() const { return mu; }
675 
679  void reset_mu();
680 
684  virtual ~LMCtrl();
685 };
686 
687 
695 class LMCtrl_GPM : public LMCtrl
696 {
697 private:
698  // Default constructor: not implemented.
699  LMCtrl_GPM();
700  // Copy constructor: not implemented.
701  LMCtrl_GPM(const LMCtrl_GPM&);
702  // Assignment operator: not implemented.
703  LMCtrl_GPM &operator=(const LMCtrl_GPM&);
704 
705 protected:
707  double K;
708 
709  yarp::sig::Vector span;
710  yarp::sig::Vector alpha_min;
711  yarp::sig::Vector alpha_max;
712  yarp::sig::Matrix Eye;
713 
714 public:
718  LMCtrl_GPM(iKinChain &c, unsigned int _ctrlPose, double _Ts, double _mu0, double _mu_inc,
719  double _mu_dec, double _mu_min, double _mu_max, double _sv_thres=1e-6);
720 
721  virtual yarp::sig::Vector computeGPM();
722 
727  void set_K(const double _K) { K=_K>0 ? _K : -_K; }
728 
733  double get_K() const { return K; }
734 
741  void set_safeAreaRatio(const double _safeAreaRatio);
742 
747  double get_safeAreaRatio() const { return safeAreaRatio; }
748 };
749 
750 
759 {
760 private:
761  // Default constructor: not implemented.
763  // Copy constructor: not implemented.
765  // Assignment operator: not implemented.
766  MultiRefMinJerkCtrl &operator=(const MultiRefMinJerkCtrl&);
767 
768 protected:
772 
773  yarp::sig::Vector q_set;
774  yarp::sig::Vector qdot;
775  yarp::sig::Vector xdot;
776  yarp::sig::Matrix W;
777  yarp::sig::Matrix Eye6;
778 
779  double Ts;
780  double execTime;
781  double gamma;
782  double guardRatio;
783 
784  yarp::sig::Vector qGuard;
787 
788  yarp::sig::Vector compensation;
789 
790  virtual void computeGuard();
791  virtual void computeWeight();
792  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
793  yarp::sig::Vector *xdot_set, const unsigned int verbose);
794 
795  virtual void inTargetFcn() { }
796  virtual void deadLockRecoveryFcn() { }
797  virtual void printIter(const unsigned int verbose);
798 
799  // disable unused father's methods
800  virtual bool test_convergence(const double) { return false; }
801  virtual yarp::sig::Vector iterate(yarp::sig::Vector&, const unsigned int) { return yarp::sig::Vector(0); }
802  virtual yarp::sig::Vector solve(yarp::sig::Vector&, const double,
803  const int, const unsigned int, int*, bool *) { return yarp::sig::Vector(0); }
804 
805 public:
820  MultiRefMinJerkCtrl(iKinChain &c, unsigned int _ctrlPose, double _Ts, bool nonIdealPlant=false);
821 
847  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
848  const unsigned int verbose=0);
849 
878  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
879  yarp::sig::Vector &xdot_set, const unsigned int verbose=0);
880 
881  virtual void restart(const yarp::sig::Vector &q0);
882 
883  virtual std::string getAlgoName() { return "multi-referential-minimum-jerk-controllers"; }
884 
892  double get_guardRatio() const { return guardRatio; }
893 
900  double get_gamma() const { return gamma; }
901 
906  double get_execTime() const { return execTime; }
907 
912  yarp::sig::Vector get_qdot() const { return qdot; }
913 
919  yarp::sig::Vector get_xdot() const { return xdot; }
920 
925  void set_guardRatio(double _guardRatio);
926 
933  void set_gamma(double _gamma) { gamma=_gamma; }
934 
939  virtual void set_q(const yarp::sig::Vector &q0);
940 
950  double set_execTime(const double _execTime, const bool warn=false);
951 
958  void add_compensation(const yarp::sig::Vector &comp);
959 
978  void setPlantParameters(const yarp::os::Property &parameters,
979  const std::string &entryTag="dimension");
980 
984  virtual ~MultiRefMinJerkCtrl();
985 };
986 
987 }
988 
989 }
990 
991 #endif
992 
993 
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:48
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
Definition: pids.cpp:128
Abstract class for minimum-jerk controllers with velocity commands.
Definition: minJerkCtrl.h:47
A class derived from LMCtrl implementing the Gradient Projection Method according to the paper availa...
Definition: iKinInv.h:696
yarp::sig::Vector span
Definition: iKinInv.h:709
yarp::sig::Matrix Eye
Definition: iKinInv.h:712
yarp::sig::Vector alpha_max
Definition: iKinInv.h:711
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.cpp:708
double get_K() const
Returns the GPM gain (1.0 by default).
Definition: iKinInv.h:733
void set_K(const double _K)
Sets the GPM gain (shall be positive).
Definition: iKinInv.h:727
void set_safeAreaRatio(const double _safeAreaRatio)
Sets the safe area ratio [0-1], which is for each joint the ratio between the angle span within which...
Definition: iKinInv.cpp:692
double get_safeAreaRatio() const
Returns the safe area ratio (0.9 by default).
Definition: iKinInv.h:747
yarp::sig::Vector alpha_min
Definition: iKinInv.h:710
A class derived from iKinCtrl implementing the Levenberg-Marquardt algorithm:
Definition: iKinInv.h:577
void reset_mu()
Sets the weighting factor mu equal to the initial value.
Definition: iKinInv.cpp:535
virtual bool test_convergence(const double tol_size)
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
Definition: iKinInv.h:650
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:628
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
Definition: iKinInv.h:668
virtual ~LMCtrl()
Destructor.
Definition: iKinInv.cpp:669
ctrl::Integrator * I
Definition: iKinInv.h:588
void resetInt()
Resets integral status at the current joint angles.
Definition: iKinInv.h:656
yarp::sig::Matrix pinvLM
Definition: iKinInv.h:594
double get_mu() const
Returns the current weighting factor mu.
Definition: iKinInv.h:674
yarp::sig::Vector gpm
Definition: iKinInv.h:593
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.h:645
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:662
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.cpp:497
virtual yarp::sig::Matrix pinv(const yarp::sig::Matrix &A, const double tol=0.0)
Definition: iKinInv.cpp:507
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:609
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:638
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)
Executes one iteration of the control algorithm.
Definition: iKinInv.cpp:570
virtual double update_mu()
Definition: iKinInv.cpp:543
virtual std::string getAlgoName()
Returns the algorithm's name.
Definition: iKinInv.h:651
yarp::sig::Vector qdot
Definition: iKinInv.h:592
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:608
A class derived from iKinCtrl implementing the multi-referential approach.
Definition: iKinInv.h:759
void setPlantParameters(const yarp::os::Property &parameters, const std::string &entryTag="dimension")
Allows user to assign values to the parameters of plant under control (for the configuration space on...
Definition: iKinInv.cpp:965
virtual yarp::sig::Vector iterate(yarp::sig::Vector &, const unsigned int)
Executes one iteration of the control algorithm.
Definition: iKinInv.h:801
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose)
yarp::sig::Vector get_xdot() const
Returns the actual derivative of End-Effector Pose (6 components; xdot=J*qdot).
Definition: iKinInv.h:919
yarp::sig::Vector xdot
Definition: iKinInv.h:775
double get_execTime() const
Returns the task execution time in seconds (1.0 by default).
Definition: iKinInv.h:906
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, const unsigned int verbose=0)
Executes one iteration of the control algorithm.
yarp::sig::Vector qGuardMaxCOG
Definition: iKinInv.h:786
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:796
yarp::sig::Vector qGuardMinExt
Definition: iKinInv.h:785
virtual ~MultiRefMinJerkCtrl()
Destructor.
Definition: iKinInv.cpp:984
yarp::sig::Vector qGuard
Definition: iKinInv.h:784
yarp::sig::Vector qGuardMinCOG
Definition: iKinInv.h:785
yarp::sig::Vector qGuardMinInt
Definition: iKinInv.h:785
yarp::sig::Vector qGuardMaxExt
Definition: iKinInv.h:786
virtual bool test_convergence(const double)
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
Definition: iKinInv.h:800
void add_compensation(const yarp::sig::Vector &comp)
Adds to the controller input a further compensation term.
Definition: iKinInv.cpp:956
double get_guardRatio() const
Returns the guard ratio for the joints span (0.1 by default).
Definition: iKinInv.h:892
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:898
yarp::sig::Vector q_set
Definition: iKinInv.h:773
double set_execTime(const double _execTime, const bool warn=false)
Sets the task execution time in seconds.
Definition: iKinInv.cpp:942
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector &xdot_set, const unsigned int verbose=0)
Executes one iteration of the control algorithm.
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:795
virtual yarp::sig::Vector solve(yarp::sig::Vector &, const double, const int, const unsigned int, int *, bool *)
Iterates the control algorithm trying to converge on the target.
Definition: iKinInv.h:802
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:912
yarp::sig::Vector qGuardMaxInt
Definition: iKinInv.h:786
ctrl::minJerkVelCtrl * mjCtrlTask
Definition: iKinInv.h:770
yarp::sig::Matrix Eye6
Definition: iKinInv.h:777
ctrl::minJerkVelCtrl * mjCtrlJoint
Definition: iKinInv.h:769
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:934
void set_guardRatio(double _guardRatio)
Sets the guard ratio (in [0 1]).
Definition: iKinInv.cpp:770
double get_gamma() const
Returns the parameter gamma which is used to blend the contribute of the task controller versus the c...
Definition: iKinInv.h:900
virtual std::string getAlgoName()
Returns the algorithm's name.
Definition: iKinInv.h:883
void set_gamma(double _gamma)
Sets the parameter gamma which is used to blend the contribute of the task controller versus the cont...
Definition: iKinInv.h:933
ctrl::Integrator * I
Definition: iKinInv.h:771
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:885
yarp::sig::Vector qdot
Definition: iKinInv.h:774
yarp::sig::Vector compensation
Definition: iKinInv.h:788
A class derived from iKinCtrl implementing two standard algorithms based on steepest descent qdot=-Kp...
Definition: iKinInv.h:412
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:372
yarp::sig::Vector gpm
Definition: iKinInv.h:430
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
Definition: iKinInv.h:487
unsigned int type
Definition: iKinInv.h:425
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:363
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.cpp:293
yarp::sig::Vector qdot
Definition: iKinInv.h:429
virtual ~SteepCtrl()
Destructor.
Definition: iKinInv.cpp:406
double get_Kp() const
Returns the gain.
Definition: iKinInv.h:493
virtual std::string getAlgoName()
Returns the algorithm's name.
Definition: iKinInv.h:470
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)
Executes one iteration of the control algorithm.
Definition: iKinInv.cpp:312
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:433
void resetInt()
Resets integral status at the current joint angles.
Definition: iKinInv.h:475
virtual bool test_convergence(const double tol_size)
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
Definition: iKinInv.h:469
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:432
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.h:464
ctrl::Integrator * I
Definition: iKinInv.h:423
virtual yarp::sig::Vector update_qdot()
Definition: iKinInv.cpp:303
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:481
A class derived from SteepCtrl implementing the variable gain algorithm.
Definition: iKinInv.h:513
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.h:557
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:532
virtual yarp::sig::Vector update_qdot()
Definition: iKinInv.cpp:437
virtual std::string getAlgoName()
Returns the algorithm's name.
Definition: iKinInv.h:558
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:498
Abstract class for inverting chain's kinematics.
Definition: iKinInv.h:65
iKinChain & chain
Definition: iKinInv.h:75
virtual void setWatchDogTol(double tol_q)
Sets tolerance for watchDog check (1e-4 by default).
Definition: iKinInv.h:290
virtual int getWatchDogMaxIter() const
Returns maximum number of iterations to trigger the watchDog.
Definition: iKinInv.h:309
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:191
yarp::sig::Matrix Jt
Definition: iKinInv.h:83
yarp::sig::Vector e
Definition: iKinInv.h:80
virtual yarp::sig::Vector calc_e()
Computes the error according to the current controller settings (complete pose/translational/rotation...
Definition: iKinInv.cpp:88
virtual bool isInTarget()
Checks if the End-Effector is in target.
Definition: iKinInv.h:315
virtual void watchDog()
Handles the watchDog.
Definition: iKinInv.cpp:172
yarp::sig::Vector x_set
Definition: iKinInv.h:78
virtual void printIter(const unsigned int verbose=0)=0
Dumps warning or status messages.
unsigned int dim
Definition: iKinInv.h:92
unsigned int get_ctrlPose() const
Returns the state of Pose control settings.
Definition: iKinInv.h:339
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:74
void switchWatchDog(bool sw)
Switch on/off the watchDog mechanism to trigger deadLocks.
Definition: iKinInv.h:272
yarp::sig::Matrix J
Definition: iKinInv.h:82
int get_state() const
Returns the algorithm's state.
Definition: iKinInv.h:324
yarp::sig::Vector q_old
Definition: iKinInv.h:87
virtual std::string getAlgoName()=0
Returns the algorithm's name.
unsigned int printHandling(const unsigned int verbose=0)
Method to be called within the printIter routine inherited by children in order to handle the highest...
Definition: iKinInv.cpp:159
yarp::sig::Vector grad
Definition: iKinInv.h:85
virtual yarp::sig::Matrix get_J() const
Returns the actual Jacobian used in computation.
Definition: iKinInv.h:387
virtual bool test_convergence(const double tol_size)=0
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
virtual void update_state()
Updates the control state.
Definition: iKinInv.cpp:126
virtual yarp::sig::Vector get_q() const
Returns the actual joint angles values.
Definition: iKinInv.h:375
virtual yarp::sig::Vector checkVelocity(const yarp::sig::Vector &_qdot, double _Ts)
Checks each joint velocity and sets it to zero if it steers the joint out of range.
Definition: iKinInv.cpp:200
virtual double getWatchDogTol() const
Returns tolerance for watchDog check.
Definition: iKinInv.h:296
virtual yarp::sig::Vector get_grad() const
Returns the actual gradient.
Definition: iKinInv.h:381
virtual void setInTargetTol(double tol_x)
Sets tolerance for in-target check (5e-3 by default).
Definition: iKinInv.h:278
unsigned int get_dim() const
Returns the number of Chain DOF.
Definition: iKinInv.h:345
virtual yarp::sig::Vector solve(yarp::sig::Vector &xd, const double tol_size=IKINCTRL_DISABLED, const int max_iter=IKINCTRL_DISABLED, const unsigned int verbose=0, int *exit_code=NULL, bool *exhalt=NULL)
Iterates the control algorithm trying to converge on the target.
Definition: iKinInv.cpp:214
yarp::sig::Vector x
Definition: iKinInv.h:79
double inTargetTol
Definition: iKinInv.h:89
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
Definition: iKinInv.cpp:64
virtual yarp::sig::Vector get_e() const
Returns the actual cartesian position error.
Definition: iKinInv.h:363
yarp::sig::Vector q
Definition: iKinInv.h:81
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
Definition: iKinInv.h:357
virtual double getInTargetTol() const
Returns tolerance for in-target check.
Definition: iKinInv.h:284
unsigned int ctrlPose
Definition: iKinInv.h:76
virtual ~iKinCtrl()
Default destructor.
Definition: iKinInv.h:399
virtual void deadLockRecoveryFcn()=0
Method called whenever the watchDog is triggered.
double watchDogTol
Definition: iKinInv.h:90
virtual void inTargetFcn()=0
Method called whenever in target.
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.h:183
virtual void setWatchDogMaxIter(int maxIter)
Sets maximum number of iterations to trigger the watchDog (200 by default).
Definition: iKinInv.h:303
unsigned int iter
Definition: iKinInv.h:93
yarp::sig::Matrix pinvJ
Definition: iKinInv.h:84
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)=0
Executes one iteration of the control algorithm.
virtual double dist() const
Returns the actual distance from the target in cartesian space (euclidean norm is used).
Definition: iKinInv.h:394
unsigned int get_iter() const
Returns the number of performed iterations.
Definition: iKinInv.h:351
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
#define IKINCTRL_DISABLED
Definition: iKinInv.h:50
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
A
Definition: sine.m:16