iCub-main
Public Member Functions | Protected Member Functions | Protected Attributes | Friends | List of all members
iCub::iKin::CartesianSolver Class Referenceabstract

Abstract class defining the core of on-line solvers. More...

#include <iKinSlv.h>

+ Inheritance diagram for iCub::iKin::CartesianSolver:

Public Member Functions

 CartesianSolver (const std::string &_slvName)
 Constructor. More...
 
virtual bool open (yarp::os::Searchable &options)
 Configure the solver and start it up. More...
 
virtual void interrupt ()
 Interrupt the open() method waiting for motor parts to be ready. More...
 
virtual void close ()
 Stop the solver and dispose it. More...
 
virtual bool isClosed () const
 To be called to check whether the solver has received a [quit] request. More...
 
virtual bool & getTimeoutFlag ()
 To be called to check whether communication timeout has been detected. More...
 
virtual void suspend ()
 Suspend the solver's main loop. More...
 
virtual void resume ()
 Resume the solver's main loop. More...
 
virtual ~CartesianSolver ()
 Default destructor. More...
 

Protected Member Functions

virtual PartDescriptorgetPartDesc (yarp::os::Searchable &options)=0
 
virtual yarp::sig::Vector solve (yarp::sig::Vector &xd)
 
virtual yarp::sig::Vector & encodeDOF ()
 
virtual bool decodeDOF (const yarp::sig::Vector &_dof)
 
virtual bool handleJointsRestPosition (const yarp::os::Bottle *options, yarp::os::Bottle *reply=NULL)
 
virtual bool handleJointsRestWeights (const yarp::os::Bottle *options, yarp::os::Bottle *reply=NULL)
 
yarp::dev::PolyDriver * waitPart (const yarp::os::Property &partOpt)
 
bool isNewDOF (const yarp::sig::Vector &_dof)
 
bool changeDOF (const yarp::sig::Vector &_dof)
 
bool alignJointsBounds ()
 
bool setLimits (int axis, double min, double max)
 
void countUncontrolledJoints ()
 
void latchUncontrolledJoints (yarp::sig::Vector &joints)
 
void getFeedback (const bool wait=false)
 
void initPos ()
 
void lock ()
 
void unlock ()
 
void waitDOFHandling ()
 
void postDOFHandling ()
 
void fillDOFInfo (yarp::os::Bottle &reply)
 
void send (const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, double *tok)
 
void printInfo (const std::string &typ, const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, const double t)
 
virtual void prepareJointsRestTask ()
 
virtual void respond (const yarp::os::Bottle &command, yarp::os::Bottle &reply)
 
virtual bool threadInit ()
 
virtual void afterStart (bool)
 
virtual void run ()
 
virtual void threadRelease ()
 

Protected Attributes

PartDescriptorprt
 
std::deque< yarp::dev::PolyDriver * > drv
 
std::deque< yarp::dev::IControlLimits * > lim
 
std::deque< yarp::dev::IEncoders * > enc
 
std::deque< int > jnt
 
std::deque< int * > rmp
 
iKinIpOptMinslv
 
SolverCallbackclb
 
RpcProcessorcmdProcessor
 
yarp::os::Port * rpcPort
 
InputPortinPort
 
yarp::os::BufferedPort< yarp::os::Bottle > * outPort
 
yarp::os::Mutex mutex
 
std::string slvName
 
std::string type
 
unsigned int period
 
unsigned int ctrlPose
 
bool fullDOF
 
bool contModeOld
 
bool configured
 
bool closing
 
bool closed
 
bool interrupting
 
bool verbosity
 
bool timeout_detected
 
int maxPartJoints
 
int unctrlJointsNum
 
double ping_robot_tmo
 
double token
 
double * pToken
 
yarp::sig::Matrix hwLimits
 
yarp::sig::Matrix swLimits
 
yarp::sig::Vector unctrlJointsOld
 
yarp::sig::Vector dof
 
yarp::sig::Vector restJntPos
 
yarp::sig::Vector restWeights
 
yarp::sig::Vector xd_2ndTask
 
yarp::sig::Vector w_2ndTask
 
yarp::sig::Vector qd_3rdTask
 
yarp::sig::Vector w_3rdTask
 
yarp::sig::Vector idx_3rdTask
 
yarp::os::Event dofEvent
 

Friends

class RpcProcessor
 
class InputPort
 
class SolverCallback
 

Additional Inherited Members

- Static Protected Member Functions inherited from iCub::iKin::CartesianHelper
static void addVectorOption (yarp::os::Bottle &b, const int vcb, const yarp::sig::Vector &v)
 
static bool getDesiredOption (const yarp::os::Bottle &reply, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
 
static void addTargetOption (yarp::os::Bottle &b, const yarp::sig::Vector &xd)
 Appends to a bottle all data needed to command a target. More...
 
static void addDOFOption (yarp::os::Bottle &b, const yarp::sig::Vector &dof)
 Appends to a bottle all data needed to reconfigure chain's dof. More...
 
static void addJointsResPosOption (yarp::os::Bottle &b, const yarp::sig::Vector &restPos)
 Appends to a bottle all data needed to modify joints rest position. More...
 
static void addJointsRestWeightsOption (yarp::os::Bottle &b, const yarp::sig::Vector &restWeights)
 Appends to a bottle all data needed to modify joints rest weights. More...
 
static void addPoseOption (yarp::os::Bottle &b, const unsigned int pose)
 Appends to a bottle all data needed to change the pose mode. More...
 
static void addModeOption (yarp::os::Bottle &b, const bool tracking)
 Appends to a bottle all data needed to change the tracking mode. More...
 
static void addTokenOption (yarp::os::Bottle &b, const double token)
 Appends to a bottle a token to be exchanged with the solver. More...
 
static yarp::os::Bottle * getTargetOption (const yarp::os::Bottle &b)
 Retrieves commanded target data from a bottle. More...
 
static yarp::os::Bottle * getEndEffectorPoseOption (const yarp::os::Bottle &b)
 Retrieves the end-effector pose data. More...
 
static yarp::os::Bottle * getJointsOption (const yarp::os::Bottle &b)
 Retrieves the joints configuration data. More...
 
static bool getTokenOption (const yarp::os::Bottle &b, double *token)
 Retrieves the token from the bottle. More...
 
static bool computeFixationPointData (iKinChain &eyeL, iKinChain &eyeR, yarp::sig::Vector &fp)
 Retrieves current fixation point given the current kinematics configuration of the eyes. More...
 
static bool computeFixationPointData (iKinChain &eyeL, iKinChain &eyeR, yarp::sig::Vector &fp, yarp::sig::Matrix &J)
 Retrieves current fixation point and its Jacobian wrt eyes tilt-pan-vergence dofs given the current kinematics configuration of the eyes. More...
 

Detailed Description

Abstract class defining the core of on-line solvers.

Definition at line 330 of file iKinSlv.h.

Constructor & Destructor Documentation

◆ CartesianSolver()

CartesianSolver::CartesianSolver ( const std::string &  _slvName)

Constructor.

Parameters
_slvNamespecifies the base name for all ports created as below:

/<_slvName>/in : the input port which accepts requests in streaming mode.

/<_slvName>/rpc : the input port where to send requests and wait for replies.

/<_slvName>/out : the port which streams out the results of optimization.

Definition at line 274 of file iKinSlv.cpp.

◆ ~CartesianSolver()

CartesianSolver::~CartesianSolver ( )
virtual

Default destructor.

Definition at line 1756 of file iKinSlv.cpp.

Member Function Documentation

◆ afterStart()

void CartesianSolver::afterStart ( bool  s)
protectedvirtual

Definition at line 1614 of file iKinSlv.cpp.

◆ alignJointsBounds()

bool CartesianSolver::alignJointsBounds ( )
protected

Definition at line 347 of file iKinSlv.cpp.

◆ changeDOF()

bool CartesianSolver::changeDOF ( const yarp::sig::Vector &  _dof)
protected

Definition at line 1465 of file iKinSlv.cpp.

◆ close()

void CartesianSolver::close ( void  )
virtual

Stop the solver and dispose it.

Called by destructor.

Definition at line 1539 of file iKinSlv.cpp.

◆ countUncontrolledJoints()

void CartesianSolver::countUncontrolledJoints ( )
protected

Definition at line 408 of file iKinSlv.cpp.

◆ decodeDOF()

bool CartesianSolver::decodeDOF ( const yarp::sig::Vector &  _dof)
protectedvirtual

Reimplemented in iCub::iKin::iCubArmCartesianSolver.

Definition at line 1182 of file iKinSlv.cpp.

◆ encodeDOF()

Vector & CartesianSolver::encodeDOF ( )
protectedvirtual

Definition at line 1167 of file iKinSlv.cpp.

◆ fillDOFInfo()

void CartesianSolver::fillDOFInfo ( yarp::os::Bottle &  reply)
protected

Definition at line 514 of file iKinSlv.cpp.

◆ getFeedback()

void CartesianSolver::getFeedback ( const bool  wait = false)
protected

Definition at line 432 of file iKinSlv.cpp.

◆ getPartDesc()

virtual PartDescriptor* iCub::iKin::CartesianSolver::getPartDesc ( yarp::os::Searchable &  options)
protectedpure virtual

◆ getTimeoutFlag()

virtual bool& iCub::iKin::CartesianSolver::getTimeoutFlag ( )
inlinevirtual

To be called to check whether communication timeout has been detected.

Returns
reference to the internal flag.
Note
At first timeout detection the flag is set and is never reset again.

Definition at line 544 of file iKinSlv.h.

◆ handleJointsRestPosition()

bool CartesianSolver::handleJointsRestPosition ( const yarp::os::Bottle *  options,
yarp::os::Bottle *  reply = NULL 
)
protectedvirtual

Definition at line 1202 of file iKinSlv.cpp.

◆ handleJointsRestWeights()

bool CartesianSolver::handleJointsRestWeights ( const yarp::os::Bottle *  options,
yarp::os::Bottle *  reply = NULL 
)
protectedvirtual

Definition at line 1233 of file iKinSlv.cpp.

◆ initPos()

void CartesianSolver::initPos ( )
protected

Definition at line 473 of file iKinSlv.cpp.

◆ interrupt()

void CartesianSolver::interrupt ( )
virtual

Interrupt the open() method waiting for motor parts to be ready.

Definition at line 1532 of file iKinSlv.cpp.

◆ isClosed()

virtual bool iCub::iKin::CartesianSolver::isClosed ( ) const
inlinevirtual

To be called to check whether the solver has received a [quit] request.

Returns
true/false if closed or not.

Definition at line 534 of file iKinSlv.h.

◆ isNewDOF()

bool CartesianSolver::isNewDOF ( const yarp::sig::Vector &  _dof)
protected

Definition at line 1261 of file iKinSlv.cpp.

◆ latchUncontrolledJoints()

void CartesianSolver::latchUncontrolledJoints ( yarp::sig::Vector &  joints)
protected

Definition at line 415 of file iKinSlv.cpp.

◆ lock()

void CartesianSolver::lock ( )
protected

Definition at line 485 of file iKinSlv.cpp.

◆ open()

bool CartesianSolver::open ( yarp::os::Searchable &  options)
virtual

Configure the solver and start it up.

Parameters
optionscontains the set of options in form of a Property object.

Available options are:

robot <string>: example (robot icub), specifies the name of the robot to connect to.

type <string>: example (type right), specifies the type of the limb to be instantiated; it can be "left" or "right".

dof <(int int ...)>: example (dof (1 1 0 1 ...)), specifies which dof of the chain are actuated (by putting 1 in the corresponding position) and which not (with 0). The length of the provided list of 1's and 0's should match the number of chain's links. The special value 2 is used to keep the link status unchanged and proceed to the next link.

rest_pos <(double double ...)>: example (rest_pos (20.0 0.0 0.0 ...)), specifies in degrees the joints rest position used as secondary task in the minimization. The length of the provided list should match the number of chain's links. Default values are (0.0 0.0 0.0 ...).

rest_weights <(double double ...)>: example (rest_weights (1.0 0.0 0.0 1.0 ...)), specifies for each link the weights used for the secondary task minimization. The length of the provided list should match the number of the chain's links. Default values are (0.0 0.0 0.0 ...).

period <int>: example (period 30), specifies the thread period in ms.

pose <vocab>: example (pose full), specifies the end-effector pose the user wants to achieve; it can be [full] (position+orientation) or [xyz] (only position).

mode : example (mode cont), selects the solver mode between [cont] which implements a continuous tracking and [shot] which does not compensate for movements induced on unacatuated joints.

verbosity <vocab>: example (verbosity on), selects whether to report or not on the screen the result of each optimization instance; allowed values are [on] or [off].

tol <double>: example (tol 0.001), specifies the desired tolerance on the task function to be minimized.

constr_tol <double>: example (constr_tol 0.000001), specifies the desired tolerance for the constraints to comply with.

maxIter <int>: example (maxIter 200), specifies the maximum number of iterations allowed for one optimization instance.

interPoints <vocab>: example (interPoints on), selects whether to force or not the solver to output on the port all intermediate points of optimization instance; allowed values are [on] or [off].

ping_robot_tmo <double>: example (ping_robot_tmo 2.0), specifies a timeout in seconds during which robot state ports are pinged prior to connecting; a timeout equal to zero disables this option.

Returns
true/false if successful/failed

Reimplemented in iCub::iKin::iCubArmCartesianSolver.

Definition at line 1277 of file iKinSlv.cpp.

◆ postDOFHandling()

void CartesianSolver::postDOFHandling ( )
protected

Definition at line 507 of file iKinSlv.cpp.

◆ prepareJointsRestTask()

void CartesianSolver::prepareJointsRestTask ( )
protectedvirtual

Definition at line 1496 of file iKinSlv.cpp.

◆ printInfo()

void CartesianSolver::printInfo ( const std::string &  typ,
const yarp::sig::Vector &  xd,
const yarp::sig::Vector &  x,
const yarp::sig::Vector &  q,
const double  t 
)
protected

Definition at line 1151 of file iKinSlv.cpp.

◆ respond()

void CartesianSolver::respond ( const yarp::os::Bottle &  command,
yarp::os::Bottle &  reply 
)
protectedvirtual

Definition at line 522 of file iKinSlv.cpp.

◆ resume()

void CartesianSolver::resume ( )
virtual

Resume the solver's main loop.

Definition at line 1637 of file iKinSlv.cpp.

◆ run()

void CartesianSolver::run ( void  )
protectedvirtual

Definition at line 1651 of file iKinSlv.cpp.

◆ send()

void CartesianSolver::send ( const yarp::sig::Vector &  xd,
const yarp::sig::Vector &  x,
const yarp::sig::Vector &  q,
double *  tok 
)
protected

Definition at line 1133 of file iKinSlv.cpp.

◆ setLimits()

bool CartesianSolver::setLimits ( int  axis,
double  min,
double  max 
)
protected

Definition at line 382 of file iKinSlv.cpp.

◆ solve()

Vector CartesianSolver::solve ( yarp::sig::Vector &  xd)
protectedvirtual

Definition at line 1522 of file iKinSlv.cpp.

◆ suspend()

void CartesianSolver::suspend ( )
virtual

Suspend the solver's main loop.

Definition at line 1624 of file iKinSlv.cpp.

◆ threadInit()

bool CartesianSolver::threadInit ( )
protectedvirtual

Definition at line 1602 of file iKinSlv.cpp.

◆ threadRelease()

void CartesianSolver::threadRelease ( )
protectedvirtual

Definition at line 1749 of file iKinSlv.cpp.

◆ unlock()

void CartesianSolver::unlock ( )
protected

Definition at line 492 of file iKinSlv.cpp.

◆ waitDOFHandling()

void CartesianSolver::waitDOFHandling ( )
protected

Definition at line 499 of file iKinSlv.cpp.

◆ waitPart()

PolyDriver * CartesianSolver::waitPart ( const yarp::os::Property &  partOpt)
protected

Definition at line 308 of file iKinSlv.cpp.

Friends And Related Function Documentation

◆ InputPort

friend class InputPort
friend

Definition at line 426 of file iKinSlv.h.

◆ RpcProcessor

friend class RpcProcessor
friend

Definition at line 425 of file iKinSlv.h.

◆ SolverCallback

friend class SolverCallback
friend

Definition at line 427 of file iKinSlv.h.

Member Data Documentation

◆ clb

SolverCallback* iCub::iKin::CartesianSolver::clb
protected

Definition at line 342 of file iKinSlv.h.

◆ closed

bool iCub::iKin::CartesianSolver::closed
protected

Definition at line 358 of file iKinSlv.h.

◆ closing

bool iCub::iKin::CartesianSolver::closing
protected

Definition at line 357 of file iKinSlv.h.

◆ cmdProcessor

RpcProcessor* iCub::iKin::CartesianSolver::cmdProcessor
protected

Definition at line 344 of file iKinSlv.h.

◆ configured

bool iCub::iKin::CartesianSolver::configured
protected

Definition at line 356 of file iKinSlv.h.

◆ contModeOld

bool iCub::iKin::CartesianSolver::contModeOld
protected

Definition at line 355 of file iKinSlv.h.

◆ ctrlPose

unsigned int iCub::iKin::CartesianSolver::ctrlPose
protected

Definition at line 353 of file iKinSlv.h.

◆ dof

yarp::sig::Vector iCub::iKin::CartesianSolver::dof
protected

Definition at line 372 of file iKinSlv.h.

◆ dofEvent

yarp::os::Event iCub::iKin::CartesianSolver::dofEvent
protected

Definition at line 384 of file iKinSlv.h.

◆ drv

std::deque<yarp::dev::PolyDriver*> iCub::iKin::CartesianSolver::drv
protected

Definition at line 335 of file iKinSlv.h.

◆ enc

std::deque<yarp::dev::IEncoders*> iCub::iKin::CartesianSolver::enc
protected

Definition at line 337 of file iKinSlv.h.

◆ fullDOF

bool iCub::iKin::CartesianSolver::fullDOF
protected

Definition at line 354 of file iKinSlv.h.

◆ hwLimits

yarp::sig::Matrix iCub::iKin::CartesianSolver::hwLimits
protected

Definition at line 368 of file iKinSlv.h.

◆ idx_3rdTask

yarp::sig::Vector iCub::iKin::CartesianSolver::idx_3rdTask
protected

Definition at line 382 of file iKinSlv.h.

◆ inPort

InputPort* iCub::iKin::CartesianSolver::inPort
protected

Definition at line 346 of file iKinSlv.h.

◆ interrupting

bool iCub::iKin::CartesianSolver::interrupting
protected

Definition at line 359 of file iKinSlv.h.

◆ jnt

std::deque<int> iCub::iKin::CartesianSolver::jnt
protected

Definition at line 338 of file iKinSlv.h.

◆ lim

std::deque<yarp::dev::IControlLimits*> iCub::iKin::CartesianSolver::lim
protected

Definition at line 336 of file iKinSlv.h.

◆ maxPartJoints

int iCub::iKin::CartesianSolver::maxPartJoints
protected

Definition at line 362 of file iKinSlv.h.

◆ mutex

yarp::os::Mutex iCub::iKin::CartesianSolver::mutex
protected

Definition at line 348 of file iKinSlv.h.

◆ outPort

yarp::os::BufferedPort<yarp::os::Bottle>* iCub::iKin::CartesianSolver::outPort
protected

Definition at line 347 of file iKinSlv.h.

◆ period

unsigned int iCub::iKin::CartesianSolver::period
protected

Definition at line 352 of file iKinSlv.h.

◆ ping_robot_tmo

double iCub::iKin::CartesianSolver::ping_robot_tmo
protected

Definition at line 364 of file iKinSlv.h.

◆ prt

PartDescriptor* iCub::iKin::CartesianSolver::prt
protected

Definition at line 334 of file iKinSlv.h.

◆ pToken

double* iCub::iKin::CartesianSolver::pToken
protected

Definition at line 366 of file iKinSlv.h.

◆ qd_3rdTask

yarp::sig::Vector iCub::iKin::CartesianSolver::qd_3rdTask
protected

Definition at line 380 of file iKinSlv.h.

◆ restJntPos

yarp::sig::Vector iCub::iKin::CartesianSolver::restJntPos
protected

Definition at line 374 of file iKinSlv.h.

◆ restWeights

yarp::sig::Vector iCub::iKin::CartesianSolver::restWeights
protected

Definition at line 375 of file iKinSlv.h.

◆ rmp

std::deque<int*> iCub::iKin::CartesianSolver::rmp
protected

Definition at line 339 of file iKinSlv.h.

◆ rpcPort

yarp::os::Port* iCub::iKin::CartesianSolver::rpcPort
protected

Definition at line 345 of file iKinSlv.h.

◆ slv

iKinIpOptMin* iCub::iKin::CartesianSolver::slv
protected

Definition at line 341 of file iKinSlv.h.

◆ slvName

std::string iCub::iKin::CartesianSolver::slvName
protected

Definition at line 350 of file iKinSlv.h.

◆ swLimits

yarp::sig::Matrix iCub::iKin::CartesianSolver::swLimits
protected

Definition at line 369 of file iKinSlv.h.

◆ timeout_detected

bool iCub::iKin::CartesianSolver::timeout_detected
protected

Definition at line 361 of file iKinSlv.h.

◆ token

double iCub::iKin::CartesianSolver::token
protected

Definition at line 365 of file iKinSlv.h.

◆ type

std::string iCub::iKin::CartesianSolver::type
protected

Definition at line 351 of file iKinSlv.h.

◆ unctrlJointsNum

int iCub::iKin::CartesianSolver::unctrlJointsNum
protected

Definition at line 363 of file iKinSlv.h.

◆ unctrlJointsOld

yarp::sig::Vector iCub::iKin::CartesianSolver::unctrlJointsOld
protected

Definition at line 371 of file iKinSlv.h.

◆ verbosity

bool iCub::iKin::CartesianSolver::verbosity
protected

Definition at line 360 of file iKinSlv.h.

◆ w_2ndTask

yarp::sig::Vector iCub::iKin::CartesianSolver::w_2ndTask
protected

Definition at line 378 of file iKinSlv.h.

◆ w_3rdTask

yarp::sig::Vector iCub::iKin::CartesianSolver::w_3rdTask
protected

Definition at line 381 of file iKinSlv.h.

◆ xd_2ndTask

yarp::sig::Vector iCub::iKin::CartesianSolver::xd_2ndTask
protected

Definition at line 377 of file iKinSlv.h.


The documentation for this class was generated from the following files: