The BasicCartesianControl class implements ICartesianControl.

#include <BasicCartesianControl.hpp>

Inheritance diagram for BasicCartesianControl:

Classes
class	StateWatcher

Public Member Functions
yarp::dev::ReturnValue	getState (roboticslab::ICartesianControl::ControllerState &state) override
	Current state and position.

yarp::dev::ReturnValue	solvePose (const std::vector< double > &xd, std::vector< double > &q) override
	Inverse kinematics.

yarp::dev::ReturnValue	moveJoint (const std::vector< double > &xd) override
	Move in joint space.

yarp::dev::ReturnValue	moveLinear (const std::vector< double > &xd) override
	Linear move to target position.

yarp::dev::ReturnValue	moveVelocity (const std::vector< double > &xdotd) override
	Linear move with given velocity.

yarp::dev::ReturnValue	gravityCompensation () override
	Gravity compensation.

yarp::dev::ReturnValue	forceControl (const std::vector< double > &fd) override
	Force control.

yarp::dev::ReturnValue	stopControl () override
	Stop control.

yarp::dev::ReturnValue	changeTool (const std::vector< double > &x) override
	Change tool.

yarp::dev::ReturnValue	actuateTool (roboticslab::ICartesianControl::Actuator command) override
	Actuate tool.

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

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

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

yarp::dev::ReturnValue	setParameter (roboticslab::ICartesianControl::Config vocab, double value) override
	Set a configuration parameter.

yarp::dev::ReturnValue	getParameter (roboticslab::ICartesianControl::Config vocab, double *value) override
	Retrieve a configuration parameter.

yarp::dev::ReturnValue	setParameters (const std::map< roboticslab::ICartesianControl::Config, double > &params) override

yarp::dev::ReturnValue	getParameters (std::map< roboticslab::ICartesianControl::Config, double > &params) override

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 bool	stat (std::vector< double > &x, int state=nullptr, double timestamp=nullptr)

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

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

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

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

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

virtual bool	gcmp ()

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

virtual bool	wait (double timeout=0.0)

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

virtual bool	act (int command)

virtual bool	setParameter (int vocab, double value)

virtual bool	getParameter (int vocab, double *value)

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

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

virtual yarp::dev::ReturnValue	setParameters (const std::map< Config, double > &params)=0
	Set multiple configuration parameters.

virtual yarp::dev::ReturnValue	getParameters (std::map< Config, double > &params)=0
	Retrieve multiple configuration parameters.

Public Member Functions inherited from BasicCartesianControl_ParamsParser
bool	parseParams (const yarp::os::Searchable &config) override

std::string	getDeviceClassName () const override

std::string	getDeviceName () const override

std::string	getDocumentationOfDeviceParams () const override

std::vector< std::string >	getListOfParams () const override

bool	getParamValue (const std::string &paramName, std::string &paramValue) const override

std::string	getConfiguration () const override

Private Member Functions
double	getTimestamp ()

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 (roboticslab::ICartesianControl::Streaming 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	handleMovelVel (const std::vector< double > &q, const StateWatcher &watcher)

void	handleMovelPosd (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

roboticslab::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}

roboticslab::ICartesianSolver::Frame	referenceFrame

int	numJoints {0}

std::atomic< roboticslab::ICartesianControl::Mode >	currentMode {roboticslab::ICartesianControl::Mode::NONE}

roboticslab::ICartesianControl::Streaming	streamingCommand {roboticslab::ICartesianControl::Streaming::POSE}

std::vector< double >	vmoStored

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

std::vector< double >	fd

int	encoderErrors {0}

double	trajectoryStartTime {0.0}

std::atomic< double >	maxTrajectoryDuration {0.0}

std::atomic< bool >	cmcSuccess {true}

std::atomic< float >	cmcProgress {1.0f}

std::vector< double >	qMin

std::vector< double >	qMax

std::vector< double >	qdotMin

std::vector< double >	qdotMax

std::vector< double >	qRefSpeeds

Additional Inherited Members
Public Types inherited from roboticslab::ICartesianControl
enum class	Vocabs { OK = yarp::os::createVocab32('o','k') , FAILED = yarp::os::createVocab32('f','a','i','l') , SET = yarp::os::createVocab32('s','e','t') , GET = yarp::os::createVocab32('g','e','t') , NOT_SET = yarp::os::createVocab32('n','s','e','t') }
	General-purpose vocabs. More...

enum class	RPC { STATE = yarp::os::createVocab32('s','t','a','t') , INV = yarp::os::createVocab32('i','n','v') , MOVEJ = yarp::os::createVocab32('m','o','v','j') , MOVEL = yarp::os::createVocab32('m','o','v','l') , MOVEV = yarp::os::createVocab32('m','o','v','v') , GCMP = yarp::os::createVocab32('g','c','m','p') , FORCE = yarp::os::createVocab32('f','o','r','c') , STOP = yarp::os::createVocab32('s','t','o','p') , TOOL = yarp::os::createVocab32('t','o','o','l') , ACT = yarp::os::createVocab32('a','c','t') }
	RPC vocabs. More...

enum class	Streaming { POSE = yarp::os::createVocab32('p','o','s','e') , TWIST = yarp::os::createVocab32('t','w','s','t') , WRENCH = yarp::os::createVocab32('w','r','n','c') }
	Streaming vocabs. More...

enum class	Mode { NONE = yarp::os::createVocab32('n','c','t','l') , MOVEJ = yarp::os::createVocab32('m','o','v','j') , MOVEL = yarp::os::createVocab32('m','o','v','l') , MOVEV = yarp::os::createVocab32('m','o','v','v') , GCMP = yarp::os::createVocab32('g','c','m','p') , FORCE = yarp::os::createVocab32('f','o','r','c') }
	Controller mode vocabs. More...

enum class	Actuator { NONE = yarp::os::createVocab32('a','c','n') , CLOSE = yarp::os::createVocab32('a','c','c','g') , OPEN = yarp::os::createVocab32('a','c','o','g') , STOP = yarp::os::createVocab32('a','c','s','g') , GENERIC = yarp::os::createVocab32('a','c','g') }
	Actuator control vocabs. More...

enum class	Config { GAIN = yarp::os::createVocab32('c','p','c','g') , TRAJ_DURATION = yarp::os::createVocab32('c','p','t','d') , TRAJ_REF_SPD = yarp::os::createVocab32('c','p','t','s') , TRAJ_REF_ACC = yarp::os::createVocab32('c','p','t','a') , CMC_PERIOD = yarp::os::createVocab32('c','p','c','p') , FRAME = yarp::os::createVocab32('c','p','f') , STREAMING_CMD = yarp::os::createVocab32('c','p','s','c') }
	Controller configuration vocabs. More...

Public Attributes inherited from BasicCartesianControl_ParamsParser
const std::string	m_device_classname = {"BasicCartesianControl"}

const std::string	m_device_name = {"BasicCartesianControl"}

bool	m_parser_is_strict = false

const parser_version_type	m_parser_version = {}

std::string	m_provided_configuration

const std::string	m_controllerGain_defaultValue = {"0.05"}

const std::string	m_trajectoryDuration_defaultValue = {"0.0"}

const std::string	m_trajectoryRefSpeed_defaultValue = {"0.05"}

const std::string	m_trajectoryRefAccel_defaultValue = {"0.02"}

const std::string	m_cmcPeriodMs_defaultValue = {"50"}

const std::string	m_usePosdMovel_defaultValue = {"false"}

const std::string	m_enableFailFast_defaultValue = {"false"}

const std::string	m_referenceFrame_defaultValue = {"base"}

const std::string	m_robot_defaultValue = {"remote_controlboard"}

const std::string	m_solver_defaultValue = {"KdlSolver"}

double	m_controllerGain = {0.05}

double	m_trajectoryDuration = {0.0}

double	m_trajectoryRefSpeed = {0.05}

double	m_trajectoryRefAccel = {0.02}

int	m_cmcPeriodMs = {50}

bool	m_usePosdMovel = {false}

bool	m_enableFailFast = {false}

std::string	m_referenceFrame = {"base"}

std::string	m_robot = {"remote_controlboard"}

std::string	m_solver = {"KdlSolver"}

Member Function Documentation

◆ actuateTool()

yarp::dev::ReturnValue BasicCartesianControl::actuateTool ( roboticslab::ICartesianControl::Actuator command )

overridevirtual

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

Parameters

command One of the available ICartesianControl::Actuator vocabs.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ changeTool()

yarp::dev::ReturnValue BasicCartesianControl::changeTool ( 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.

◆ forceControl()

yarp::dev::ReturnValue BasicCartesianControl::forceControl ( 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.

◆ getParameter()

yarp::dev::ReturnValue BasicCartesianControl::getParameter	(	roboticslab::ICartesianControl::Config	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.

◆ getState()

yarp::dev::ReturnValue BasicCartesianControl::getState ( roboticslab::ICartesianControl::ControllerState & state )

overridevirtual

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

Parameters

state Controller state data.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ gravityCompensation()

yarp::dev::ReturnValue BasicCartesianControl::gravityCompensation ( )

overridevirtual

Enable gravity compensation.

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ moveJoint()

yarp::dev::ReturnValue BasicCartesianControl::moveJoint ( 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).

Returns: true on success, false otherwise

Implements roboticslab::ICartesianControl.

◆ moveLinear()

yarp::dev::ReturnValue BasicCartesianControl::moveLinear ( 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.

◆ moveVelocity()

yarp::dev::ReturnValue BasicCartesianControl::moveVelocity ( 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.

◆ setParameter()

yarp::dev::ReturnValue BasicCartesianControl::setParameter	(	roboticslab::ICartesianControl::Config	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.

◆ solvePose()

yarp::dev::ReturnValue BasicCartesianControl::solvePose	(	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.

◆ stopControl()

yarp::dev::ReturnValue BasicCartesianControl::stopControl ( )

overridevirtual

Halt current control loop if any and cease movement.

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.

◆ 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> BasicCartesianControl::fd

private

FORCE desired Cartesian force

◆ trajectories

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

private

MOVEL store Cartesian trajectory

◆ vmoStored

std::vector<double> BasicCartesianControl::vmoStored

private

MOVEJ 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

Additional Inherited Members

Member Function Documentation

◆ actuateTool()

◆ changeTool()

◆ forceControl()

◆ getParameter()

◆ getState()

◆ gravityCompensation()

◆ moveJoint()

◆ moveLinear()

◆ moveVelocity()

◆ pose()

◆ setParameter()

◆ solvePose()

◆ stopControl()

◆ twist()

◆ wrench()

Member Data Documentation

◆ fd

◆ trajectories

◆ vmoStored