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 
760 {
761 private:
762  // Default constructor: not implemented.
764  // Copy constructor: not implemented.
766  // Assignment operator: not implemented.
767  MultiRefMinJerkCtrl &operator=(const MultiRefMinJerkCtrl&);
768 
769 protected:
773 
774  yarp::sig::Vector q_set;
775  yarp::sig::Vector qdot;
776  yarp::sig::Vector xdot;
777  yarp::sig::Matrix W;
778  yarp::sig::Matrix Eye6;
779 
780  double Ts;
781  double execTime;
782  double gamma;
783  double guardRatio;
784 
785  yarp::sig::Vector qGuard;
786  yarp::sig::Vector qGuardMinInt, qGuardMinExt, qGuardMinCOG;
787  yarp::sig::Vector qGuardMaxInt, qGuardMaxExt, qGuardMaxCOG;
788 
789  yarp::sig::Vector compensation;
790 
791  virtual void computeGuard();
792  virtual void computeWeight();
793  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
794  yarp::sig::Vector *xdot_set, const unsigned int verbose);
795 
796  virtual void inTargetFcn() { }
797  virtual void deadLockRecoveryFcn() { }
798  virtual void printIter(const unsigned int verbose);
799 
800  // disable unused father's methods
801  virtual bool test_convergence(const double) { return false; }
802  virtual yarp::sig::Vector iterate(yarp::sig::Vector&, const unsigned int) { return yarp::sig::Vector(0); }
803  virtual yarp::sig::Vector solve(yarp::sig::Vector&, const double,
804  const int, const unsigned int, int*, bool *) { return yarp::sig::Vector(0); }
805 
806 public:
821  MultiRefMinJerkCtrl(iKinChain &c, unsigned int _ctrlPose, double _Ts, bool nonIdealPlant=false);
822 
848  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
849  const unsigned int verbose=0);
850 
879  virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
880  yarp::sig::Vector &xdot_set, const unsigned int verbose=0);
881 
882  virtual void restart(const yarp::sig::Vector &q0);
883 
884  virtual std::string getAlgoName() { return "multi-referential-minimum-jerk-controllers"; }
885 
893  double get_guardRatio() const { return guardRatio; }
894 
901  double get_gamma() const { return gamma; }
902 
907  double get_execTime() const { return execTime; }
908 
913  yarp::sig::Vector get_qdot() const { return qdot; }
914 
920  yarp::sig::Vector get_xdot() const { return xdot; }
921 
926  void set_guardRatio(double _guardRatio);
927 
934  void set_gamma(double _gamma) { gamma=_gamma; }
935 
940  virtual void set_q(const yarp::sig::Vector &q0);
941 
951  double set_execTime(const double _execTime, const bool warn=false);
952 
959  void add_compensation(const yarp::sig::Vector &comp);
960 
979  void setPlantParameters(const yarp::os::Property &parameters,
980  const std::string &entryTag="dimension");
981 
985  virtual ~MultiRefMinJerkCtrl();
986 };
987 
988 }
989 
990 }
991 
992 #endif
993 
994 
double watchDogTol
Definition: iKinInv.h:90
virtual std::string getAlgoName()
Returns the algorithm&#39;s name.
Definition: iKinInv.h:470
#define IKINCTRL_DISABLED
Definition: iKinInv.h:50
void set_K(const double _K)
Sets the GPM gain (shall be positive).
Definition: iKinInv.h:727
yarp::sig::Vector qGuard
Definition: iKinInv.h:785
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.h:183
A class derived from iKinCtrl implementing two standard algorithms based on steepest descent qdot=-Kp...
Definition: iKinInv.h:411
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:934
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm&#39;s internal state and resets the starting point.
Definition: iKinInv.cpp:363
yarp::sig::Matrix pinvLM
Definition: iKinInv.h:594
virtual std::string getAlgoName()
Returns the algorithm&#39;s name.
Definition: iKinInv.h:651
double get_guardRatio() const
Returns the guard ratio for the joints span (0.1 by default).
Definition: iKinInv.h:893
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:532
A class derived from iKinCtrl implementing the Levenberg-Marquardt algorithm:
Definition: iKinInv.h:576
yarp::sig::Vector x
Definition: iKinInv.h:79
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm&#39;s internal state and resets the starting point.
Definition: iKinInv.cpp:191
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.h:645
ctrl::Integrator * I
Definition: iKinInv.h:423
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:901
Abstract class for minimum-jerk controllers with velocity commands.
Definition: minJerkCtrl.h:46
yarp::sig::Vector get_xdot() const
Returns the actual derivative of End-Effector Pose (6 components; xdot=J*qdot).
Definition: iKinInv.h:920
yarp::sig::Vector grad
Definition: iKinInv.h:85
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.h:464
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:481
unsigned int dim
Definition: iKinInv.h:92
yarp::sig::Vector e
Definition: iKinInv.h:80
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:608
void resetInt()
Resets integral status at the current joint angles.
Definition: iKinInv.h:475
virtual void update_state()
Updates the control state.
Definition: iKinInv.cpp:126
int get_state() const
Returns the algorithm&#39;s state.
Definition: iKinInv.h:324
virtual double dist() const
Returns the actual distance from the target in cartesian space (euclidean norm is used)...
Definition: iKinInv.h:394
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
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
virtual bool isInTarget()
Checks if the End-Effector is in target.
Definition: iKinInv.h:315
yarp::sig::Matrix J
Definition: iKinInv.h:82
virtual double getInTargetTol() const
Returns tolerance for in-target check.
Definition: iKinInv.h:284
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:433
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:803
yarp::sig::Matrix Jt
Definition: iKinInv.h:83
yarp::sig::Vector qdot
Definition: iKinInv.h:592
yarp::sig::Vector qGuardMinInt
Definition: iKinInv.h:786
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
void resetInt()
Resets integral status at the current joint angles.
Definition: iKinInv.h:656
yarp::sig::Vector qdot
Definition: iKinInv.h:775
yarp::sig::Vector gpm
Definition: iKinInv.h:430
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:662
yarp::sig::Vector q_set
Definition: iKinInv.h:774
virtual std::string getAlgoName()=0
Returns the algorithm&#39;s name.
yarp::sig::Matrix Eye
Definition: iKinInv.h:712
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
Definition: pids.cpp:128
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:797
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...
double get_Kp() const
Returns the gain.
Definition: iKinInv.h:493
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
unsigned int get_iter() const
Returns the number of performed iterations.
Definition: iKinInv.h:351
void switchWatchDog(bool sw)
Switch on/off the watchDog mechanism to trigger deadLocks.
Definition: iKinInv.h:272
virtual std::string getAlgoName()
Returns the algorithm&#39;s name.
Definition: iKinInv.h:558
A class derived from LMCtrl implementing the Gradient Projection Method according to the paper availa...
Definition: iKinInv.h:695
yarp::sig::Vector alpha_min
Definition: iKinInv.h:710
virtual yarp::sig::Vector get_grad() const
Returns the actual gradient.
Definition: iKinInv.h:381
yarp::sig::Vector xdot
Definition: iKinInv.h:776
virtual yarp::sig::Vector calc_e()
Computes the error according to the current controller settings (complete pose/translational/rotation...
Definition: iKinInv.cpp:88
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
virtual double getWatchDogTol() const
Returns tolerance for watchDog check.
Definition: iKinInv.h:296
ctrl::minJerkVelCtrl * mjCtrlTask
Definition: iKinInv.h:771
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
Definition: iKinInv.h:668
double get_safeAreaRatio() const
Returns the safe area ratio (0.9 by default).
Definition: iKinInv.h:747
yarp::sig::Vector qdot
Definition: iKinInv.h:429
yarp::sig::Vector q_old
Definition: iKinInv.h:87
double get_execTime() const
Returns the task execution time in seconds (1.0 by default).
Definition: iKinInv.h:907
ctrl::Integrator * I
Definition: iKinInv.h:772
unsigned int iter
Definition: iKinInv.h:93
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:74
Abstract class for inverting chain&#39;s kinematics.
Definition: iKinInv.h:64
yarp::sig::Vector gpm
Definition: iKinInv.h:593
A
Definition: sine.m:16
virtual yarp::sig::Vector get_e() const
Returns the actual cartesian position error.
Definition: iKinInv.h:363
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:47
virtual void deadLockRecoveryFcn()=0
Method called whenever the watchDog is triggered.
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:913
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:801
GtkWidget * sw
Definition: main.cpp:170
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:432
double inTargetTol
Definition: iKinInv.h:89
yarp::sig::Vector span
Definition: iKinInv.h:709
yarp::sig::Vector qGuardMaxInt
Definition: iKinInv.h:787
unsigned int ctrlPose
Definition: iKinInv.h:76
yarp::sig::Vector alpha_max
Definition: iKinInv.h:711
virtual void printIter(const unsigned int verbose=0)=0
Dumps warning or status messages.
virtual void setInTargetTol(double tol_x)
Sets tolerance for in-target check (5e-3 by default).
Definition: iKinInv.h:278
A class derived from iKinCtrl implementing the multi-referential approach (pdf).
Definition: iKinInv.h:759
ctrl::minJerkVelCtrl * mjCtrlJoint
Definition: iKinInv.h:770
virtual yarp::sig::Matrix get_J() const
Returns the actual Jacobian used in computation.
Definition: iKinInv.h:387
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)=0
Executes one iteration of the control algorithm.
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm&#39;s internal state and resets the starting point.
Definition: iKinInv.h:557
unsigned int type
Definition: iKinInv.h:425
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 deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:609
virtual yarp::sig::Vector iterate(yarp::sig::Vector &, const unsigned int)
Executes one iteration of the control algorithm.
Definition: iKinInv.h:802
A class derived from SteepCtrl implementing the variable gain algorithm.
Definition: iKinInv.h:512
virtual void watchDog()
Handles the watchDog.
Definition: iKinInv.cpp:172
virtual void setWatchDogMaxIter(int maxIter)
Sets maximum number of iterations to trigger the watchDog (200 by default).
Definition: iKinInv.h:303
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:796
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
unsigned int get_dim() const
Returns the number of Chain DOF.
Definition: iKinInv.h:345
yarp::sig::Matrix Eye6
Definition: iKinInv.h:778
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
Definition: iKinInv.h:487
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
unsigned int get_ctrlPose() const
Returns the state of Pose control settings.
Definition: iKinInv.h:339
virtual ~iKinCtrl()
Default destructor.
Definition: iKinInv.h:399
virtual void inTargetFcn()=0
Method called whenever in target.
double get_K() const
Returns the GPM gain (1.0 by default).
Definition: iKinInv.h:733
ctrl::Integrator * I
Definition: iKinInv.h:588
iKinChain & chain
Definition: iKinInv.h:75
yarp::sig::Vector x_set
Definition: iKinInv.h:78
virtual int getWatchDogMaxIter() const
Returns maximum number of iterations to trigger the watchDog.
Definition: iKinInv.h:309
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:497
yarp::sig::Matrix pinvJ
Definition: iKinInv.h:84
double get_mu() const
Returns the current weighting factor mu.
Definition: iKinInv.h:674
virtual yarp::sig::Vector get_q() const
Returns the actual joint angles values.
Definition: iKinInv.h:375
virtual void setWatchDogTol(double tol_q)
Sets tolerance for watchDog check (1e-4 by default).
Definition: iKinInv.h:290
virtual std::string getAlgoName()
Returns the algorithm&#39;s name.
Definition: iKinInv.h:884
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
Definition: iKinInv.h:357
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
Definition: iKinInv.cpp:64
yarp::sig::Vector compensation
Definition: iKinInv.h:789
yarp::sig::Vector q
Definition: iKinInv.h:81