The BasicCartesianControl class implements ICartesianControl.

#include <BasicCartesianControl.hpp>

Inheritance diagram for roboticslab::BasicCartesianControl:

Classes
class	StateWatcher

Public Member Functions
bool	stat (std::vector< double > &x, int state=nullptr, double timestamp=nullptr) override
	Current state and position. More...

bool	inv (const std::vector< double > &xd, std::vector< double > &q) override
	Inverse kinematics. More...

bool	movj (const std::vector< double > &xd) override
	Move in joint space, absolute coordinates. More...

bool	relj (const std::vector< double > &xd) override
	Move in joint space, relative coordinates. More...

bool	movl (const std::vector< double > &xd) override
	Linear move to target position. More...

bool	movv (const std::vector< double > &xdotd) override
	Linear move with given velocity. More...

bool	gcmp () override
	Gravity compensation. More...

bool	forc (const std::vector< double > &fd) override
	Force control. More...

bool	stopControl () override
	Stop control. More...

bool	wait (double timeout) override
	Wait until completion. More...

bool	tool (const std::vector< double > &x) override
	Change tool. More...

bool	act (int command) override
	Actuate tool. More...

void	pose (const std::vector< double > &x) override
	Achieve pose. More...

void	twist (const std::vector< double > &xdot) override
	Instantaneous velocity steps. More...

void	wrench (const std::vector< double > &w) override
	Exert force. More...

bool	setParameter (int vocab, double value) override
	Set a configuration parameter. More...

bool	getParameter (int vocab, double *value) override
	Retrieve a configuration parameter. More...

bool	setParameters (const std::map< int, double > &params) override
	Set multiple configuration parameters. More...

bool	getParameters (std::map< int, double > &params) override
	Retrieve multiple configuration parameters. More...

void	run () override

bool	open (yarp::os::Searchable &config) override

bool	close () override

Public Member Functions inherited from roboticslab::ICartesianControl
virtual	~ICartesianControl ()=default
	Destructor.

virtual void	movi (const std::vector< double > &x)

Private Member Functions
int	getCurrentState () const

void	setCurrentState (int value)

bool	checkJointLimits (const std::vector< double > &q)

bool	checkJointLimits (const std::vector< double > &q, const std::vector< double > &qdot)

bool	checkJointVelocities (const std::vector< double > &qdot)

bool	doFailFastChecks (const std::vector< double > &initialQ)

bool	checkControlModes (int mode)

bool	setControlModes (int mode)

bool	presetStreamingCommand (int command)

bool	computeIsocronousSpeeds (const std::vector< double > &q, const std::vector< double > &qd, std::vector< double > &qdot)

void	handleMovj (const std::vector< double > &q, const StateWatcher &watcher)

void	handleMovlVel (const std::vector< double > &q, const StateWatcher &watcher)

void	handleMovlPosd (const std::vector< double > &q, const StateWatcher &watcher)

void	handleMovv (const std::vector< double > &q, const StateWatcher &watcher)

void	handleGcmp (const std::vector< double > &q, const StateWatcher &watcher)

void	handleForc (const std::vector< double > &q, const std::vector< double > &qdot, const std::vector< double > &qdotdot, const StateWatcher &watcher)

Private Attributes
yarp::dev::PolyDriver	solverDevice

ICartesianSolver *	iCartesianSolver {nullptr}

yarp::dev::PolyDriver	robotDevice

yarp::dev::IControlMode *	iControlMode {nullptr}

yarp::dev::IEncoders *	iEncoders {nullptr}

yarp::dev::IPositionControl *	iPositionControl {nullptr}

yarp::dev::IPositionDirect *	iPositionDirect {nullptr}

yarp::dev::IPreciselyTimed *	iPreciselyTimed {nullptr}

yarp::dev::ITorqueControl *	iTorqueControl {nullptr}

yarp::dev::IVelocityControl *	iVelocityControl {nullptr}

ICartesianSolver::reference_frame	referenceFrame

double	gain

double	duration

int	cmcPeriodMs

int	waitPeriodMs

int	numJoints

int	currentState

int	streamingCommand

bool	usePosdMovl

bool	enableFailFast

std::mutex	stateMutex

std::vector< double >	vmoStored

double	movementStartTime

std::vector< std::unique_ptr< KDL::Trajectory > >	trajectories

std::vector< double >	fd

int	encoderErrors {0}

std::atomic_bool	cmcSuccess

std::vector< double >	qMin

std::vector< double >	qMax

std::vector< double >	qdotMin

std::vector< double >	qdotMax

std::vector< double >	qRefSpeeds

Member Function Documentation

◆ act()

bool BasicCartesianControl::act ( int command )

overridevirtual

Send control command to actuate the robot's tool, if available.

Parameters

command One of available actuator vocabs.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ forc()

bool BasicCartesianControl::forc ( const std::vector< double > & fd )

overridevirtual

Apply desired forces in task space.

Parameters

fd	6-element vector describing desired force exerted by the TCP in cartesian space; first three elements denote linear force (Newton), last three denote torque (Newton*meters).

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ gcmp()

bool BasicCartesianControl::gcmp ( )

overridevirtual

Enable gravity compensation.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ getParameter()

bool BasicCartesianControl::getParameter	(	int	vocab,
		double *	value
	)

overridevirtual

Ask the controller to retrieve a parameter of 'double' type.

Parameters

vocab	YARP-encoded vocab (parameter key).
value	Parameter value encoded as a double.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ getParameters()

bool BasicCartesianControl::getParameters ( std::map< int, double > & params )

overridevirtual

Ask the controller to retrieve all available parameters at once.

Parameters

params Dictionary of YARP-encoded vocabs as keys and their values.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ inv()

bool BasicCartesianControl::inv	(	const std::vector< double > &	xd,
		std::vector< double > &	q
	)

overridevirtual

Perform inverse kinematics (using robot position as initial guess), but do not move.

Parameters

xd	6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
q	Vector describing current position in joint space (meters or degrees).

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ movj()

bool BasicCartesianControl::movj ( const std::vector< double > & xd )

overridevirtual

Perform inverse kinematics and move to desired position in joint space using absolute coordinates.

Parameters

xd	6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).

See also: relj (relative coordinates)

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ movl()

bool BasicCartesianControl::movl ( const std::vector< double > & xd )

overridevirtual

Move to end position along a line trajectory.

Parameters

xd	6-element vector describing desired position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ movv()

bool BasicCartesianControl::movv ( const std::vector< double > & xdotd )

overridevirtual

Move along a line with constant velocity.

Parameters

xdotd 6-element vector describing desired velocity in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ pose()

void BasicCartesianControl::pose ( const std::vector< double > & x )

overridevirtual

Move to desired position instantaneously, no further intermediate calculations are expected other than computing the inverse kinematics.

Parameters

x	6-element vector describing desired instantaneous pose in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).

Implements roboticslab::ICartesianControl.

◆ relj()

bool BasicCartesianControl::relj ( const std::vector< double > & xd )

overridevirtual

Perform inverse kinematics and move to desired position in joint space using relative coordinates.

Parameters

xd	6-element vector describing desired offset in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).

See also: movj (absolute coordinates)

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ setParameter()

bool BasicCartesianControl::setParameter	(	int	vocab,
		double	value
	)

overridevirtual

Ask the controller to store or update a parameter of 'double' type.

Parameters

vocab	YARP-encoded vocab (parameter key).
value	Parameter value encoded as a double.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ setParameters()

bool BasicCartesianControl::setParameters ( const std::map< int, double > & params )

overridevirtual

Ask the controller to store or update multiple parameters at once.

Parameters

params Dictionary of YARP-encoded vocabs as keys and their values.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ stat()

bool BasicCartesianControl::stat	(	std::vector< double > &	x,
		int *	state = `nullptr`,
		double *	timestamp = `nullptr`
	)

overridevirtual

Inform on control state, get robot position and perform forward kinematics.

Parameters

x	6-element vector describing current position in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).
state	Identifier for a cartesian control vocab.
timestamp	Remote encoder acquisition time.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ stopControl()

bool BasicCartesianControl::stopControl ( )

overridevirtual

Halt current control loop if any and cease movement.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ tool()

bool BasicCartesianControl::tool ( const std::vector< double > & x )

overridevirtual

Unload current tool if any and append new tool frame to the kinematic chain.

Parameters

x	6-element vector describing new tool tip with regard to current end-effector frame in cartesian space; first three elements denote translation (meters), last three denote rotation in scaled axis-angle representation (radians).

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ twist()

void BasicCartesianControl::twist ( const std::vector< double > & xdot )

overridevirtual

Move in instantaneous velocity increments.

Parameters

xdot	6-element vector describing velocity increments in cartesian space; first three elements denote translational velocity (meters/second), last three denote angular velocity (radians/second).

Implements roboticslab::ICartesianControl.

◆ wait()

bool BasicCartesianControl::wait ( double timeout )

overridevirtual

Block execution until the movement is completed, errors occur or timeout is reached.

Parameters

timeout Timeout in seconds, '0.0' means no timeout.

Returns: true on success, false if errors occurred during the execution of the trajectory

Implements roboticslab::ICartesianControl.

◆ wrench()

void BasicCartesianControl::wrench ( const std::vector< double > & w )

overridevirtual

Make the TCP exert the desired force instantaneously.

Parameters

w	6-element vector describing desired force exerted by the TCP in cartesian space; first three elements denote linear force (Newton), last three denote torque (Newton*meters).

Implements roboticslab::ICartesianControl.

Member Data Documentation

◆ fd

std::vector<double> roboticslab::BasicCartesianControl::fd

private

FORC desired Cartesian force

◆ movementStartTime

double roboticslab::BasicCartesianControl::movementStartTime

private

MOVL keep track of movement start time to know at what time of trajectory movement we are

◆ trajectories

std::vector<std::unique_ptr<KDL::Trajectory> > roboticslab::BasicCartesianControl::trajectories

private

MOVL store Cartesian trajectory

◆ vmoStored

std::vector<double> roboticslab::BasicCartesianControl::vmoStored

private

MOVJ store previous reference speeds

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

libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp
libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
libraries/YarpPlugins/BasicCartesianControl/PeriodicThreadImpl.cpp

Classes

Public Member Functions

Private Member Functions

Private Attributes

Member Function Documentation

◆ act()

◆ forc()

◆ gcmp()

◆ getParameter()

◆ getParameters()

◆ inv()

◆ movj()

◆ movl()

◆ movv()

◆ pose()

◆ relj()

◆ setParameter()

◆ setParameters()

◆ stat()

◆ stopControl()

◆ tool()

◆ twist()

◆ wait()

◆ wrench()

Member Data Documentation

◆ fd

◆ movementStartTime

◆ trajectories

◆ vmoStored