iCub-main
|
A class derived from iKinCtrl implementing the Levenberg-Marquardt algorithm: More...
#include <iKinInv.h>
Public Member Functions | |
LMCtrl (iKinChain &c, unsigned int _ctrlPose, double _Ts, double _mu0, double _mu_inc, double _mu_dec, double _mu_min, double _mu_max, double _sv_thres=1e-6) | |
Constructor. | |
virtual yarp::sig::Vector | computeGPM () |
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection Method, i.e. | |
virtual void | setChainConstraints (bool _constrained) |
Enables/Disables joint angles constraints. | |
virtual yarp::sig::Vector | iterate (yarp::sig::Vector &xd, const unsigned int verbose=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 or the simplex size or whatever) to a certain tolerance. | |
virtual std::string | getAlgoName () |
Returns the algorithm's name. | |
void | resetInt () |
Resets integral status at the current joint angles. | |
yarp::sig::Vector | get_qdot () const |
Returns the actual derivative of joint angles. | |
yarp::sig::Vector | get_gpm () const |
Returns the actual value of Gradient Projected. | |
double | get_mu () const |
Returns the current weighting factor mu. | |
void | reset_mu () |
Sets the weighting factor mu equal to the initial value. | |
virtual | ~LMCtrl () |
Destructor. | |
Public Member Functions inherited from iCub::iKin::iKinCtrl | |
iKinCtrl (iKinChain &c, unsigned int _ctrlPose) | |
Constructor. | |
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. | |
void | switchWatchDog (bool sw) |
Switch on/off the watchDog mechanism to trigger deadLocks. | |
virtual void | setInTargetTol (double tol_x) |
Sets tolerance for in-target check (5e-3 by default). | |
virtual double | getInTargetTol () const |
Returns tolerance for in-target check. | |
virtual void | setWatchDogTol (double tol_q) |
Sets tolerance for watchDog check (1e-4 by default). | |
virtual double | getWatchDogTol () const |
Returns tolerance for watchDog check. | |
virtual void | setWatchDogMaxIter (int maxIter) |
Sets maximum number of iterations to trigger the watchDog (200 by default). | |
virtual int | getWatchDogMaxIter () const |
Returns maximum number of iterations to trigger the watchDog. | |
virtual bool | isInTarget () |
Checks if the End-Effector is in target. | |
int | get_state () const |
Returns the algorithm's state. | |
void | set_ctrlPose (unsigned int _ctrlPose) |
Sets the state of Pose control settings. | |
unsigned int | get_ctrlPose () const |
Returns the state of Pose control settings. | |
unsigned int | get_dim () const |
Returns the number of Chain DOF. | |
unsigned int | get_iter () const |
Returns the number of performed iterations. | |
virtual yarp::sig::Vector | get_x () const |
Returns the actual cartesian position of the End-Effector. | |
virtual yarp::sig::Vector | get_e () const |
Returns the actual cartesian position error. | |
virtual void | set_q (const yarp::sig::Vector &q0) |
Sets the joint angles values. | |
virtual yarp::sig::Vector | get_q () const |
Returns the actual joint angles values. | |
virtual yarp::sig::Vector | get_grad () const |
Returns the actual gradient. | |
virtual yarp::sig::Matrix | get_J () const |
Returns the actual Jacobian used in computation. | |
virtual double | dist () const |
Returns the actual distance from the target in cartesian space (euclidean norm is used). | |
virtual | ~iKinCtrl () |
Default destructor. | |
Protected Member Functions | |
virtual double | update_mu () |
virtual void | inTargetFcn () |
Method called whenever in target. | |
virtual void | deadLockRecoveryFcn () |
Method called whenever the watchDog is triggered. | |
virtual void | printIter (const unsigned int verbose) |
Dumps warning or status messages. | |
virtual yarp::sig::Matrix | pinv (const yarp::sig::Matrix &A, const double tol=0.0) |
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). | |
virtual void | update_state () |
Updates the control state. | |
virtual void | watchDog () |
Handles the watchDog. | |
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. | |
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. | |
Protected Attributes | |
double | Ts |
ctrl::Integrator * | I |
bool | constrained |
yarp::sig::Vector | qdot |
yarp::sig::Vector | gpm |
yarp::sig::Matrix | pinvLM |
double | mu |
double | mu0 |
double | mu_inc |
double | mu_dec |
double | mu_min |
double | mu_max |
double | dist_old |
double | svMin |
double | svThres |
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 Levenberg-Marquardt algorithm:
qdot=-pinv(Jt*J+mu*I)*grad
r(k)=dist(k)/dist(k-1) r(k)<1 => mu(k)=mu(k-1)*mu_dec; r(k)>1 => mu(k)=mu(k-1)*mu_inc;
H=Jt*J is the approximation of Hessian matrix
LMCtrl::LMCtrl | ( | iKinChain & | c, |
unsigned int | _ctrlPose, | ||
double | _Ts, | ||
double | _mu0, | ||
double | _mu_inc, | ||
double | _mu_dec, | ||
double | _mu_min, | ||
double | _mu_max, | ||
double | _sv_thres = 1e-6 |
||
) |
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 controller sample time. |
_mu0 | is the initial value for weighting factor mu. |
_mu_inc | is the increasing factor. |
_mu_dec | is the drecreasing factor. |
_mu_min | is the minimum value for mu. |
_mu_max | is the maximum value for mu. |
_sv_thres | is the minimum singular value under which the mu is constantly kept equal to _mu_max. |
Definition at line 466 of file iKinInv.cpp.
|
virtual |
Destructor.
Definition at line 669 of file iKinInv.cpp.
|
inlinevirtual |
Returns the further contribution to the qdot=pinvJ*xdot equation according to the Gradient Projection Method, i.e.
qdot=pinvJ*xdot+(I-pinvJ*J)*w
Reimplemented in iCub::iKin::LMCtrl_GPM.
|
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 |
|
inlinevirtual |
Returns the algorithm's name.
Implements iCub::iKin::iKinCtrl.
|
inlineprotectedvirtual |
|
virtual |
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.
Definition at line 570 of file iKinInv.cpp.
|
protectedvirtual |
Definition at line 507 of file iKinInv.cpp.
|
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 638 of file iKinInv.cpp.
void LMCtrl::reset_mu | ( | ) |
Sets the weighting factor mu equal to the initial value.
Definition at line 535 of file iKinInv.cpp.
|
inline |
|
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 628 of file iKinInv.cpp.
|
virtual |
Enables/Disables joint angles constraints.
_constrained | if true then constraints are applied. |
Reimplemented from iCub::iKin::iKinCtrl.
Definition at line 497 of file iKinInv.cpp.
|
inlinevirtual |
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.
|
protectedvirtual |
Definition at line 543 of file iKinInv.cpp.
|
protected |