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;
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 
iCub::iKin::iKinCtrl::watchDogOn
bool watchDogOn
Definition: iKinInv.h:97
iCub::iKin::VarKpSteepCtrl
Definition: iKinInv.h:512
iCub::iKin::iKinCtrl::get_state
int get_state() const
Returns the algorithm's state.
Definition: iKinInv.h:324
iCub::iKin::LMCtrl::mu_dec
double mu_dec
Definition: iKinInv.h:599
iCub::iKin::MultiRefMinJerkCtrl::qGuardMinExt
yarp::sig::Vector qGuardMinExt
Definition: iKinInv.h:786
iCub::iKin::iKinCtrl::chain
iKinChain & chain
Definition: iKinInv.h:75
iCub::iKin::MultiRefMinJerkCtrl::mjCtrlJoint
ctrl::minJerkVelCtrl * mjCtrlJoint
Definition: iKinInv.h:770
iCub::iKin::VarKpSteepCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.h:557
iKinFwd.h
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::ctrl::Integrator
Definition: pids.h:47
iCub::iKin::iKinCtrl::calc_e
virtual yarp::sig::Vector calc_e()
Computes the error according to the current controller settings (complete pose/translational/rotation...
Definition: iKinInv.cpp:88
iCub::iKin::VarKpSteepCtrl::Kp_max
double Kp_max
Definition: iKinInv.h:526
iCub::iKin::VarKpSteepCtrl::dist_old
double dist_old
Definition: iKinInv.h:529
iCub::iKin::iKinCtrl::get_J
virtual yarp::sig::Matrix get_J() const
Returns the actual Jacobian used in computation.
Definition: iKinInv.h:387
iCub::iKin::SteepCtrl::get_qdot
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:481
iCub::iKin::iKinCtrl::inTargetTol
double inTargetTol
Definition: iKinInv.h:89
iCub::iKin::LMCtrl_GPM::Eye
yarp::sig::Matrix Eye
Definition: iKinInv.h:712
iCub::iKin::LMCtrl::update_mu
virtual double update_mu()
Definition: iKinInv.cpp:543
iCub::iKin::LMCtrl
Definition: iKinInv.h:576
iCub::iKin::iKinCtrl::watchDogTol
double watchDogTol
Definition: iKinInv.h:90
iCub::iKin::MultiRefMinJerkCtrl::inTargetFcn
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:796
iCub::iKin::iKinCtrl::inTargetFcn
virtual void inTargetFcn()=0
Method called whenever in target.
iCub::iKin::LMCtrl::mu_min
double mu_min
Definition: iKinInv.h:600
iCub::iKin::LMCtrl::svMin
double svMin
Definition: iKinInv.h:604
iCub::iKin::SteepCtrl::computeGPM
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.h:464
iCub::iKin::VarKpSteepCtrl::getAlgoName
virtual std::string getAlgoName()
Returns the algorithm's name.
Definition: iKinInv.h:558
iCub::iKin::MultiRefMinJerkCtrl::set_execTime
double set_execTime(const double _execTime, const bool warn=false)
Sets the task execution time in seconds.
Definition: iKinInv.cpp:942
iCub::iKin::SteepCtrl::resetInt
void resetInt()
Resets integral status at the current joint angles.
Definition: iKinInv.h:475
iCub::iKin::MultiRefMinJerkCtrl::qdot
yarp::sig::Vector qdot
Definition: iKinInv.h:775
iCub::iKin::MultiRefMinJerkCtrl::Eye6
yarp::sig::Matrix Eye6
Definition: iKinInv.h:778
iCub::iKin::MultiRefMinJerkCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose)
iCub::iKin::LMCtrl_GPM::computeGPM
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.cpp:708
iCub::iKin::iKinCtrl::getWatchDogTol
virtual double getWatchDogTol() const
Returns tolerance for watchDog check.
Definition: iKinInv.h:296
iCub::iKin::LMCtrl::I
ctrl::Integrator * I
Definition: iKinInv.h:588
iCub::iKin::iKinCtrl::get_x
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
Definition: iKinInv.h:357
iCub::iKin::iKinCtrl::deadLockRecoveryFcn
virtual void deadLockRecoveryFcn()=0
Method called whenever the watchDog is triggered.
iCub::iKin::iKinCtrl::get_ctrlPose
unsigned int get_ctrlPose() const
Returns the state of Pose control settings.
Definition: iKinInv.h:339
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iCub::iKin::LMCtrl::deadLockRecoveryFcn
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:609
iCub::iKin::MultiRefMinJerkCtrl::get_execTime
double get_execTime() const
Returns the task execution time in seconds (1.0 by default).
Definition: iKinInv.h:907
IKINCTRL_DISABLED
#define IKINCTRL_DISABLED
Definition: iKinInv.h:50
iCub::iKin::MultiRefMinJerkCtrl::set_guardRatio
void set_guardRatio(double _guardRatio)
Sets the guard ratio (in [0 1]).
Definition: iKinInv.cpp:770
iCub::iKin::SteepCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:363
iCub::iKin::iKinCtrl::setChainConstraints
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.h:183
iCub::iKin::LMCtrl::test_convergence
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
iCub::iKin::iKinCtrl::dist
virtual double dist() const
Returns the actual distance from the target in cartesian space (euclidean norm is used).
Definition: iKinInv.h:394
iCub::iKin::MultiRefMinJerkCtrl::compensation
yarp::sig::Vector compensation
Definition: iKinInv.h:789
iCub::iKin::MultiRefMinJerkCtrl::W
yarp::sig::Matrix W
Definition: iKinInv.h:777
iCub::iKin::MultiRefMinJerkCtrl::xdot
yarp::sig::Vector xdot
Definition: iKinInv.h:776
iCub::iKin::iKinCtrl::ctrlPose
unsigned int ctrlPose
Definition: iKinInv.h:76
iCub::iKin::LMCtrl::pinvLM
yarp::sig::Matrix pinvLM
Definition: iKinInv.h:594
iCub::iKin::iKinCtrl::e
yarp::sig::Vector e
Definition: iKinInv.h:80
iCub::iKin::SteepCtrl::Ts
double Ts
Definition: iKinInv.h:422
iCub::iKin::SteepCtrl::iterate
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
iCub::iKin::MultiRefMinJerkCtrl::I
ctrl::Integrator * I
Definition: iKinInv.h:772
iCub::iKin::iKinCtrl::~iKinCtrl
virtual ~iKinCtrl()
Default destructor.
Definition: iKinInv.h:399
iCub::iKin::LMCtrl::svThres
double svThres
Definition: iKinInv.h:605
iCub::iKin::iKinCtrl::solve
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
iCub::iKin::LMCtrl::pinv
virtual yarp::sig::Matrix pinv(const yarp::sig::Matrix &A, const double tol=0.0)
Definition: iKinInv.cpp:507
iCub::iKin::MultiRefMinJerkCtrl::getAlgoName
virtual std::string getAlgoName()
Returns the algorithm's name.
Definition: iKinInv.h:884
iCub::iKin::iKinCtrl::watchDogCnt
int watchDogCnt
Definition: iKinInv.h:98
iCub::iKin::LMCtrl::gpm
yarp::sig::Vector gpm
Definition: iKinInv.h:593
iCub::iKin::MultiRefMinJerkCtrl::get_guardRatio
double get_guardRatio() const
Returns the guard ratio for the joints span (0.1 by default).
Definition: iKinInv.h:893
iCub::iKin::MultiRefMinJerkCtrl::printIter
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:898
iCub::iKin::LMCtrl::~LMCtrl
virtual ~LMCtrl()
Destructor.
Definition: iKinInv.cpp:669
iCub::iKin::LMCtrl::reset_mu
void reset_mu()
Sets the weighting factor mu equal to the initial value.
Definition: iKinInv.cpp:535
iCub::iKin::LMCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:628
iCub::iKin::iKinCtrl::setInTargetTol
virtual void setInTargetTol(double tol_x)
Sets tolerance for in-target check (5e-3 by default).
Definition: iKinInv.h:278
iCub::iKin::SteepCtrl::deadLockRecoveryFcn
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:433
iCub::iKin::LMCtrl_GPM::K
double K
Definition: iKinInv.h:707
iCub::iKin::LMCtrl_GPM::get_K
double get_K() const
Returns the GPM gain (1.0 by default).
Definition: iKinInv.h:733
iCub::iKin::SteepCtrl
Definition: iKinInv.h:411
iCub::iKin::MultiRefMinJerkCtrl::qGuardMinCOG
yarp::sig::Vector qGuardMinCOG
Definition: iKinInv.h:786
iCub::iKin::MultiRefMinJerkCtrl::test_convergence
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
iCub::iKin::LMCtrl_GPM::alpha_max
yarp::sig::Vector alpha_max
Definition: iKinInv.h:711
iCub::iKin::iKinCtrl::update_state
virtual void update_state()
Updates the control state.
Definition: iKinInv.cpp:126
iCub::iKin::MultiRefMinJerkCtrl::qGuardMaxInt
yarp::sig::Vector qGuardMaxInt
Definition: iKinInv.h:787
iCub::iKin::iKinCtrl::isInTarget
virtual bool isInTarget()
Checks if the End-Effector is in target.
Definition: iKinInv.h:315
iCub::iKin::iKinCtrl::setWatchDogMaxIter
virtual void setWatchDogMaxIter(int maxIter)
Sets maximum number of iterations to trigger the watchDog (200 by default).
Definition: iKinInv.h:303
iCub::iKin::SteepCtrl::gpm
yarp::sig::Vector gpm
Definition: iKinInv.h:430
iCub::iKin::LMCtrl_GPM::set_safeAreaRatio
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
iCub::iKin::MultiRefMinJerkCtrl
Definition: iKinInv.h:759
iCub::iKin::iKinCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)=0
Executes one iteration of the control algorithm.
iCub::iKin::SteepCtrl::get_gpm
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
Definition: iKinInv.h:487
iCub::iKin::VarKpSteepCtrl::max_perf_inc
double max_perf_inc
Definition: iKinInv.h:527
iCub::iKin::iKinCtrl::x_set
yarp::sig::Vector x_set
Definition: iKinInv.h:78
iCub::ctrl::Integrator::reset
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
Definition: pids.cpp:128
iCub::iKin::MultiRefMinJerkCtrl::get_gamma
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
iCub::iKin::SteepCtrl::I
ctrl::Integrator * I
Definition: iKinInv.h:423
iCub::iKin::LMCtrl::printIter
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:638
iCub::iKin::iKinCtrl::x
yarp::sig::Vector x
Definition: iKinInv.h:79
iCub::iKin::iKinCtrl::printIter
virtual void printIter(const unsigned int verbose=0)=0
Dumps warning or status messages.
iCub::iKin::MultiRefMinJerkCtrl::qGuardMaxExt
yarp::sig::Vector qGuardMaxExt
Definition: iKinInv.h:787
iCub::iKin::MultiRefMinJerkCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &, const unsigned int)
Executes one iteration of the control algorithm.
Definition: iKinInv.h:802
iCub::iKin::LMCtrl::mu_max
double mu_max
Definition: iKinInv.h:601
iCub::iKin::MultiRefMinJerkCtrl::get_xdot
yarp::sig::Vector get_xdot() const
Returns the actual derivative of End-Effector Pose (6 components; xdot=J*qdot).
Definition: iKinInv.h:920
iCub::iKin::LMCtrl::Ts
double Ts
Definition: iKinInv.h:587
iCub::iKin::VarKpSteepCtrl::Kp_inc
double Kp_inc
Definition: iKinInv.h:524
iCub::iKin::iKinCtrl::q
yarp::sig::Vector q
Definition: iKinInv.h:81
iCub::iKin::MultiRefMinJerkCtrl::execTime
double execTime
Definition: iKinInv.h:781
iCub::iKin::iKinCtrl::q_old
yarp::sig::Vector q_old
Definition: iKinInv.h:87
iCub::iKin::LMCtrl::resetInt
void resetInt()
Resets integral status at the current joint angles.
Definition: iKinInv.h:656
iCub::iKin::MultiRefMinJerkCtrl::gamma
double gamma
Definition: iKinInv.h:782
iCub::iKin::iKinCtrl::watchDog
virtual void watchDog()
Handles the watchDog.
Definition: iKinInv.cpp:172
iCub::iKin::SteepCtrl::get_Kp
double get_Kp() const
Returns the gain.
Definition: iKinInv.h:493
iCub::iKin::LMCtrl::constrained
bool constrained
Definition: iKinInv.h:590
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
iCub::iKin::iKinCtrl::pinvJ
yarp::sig::Matrix pinvJ
Definition: iKinInv.h:84
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::iKin::LMCtrl::qdot
yarp::sig::Vector qdot
Definition: iKinInv.h:592
iCub::iKin::MultiRefMinJerkCtrl::deadLockRecoveryFcn
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
Definition: iKinInv.h:797
iCub::iKin::SteepCtrl::qdot
yarp::sig::Vector qdot
Definition: iKinInv.h:429
iCub::iKin::SteepCtrl::type
unsigned int type
Definition: iKinInv.h:425
iCub::iKin::iKinCtrl::get_dim
unsigned int get_dim() const
Returns the number of Chain DOF.
Definition: iKinInv.h:345
iCub::iKin::MultiRefMinJerkCtrl::Ts
double Ts
Definition: iKinInv.h:780
iCub::iKin::iKinCtrl::Jt
yarp::sig::Matrix Jt
Definition: iKinInv.h:83
iCub::iKin::iKinCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:191
iCub::iKin::MultiRefMinJerkCtrl::qGuardMinInt
yarp::sig::Vector qGuardMinInt
Definition: iKinInv.h:786
iCub::iKin::MultiRefMinJerkCtrl::computeWeight
virtual void computeWeight()
Definition: iKinInv.cpp:797
iCub::iKin::LMCtrl::dist_old
double dist_old
Definition: iKinInv.h:603
iCub::iKin::iKinCtrl::get_grad
virtual yarp::sig::Vector get_grad() const
Returns the actual gradient.
Definition: iKinInv.h:381
iCub::iKin::SteepCtrl::getAlgoName
virtual std::string getAlgoName()
Returns the algorithm's name.
Definition: iKinInv.h:470
iCub::iKin::SteepCtrl::constrained
bool constrained
Definition: iKinInv.h:426
iCub::iKin::iKinCtrl::setWatchDogTol
virtual void setWatchDogTol(double tol_q)
Sets tolerance for watchDog check (1e-4 by default).
Definition: iKinInv.h:290
iCub::iKin::iKinCtrl::get_iter
unsigned int get_iter() const
Returns the number of performed iterations.
Definition: iKinInv.h:351
iCub::iKin::iKinCtrl::switchWatchDog
void switchWatchDog(bool sw)
Switch on/off the watchDog mechanism to trigger deadLocks.
Definition: iKinInv.h:272
iCub::iKin::SteepCtrl::~SteepCtrl
virtual ~SteepCtrl()
Destructor.
Definition: iKinInv.cpp:406
iCub::iKin::VarKpSteepCtrl::inTargetFcn
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:532
iCub::iKin::MultiRefMinJerkCtrl::~MultiRefMinJerkCtrl
virtual ~MultiRefMinJerkCtrl()
Destructor.
Definition: iKinInv.cpp:984
iCub::iKin::VarKpSteepCtrl::update_qdot
virtual yarp::sig::Vector update_qdot()
Definition: iKinInv.cpp:437
iCub::iKin::iKinCtrl::grad
yarp::sig::Vector grad
Definition: iKinInv.h:85
iCub::iKin::SteepCtrl::printIter
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
Definition: iKinInv.cpp:372
iCub::iKin::LMCtrl::getAlgoName
virtual std::string getAlgoName()
Returns the algorithm's name.
Definition: iKinInv.h:651
iCub::iKin::MultiRefMinJerkCtrl::q_set
yarp::sig::Vector q_set
Definition: iKinInv.h:774
iCub::iKin::iKinCtrl::set_ctrlPose
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
Definition: iKinInv.cpp:64
iCub::iKin::iKinCtrl::printHandling
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
iCub::iKin::LMCtrl::iterate
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
A
A
Definition: sine.m:16
iCub::iKin::VarKpSteepCtrl::reset_Kp
void reset_Kp()
Definition: iKinInv.cpp:429
iCub::iKin::iKinCtrl::get_e
virtual yarp::sig::Vector get_e() const
Returns the actual cartesian position error.
Definition: iKinInv.h:363
iCub::iKin::LMCtrl_GPM::alpha_min
yarp::sig::Vector alpha_min
Definition: iKinInv.h:710
iCub::iKin::MultiRefMinJerkCtrl::qGuardMaxCOG
yarp::sig::Vector qGuardMaxCOG
Definition: iKinInv.h:787
iCub::iKin::SteepCtrl::inTargetFcn
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:432
iCub::iKin::VarKpSteepCtrl::Kp0
double Kp0
Definition: iKinInv.h:523
iCub::iKin::LMCtrl_GPM
Definition: iKinInv.h:695
iCub::iKin::MultiRefMinJerkCtrl::guardRatio
double guardRatio
Definition: iKinInv.h:783
iCub::iKin::iKinCtrl::set_q
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:74
iCub::iKin::LMCtrl::mu0
double mu0
Definition: iKinInv.h:597
iCub::iKin::LMCtrl::mu
double mu
Definition: iKinInv.h:596
minJerkCtrl.h
iCub::iKin::LMCtrl_GPM::set_K
void set_K(const double _K)
Sets the GPM gain (shall be positive).
Definition: iKinInv.h:727
iCub::iKin::MultiRefMinJerkCtrl::mjCtrlTask
ctrl::minJerkVelCtrl * mjCtrlTask
Definition: iKinInv.h:771
iCub::iKin::MultiRefMinJerkCtrl::computeGuard
virtual void computeGuard()
Definition: iKinInv.cpp:779
iCub::iKin::iKinCtrl::test_convergence
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...
iCub::iKin::MultiRefMinJerkCtrl::get_qdot
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:913
iCub::iKin::MultiRefMinJerkCtrl::setPlantParameters
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
iCub::iKin::LMCtrl::get_mu
double get_mu() const
Returns the current weighting factor mu.
Definition: iKinInv.h:674
iCub::iKin::iKinCtrl::getAlgoName
virtual std::string getAlgoName()=0
Returns the algorithm's name.
iCub::iKin::LMCtrl::setChainConstraints
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.cpp:497
iCub::iKin::MultiRefMinJerkCtrl::qGuard
yarp::sig::Vector qGuard
Definition: iKinInv.h:785
iCub::iKin::iKinCtrl::getInTargetTol
virtual double getInTargetTol() const
Returns tolerance for in-target check.
Definition: iKinInv.h:284
iCub::iKin::LMCtrl::get_gpm
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
Definition: iKinInv.h:668
iCub::iKin::LMCtrl::inTargetFcn
virtual void inTargetFcn()
Method called whenever in target.
Definition: iKinInv.h:608
iCub::iKin::SteepCtrl::setChainConstraints
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
Definition: iKinInv.cpp:293
iCub::iKin::iKinCtrl::iter
unsigned int iter
Definition: iKinInv.h:93
iCub::iKin::SteepCtrl::test_convergence
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
iCub::iKin::LMCtrl::get_qdot
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:662
iCub::iKin::iKinChain::setAllConstraints
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:506
iCub::iKin::MultiRefMinJerkCtrl::set_gamma
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
iCub::iKin::iKinCtrl::J
yarp::sig::Matrix J
Definition: iKinInv.h:82
iCub::iKin::iKinCtrl::checkVelocity
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
iCub::iKin::LMCtrl::mu_inc
double mu_inc
Definition: iKinInv.h:598
iCub::ctrl::minJerkVelCtrl
Definition: minJerkCtrl.h:46
iCub::iKin::iKinCtrl::watchDogMaxIter
int watchDogMaxIter
Definition: iKinInv.h:99
iCub::iKin::SteepCtrl::Kp
double Kp
Definition: iKinInv.h:428
iCub::iKin::iKinCtrl::dim
unsigned int dim
Definition: iKinInv.h:92
iCub::iKin::LMCtrl_GPM::span
yarp::sig::Vector span
Definition: iKinInv.h:709
iCub::iKin::MultiRefMinJerkCtrl::set_q
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:934
iCub::iKin::VarKpSteepCtrl::Kp_dec
double Kp_dec
Definition: iKinInv.h:525
iCub::iKin::SteepCtrl::update_qdot
virtual yarp::sig::Vector update_qdot()
Definition: iKinInv.cpp:303
iCub::iKin::LMCtrl_GPM::safeAreaRatio
double safeAreaRatio
Definition: iKinInv.h:706
iCub::iKin::iKinCtrl
Definition: iKinInv.h:64
pids.h
iCub::iKin::MultiRefMinJerkCtrl::add_compensation
void add_compensation(const yarp::sig::Vector &comp)
Adds to the controller input a further compensation term.
Definition: iKinInv.cpp:956
iCub::iKin::LMCtrl::computeGPM
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
Definition: iKinInv.h:645
iCub::iKin::MultiRefMinJerkCtrl::solve
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
iCub::iKin::iKinCtrl::get_q
virtual yarp::sig::Vector get_q() const
Returns the actual joint angles values.
Definition: iKinInv.h:375
iCub::iKin::iKinCtrl::state
int state
Definition: iKinInv.h:95
iCub::iKin::MultiRefMinJerkCtrl::restart
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
Definition: iKinInv.cpp:885
iCub::iKin::iKinCtrl::getWatchDogMaxIter
virtual int getWatchDogMaxIter() const
Returns maximum number of iterations to trigger the watchDog.
Definition: iKinInv.h:309
iCub::iKin::LMCtrl_GPM::get_safeAreaRatio
double get_safeAreaRatio() const
Returns the safe area ratio (0.9 by default).
Definition: iKinInv.h:747