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 PartDescriptor *	getPartDesc (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
PartDescriptor *	prt

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

iKinIpOptMin *	slv

SolverCallback *	clb

RpcProcessor *	cmdProcessor

yarp::os::Port *	rpcPort

InputPort *	inPort

yarp::os::BufferedPort< yarp::os::Bottle > *	outPort

std::mutex	mtx

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

std::mutex	mtx_dofEvent

std::condition_variable	cv_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 343 of file iKinSlv.h.

Constructor & Destructor Documentation

◆ CartesianSolver()

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

Constructor.

Parameters

_slvName specifies 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 267 of file iKinSlv.cpp.

◆ ~CartesianSolver()

CartesianSolver::~CartesianSolver ( )

virtual

Default destructor.

Definition at line 1749 of file iKinSlv.cpp.

Member Function Documentation

◆ afterStart()

void CartesianSolver::afterStart ( bool s )

protectedvirtual

Definition at line 1607 of file iKinSlv.cpp.

◆ alignJointsBounds()

bool CartesianSolver::alignJointsBounds ( )

protected

Definition at line 340 of file iKinSlv.cpp.

◆ changeDOF()

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

protected

Definition at line 1458 of file iKinSlv.cpp.

◆ close()

void CartesianSolver::close ( void )

virtual

Stop the solver and dispose it.

Called by destructor.

Definition at line 1532 of file iKinSlv.cpp.

◆ countUncontrolledJoints()

void CartesianSolver::countUncontrolledJoints ( )

protected

Definition at line 401 of file iKinSlv.cpp.

◆ decodeDOF()

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

protectedvirtual

Reimplemented in iCub::iKin::iCubArmCartesianSolver.

Definition at line 1175 of file iKinSlv.cpp.

◆ encodeDOF()

Vector & CartesianSolver::encodeDOF ( )

protectedvirtual

Definition at line 1160 of file iKinSlv.cpp.

◆ fillDOFInfo()

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

protected

Definition at line 507 of file iKinSlv.cpp.

◆ getFeedback()

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

protected

Definition at line 425 of file iKinSlv.cpp.

◆ getPartDesc()

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

protectedpure virtual

Implemented in iCub::iKin::iCubLegCartesianSolver, and iCub::iKin::iCubArmCartesianSolver.

◆ 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 558 of file iKinSlv.h.

◆ handleJointsRestPosition()

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

protectedvirtual

Definition at line 1195 of file iKinSlv.cpp.

◆ handleJointsRestWeights()

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

protectedvirtual

Definition at line 1226 of file iKinSlv.cpp.

◆ initPos()

void CartesianSolver::initPos ( )

protected

Definition at line 466 of file iKinSlv.cpp.

◆ interrupt()

void CartesianSolver::interrupt ( )

virtual

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

Definition at line 1525 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 548 of file iKinSlv.h.

◆ isNewDOF()

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

protected

Definition at line 1254 of file iKinSlv.cpp.

◆ latchUncontrolledJoints()

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

protected

Definition at line 408 of file iKinSlv.cpp.

◆ lock()

void CartesianSolver::lock ( )

protected

Definition at line 478 of file iKinSlv.cpp.

◆ open()

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

virtual

Configure the solver and start it up.

Parameters

options contains 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 1270 of file iKinSlv.cpp.

◆ postDOFHandling()

void CartesianSolver::postDOFHandling ( )

protected

Definition at line 500 of file iKinSlv.cpp.

◆ prepareJointsRestTask()

void CartesianSolver::prepareJointsRestTask ( )

protectedvirtual

Definition at line 1489 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 1144 of file iKinSlv.cpp.

◆ respond()

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

protectedvirtual

Definition at line 515 of file iKinSlv.cpp.

◆ resume()

void CartesianSolver::resume ( )

virtual

Resume the solver's main loop.

Definition at line 1630 of file iKinSlv.cpp.

◆ run()

void CartesianSolver::run ( void )

protectedvirtual

Definition at line 1644 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 1126 of file iKinSlv.cpp.

◆ setLimits()

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

protected

Definition at line 375 of file iKinSlv.cpp.

◆ solve()

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

protectedvirtual

Definition at line 1515 of file iKinSlv.cpp.

◆ suspend()

void CartesianSolver::suspend ( )

virtual

Suspend the solver's main loop.

Definition at line 1617 of file iKinSlv.cpp.

◆ threadInit()

bool CartesianSolver::threadInit ( )

protectedvirtual

Definition at line 1595 of file iKinSlv.cpp.

◆ threadRelease()

void CartesianSolver::threadRelease ( )

protectedvirtual

Definition at line 1742 of file iKinSlv.cpp.

◆ unlock()

void CartesianSolver::unlock ( )

protected

Definition at line 485 of file iKinSlv.cpp.

◆ waitDOFHandling()

void CartesianSolver::waitDOFHandling ( )

protected

Definition at line 492 of file iKinSlv.cpp.

◆ waitPart()

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

protected

Definition at line 301 of file iKinSlv.cpp.

Friends And Related Function Documentation

◆ InputPort

friend class InputPort

friend

Definition at line 440 of file iKinSlv.h.

◆ RpcProcessor

friend class RpcProcessor

friend

Definition at line 439 of file iKinSlv.h.

◆ SolverCallback

friend class SolverCallback

friend

Definition at line 441 of file iKinSlv.h.

Member Data Documentation

◆ clb

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

protected

Definition at line 355 of file iKinSlv.h.

◆ closed

bool iCub::iKin::CartesianSolver::closed

protected

Definition at line 371 of file iKinSlv.h.

◆ closing

bool iCub::iKin::CartesianSolver::closing

protected

Definition at line 370 of file iKinSlv.h.

◆ cmdProcessor

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

protected

Definition at line 357 of file iKinSlv.h.

◆ configured

bool iCub::iKin::CartesianSolver::configured

protected

Definition at line 369 of file iKinSlv.h.

◆ contModeOld

bool iCub::iKin::CartesianSolver::contModeOld

protected

Definition at line 368 of file iKinSlv.h.

◆ ctrlPose

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

protected

Definition at line 366 of file iKinSlv.h.

◆ cv_dofEvent

std::condition_variable iCub::iKin::CartesianSolver::cv_dofEvent

protected

Definition at line 398 of file iKinSlv.h.

◆ dof

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

protected

Definition at line 385 of file iKinSlv.h.

◆ drv

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

protected

Definition at line 348 of file iKinSlv.h.

◆ enc

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

protected

Definition at line 350 of file iKinSlv.h.

◆ fullDOF

bool iCub::iKin::CartesianSolver::fullDOF

protected

Definition at line 367 of file iKinSlv.h.

◆ hwLimits

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

protected

Definition at line 381 of file iKinSlv.h.

◆ idx_3rdTask

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

protected

Definition at line 395 of file iKinSlv.h.

◆ inPort

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

protected

Definition at line 359 of file iKinSlv.h.

◆ interrupting

bool iCub::iKin::CartesianSolver::interrupting

protected

Definition at line 372 of file iKinSlv.h.

◆ jnt

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

protected

Definition at line 351 of file iKinSlv.h.

◆ lim

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

protected

Definition at line 349 of file iKinSlv.h.

◆ maxPartJoints

int iCub::iKin::CartesianSolver::maxPartJoints

protected

Definition at line 375 of file iKinSlv.h.

◆ mtx

std::mutex iCub::iKin::CartesianSolver::mtx

protected

Definition at line 361 of file iKinSlv.h.

◆ mtx_dofEvent

std::mutex iCub::iKin::CartesianSolver::mtx_dofEvent

protected

Definition at line 397 of file iKinSlv.h.

◆ outPort

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

protected

Definition at line 360 of file iKinSlv.h.

◆ period

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

protected

Definition at line 365 of file iKinSlv.h.

◆ ping_robot_tmo

double iCub::iKin::CartesianSolver::ping_robot_tmo

protected

Definition at line 377 of file iKinSlv.h.

◆ prt

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

protected

Definition at line 347 of file iKinSlv.h.

◆ pToken

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

protected

Definition at line 379 of file iKinSlv.h.

◆ qd_3rdTask

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

protected

Definition at line 393 of file iKinSlv.h.

◆ restJntPos

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

protected

Definition at line 387 of file iKinSlv.h.

◆ restWeights

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

protected

Definition at line 388 of file iKinSlv.h.

◆ rmp

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

protected

Definition at line 352 of file iKinSlv.h.

◆ rpcPort

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

protected

Definition at line 358 of file iKinSlv.h.

◆ slv

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

protected

Definition at line 354 of file iKinSlv.h.

◆ slvName

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

protected

Definition at line 363 of file iKinSlv.h.

◆ swLimits

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

protected

Definition at line 382 of file iKinSlv.h.

◆ timeout_detected

bool iCub::iKin::CartesianSolver::timeout_detected

protected

Definition at line 374 of file iKinSlv.h.

◆ token

double iCub::iKin::CartesianSolver::token

protected

Definition at line 378 of file iKinSlv.h.

◆ type

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

protected

Definition at line 364 of file iKinSlv.h.

◆ unctrlJointsNum

int iCub::iKin::CartesianSolver::unctrlJointsNum

protected

Definition at line 376 of file iKinSlv.h.

◆ unctrlJointsOld

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

protected

Definition at line 384 of file iKinSlv.h.

◆ verbosity

bool iCub::iKin::CartesianSolver::verbosity

protected

Definition at line 373 of file iKinSlv.h.

◆ w_2ndTask

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

protected

Definition at line 391 of file iKinSlv.h.

◆ w_3rdTask

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

protected

Definition at line 394 of file iKinSlv.h.

◆ xd_2ndTask

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

protected

Definition at line 390 of file iKinSlv.h.

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

icub-main/src/libraries/iKin/include/iCub/iKin/iKinSlv.h
icub-main/src/libraries/iKin/src/iKinSlv.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CartesianSolver()

◆ ~CartesianSolver()

Member Function Documentation

◆ afterStart()

◆ alignJointsBounds()

◆ changeDOF()

◆ close()

◆ countUncontrolledJoints()

◆ decodeDOF()

◆ encodeDOF()

◆ fillDOFInfo()

◆ getFeedback()

◆ getPartDesc()

◆ getTimeoutFlag()

◆ handleJointsRestPosition()

◆ handleJointsRestWeights()

◆ initPos()

◆ interrupt()

◆ isClosed()

◆ isNewDOF()

◆ latchUncontrolledJoints()

◆ lock()

◆ open()

◆ postDOFHandling()

◆ prepareJointsRestTask()

◆ printInfo()

◆ respond()

◆ resume()

◆ run()

◆ send()

◆ setLimits()

◆ solve()

◆ suspend()

◆ threadInit()

◆ threadRelease()

◆ unlock()

◆ waitDOFHandling()

◆ waitPart()

Friends And Related Function Documentation

◆ InputPort

◆ RpcProcessor

◆ SolverCallback

Member Data Documentation

◆ clb

◆ closed

◆ closing

◆ cmdProcessor

◆ configured

◆ contModeOld

◆ ctrlPose

◆ cv_dofEvent

◆ dof

◆ drv

◆ enc

◆ fullDOF

◆ hwLimits

◆ idx_3rdTask

◆ inPort

◆ interrupting

◆ jnt

◆ lim

◆ maxPartJoints

◆ mtx

◆ mtx_dofEvent

◆ outPort

◆ period

◆ ping_robot_tmo

◆ prt

◆ pToken

◆ qd_3rdTask

◆ restJntPos

◆ restWeights

◆ rmp