A class that inherits from ActionPrimitivesLayer1 and integrates the force-torque sensing in order to stop the limb while reaching as soon as a contact with external objects is detected. More...

#include <actionPrimitives.h>

Inheritance diagram for iCub::action::ActionPrimitivesLayer2:

Public Member Functions
	ActionPrimitivesLayer2 ()
	Default Constructor.
	ActionPrimitivesLayer2 (yarp::os::Property &opt)
	Constructor.
virtual	~ActionPrimitivesLayer2 ()
	Destructor.
virtual bool	open (yarp::os::Property &opt)
	Configure the object.
virtual bool	isValid () const
	Check if the object is initialized correctly.
virtual void	close ()
	Deallocate the object.
virtual bool	grasp (const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d1, const yarp::sig::Vector &d2)
	More evolute version of grasp.
virtual bool	grasp (const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
	The usual grasp is still available.
virtual bool	touch (const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
	More evolute version of touch, exploiting contact detection.
virtual bool	getExtWrench (yarp::sig::Vector &wrench) const
	Retrieve the current wrench on the end-effector.
virtual bool	getExtForceThres (double &thres) const
	Retrieve the current threshold on the external force used to stop the limb while reaching.
virtual bool	setExtForceThres (const double thres)
	Set the threshold on the external force used to stop the limb while reaching.
virtual bool	enableContactDetection ()
	Self-explaining :).
virtual bool	disableContactDetection ()
	Self-explaining :).
virtual bool	isContactDetectionEnabled (bool &f) const
	Self-explaining :).
virtual bool	checkContact (bool &f) const
	Check whether the reaching has been stopped due to a contact with external objects.
Public Member Functions inherited from iCub::action::ActionPrimitivesLayer1
	ActionPrimitivesLayer1 ()
	Default Constructor.
	ActionPrimitivesLayer1 (yarp::os::Property &opt)
	Constructor.
virtual bool	tap (const yarp::sig::Vector &x1, const yarp::sig::Vector &o1, const yarp::sig::Vector &x2, const yarp::sig::Vector &o2, const double execTime=ACTIONPRIM_DISABLE_EXECTIME)
	Tap the given target (combined action).
Public Member Functions inherited from iCub::action::ActionPrimitives
	ActionPrimitives ()
	Default Constructor.
	ActionPrimitives (yarp::os::Property &opt)
	Constructor.
virtual	~ActionPrimitives ()
	Destructor.
virtual bool	pushAction (const yarp::sig::Vector &x, const yarp::sig::Vector &o, const std::string &handSeqKey, const double execTime=ACTIONPRIM_DISABLE_EXECTIME, ActionPrimitivesCallback *clb=NULL)
	Insert a combination of arm and hand primitive actions in the actions queue.
virtual bool	pushAction (const yarp::sig::Vector &x, const std::string &handSeqKey, const double execTime=ACTIONPRIM_DISABLE_EXECTIME, ActionPrimitivesCallback *clb=NULL)
	Insert a combination of arm and hand primitive actions in the actions queue.
virtual bool	pushAction (const yarp::sig::Vector &x, const yarp::sig::Vector &o, const double execTime=ACTIONPRIM_DISABLE_EXECTIME, ActionPrimitivesCallback *clb=NULL)
	Insert the arm-primitive action reach for target in the actions queue.
virtual bool	pushAction (const yarp::sig::Vector &x, const double execTime=ACTIONPRIM_DISABLE_EXECTIME, ActionPrimitivesCallback *clb=NULL)
	Insert the arm-primitive action reach for target in the actions queue.
virtual bool	pushAction (const std::string &handSeqKey, ActionPrimitivesCallback *clb=NULL)
	Insert a hand-primitive action in the actions queue.
virtual bool	pushAction (const std::deque< ActionPrimitivesWayPoint > &wayPoints, ActionPrimitivesCallback *clb=NULL)
	Insert in the actions queue a trajectory in the operational space parametrized in terms of waypoints.
virtual bool	pushAction (const std::deque< ActionPrimitivesWayPoint > &wayPoints, const std::string &handSeqKey, ActionPrimitivesCallback *clb=NULL)
	Insert in the actions queue a combination of hand and arm trajectory in the operational space parametrized in terms of waypoints.
virtual bool	pushWaitState (const double tmo, ActionPrimitivesCallback *clb=NULL)
	Insert a wait state in the actions queue.
virtual bool	reachPose (const yarp::sig::Vector &x, const yarp::sig::Vector &o, const double execTime=ACTIONPRIM_DISABLE_EXECTIME)
	Immediately update the current reaching target (without affecting the actions queue) or initiate a new reach (if the actions queue is empty).
virtual bool	reachPosition (const yarp::sig::Vector &x, const double execTime=ACTIONPRIM_DISABLE_EXECTIME)
	Immediately update the current reaching target (without affecting the actions queue) or initiate a new reach (if the actions queue is empty).
virtual bool	clearActionsQueue ()
	Empty the actions queue.
virtual bool	lockActions ()
	Disable the possibility to yield any new action.
virtual bool	unlockActions ()
	Enable the possibility to yield new actions.
virtual bool	getActionsLockStatus () const
	Return the actions lock status.
virtual bool	addHandSeqWP (const std::string &handSeqKey, const yarp::sig::Vector &poss, const yarp::sig::Vector &vels, const yarp::sig::Vector &tols, const yarp::sig::Vector &thres, const double tmo)
	Define an hand WayPoint (WP) to be added at the bottom of the hand motion sequence pointed by the key.
virtual bool	addHandSequence (const std::string &handSeqKey, const yarp::os::Bottle &sequence)
	Define an hand motion sequence from a configuration bottle.
virtual bool	isValidHandSeq (const std::string &handSeqKey)
	Check whether a sequence key is defined.
virtual bool	removeHandSeq (const std::string &handSeqKey)
	Remove an already existing hand motion sequence.
std::deque< std::string >	getHandSeqList ()
	Return the list of available hand sequence keys.
virtual bool	getHandSequence (const std::string &handSeqKey, yarp::os::Bottle &sequence)
	Return a hand sequence.
virtual bool	areFingersMoving (bool &f)
	Query if fingers are moving.
virtual bool	areFingersInPosition (bool &f)
	Query if fingers are in position (cumulative response).
virtual bool	areFingersInPosition (std::deque< bool > &f)
	Query if fingers are in position (finger-wise response).
virtual bool	getGraspModel (perception::Model *&model) const
	Return the model used internally to detect external contacts.
virtual bool	getCartesianIF (yarp::dev::ICartesianControl *&ctrl) const
	Return the cartesian interface used internally to control the limb.
virtual bool	getTorsoJoints (yarp::sig::Vector &torso)
	Return the control status of torso joints.
virtual bool	setTorsoJoints (const yarp::sig::Vector &torso)
	Change the control status of torso joints.
virtual bool	getPose (yarp::sig::Vector &x, yarp::sig::Vector &o) const
	Get the current arm pose.
virtual bool	stopControl ()
	Stop any ongoing arm/hand movements.
virtual bool	setDefaultExecTime (const double execTime)
	Set the default arm movement execution time.
virtual double	getDefaultExecTime () const
	Get the current default arm movement execution time.
virtual bool	setTrackingMode (const bool f)
	Set the task space controller in tracking or non-tracking mode.
virtual bool	getTrackingMode () const
	Get the current controller mode.
virtual bool	enableArmWaving (const yarp::sig::Vector &restPos)
	Enable the waving mode that keeps on moving the arm around a predefined position.
virtual bool	disableArmWaving ()
	Disable the waving mode.
virtual bool	enableReachingTimeout (const double tmo)
	Enable timeout while reaching.
virtual bool	disableReachingTimeout ()
	Disable timeout while reaching.
virtual bool	checkActionsDone (bool &f, const bool sync=false)
	Check whether all the actions in queue are accomplished.
virtual bool	checkActionOnGoing (bool &f, const bool sync=false)
	Check whether an action is still ongoing.
virtual bool	syncCheckInterrupt (const bool disable=false)
	Suddenly interrupt any blocking call that is pending on querying the action status.
virtual bool	syncCheckReinstate ()
	Reinstate the blocking feature for future calls with sync switch on.

Protected Member Functions
virtual void	init ()
virtual void	postReachCallback ()
virtual void	run ()
Protected Member Functions inherited from iCub::action::ActionPrimitives
virtual void	printMessage (const int logtype, const char *format,...) const
virtual bool	handleTorsoDOF (yarp::os::Property &opt, const std::string &key)
virtual void	disableTorsoDof ()
virtual void	enableTorsoDof ()
virtual bool	configHandSeq (yarp::os::Property &opt)
virtual bool	configGraspModel (yarp::os::Property &opt)
virtual bool	_pushAction (const bool execArm, const yarp::sig::Vector &x, const yarp::sig::Vector &o, const double execTime, const bool oEnabled, const bool execHand, const HandWayPoint &handWP, const bool handSeqTerminator, ActionPrimitivesCallback *clb)
virtual bool	_pushAction (const yarp::sig::Vector &x, const yarp::sig::Vector &o, const std::string &handSeqKey, const double execTime, ActionPrimitivesCallback *clb, const bool oEnabled)
virtual bool	handCheckMotionDone (const int jnt)
virtual bool	wait (const Action &action)
virtual bool	cmdArm (const Action &action)
virtual bool	cmdArmWP (const Action &action)
virtual bool	cmdHand (const Action &action)
virtual bool	isHandSeqEnded ()
virtual bool	execQueuedAction ()
virtual bool	execPendingHandSequences ()

Protected Attributes
bool	skipFatherPart
bool	configuredLayer2
bool	contactDetectionOn
bool	contactDetected
double	ext_force_thres
liftAndGraspCallback *	execLiftAndGrasp
touchCallback *	execTouch
yarp::os::BufferedPort< yarp::sig::Vector >	wbdynPortIn
yarp::sig::Vector	wrenchExternal
yarp::sig::Vector	grasp_d2
yarp::sig::Vector	grasp_o
yarp::sig::Vector	encDataTorso
yarp::sig::Vector	encDataArm
Protected Attributes inherited from iCub::action::ActionPrimitives
std::string	robot
std::string	local
std::string	part
yarp::dev::PolyDriver	polyHand
yarp::dev::PolyDriver	polyCart
yarp::dev::IControlMode *	modCtrl
yarp::dev::IEncoders *	encCtrl
yarp::dev::IPositionControl *	posCtrl
yarp::dev::ICartesianControl *	cartCtrl
perception::Model *	graspModel
yarp::os::PeriodicThread *	armWaver
std::mutex	mtx
std::mutex	mtx_motionStartEvent
std::condition_variable	cv_motionStartEvent
std::mutex	mtx_motionDoneEvent
std::condition_variable	cv_motionDoneEvent
bool	armMoveDone
bool	handMoveDone
bool	latchArmMoveDone
bool	latchHandMoveDone
bool	handSeqTerminator
bool	fingersInPosition
std::deque< bool >	fingerInPosition
bool	configured
bool	closed
bool	checkEnabled
bool	tracking_mode
bool	torsoActive
bool	reachTmoEnabled
bool	locked
bool	verbose
double	default_exec_time
double	waitTmo
double	reachTmo
double	latchTimerWait
double	latchTimerReach
double	latchTimerReachLog
int	jHandMin
int	jHandMax
yarp::sig::Vector	enableTorsoSw
yarp::sig::Vector	disableTorsoSw
yarp::sig::Vector	curHandFinalPoss
yarp::sig::Vector	curHandTols
yarp::sig::Vector	curGraspDetectionThres
double	curHandTmo
double	latchTimerHand
std::vector< int >	fingersJnts
std::set< int >	fingersJntsSet
std::set< int >	fingersMovingJntsSet
std::multimap< int, int >	fingers2JntsMap
ActionPrimitivesCallback *	actionClb
yarp::os::PeriodicThread *	actionWP
iCub::action::ActionPrimitives::ActionsQueue	actionsQueue
std::map< std::string, std::deque< HandWayPoint > >	handSeqMap

Friends
class	liftAndGraspCallback
class	touchCallback

Detailed Description

A class that inherits from ActionPrimitivesLayer1 and integrates the force-torque sensing in order to stop the limb while reaching as soon as a contact with external objects is detected.

The module wholeBodyDynamics - in charge of computing the robot dynamics - must be running.

Definition at line 1069 of file actionPrimitives.h.

Constructor & Destructor Documentation

◆ ActionPrimitivesLayer2() [1/2]

ActionPrimitivesLayer2::ActionPrimitivesLayer2 ( )

Default Constructor.

Definition at line 2155 of file actionPrimitives.cpp.

◆ ActionPrimitivesLayer2() [2/2]

iCub::action::ActionPrimitivesLayer2::ActionPrimitivesLayer2 ( yarp::os::Property & opt )

Constructor.

Parameters

opt	the Property used to configure the object after its creation.

◆ ~ActionPrimitivesLayer2()

ActionPrimitivesLayer2::~ActionPrimitivesLayer2 ( )

virtual

Destructor.

Note: it calls the close() method.

Definition at line 2465 of file actionPrimitives.cpp.

Member Function Documentation

◆ checkContact()

bool ActionPrimitivesLayer2::checkContact ( bool & f ) const

virtual

Check whether the reaching has been stopped due to a contact with external objects.

Parameters

f	the result of the check.

Returns: true/false on success/fail.

Definition at line 2452 of file actionPrimitives.cpp.

◆ close()

void ActionPrimitivesLayer2::close ( )

virtual

Deallocate the object.

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2292 of file actionPrimitives.cpp.

◆ disableContactDetection()

bool ActionPrimitivesLayer2::disableContactDetection ( )

virtual

Self-explaining :).

Returns: true/false on success/fail.

Definition at line 2426 of file actionPrimitives.cpp.

◆ enableContactDetection()

bool ActionPrimitivesLayer2::enableContactDetection ( )

virtual

Self-explaining :).

Returns: true/false on success/fail.

Definition at line 2413 of file actionPrimitives.cpp.

◆ getExtForceThres()

bool ActionPrimitivesLayer2::getExtForceThres ( double & thres ) const

virtual

Retrieve the current threshold on the external force used to stop the limb while reaching.

Parameters

thres where to return the threshold.

Returns: true/false on success/fail.

Definition at line 2387 of file actionPrimitives.cpp.

◆ getExtWrench()

bool ActionPrimitivesLayer2::getExtWrench ( yarp::sig::Vector & wrench ) const

virtual

Retrieve the current wrench on the end-effector.

Parameters

wrench a vector containing the external forces/moments acting on the end-effector.

Returns: true/false on success/fail.

Definition at line 2374 of file actionPrimitives.cpp.

◆ grasp() [1/2]

virtual bool iCub::action::ActionPrimitivesLayer2::grasp	(	const yarp::sig::Vector &	x,
		const yarp::sig::Vector &	o,
		const yarp::sig::Vector &	d )

virtual

The usual grasp is still available.

Parameters

x	the 3-d target position [m].
o	the 4-d hand orientation used while reaching/grasping (given in axis-angle representation: ax ay az angle in rad).
d	the displacement [m] wrt the target position that identifies a location to be reached prior to grasping.

Returns: true/false on success/fail.

Reimplemented from iCub::action::ActionPrimitivesLayer1.

◆ grasp() [2/2]

virtual bool iCub::action::ActionPrimitivesLayer2::grasp	(	const yarp::sig::Vector &	x,
		const yarp::sig::Vector &	o,
		const yarp::sig::Vector &	d1,
		const yarp::sig::Vector &	d2 )

virtual

More evolute version of grasp.

It exploits the contact detection in order to lift up a bit the hand prior to grasping (this happens only after contact).

Parameters

x	the 3-d target position [m].
o	the 4-d hand orientation used while reaching/grasping (given in axis-angle representation: ax ay az angle in rad).
d1	the displacement [m] wrt the target position that identifies a location to be reached prior to grasping.
d2	the displacement [m] that identifies the amount of the lift after the contact.

Returns: true/false on success/fail.

◆ init()

void ActionPrimitivesLayer2::init ( )

protectedvirtual

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2172 of file actionPrimitives.cpp.

◆ isContactDetectionEnabled()

bool ActionPrimitivesLayer2::isContactDetectionEnabled ( bool & f ) const

virtual

Self-explaining :).

Parameters

f	the result of the check.

Returns: true/false on success/fail.

Definition at line 2439 of file actionPrimitives.cpp.

◆ isValid()

bool ActionPrimitivesLayer2::isValid ( ) const

virtual

Check if the object is initialized correctly.

Returns: true/fail on success/fail.

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2285 of file actionPrimitives.cpp.

◆ open()

bool ActionPrimitivesLayer2::open ( yarp::os::Property & opt )

virtual

Configure the object.

Parameters

opt	the Property used to configure the object after its creation.

Note: To be called after object creation.

Further available options are:

ext_force_thres <double>: specify the maximum external force magnitude applied to the end-effector in order to detect contact between end-effector and objects while reaching.

wbdyn_stem_name <string>: specify the stem-name of the wholeBodyDynamics module.

wbdyn_port_name <string>: specify the tag-name of the port used by wholeBodyDynamics to stream out the wrench at the end-effector.

Note: A port called /<local>/<part>/wbdyn:i is open to acquire data provided by wholeBodyDynamics. The port is automatically connected to /<wbdyn_stem_name>/<part>/<wbdyn_port_name>.

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2243 of file actionPrimitives.cpp.

◆ postReachCallback()

void ActionPrimitivesLayer2::postReachCallback ( )

protectedvirtual

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2185 of file actionPrimitives.cpp.

◆ run()

void ActionPrimitivesLayer2::run ( )

protectedvirtual

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2196 of file actionPrimitives.cpp.

◆ setExtForceThres()

bool ActionPrimitivesLayer2::setExtForceThres ( const double thres )

virtual

Set the threshold on the external force used to stop the limb while reaching.

Parameters

thres the new threshold.

Returns: true/false on success/fail.

Definition at line 2400 of file actionPrimitives.cpp.

◆ touch()

bool ActionPrimitivesLayer2::touch	(	const yarp::sig::Vector &	x,
		const yarp::sig::Vector &	o,
		const yarp::sig::Vector &	d )

virtual

More evolute version of touch, exploiting contact detection.

Parameters

x	the 3-d target position [m].
o	the 4-d hand orientation used while reaching/touching (given in axis-angle representation: ax ay az angle in rad).
d	the displacement [m] wrt the target position that identifies a location to be reached prior to touching.

Returns: true/false on success/fail.

Reimplemented from iCub::action::ActionPrimitivesLayer1.

Definition at line 2355 of file actionPrimitives.cpp.

◆ liftAndGraspCallback

friend class liftAndGraspCallback

friend

Definition at line 1092 of file actionPrimitives.h.

◆ touchCallback

friend class touchCallback

friend

Definition at line 1093 of file actionPrimitives.h.

Member Data Documentation

◆ configuredLayer2

bool iCub::action::ActionPrimitivesLayer2::configuredLayer2

protected

Definition at line 1073 of file actionPrimitives.h.

◆ contactDetected

bool iCub::action::ActionPrimitivesLayer2::contactDetected

protected

Definition at line 1075 of file actionPrimitives.h.

◆ contactDetectionOn

bool iCub::action::ActionPrimitivesLayer2::contactDetectionOn

protected

Definition at line 1074 of file actionPrimitives.h.

◆ encDataArm

yarp::sig::Vector iCub::action::ActionPrimitivesLayer2::encDataArm

protected

Definition at line 1090 of file actionPrimitives.h.

◆ encDataTorso

yarp::sig::Vector iCub::action::ActionPrimitivesLayer2::encDataTorso

protected

Definition at line 1089 of file actionPrimitives.h.

◆ execLiftAndGrasp

liftAndGraspCallback* iCub::action::ActionPrimitivesLayer2::execLiftAndGrasp

protected

Definition at line 1079 of file actionPrimitives.h.

◆ execTouch

touchCallback* iCub::action::ActionPrimitivesLayer2::execTouch

protected

Definition at line 1080 of file actionPrimitives.h.

◆ ext_force_thres

double iCub::action::ActionPrimitivesLayer2::ext_force_thres

protected

Definition at line 1077 of file actionPrimitives.h.

◆ grasp_d2

yarp::sig::Vector iCub::action::ActionPrimitivesLayer2::grasp_d2

protected

Definition at line 1086 of file actionPrimitives.h.

◆ grasp_o

yarp::sig::Vector iCub::action::ActionPrimitivesLayer2::grasp_o

protected

Definition at line 1087 of file actionPrimitives.h.

◆ skipFatherPart

bool iCub::action::ActionPrimitivesLayer2::skipFatherPart

protected

Definition at line 1072 of file actionPrimitives.h.

◆ wbdynPortIn

yarp::os::BufferedPort<yarp::sig::Vector> iCub::action::ActionPrimitivesLayer2::wbdynPortIn

protected

Definition at line 1082 of file actionPrimitives.h.

◆ wrenchExternal

yarp::sig::Vector iCub::action::ActionPrimitivesLayer2::wrenchExternal

protected

Definition at line 1084 of file actionPrimitives.h.

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

icub-main/src/libraries/actionPrimitives/include/iCub/action/actionPrimitives.h
icub-main/src/libraries/actionPrimitives/src/actionPrimitives.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ ActionPrimitivesLayer2() [1/2]

◆ ActionPrimitivesLayer2() [2/2]

◆ ~ActionPrimitivesLayer2()

Member Function Documentation

◆ checkContact()

◆ close()

◆ disableContactDetection()

◆ enableContactDetection()

◆ getExtForceThres()

◆ getExtWrench()

◆ grasp() [1/2]

◆ grasp() [2/2]

◆ init()

◆ isContactDetectionEnabled()

◆ isValid()

◆ open()

◆ postReachCallback()

◆ run()

◆ setExtForceThres()

◆ touch()

Friends And Related Symbol Documentation

◆ liftAndGraspCallback

◆ touchCallback

Member Data Documentation

◆ configuredLayer2

◆ contactDetected

◆ contactDetectionOn

◆ encDataArm

◆ encDataTorso

◆ execLiftAndGrasp

◆ execTouch

◆ ext_force_thres

◆ grasp_d2

◆ grasp_o

◆ skipFatherPart

◆ wbdynPortIn

◆ wrenchExternal