iCub-main
|
A class derived from iKinCtrl implementing the multi-referential approach. More...
#include <iKinInv.h>
Public Member Functions | |
MultiRefMinJerkCtrl (iKinChain &c, unsigned int _ctrlPose, double _Ts, bool nonIdealPlant=false) | |
Constructor. More... | |
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. More... | |
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. More... | |
virtual void | restart (const yarp::sig::Vector &q0) |
Reinitializes the algorithm's internal state and resets the starting point. More... | |
virtual std::string | getAlgoName () |
Returns the algorithm's name. More... | |
double | get_guardRatio () const |
Returns the guard ratio for the joints span (0.1 by default). More... | |
double | get_gamma () const |
Returns the parameter gamma which is used to blend the contribute of the task controller versus the contribute of the joint controller. More... | |
double | get_execTime () const |
Returns the task execution time in seconds (1.0 by default). More... | |
yarp::sig::Vector | get_qdot () const |
Returns the actual derivative of joint angles. More... | |
yarp::sig::Vector | get_xdot () const |
Returns the actual derivative of End-Effector Pose (6 components; xdot=J*qdot). More... | |
void | set_guardRatio (double _guardRatio) |
Sets the guard ratio (in [0 1]). More... | |
void | set_gamma (double _gamma) |
Sets the parameter gamma which is used to blend the contribute of the task controller versus the contribute of the joint controller. More... | |
virtual void | set_q (const yarp::sig::Vector &q0) |
Sets the joint angles values. More... | |
double | set_execTime (const double _execTime, const bool warn=false) |
Sets the task execution time in seconds. More... | |
void | add_compensation (const yarp::sig::Vector &comp) |
Adds to the controller input a further compensation term. More... | |
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 only). More... | |
virtual | ~MultiRefMinJerkCtrl () |
Destructor. More... | |
Public Member Functions inherited from iCub::iKin::iKinCtrl | |
iKinCtrl (iKinChain &c, unsigned int _ctrlPose) | |
Constructor. More... | |
virtual void | setChainConstraints (bool _constrained) |
Enables/Disables joint angles constraints. More... | |
void | switchWatchDog (bool sw) |
Switch on/off the watchDog mechanism to trigger deadLocks. More... | |
virtual void | setInTargetTol (double tol_x) |
Sets tolerance for in-target check (5e-3 by default). More... | |
virtual double | getInTargetTol () const |
Returns tolerance for in-target check. More... | |
virtual void | setWatchDogTol (double tol_q) |
Sets tolerance for watchDog check (1e-4 by default). More... | |
virtual double | getWatchDogTol () const |
Returns tolerance for watchDog check. More... | |
virtual void | setWatchDogMaxIter (int maxIter) |
Sets maximum number of iterations to trigger the watchDog (200 by default). More... | |
virtual int | getWatchDogMaxIter () const |
Returns maximum number of iterations to trigger the watchDog. More... | |
virtual bool | isInTarget () |
Checks if the End-Effector is in target. More... | |
int | get_state () const |
Returns the algorithm's state. More... | |
void | set_ctrlPose (unsigned int _ctrlPose) |
Sets the state of Pose control settings. More... | |
unsigned int | get_ctrlPose () const |
Returns the state of Pose control settings. More... | |
unsigned int | get_dim () const |
Returns the number of Chain DOF. More... | |
unsigned int | get_iter () const |
Returns the number of performed iterations. More... | |
virtual yarp::sig::Vector | get_x () const |
Returns the actual cartesian position of the End-Effector. More... | |
virtual yarp::sig::Vector | get_e () const |
Returns the actual cartesian position error. More... | |
virtual yarp::sig::Vector | get_q () const |
Returns the actual joint angles values. More... | |
virtual yarp::sig::Vector | get_grad () const |
Returns the actual gradient. More... | |
virtual yarp::sig::Matrix | get_J () const |
Returns the actual Jacobian used in computation. More... | |
virtual double | dist () const |
Returns the actual distance from the target in cartesian space (euclidean norm is used). More... | |
virtual | ~iKinCtrl () |
Default destructor. More... | |
Protected Member Functions | |
virtual void | computeGuard () |
virtual void | computeWeight () |
virtual yarp::sig::Vector | iterate (yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose) |
virtual void | inTargetFcn () |
Method called whenever in target. More... | |
virtual void | deadLockRecoveryFcn () |
Method called whenever the watchDog is triggered. More... | |
virtual void | printIter (const unsigned int verbose) |
Dumps warning or status messages. More... | |
virtual bool | test_convergence (const double) |
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm or the simplex size or whatever) to a certain tolerance. More... | |
virtual yarp::sig::Vector | iterate (yarp::sig::Vector &, const unsigned int) |
Executes one iteration of the control algorithm. More... | |
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. More... | |
Protected Member Functions inherited from iCub::iKin::iKinCtrl | |
virtual yarp::sig::Vector | calc_e () |
Computes the error according to the current controller settings (complete pose/translational/rotational part). More... | |
virtual void | update_state () |
Updates the control state. More... | |
virtual void | watchDog () |
Handles the watchDog. More... | |
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. More... | |
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 word of verbose integer. More... | |
Protected Attributes | |
ctrl::minJerkVelCtrl * | mjCtrlJoint |
ctrl::minJerkVelCtrl * | mjCtrlTask |
ctrl::Integrator * | I |
yarp::sig::Vector | q_set |
yarp::sig::Vector | qdot |
yarp::sig::Vector | xdot |
yarp::sig::Matrix | W |
yarp::sig::Matrix | Eye6 |
double | Ts |
double | execTime |
double | gamma |
double | guardRatio |
yarp::sig::Vector | qGuard |
yarp::sig::Vector | qGuardMinInt |
yarp::sig::Vector | qGuardMinExt |
yarp::sig::Vector | qGuardMinCOG |
yarp::sig::Vector | qGuardMaxInt |
yarp::sig::Vector | qGuardMaxExt |
yarp::sig::Vector | qGuardMaxCOG |
yarp::sig::Vector | compensation |
Protected Attributes inherited from iCub::iKin::iKinCtrl | |
iKinChain & | chain |
unsigned int | ctrlPose |
yarp::sig::Vector | x_set |
yarp::sig::Vector | x |
yarp::sig::Vector | e |
yarp::sig::Vector | q |
yarp::sig::Matrix | J |
yarp::sig::Matrix | Jt |
yarp::sig::Matrix | pinvJ |
yarp::sig::Vector | grad |
yarp::sig::Vector | q_old |
double | inTargetTol |
double | watchDogTol |
unsigned int | dim |
unsigned int | iter |
int | state |
bool | watchDogOn |
int | watchDogCnt |
int | watchDogMaxIter |
A class derived from iKinCtrl implementing the multi-referential approach.
MultiRefMinJerkCtrl::MultiRefMinJerkCtrl | ( | iKinChain & | c, |
unsigned int | _ctrlPose, | ||
double | _Ts, | ||
bool | nonIdealPlant = false |
||
) |
Constructor.
c | is the Chain object on which the control operates. Do not change Chain DOF from this point onwards!! |
_ctrlPose | one of the following: IKINCTRL_POSE_FULL => complete pose control. IKINCTRL_POSE_XYZ => translational part of pose controlled. IKINCTRL_POSE_ANG => rotational part of pose controlled. |
_Ts | is the nominal controller sample time. |
nonIdealPlant | if true allocate a dedicated min-jerk controller for the configuration space capable of compansating plants that differ from pure integrators. |
Definition at line 725 of file iKinInv.cpp.
|
virtual |
Destructor.
Definition at line 984 of file iKinInv.cpp.
void MultiRefMinJerkCtrl::add_compensation | ( | const yarp::sig::Vector & | comp | ) |
Adds to the controller input a further compensation term.
comp | the compensation term. |
Definition at line 956 of file iKinInv.cpp.
|
protectedvirtual |
Definition at line 779 of file iKinInv.cpp.
|
protectedvirtual |
Definition at line 797 of file iKinInv.cpp.
|
inlineprotectedvirtual |
Method called whenever the watchDog is triggered.
Put here the code to recover from deadLock. Shall be implemented.
Implements iCub::iKin::iKinCtrl.
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inlinevirtual |
Returns the algorithm's name.
Implements iCub::iKin::iKinCtrl.
|
inlineprotectedvirtual |
|
inlineprotectedvirtual |
Executes one iteration of the control algorithm.
xd | is the End-Effector target Pose to be tracked. |
verbose | is a integer whose 32 bits are intended as follows. The lowest word (16 bits) progressively enables different levels of warning messages or status dump: the larger this value the more detailed is the output (0x####0000=>off by default). The highest word indicates how many successive calls to the dump shall be skipped in order to reduce the amount of information on the screen (ex: 0x0000####=>print all iterations, 0x0001####=>print one iteration and skip the next one, 0x0002####=> print one iteration and skip the next two). |
Implements iCub::iKin::iKinCtrl.
|
virtual |
Executes one iteration of the control algorithm.
xd | is the End-Effector target Pose to be tracked. |
qd | is the target joint angles (it shall satisfy the forward kinematic function xd=f(qd)). |
verbose | is a integer whose 32 bits are intended as follows. The lowest word (16 bits) progressively enables different levels of warning messages or status dump: the larger this value the more detailed is the output (0x####0000=>off by default). The highest word indicates how many successive calls to the dump shall be skipped in order to reduce the amount of information on the screen (ex: 0x0000####=>print all iterations, 0x0001####=>print one iteration and skip the next one, 0x0002####=> print one iteration and skip the next two). |
|
virtual |
Executes one iteration of the control algorithm.
xd | is the End-Effector target Pose to be tracked. |
qd | is the target joint angles (it shall satisfy the forward kinematic function xd=f(qd)). |
xdot_set | is the Task Space reference velocity; the vector size is 7 (due to the axis-angle notation) and units are [rad/s]. |
verbose | is a integer whose 32 bits are intended as follows. The lowest word (16 bits) progressively enables different levels of warning messages or status dump: the larger this value the more detailed is the output (0x####0000=>off by default). The highest word indicates how many successive calls to the dump shall be skipped in order to reduce the amount of information on the screen (ex: 0x0000####=>print all iterations, 0x0001####=>print one iteration and skip the next one, 0x0002####=> print one iteration and skip the next two). |
|
protectedvirtual |
|
protectedvirtual |
Dumps warning or status messages.
verbose | is a integer whose 32 bits are intended as follows. The lowest word (16 bits) progressively enables different levels of warning messages or status dump: the larger this value the more detailed is the output (0x####0000=>off by default). The highest word indicates how many successive calls to the dump shall be skipped in order to reduce the amount of information on the screen (ex: 0x0000####=>print all iterations, 0x0001####=>print one iteration and skip the next one, 0x0002####=> print one iteration and skip the next two). |
Implements iCub::iKin::iKinCtrl.
Definition at line 898 of file iKinInv.cpp.
|
virtual |
Reinitializes the algorithm's internal state and resets the starting point.
q0 | is the new starting point. |
Reimplemented from iCub::iKin::iKinCtrl.
Definition at line 885 of file iKinInv.cpp.
double MultiRefMinJerkCtrl::set_execTime | ( | const double | _execTime, |
const bool | warn = false |
||
) |
Sets the task execution time in seconds.
_execTime. | |
warn | enable/disable warning message for thresholding (disabled by default). |
Definition at line 942 of file iKinInv.cpp.
|
inline |
void MultiRefMinJerkCtrl::set_guardRatio | ( | double | _guardRatio | ) |
Sets the guard ratio (in [0 1]).
_guardRatio. |
Definition at line 770 of file iKinInv.cpp.
|
virtual |
Sets the joint angles values.
q0 | is the joint angles vector. |
Reimplemented from iCub::iKin::iKinCtrl.
Definition at line 934 of file iKinInv.cpp.
void MultiRefMinJerkCtrl::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 only).
In case the controlled plant is not a pure integrator, then it can be modelled as the following transfer function: (Kp/s)*((1+Tz*s)/(1+2*Zeta*Tw*s+(Tw*s)^2))
parameters | contains the set of plant parameters for each dimension in form of a Property object. |
Available parameters are:
dimension_# < list>: example (dimension_2 ((Kp 1.0) (Tw 0.1) ...)), specifies the Kp, Tz, Tw and Zeta parameters for a given dimension of the plant ("dimension_2" in the example). Dimensions are 0-based numbers.
entryTag | specifies an entry tag different from "dimension". |
Definition at line 965 of file iKinInv.cpp.
|
inlineprotectedvirtual |
Iterates the control algorithm trying to converge on the target.
xd | is the End-Effector target Pose to be tracked. |
tol_size | exits if test_convergence(tol_size) is true (tol_size<0 disables this check, default). |
max_iter | exits if iter>=max_iter (max_iter<0 disables this check, default). |
verbose | is a integer whose 32 bits are intended as follows. The lowest word (16 bits) progressively enables different levels of warning messages or status dump: the larger this value the more detailed is the output (0x####0000=>off by default). The highest word indicates how many successive calls to the dump shall be skipped in order to reduce the amount of information on the screen (ex: 0x0000####=>print all iterations, 0x0001####=>print one iteration and skip the next one, 0x0002####=> print one iteration and skip the next two). |
exit_code | stores the exit code (NULL by default). Test for one of this: IKINCTRL_RET_TOLX IKINCTRL_RET_TOLSIZE IKINCTRL_RET_TOLQ IKINCTRL_RET_MAXITER IKINCTRL_RET_EXHALT |
exhalt | checks for an external request to exit (NULL by default). |
Reimplemented from iCub::iKin::iKinCtrl.
|
inlineprotectedvirtual |
Tests convergence by comparing the size of the algorithm internal structure (may be the gradient norm or the simplex size or whatever) to a certain tolerance.
tol_size | is tolerance to compare to. Shall be implemented. |
Implements iCub::iKin::iKinCtrl.
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |