Go to the documentation of this file.
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);
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);
555 double _Kp0,
double _Kp_inc,
double _Kp_dec,
double _Kp_max,
double _max_perf_inc);
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);
719 double _mu_dec,
double _mu_min,
double _mu_max,
double _sv_thres=1
e-6);
727 void set_K(
const double _K) {
K=_K>0 ? _K : -_K; }
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);
940 virtual void set_q(
const yarp::sig::Vector &q0);
951 double set_execTime(
const double _execTime,
const bool warn=
false);
int get_state() const
Returns the algorithm's state.
yarp::sig::Vector qGuardMinExt
ctrl::minJerkVelCtrl * mjCtrlJoint
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
virtual yarp::sig::Vector calc_e()
Computes the error according to the current controller settings (complete pose/translational/rotation...
virtual yarp::sig::Matrix get_J() const
Returns the actual Jacobian used in computation.
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
virtual double update_mu()
virtual void inTargetFcn()
Method called whenever in target.
virtual void inTargetFcn()=0
Method called whenever in target.
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
virtual std::string getAlgoName()
Returns the algorithm's name.
double set_execTime(const double _execTime, const bool warn=false)
Sets the task execution time in seconds.
void resetInt()
Resets integral status at the current joint angles.
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose)
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
virtual double getWatchDogTol() const
Returns tolerance for watchDog check.
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
virtual void deadLockRecoveryFcn()=0
Method called whenever the watchDog is triggered.
unsigned int get_ctrlPose() const
Returns the state of Pose control settings.
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
double get_execTime() const
Returns the task execution time in seconds (1.0 by default).
#define IKINCTRL_DISABLED
void set_guardRatio(double _guardRatio)
Sets the guard ratio (in [0 1]).
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
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 double dist() const
Returns the actual distance from the target in cartesian space (euclidean norm is used).
yarp::sig::Vector compensation
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)
Executes one iteration of the control algorithm.
virtual ~iKinCtrl()
Default destructor.
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.
virtual yarp::sig::Matrix pinv(const yarp::sig::Matrix &A, const double tol=0.0)
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 printIter(const unsigned int verbose)
Dumps warning or status messages.
virtual ~LMCtrl()
Destructor.
void reset_mu()
Sets the weighting factor mu equal to the initial value.
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
virtual void setInTargetTol(double tol_x)
Sets tolerance for in-target check (5e-3 by default).
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
double get_K() const
Returns the GPM gain (1.0 by default).
yarp::sig::Vector qGuardMinCOG
virtual bool test_convergence(const double)
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
yarp::sig::Vector alpha_max
virtual void update_state()
Updates the control state.
yarp::sig::Vector qGuardMaxInt
virtual bool isInTarget()
Checks if the End-Effector is in target.
virtual void setWatchDogMaxIter(int maxIter)
Sets maximum number of iterations to trigger the watchDog (200 by default).
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...
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)=0
Executes one iteration of the control algorithm.
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
void reset(const yarp::sig::Vector &y0)
Resets the internal state and sets the output vector to the given value.
double get_gamma() const
Returns the parameter gamma which is used to blend the contribute of the task controller versus the c...
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
virtual void printIter(const unsigned int verbose=0)=0
Dumps warning or status messages.
yarp::sig::Vector qGuardMaxExt
virtual yarp::sig::Vector iterate(yarp::sig::Vector &, const unsigned int)
Executes one iteration of the control algorithm.
yarp::sig::Vector get_xdot() const
Returns the actual derivative of End-Effector Pose (6 components; xdot=J*qdot).
void resetInt()
Resets integral status at the current joint angles.
virtual void watchDog()
Handles the watchDog.
double get_Kp() const
Returns the gain.
double norm(const yarp::sig::Matrix &M, int col)
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
virtual void deadLockRecoveryFcn()
Method called whenever the watchDog is triggered.
unsigned int get_dim() const
Returns the number of Chain DOF.
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
yarp::sig::Vector qGuardMinInt
virtual void computeWeight()
virtual yarp::sig::Vector get_grad() const
Returns the actual gradient.
virtual std::string getAlgoName()
Returns the algorithm's name.
virtual void setWatchDogTol(double tol_q)
Sets tolerance for watchDog check (1e-4 by default).
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 ~SteepCtrl()
Destructor.
virtual void inTargetFcn()
Method called whenever in target.
virtual ~MultiRefMinJerkCtrl()
Destructor.
virtual yarp::sig::Vector update_qdot()
virtual void printIter(const unsigned int verbose)
Dumps warning or status messages.
virtual std::string getAlgoName()
Returns the algorithm's name.
void set_ctrlPose(unsigned int _ctrlPose)
Sets the state of Pose control settings.
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...
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, const unsigned int verbose=0)
Executes one iteration of the control algorithm.
virtual yarp::sig::Vector get_e() const
Returns the actual cartesian position error.
yarp::sig::Vector alpha_min
yarp::sig::Vector qGuardMaxCOG
virtual void inTargetFcn()
Method called whenever in target.
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
void set_K(const double _K)
Sets the GPM gain (shall be positive).
ctrl::minJerkVelCtrl * mjCtrlTask
virtual void computeGuard()
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...
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
void setPlantParameters(const yarp::os::Property ¶meters, const std::string &entryTag="dimension")
Allows user to assign values to the parameters of plant under control (for the configuration space on...
double get_mu() const
Returns the current weighting factor mu.
virtual std::string getAlgoName()=0
Returns the algorithm's name.
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
virtual double getInTargetTol() const
Returns tolerance for in-target check.
yarp::sig::Vector get_gpm() const
Returns the actual value of Gradient Projected.
virtual void inTargetFcn()
Method called whenever in target.
virtual void setChainConstraints(bool _constrained)
Enables/Disables joint angles constraints.
virtual bool test_convergence(const double tol_size)
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm...
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
void set_gamma(double _gamma)
Sets the parameter gamma which is used to blend the contribute of the task controller versus the cont...
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.
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
virtual yarp::sig::Vector update_qdot()
void add_compensation(const yarp::sig::Vector &comp)
Adds to the controller input a further compensation term.
virtual yarp::sig::Vector computeGPM()
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection...
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.
virtual yarp::sig::Vector get_q() const
Returns the actual joint angles values.
virtual void restart(const yarp::sig::Vector &q0)
Reinitializes the algorithm's internal state and resets the starting point.
virtual int getWatchDogMaxIter() const
Returns maximum number of iterations to trigger the watchDog.
double get_safeAreaRatio() const
Returns the safe area ratio (0.9 by default).