Public Member Functions | Protected Member Functions | Protected Attributes

iCub::iKin::MultiRefMinJerkCtrl Class Reference
[iKinInv]

A class derived from iKinCtrl implementing the multi-referential approach (pdf). More...

#include <iKinInv.h>

Inheritance diagram for iCub::iKin::MultiRefMinJerkCtrl:
iCub::iKin::iKinCtrl

Public Member Functions

 MultiRefMinJerkCtrl (iKinChain &c, unsigned int _ctrlPose, double _Ts, bool nonIdealPlant=false)
 Constructor.
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.
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.
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.
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.
virtual yarp::sig::Vector iterate (yarp::sig::Vector &, const unsigned int)
 Executes one iteration of the control algorithm.
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.
double get_guardRatio () const
 Returns the guard ratio for the joints span (0.1 by default).
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.
double get_execTime () const
 Returns the task execution time in seconds (1.0 by default).
yarp::sig::Vector get_qdot () const
 Returns the actual derivative of joint angles.
yarp::sig::Vector get_xdot () const
 Returns the actual derivative of End-Effector Pose (6 components; xdot=J*qdot).
void set_guardRatio (double _guardRatio)
 Sets the guard ratio (in [0 1]).
void set_gamma (double _gamma)
 Sets the parameter gamma which is used to blend the contribute of the task contorller versus the contribute of the joint controller.
virtual void set_q (const yarp::sig::Vector &q0)
 Sets the joint angles values.
double set_execTime (const double _execTime, const bool warn=false)
 Sets the task execution time in seconds.
void add_compensation (const yarp::sig::Vector &comp)
 Adds to the controller input a further compensation term.
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 only).
virtual ~MultiRefMinJerkCtrl ()
 Destructor.
virtual void setChainConstraints (bool _constrained)
 Enables/Disables joint angles constraints.
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 (1e-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 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).

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 yarp::sig::Vector calc_e ()
 Computes the error according to the current controller settings (complete pose/translational/rotational part).
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 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

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
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

Detailed Description

A class derived from iKinCtrl implementing the multi-referential approach (pdf).

Note:
Minimum-Jerk controllers in Task Space and Joint Space

Definition at line 895 of file iKinInv.h.

Constructor & Destructor Documentation

MultiRefMinJerkCtrl::MultiRefMinJerkCtrl ( iKinChain &  c,
unsigned int  _ctrlPose,
double  _Ts,
bool  nonIdealPlant = false 
)

Constructor.

Parameters:
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 1117 of file iKinInv.cpp.

                                                             : 
                                         iKinCtrl(c,_ctrlPose), Ts(_Ts)
{
    q_set.resize(dim,0.0);
    qdot.resize(dim,0.0);
    xdot.resize(6,0.0);
    compensation.resize(dim,0.0);

    W=eye(dim,dim);
    Eye6=eye(6,6);

    execTime=1.0;

    Matrix lim(dim,2);
    for (unsigned int i=0; i<dim; i++)
    {
        lim(i,0)=chain(i).getMin();
        lim(i,1)=chain(i).getMax();
    }

    if (nonIdealPlant)
        mjCtrlJoint=new minJerkVelCtrlForNonIdealPlant(Ts,dim);
    else
        mjCtrlJoint=new minJerkVelCtrlForIdealPlant(Ts,dim);

    mjCtrlTask =new minJerkVelCtrlForIdealPlant(Ts,x.length());
    I=new Integrator(Ts,q,lim);

    gamma=0.05;
    guardRatio=0.1;

    qGuard.resize(dim);
    qGuardMinInt.resize(dim);
    qGuardMinExt.resize(dim);
    qGuardMaxInt.resize(dim);
    qGuardMaxExt.resize(dim);
    qGuardMinCOG.resize(dim);
    qGuardMaxCOG.resize(dim);

    computeGuard();
}

Member Function Documentation

void iCub::iKin::MultiRefMinJerkCtrl::add_compensation ( const yarp::sig::Vector &  comp  ) 

Adds to the controller input a further compensation term.

Parameters:
comp the compensation term.
Note:
The compensation term holds for one iteration step, then it is automatically set to zero for safety reasons.
Vector MultiRefMinJerkCtrl::calc_e (  )  [protected, virtual]

Computes the error according to the current controller settings (complete pose/translational/rotational part).

Note that x must be previously set.

Returns:
the error.

Reimplemented from iCub::iKin::iKinCtrl.

Definition at line 1206 of file iKinInv.cpp.

{
    // x must be previously set
    Vector __e=x_set-x;

    if (__e.length()>6)
    {
       e[0]=__e[0];
       e[1]=__e[1];
       e[2]=__e[2];
       e[3]=__e[3]*__e[6];
       e[4]=__e[4]*__e[6];
       e[5]=__e[5]*__e[6];
    }
    else
        e=__e;

    if (ctrlPose==IKINCTRL_POSE_XYZ)
        e[3]=e[4]=e[5]=0.0;
    else if (ctrlPose==IKINCTRL_POSE_ANG)
        e[0]=e[1]=e[2]=0.0;

    return e;
}

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::checkVelocity ( const yarp::sig::Vector &  _qdot,
double  _Ts 
) [protected, virtual, inherited]

Checks each joint velocity and sets it to zero if it steers the joint out of range.

Parameters:
_qdot is the joint velocities vector to be checked.
_Ts is the joint velocities sample time.
Returns:
the new velocity.
virtual void iCub::iKin::MultiRefMinJerkCtrl::deadLockRecoveryFcn (  )  [inline, protected, virtual]

Method called whenever the watchDog is triggered.

Put here the code to recover from deadLock. Shall be implemented.

Implements iCub::iKin::iKinCtrl.

Definition at line 934 of file iKinInv.h.

{ }

virtual double iCub::iKin::iKinCtrl::dist (  )  const [inline, virtual, inherited]

Returns the actual distance from the target in cartesian space (euclidean norm is used).

Returns:
actual distance from the target.

Definition at line 410 of file iKinInv.h.

References iCub::ctrl::norm().

Referenced by iCub::iKin::iKinCtrl::isInTarget(), printIter(), iCub::iKin::LMCtrl::printIter(), and iCub::iKin::SteepCtrl::printIter().

{ return yarp::math::norm(e); }

unsigned int iCub::iKin::iKinCtrl::get_ctrlPose (  )  const [inline, inherited]

Returns the state of Pose control settings.

Returns:
Pose control settings.

Definition at line 355 of file iKinInv.h.

{ return ctrlPose; }

unsigned int iCub::iKin::iKinCtrl::get_dim (  )  const [inline, inherited]

Returns the number of Chain DOF.

Returns:
number of Chain DOF.

Definition at line 361 of file iKinInv.h.

{ return dim; }

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::get_e (  )  const [inline, virtual, inherited]

Returns the actual cartesian position error.

Returns:
actual cartesian position error.

Definition at line 379 of file iKinInv.h.

{ return e; }

double iCub::iKin::MultiRefMinJerkCtrl::get_execTime (  )  const [inline]

Returns the task execution time in seconds (1.0 by default).

Returns:
task execution time.

Definition at line 1044 of file iKinInv.h.

{ return execTime; }

double iCub::iKin::MultiRefMinJerkCtrl::get_gamma (  )  const [inline]

Returns the parameter gamma which is used to blend the contribute of the task controller versus the contribute of the joint controller.

Returns:
gamma.

Definition at line 1038 of file iKinInv.h.

{ return gamma; }

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::get_grad (  )  const [inline, virtual, inherited]

Returns the actual gradient.

Returns:
actual gradient.

Definition at line 397 of file iKinInv.h.

{ return grad; }

double iCub::iKin::MultiRefMinJerkCtrl::get_guardRatio (  )  const [inline]

Returns the guard ratio for the joints span (0.1 by default).

Note:
The weights W_theta^-1 are non-zero only within the range [ q_min+0.5*guardRatio*D, q_max-0.5*guardRatio*D ] for each join, where D=q_max-q_min.
Returns:
guard ratio.

Definition at line 1030 of file iKinInv.h.

{ return guardRatio; }

unsigned int iCub::iKin::iKinCtrl::get_iter (  )  const [inline, inherited]

Returns the number of performed iterations.

Returns:
number of performed iterations.

Definition at line 367 of file iKinInv.h.

{ return iter; }

virtual yarp::sig::Matrix iCub::iKin::iKinCtrl::get_J (  )  const [inline, virtual, inherited]

Returns the actual Jacobian used in computation.

Returns:
actual Jacobian.

Definition at line 403 of file iKinInv.h.

{ return J; }

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::get_q (  )  const [inline, virtual, inherited]

Returns the actual joint angles values.

Returns:
actual joint angles values.

Definition at line 391 of file iKinInv.h.

{ return q; }

yarp::sig::Vector iCub::iKin::MultiRefMinJerkCtrl::get_qdot (  )  const [inline]

Returns the actual derivative of joint angles.

Returns:
the actual derivative of joint angles.

Definition at line 1050 of file iKinInv.h.

{ return qdot; }

int iCub::iKin::iKinCtrl::get_state (  )  const [inline, inherited]

Returns the algorithm's state.

Returns:
algorithm's state: IKINCTRL_STATE_RUNNING IKINCTRL_STATE_INTARGET IKINCTRL_STATE_DEADLOCK

Definition at line 340 of file iKinInv.h.

{ return state; }

virtual yarp::sig::Vector iCub::iKin::iKinCtrl::get_x (  )  const [inline, virtual, inherited]

Returns the actual cartesian position of the End-Effector.

Returns:
actual cartesian position of the End-Effector.

Definition at line 373 of file iKinInv.h.

{ return x; }

yarp::sig::Vector iCub::iKin::MultiRefMinJerkCtrl::get_xdot (  )  const [inline]

Returns the actual derivative of End-Effector Pose (6 components; xdot=J*qdot).

Returns:
the actual derivative of End-Effector Pose.

Definition at line 1057 of file iKinInv.h.

{ return xdot; }

virtual std::string iCub::iKin::MultiRefMinJerkCtrl::getAlgoName (  )  [inline, virtual]

Returns the algorithm's name.

Returns:
algorithm name as string. Shall be implemented.

Implements iCub::iKin::iKinCtrl.

Definition at line 1015 of file iKinInv.h.

{ return "multi-referential-minimum-jerk-controllers"; }

virtual double iCub::iKin::iKinCtrl::getInTargetTol (  )  const [inline, virtual, inherited]

Returns tolerance for in-target check.

Returns:
tolerance

Definition at line 300 of file iKinInv.h.

{ return inTargetTol; }

virtual int iCub::iKin::iKinCtrl::getWatchDogMaxIter (  )  const [inline, virtual, inherited]

Returns maximum number of iterations to trigger the watchDog.

Returns:
iterations limit.

Definition at line 325 of file iKinInv.h.

{ return watchDogMaxIter; }

virtual double iCub::iKin::iKinCtrl::getWatchDogTol (  )  const [inline, virtual, inherited]

Returns tolerance for watchDog check.

Returns:
tolerance

Definition at line 312 of file iKinInv.h.

{ return watchDogTol; }

virtual void iCub::iKin::MultiRefMinJerkCtrl::inTargetFcn (  )  [inline, protected, virtual]

Method called whenever in target.

Shall be implemented.

Implements iCub::iKin::iKinCtrl.

Definition at line 933 of file iKinInv.h.

{ }

virtual bool iCub::iKin::iKinCtrl::isInTarget (  )  [inline, virtual, inherited]

Checks if the End-Effector is in target.

Returns:
true if in target.

Definition at line 331 of file iKinInv.h.

References iCub::iKin::iKinCtrl::dist().

Referenced by iCub::iKin::iKinCtrl::update_state().

{ return dist()<inTargetTol; }

virtual yarp::sig::Vector iCub::iKin::MultiRefMinJerkCtrl::iterate ( yarp::sig::Vector &  xd,
yarp::sig::Vector &  qd,
const unsigned int  verbose = 0 
) [virtual]

Executes one iteration of the control algorithm.

Parameters:
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).
Returns:
current estimation of joints configuration.
Note:
The reason why qd is provided externally instead of computed here is to discouple the inverse kinematic problem (which may require some computational effort depending on the current pose xd) from the reaching issue.
virtual yarp::sig::Vector iCub::iKin::MultiRefMinJerkCtrl::iterate ( yarp::sig::Vector &  xd,
yarp::sig::Vector &  qd,
yarp::sig::Vector &  xdot_set,
const unsigned int  verbose = 0 
) [virtual]

Executes one iteration of the control algorithm.

Parameters:
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).
Returns:
current estimation of joints configuration.
Note:
The reason why qd is provided externally instead of computed here is to discouple the inverse kinematic problem (which may require some computational effort depending on the current pose xd) from the reaching issue.
virtual yarp::sig::Vector iCub::iKin::MultiRefMinJerkCtrl::iterate ( yarp::sig::Vector &  xd,
const unsigned  verbose 
) [inline, virtual]

Executes one iteration of the control algorithm.

Parameters:
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).
Returns:
current estimation of joints configuration. Shall be implemented.

Implements iCub::iKin::iKinCtrl.

Definition at line 1019 of file iKinInv.h.

{ return yarp::sig::Vector(0); }

void MultiRefMinJerkCtrl::printIter ( const unsigned int  verbose  )  [protected, virtual]

Dumps warning or status messages.

Parameters:
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).
Note:
Angles are dumperd as degrees. Shall be implemented.

Implements iCub::iKin::iKinCtrl.

Definition at line 1314 of file iKinInv.cpp.

References iCub::iKin::iKinCtrl::dist(), and iCub::iKin::iKinCtrl::printHandling().

{
    // This should be the first line of any printIter method
    unsigned int _verbose=printHandling(verbose);

    if (_verbose)
    {
        string strState[3];

        strState[IKINCTRL_STATE_RUNNING] ="running";
        strState[IKINCTRL_STATE_INTARGET]="inTarget";
        strState[IKINCTRL_STATE_DEADLOCK]="deadLock";

        fprintf(stdout,"state   = %s\n",strState[state].c_str());
        fprintf(stdout,"norm(e) = %g\n",dist());
        fprintf(stdout,"xd      = %s\n",x_set.toString().c_str());
        fprintf(stdout,"x       = %s\n",x.toString().c_str());
        fprintf(stdout,"qd      = %s\n",(CTRL_RAD2DEG*q_set).toString().c_str());
        fprintf(stdout,"q       = %s\n",(CTRL_RAD2DEG*q).toString().c_str());

        if (_verbose>1)
        {
            fprintf(stdout,"qdot    = %s\n",(CTRL_RAD2DEG*qdot).toString().c_str());
            fprintf(stdout,"xdot    = %s\n",xdot.toString().c_str());
        }

        if (_verbose>2)
            fprintf(stdout,"comp    = %s\n",compensation.toString().c_str());

        fprintf(stdout,"\n\n");
    }
}

virtual void iCub::iKin::MultiRefMinJerkCtrl::restart ( const yarp::sig::Vector &  q0  )  [virtual]

Reinitializes the algorithm's internal state and resets the starting point.

Parameters:
q0 is the new starting point.

Reimplemented from iCub::iKin::iKinCtrl.

void iKinCtrl::set_ctrlPose ( unsigned int  _ctrlPose  )  [inherited]

Sets the state of Pose control settings.

Parameters:
_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.

Definition at line 75 of file iKinInv.cpp.

Referenced by iCub::iKin::iKinCtrl::iKinCtrl().

{
    ctrlPose=_ctrlPose;

    if (ctrlPose>IKINCTRL_POSE_ANG)
        ctrlPose=IKINCTRL_POSE_ANG;
}

double MultiRefMinJerkCtrl::set_execTime ( const double  _execTime,
const bool  warn = false 
)

Sets the task execution time in seconds.

Parameters:
_execTime. 
warn enable/disable warning message for thresholding (disabled by default).
Returns:
the actual execTime.
Note:
A lower bound equal to 10*Ts (Ts=controller's sample time) is imposed.

Definition at line 1357 of file iKinInv.cpp.

{
    double lowerThres=10.0*Ts;

    execTime=_execTime>lowerThres ? _execTime : lowerThres;

    if (warn && (execTime!=_execTime))
        fprintf(stderr,"Warning: task execution time limited to the lower bound %g\n",lowerThres);

    return execTime;
}

void iCub::iKin::MultiRefMinJerkCtrl::set_gamma ( double  _gamma  )  [inline]

Sets the parameter gamma which is used to blend the contribute of the task contorller versus the contribute of the joint controller.

Parameters:
_gamma. 

Definition at line 1071 of file iKinInv.h.

{ gamma=_gamma; }

void MultiRefMinJerkCtrl::set_guardRatio ( double  _guardRatio  ) 

Sets the guard ratio (in [0 1]).

Parameters:
_guardRatio. 

Definition at line 1162 of file iKinInv.cpp.

{
    guardRatio=_guardRatio>1.0 ? 1.0 : (_guardRatio<0.0 ? 0.0 : _guardRatio);

    computeGuard();
}

virtual void iCub::iKin::MultiRefMinJerkCtrl::set_q ( const yarp::sig::Vector &  q0  )  [virtual]

Sets the joint angles values.

Parameters:
q0 is the joint angles vector.

Reimplemented from iCub::iKin::iKinCtrl.

virtual void iCub::iKin::iKinCtrl::setChainConstraints ( bool  _constrained  )  [inline, virtual, inherited]

Enables/Disables joint angles constraints.

Parameters:
_constrained if true then constraints are applied.

Reimplemented in iCub::iKin::SteepCtrl, and iCub::iKin::LMCtrl.

Definition at line 199 of file iKinInv.h.

References iCub::iKin::iKinChain::setAllConstraints().

{ chain.setAllConstraints(_constrained); }

virtual void iCub::iKin::iKinCtrl::setInTargetTol ( double  tol_x  )  [inline, virtual, inherited]

Sets tolerance for in-target check (1e-3 by default).

Parameters:
tol_x is the tolerance

Definition at line 294 of file iKinInv.h.

{ inTargetTol=tol_x; }

void iCub::iKin::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:
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.

Parameters:
entryTag specifies an entry tag different from "dimension".
virtual void iCub::iKin::iKinCtrl::setWatchDogMaxIter ( int  maxIter  )  [inline, virtual, inherited]

Sets maximum number of iterations to trigger the watchDog (200 by default).

Parameters:
maxIter is the iterations limit.

Definition at line 319 of file iKinInv.h.

{ watchDogMaxIter=maxIter; }

virtual void iCub::iKin::iKinCtrl::setWatchDogTol ( double  tol_q  )  [inline, virtual, inherited]

Sets tolerance for watchDog check (1e-4 by default).

Parameters:
tol_q is the tolerance

Definition at line 306 of file iKinInv.h.

{ watchDogTol=tol_q; }

virtual yarp::sig::Vector iCub::iKin::MultiRefMinJerkCtrl::solve ( yarp::sig::Vector &  xd,
const   tol_size,
const   max_iter,
const unsigned  verbose,
int *  exit_code,
bool *  exhalt 
) [inline, virtual]

Iterates the control algorithm trying to converge on the target.

Parameters:
xd is the End-Effector target Pose to be tracked.
See also:
setInTargetTol()
getInTargetTol()
Parameters:
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.

Definition at line 1020 of file iKinInv.h.

                                                                                 { return yarp::sig::Vector(0); }

void iCub::iKin::iKinCtrl::switchWatchDog ( bool  sw  )  [inline, inherited]

Switch on/off the watchDog mechanism to trigger deadLocks.

A deadLock is triggered whenerver norm(q(k)-q(k-1))<tol_q for a specified number of iterations.

Parameters:
sw control the watchDog activation.

Definition at line 288 of file iKinInv.h.

{ watchDogOn=sw; }

virtual bool iCub::iKin::MultiRefMinJerkCtrl::test_convergence ( const   tol_size  )  [inline, virtual]

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.

Parameters:
tol_size is tolerance to compare to. Shall be implemented.

Implements iCub::iKin::iKinCtrl.

Definition at line 1018 of file iKinInv.h.

{ return false; }

The documentation for this class was generated from the following files:
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations
Generated on Fri May 18 2012 01:31:40 for iCub-main by  doxygen 1.7.1