28 #include <yarp/os/Property.h> 33 #define IKINCTRL_STATE_RUNNING 0 34 #define IKINCTRL_STATE_INTARGET 1 35 #define IKINCTRL_STATE_DEADLOCK 2 37 #define IKINCTRL_POSE_FULL 0 38 #define IKINCTRL_POSE_XYZ 1 39 #define IKINCTRL_POSE_ANG 2 41 #define IKINCTRL_STEEP_JT 0 42 #define IKINCTRL_STEEP_PINV 1 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 50 #define IKINCTRL_DISABLED -1 107 virtual yarp::sig::Vector
calc_e();
126 virtual yarp::sig::Vector
checkVelocity(
const yarp::sig::Vector &_qdot,
double _Ts);
158 virtual void printIter(
const unsigned int verbose=0) = 0;
204 virtual yarp::sig::Vector
iterate(yarp::sig::Vector &xd,
const unsigned int verbose=0) = 0;
241 int *exit_code=NULL,
bool *exhalt=NULL);
257 virtual void restart(
const yarp::sig::Vector &q0);
357 virtual yarp::sig::Vector
get_x()
const {
return x; }
363 virtual yarp::sig::Vector
get_e()
const {
return e; }
369 virtual void set_q(
const yarp::sig::Vector &q0);
375 virtual yarp::sig::Vector
get_q()
const {
return q; }
387 virtual yarp::sig::Matrix
get_J()
const {
return J; }
434 virtual void printIter(
const unsigned int verbose);
435 virtual yarp::sig::Vector update_qdot();
453 double _Ts,
double _Kp);
464 virtual yarp::sig::Vector
computeGPM() {
return yarp::sig::Vector(
dim,0.0); }
467 virtual yarp::sig::Vector
iterate(yarp::sig::Vector &xd,
const unsigned int verbose=0);
468 virtual void restart(
const yarp::sig::Vector &q0);
481 yarp::sig::Vector
get_qdot()
const {
return qdot; }
487 yarp::sig::Vector
get_gpm()
const {
return gpm; }
533 virtual yarp::sig::Vector update_qdot();
555 double _Kp0,
double _Kp_inc,
double _Kp_dec,
double _Kp_max,
double _max_perf_inc);
558 virtual std::string
getAlgoName() {
return "variable-gain-steepest-descent"; }
607 virtual double update_mu();
610 virtual void printIter(
const unsigned int verbose);
612 virtual yarp::sig::Matrix pinv(
const yarp::sig::Matrix &
A,
const double tol=0.0);
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=1
e-6);
645 virtual yarp::sig::Vector
computeGPM() {
return yarp::sig::Vector(
dim,0.0); }
648 virtual yarp::sig::Vector
iterate(yarp::sig::Vector &xd,
const unsigned int verbose=0);
649 virtual void restart(
const yarp::sig::Vector &q0);
651 virtual std::string
getAlgoName() {
return "levenberg-marquardt"; }
662 yarp::sig::Vector
get_qdot()
const {
return qdot; }
668 yarp::sig::Vector
get_gpm()
const {
return gpm; }
719 double _mu_dec,
double _mu_min,
double _mu_max,
double _sv_thres=1
e-6);
721 virtual yarp::sig::Vector computeGPM();
727 void set_K(
const double _K) { K=_K>0 ? _K : -_K; }
741 void set_safeAreaRatio(
const double _safeAreaRatio);
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);
798 virtual void printIter(
const unsigned int verbose);
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); }
848 virtual yarp::sig::Vector
iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd,
849 const unsigned int verbose=0);
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);
882 virtual void restart(
const yarp::sig::Vector &q0);
884 virtual std::string
getAlgoName() {
return "multi-referential-minimum-jerk-controllers"; }
913 yarp::sig::Vector
get_qdot()
const {
return qdot; }
920 yarp::sig::Vector
get_xdot()
const {
return xdot; }
926 void set_guardRatio(
double _guardRatio);
940 virtual void set_q(
const yarp::sig::Vector &q0);
951 double set_execTime(
const double _execTime,
const bool warn=
false);
959 void add_compensation(
const yarp::sig::Vector &comp);
979 void setPlantParameters(
const yarp::os::Property ¶meters,
980 const std::string &entryTag=
"dimension");
virtual std::string getAlgoName()
Returns the algorithm's name.
#define IKINCTRL_DISABLED
void set_K(const double _K)
Sets the GPM gain (shall be positive).
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
A class derived from iKinCtrl implementing two standard algorithms based on steepest descent qdot=-Kp...
void set_gamma(double _gamma)
Sets the parameter gamma which is used to blend the contribute of the task controller versus the cont...
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
virtual std::string getAlgoName()
Returns the algorithm's name.
double get_guardRatio() const
Returns the guard ratio for the joints span (0.1 by default).
virtual void inTargetFcn()
Method called whenever in target.
A class derived from iKinCtrl implementing the Levenberg-Marquardt algorithm:
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
double get_gamma() const
Returns the parameter gamma which is used to blend the contribute of the task controller versus the c...
Abstract class for minimum-jerk controllers with velocity commands.
yarp::sig::Vector get_xdot() const
Returns the actual derivative of End-Effector Pose (6 components; xdot=J*qdot).
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
virtual void inTargetFcn()
Method called whenever in target.
void resetInt()
Resets integral status at the current joint angles.
virtual void update_state()
Updates the control state.
int get_state() const
Returns the algorithm's state.
virtual double dist() const
Returns the actual distance from the target in cartesian space (euclidean norm is used)...
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.
A Base class for defining a Serial Link Chain.
virtual bool isInTarget()
Checks if the End-Effector is in target.
virtual double getInTargetTol() const
Returns tolerance for in-target check.
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
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.
yarp::sig::Vector qGuardMinInt
virtual bool test_convergence(const double tol_size)
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
void resetInt()
Resets integral status at the current joint angles.
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
virtual std::string getAlgoName()=0
Returns the algorithm's name.
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
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.
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.
unsigned int get_iter() const
Returns the number of performed iterations.
void switchWatchDog(bool sw)
Switch on/off the watchDog mechanism to trigger deadLocks.
virtual std::string getAlgoName()
Returns the algorithm's name.
A class derived from LMCtrl implementing the Gradient Projection Method according to the paper availa...
yarp::sig::Vector alpha_min
virtual yarp::sig::Vector get_grad() const
Returns the actual gradient.
virtual yarp::sig::Vector calc_e()
Computes the error according to the current controller settings (complete pose/translational/rotation...
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.
ctrl::minJerkVelCtrl * mjCtrlTask
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
double get_safeAreaRatio() const
Returns the safe area ratio (0.9 by default).
double get_execTime() const
Returns the task execution time in seconds (1.0 by default).
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Abstract class for inverting chain's kinematics.
virtual yarp::sig::Vector get_e() const
Returns the actual cartesian position error.
A class for defining a saturated integrator based on Tustin formula: .
virtual void deadLockRecoveryFcn()=0
Method called whenever the watchDog is triggered.
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
virtual bool test_convergence(const double)
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
virtual void inTargetFcn()
Method called whenever in target.
yarp::sig::Vector qGuardMaxInt
yarp::sig::Vector alpha_max
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).
A class derived from iKinCtrl implementing the multi-referential approach (pdf).
ctrl::minJerkVelCtrl * mjCtrlJoint
virtual yarp::sig::Matrix get_J() const
Returns the actual Jacobian used in computation.
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's internal state and resets the starting point.
virtual bool test_convergence(const double tol_size)
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
virtual yarp::sig::Vector iterate(yarp::sig::Vector &, const unsigned int)
Executes one iteration of the control algorithm.
A class derived from SteepCtrl implementing the variable gain algorithm.
virtual void watchDog()
Handles the watchDog.
virtual void setWatchDogMaxIter(int maxIter)
Sets maximum number of iterations to trigger the watchDog (200 by default).
virtual void inTargetFcn()
Method called whenever in target.
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.
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
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...
unsigned int get_ctrlPose() const
Returns the state of Pose control settings.
virtual ~iKinCtrl()
Default destructor.
virtual void inTargetFcn()=0
Method called whenever in target.
double get_K() const
Returns the GPM gain (1.0 by default).
virtual int getWatchDogMaxIter() const
Returns maximum number of iterations to trigger the watchDog.
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
double get_mu() const
Returns the current weighting factor mu.
virtual yarp::sig::Vector get_q() const
Returns the actual joint angles values.
virtual void setWatchDogTol(double tol_q)
Sets tolerance for watchDog check (1e-4 by default).
virtual std::string getAlgoName()
Returns the algorithm's name.
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
yarp::sig::Vector compensation