iCub-main
Classes | Public Member Functions | Protected Member Functions | Protected Attributes | Friends | List of all members
iCub::action::ActionPrimitives Class Reference

#include <actionPrimitives.h>

+ Inheritance diagram for iCub::action::ActionPrimitives:

Classes

struct  Action
 
class  ActionsQueue
 
struct  HandWayPoint
 

Public Member Functions

 ActionPrimitives ()
 Default Constructor. More...
 
 ActionPrimitives (yarp::os::Property &opt)
 Constructor. More...
 
virtual ~ActionPrimitives ()
 Destructor. More...
 
virtual bool open (yarp::os::Property &opt)
 Configure the object. More...
 
virtual bool isValid () const
 Check if the object is initialized correctly. More...
 
virtual void close ()
 Deallocate the object. More...
 
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. More...
 
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. More...
 
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. More...
 
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. More...
 
virtual bool pushAction (const std::string &handSeqKey, ActionPrimitivesCallback *clb=NULL)
 Insert a hand-primitive action in the actions queue. More...
 
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. More...
 
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. More...
 
virtual bool pushWaitState (const double tmo, ActionPrimitivesCallback *clb=NULL)
 Insert a wait state in the actions queue. More...
 
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). More...
 
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). More...
 
virtual bool clearActionsQueue ()
 Empty the actions queue. More...
 
virtual bool lockActions ()
 Disable the possibility to yield any new action. More...
 
virtual bool unlockActions ()
 Enable the possibility to yield new actions. More...
 
virtual bool getActionsLockStatus () const
 Return the actions lock status. More...
 
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. More...
 
virtual bool addHandSequence (const std::string &handSeqKey, const yarp::os::Bottle &sequence)
 Define an hand motion sequence from a configuration bottle. More...
 
virtual bool isValidHandSeq (const std::string &handSeqKey)
 Check whether a sequence key is defined. More...
 
virtual bool removeHandSeq (const std::string &handSeqKey)
 Remove an already existing hand motion sequence. More...
 
std::deque< std::stringgetHandSeqList ()
 Return the list of available hand sequence keys. More...
 
virtual bool getHandSequence (const std::string &handSeqKey, yarp::os::Bottle &sequence)
 Return a hand sequence. More...
 
virtual bool areFingersMoving (bool &f)
 Query if fingers are moving. More...
 
virtual bool areFingersInPosition (bool &f)
 Query if fingers are in position (cumulative response). More...
 
virtual bool areFingersInPosition (std::deque< bool > &f)
 Query if fingers are in position (finger-wise response). More...
 
virtual bool getGraspModel (perception::Model *&model) const
 Return the model used internally to detect external contacts. More...
 
virtual bool getCartesianIF (yarp::dev::ICartesianControl *&ctrl) const
 Return the cartesian interface used internally to control the limb. More...
 
virtual bool getTorsoJoints (yarp::sig::Vector &torso)
 Return the control status of torso joints. More...
 
virtual bool setTorsoJoints (const yarp::sig::Vector &torso)
 Change the control status of torso joints. More...
 
virtual bool getPose (yarp::sig::Vector &x, yarp::sig::Vector &o) const
 Get the current arm pose. More...
 
virtual bool stopControl ()
 Stop any ongoing arm/hand movements. More...
 
virtual bool setDefaultExecTime (const double execTime)
 Set the default arm movement execution time. More...
 
virtual double getDefaultExecTime () const
 Get the current default arm movement execution time. More...
 
virtual bool setTrackingMode (const bool f)
 Set the task space controller in tracking or non-tracking mode. More...
 
virtual bool getTrackingMode () const
 Get the current controller mode. More...
 
virtual bool enableArmWaving (const yarp::sig::Vector &restPos)
 Enable the waving mode that keeps on moving the arm around a predefined position. More...
 
virtual bool disableArmWaving ()
 Disable the waving mode. More...
 
virtual bool enableReachingTimeout (const double tmo)
 Enable timeout while reaching. More...
 
virtual bool disableReachingTimeout ()
 Disable timeout while reaching. More...
 
virtual bool checkActionsDone (bool &f, const bool sync=false)
 Check whether all the actions in queue are accomplished. More...
 
virtual bool checkActionOnGoing (bool &f, const bool sync=false)
 Check whether an action is still ongoing. More...
 
virtual bool syncCheckInterrupt (const bool disable=false)
 Suddenly interrupt any blocking call that is pending on querying the action status. More...
 
virtual bool syncCheckReinstate ()
 Reinstate the blocking feature for future calls with sync switch on. More...
 

Protected Member Functions

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 void postReachCallback ()
 
virtual void init ()
 
virtual bool execQueuedAction ()
 
virtual bool execPendingHandSequences ()
 
virtual void run ()
 

Protected Attributes

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::ModelgraspModel
 
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
 
ActionPrimitivesCallbackactionClb
 
yarp::os::PeriodicThread * actionWP
 
iCub::action::ActionPrimitives::ActionsQueue actionsQueue
 
std::map< std::string, std::deque< HandWayPoint > > handSeqMap
 

Friends

class ArmWayPoints
 

Detailed Description

The base class defining actions.

It allows executing arm (in task-space, e.g. reach()) and hand (in joint-space) primitive actions and to combine them in the actions queue.

Definition at line 193 of file actionPrimitives.h.

Constructor & Destructor Documentation

◆ ActionPrimitives() [1/2]

ActionPrimitives::ActionPrimitives ( )

Default Constructor.

Definition at line 321 of file actionPrimitives.cpp.

◆ ActionPrimitives() [2/2]

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

Constructor.

Parameters
optthe Property used to configure the object after its creation.

◆ ~ActionPrimitives()

ActionPrimitives::~ActionPrimitives ( )
virtual

Destructor.

Note
it calls the close() method.

Definition at line 1357 of file actionPrimitives.cpp.

Member Function Documentation

◆ _pushAction() [1/2]

virtual bool iCub::action::ActionPrimitives::_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 
)
protectedvirtual

◆ _pushAction() [2/2]

virtual bool iCub::action::ActionPrimitives::_pushAction ( const yarp::sig::Vector &  x,
const yarp::sig::Vector &  o,
const std::string handSeqKey,
const double  execTime,
ActionPrimitivesCallback clb,
const bool  oEnabled 
)
protectedvirtual

◆ addHandSequence()

bool ActionPrimitives::addHandSequence ( const std::string handSeqKey,
const yarp::os::Bottle &  sequence 
)
virtual

Define an hand motion sequence from a configuration bottle.

Parameters
handSeqKeythe hand sequence key.
sequencethe configuration bottle.
See also
open
Returns
true/false on success/fail.

Definition at line 1557 of file actionPrimitives.cpp.

◆ addHandSeqWP()

bool ActionPrimitives::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 
)
virtual

Define an hand WayPoint (WP) to be added at the bottom of the hand motion sequence pointed by the key.

Parameters
handSeqKeythe hand sequence key.
possthe 9 fingers joints WP positions to be attained [deg].
velsthe 9 fingers joints velocities [deg/s].
tolsthe 9 fingers joints tolerances [deg] used to detect end motion condition (motion is considered finished if |des(i)-fb(i)|<tols(i)).
thresthe 5 fingers thresholds used for grasp detection.
tmothe wayPoint timeout.
Returns
true/false on success/fail.
Note
this method creates a new empty wayPoint referred by the passed key if the key does not point to any valid sequence; hence the set (poss,vels,thres,tmo) will be the first WP of the new sequence.

Definition at line 1532 of file actionPrimitives.cpp.

◆ areFingersInPosition() [1/2]

bool ActionPrimitives::areFingersInPosition ( bool &  f)
virtual

Query if fingers are in position (cumulative response).

Parameters
fthe result of the check.
Returns
true/false on success/fail.
Note
Fingers are intended to be in position if they have attained the desired position or while moving they follow the desired trajectory. Any possible contact among fingers or with objects causes the method to return false.

Definition at line 1765 of file actionPrimitives.cpp.

◆ areFingersInPosition() [2/2]

bool ActionPrimitives::areFingersInPosition ( std::deque< bool > &  f)
virtual

Query if fingers are in position (finger-wise response).

Parameters
fa list containing the result of the single checks.
Returns
true/false on success/fail.
Note
Fingers are intended to be in position if they have attained the desired position or while moving they follow the desired trajectory. Any possible contact among fingers or with objects causes the method to return false.

Definition at line 1779 of file actionPrimitives.cpp.

◆ areFingersMoving()

bool ActionPrimitives::areFingersMoving ( bool &  f)
virtual

Query if fingers are moving.

Parameters
fthe result of the check.
Returns
true/false on success/fail.

Definition at line 1751 of file actionPrimitives.cpp.

◆ checkActionOnGoing()

bool ActionPrimitives::checkActionOnGoing ( bool &  f,
const bool  sync = false 
)
virtual

Check whether an action is still ongoing.

Parameters
fthe result of the check.
syncif true wait that at least one action has just started (blocking call).
Returns
true/false on success/fail.
Note
Sometimes it might be helpful to wait in the calling module until the beginning of the action inserted in the queue. For example:
// perform some computations that require the motion has been
// started off

Definition at line 2021 of file actionPrimitives.cpp.

◆ checkActionsDone()

bool ActionPrimitives::checkActionsDone ( bool &  f,
const bool  sync = false 
)
virtual

Check whether all the actions in queue are accomplished.

Parameters
fthe result of the check.
syncif true wait until all actions are accomplished (blocking call).
Returns
true/false on success/fail.
Note
As specified the check is performed on the content of the actions queue so that the blocking call returns as soon as the queue is empty.

Definition at line 2001 of file actionPrimitives.cpp.

◆ clearActionsQueue()

bool ActionPrimitives::clearActionsQueue ( )
virtual

Empty the actions queue.

Returns
true/false on success/fail.

Definition at line 823 of file actionPrimitives.cpp.

◆ close()

void ActionPrimitives::close ( )
virtual

Deallocate the object.

Reimplemented in iCub::action::ActionPrimitivesLayer2.

Definition at line 701 of file actionPrimitives.cpp.

◆ cmdArm()

bool ActionPrimitives::cmdArm ( const Action action)
protectedvirtual

Definition at line 1417 of file actionPrimitives.cpp.

◆ cmdArmWP()

bool ActionPrimitives::cmdArmWP ( const Action action)
protectedvirtual

Definition at line 1466 of file actionPrimitives.cpp.

◆ cmdHand()

bool ActionPrimitives::cmdHand ( const Action action)
protectedvirtual

Definition at line 1484 of file actionPrimitives.cpp.

◆ configGraspModel()

bool ActionPrimitives::configGraspModel ( yarp::os::Property &  opt)
protectedvirtual

Definition at line 529 of file actionPrimitives.cpp.

◆ configHandSeq()

bool ActionPrimitives::configHandSeq ( yarp::os::Property &  opt)
protectedvirtual

Definition at line 465 of file actionPrimitives.cpp.

◆ disableArmWaving()

bool ActionPrimitives::disableArmWaving ( )
virtual

Disable the waving mode.

Returns
true/false on success/failure.

Definition at line 1958 of file actionPrimitives.cpp.

◆ disableReachingTimeout()

bool ActionPrimitives::disableReachingTimeout ( )
virtual

Disable timeout while reaching.

Returns
true/false on success/failure.

Definition at line 1988 of file actionPrimitives.cpp.

◆ disableTorsoDof()

void ActionPrimitives::disableTorsoDof ( )
protectedvirtual

Definition at line 1388 of file actionPrimitives.cpp.

◆ enableArmWaving()

bool ActionPrimitives::enableArmWaving ( const yarp::sig::Vector &  restPos)
virtual

Enable the waving mode that keeps on moving the arm around a predefined position.

Parameters
restPosthe 3-d position around which to wave.
Note
useful to give a kind of more human perception when the arm is homed. This mode is automatically disabled each time a new action is commanded.
Returns
true/false on success/failure.

Definition at line 1939 of file actionPrimitives.cpp.

◆ enableReachingTimeout()

bool ActionPrimitives::enableReachingTimeout ( const double  tmo)
virtual

Enable timeout while reaching.

Parameters
tmothe timeout given in seconds.
Note
if a reaching task is not accomplished within the timeout, then the action is considered done.
Returns
true/false on success/failure.

Definition at line 1973 of file actionPrimitives.cpp.

◆ enableTorsoDof()

void ActionPrimitives::enableTorsoDof ( )
protectedvirtual

Definition at line 1375 of file actionPrimitives.cpp.

◆ execPendingHandSequences()

bool ActionPrimitives::execPendingHandSequences ( )
protectedvirtual

Definition at line 1245 of file actionPrimitives.cpp.

◆ execQueuedAction()

bool ActionPrimitives::execQueuedAction ( )
protectedvirtual

Definition at line 1204 of file actionPrimitives.cpp.

◆ getActionsLockStatus()

bool ActionPrimitives::getActionsLockStatus ( ) const
virtual

Return the actions lock status.

Returns
true/false on locked/unlocked.

Definition at line 863 of file actionPrimitives.cpp.

◆ getCartesianIF()

bool ActionPrimitives::getCartesianIF ( yarp::dev::ICartesianControl *&  ctrl) const
virtual

Return the cartesian interface used internally to control the limb.

Parameters
ctrlthe cartesian interface.
Returns
true/false on success/fail.
Note
Useful to access solver's options through the Cartesian Interface.

Definition at line 1806 of file actionPrimitives.cpp.

◆ getDefaultExecTime()

double ActionPrimitives::getDefaultExecTime ( ) const
virtual

Get the current default arm movement execution time.

Returns
current execution time given in seconds.

Definition at line 1907 of file actionPrimitives.cpp.

◆ getGraspModel()

bool ActionPrimitives::getGraspModel ( perception::Model *&  model) const
virtual

Return the model used internally to detect external contacts.

Parameters
modelthe perceptive model.
Returns
true/false on success/fail.
Note
Useful to access the model methods as defined in perceptiveModels library, such as the calibration procedures.

Definition at line 1793 of file actionPrimitives.cpp.

◆ getHandSeqList()

deque< string > ActionPrimitives::getHandSeqList ( )

Return the list of available hand sequence keys.

Returns
the list of available hand sequence keys.

Definition at line 1675 of file actionPrimitives.cpp.

◆ getHandSequence()

bool ActionPrimitives::getHandSequence ( const std::string handSeqKey,
yarp::os::Bottle &  sequence 
)
virtual

Return a hand sequence.

Parameters
handSeqKeythe hand sequence key.
sequencethe bottle containing the sequence
Returns
true iff valid key.

Definition at line 1688 of file actionPrimitives.cpp.

◆ getPose()

bool ActionPrimitives::getPose ( yarp::sig::Vector &  x,
yarp::sig::Vector &  o 
) const
virtual

Get the current arm pose.

Parameters
xa 3-d vector which is filled with the actual position x,y,z (meters).
oda 4-d vector which is filled with the actual orientation using axis-angle representation xa, ya, za, theta (meters and radians).
Returns
true/false on success/failure.

Definition at line 1848 of file actionPrimitives.cpp.

◆ getTorsoJoints()

bool ActionPrimitives::getTorsoJoints ( yarp::sig::Vector &  torso)
virtual

Return the control status of torso joints.

Parameters
torsothe vector containing the control status of torso joints.
See also
getDOF
Returns
true/false on success/fail.
Note
Unlike the arm, the torso is a part that can be shared, therefore the enabling/disabling of its joints must be properly notified.

Definition at line 1819 of file actionPrimitives.cpp.

◆ getTrackingMode()

bool ActionPrimitives::getTrackingMode ( ) const
virtual

Get the current controller mode.

Returns
true/false on tracking/non-tracking mode.

Definition at line 1932 of file actionPrimitives.cpp.

◆ handCheckMotionDone()

bool ActionPrimitives::handCheckMotionDone ( const int  jnt)
protectedvirtual

Definition at line 1364 of file actionPrimitives.cpp.

◆ handleTorsoDOF()

bool ActionPrimitives::handleTorsoDOF ( yarp::os::Property &  opt,
const std::string key 
)
protectedvirtual

Definition at line 404 of file actionPrimitives.cpp.

◆ init()

void ActionPrimitives::init ( )
protectedvirtual

Reimplemented in iCub::action::ActionPrimitivesLayer2.

Definition at line 338 of file actionPrimitives.cpp.

◆ isHandSeqEnded()

bool ActionPrimitives::isHandSeqEnded ( )
protectedvirtual

Definition at line 746 of file actionPrimitives.cpp.

◆ isValid()

bool ActionPrimitives::isValid ( ) const
virtual

Check if the object is initialized correctly.

Returns
true/fail on success/fail.

Reimplemented in iCub::action::ActionPrimitivesLayer2.

Definition at line 364 of file actionPrimitives.cpp.

◆ isValidHandSeq()

bool ActionPrimitives::isValidHandSeq ( const std::string handSeqKey)
virtual

Check whether a sequence key is defined.

Parameters
handSeqKeythe hand sequence key.
Returns
true iff valid key.

Definition at line 1648 of file actionPrimitives.cpp.

◆ lockActions()

bool ActionPrimitives::lockActions ( )
virtual

Disable the possibility to yield any new action.

Returns
true/false on success/fail.

Definition at line 837 of file actionPrimitives.cpp.

◆ open()

bool ActionPrimitives::open ( yarp::os::Property &  opt)
virtual

Configure the object.

Parameters
optthe Property used to configure the object after its creation.
Returns
true/false on success/fail.
Note
To be called after object creation.
Available options are:

local <string>: specify a stem name used to open local ports and to highlight messages printed on the screen.

robot <string>: the robot name to connect to (e.g. icub).

part <string>: the arm to be controlled (e.g. left_arm).

thread_period <int>: the thread period [ms] which selects the time granularity as well.

default_exec_time <double>: the arm movement execution time [s].

reach_tol <double>: the reaching tolerance [m].

tracking_mode <string>: enable/disable the tracking mode; possible values: "on"/"off".

Note
In tracking mode the cartesian position is mantained on the reached target.

verbosity <string>: enable/disable the verbose mode; possible values: "on"/"off".

torso_pitch <string>: if "on" it enables the control of the pitch of the torso.

torso_pitch_min <double>: set the pitch minimum value [deg].

torso_pitch_max <double>: set the pitch maximum value [deg].

torso_roll <string>: if "on" it enables the control of the roll of the torso.

torso_roll_min <double>: set the roll minimum value [deg].

torso_roll_max <double>: set the roll maximum value [deg].

torso_yaw <string>: if "on" it enables the control of the yaw of the torso.

torso_yaw_min <double>: set the yaw minimum value [deg].

torso_yaw_max <double>: set the yaw maximum value [deg].

grasp_model_type <string>: establish the type of the model used to detect external contacts while moving fingers. It refers to the types implemented within the perceptiveModels library, such as the "springy" or "tactile".

Note
the special tag "none" is used to skip this part.

grasp_model_file <string>: complete path to the file containing the options to initialize the grasp model.

hand_sequences_file <string>: complete path to the file containing the hand motions sequences.
Here is the format of motion sequences:

[GENERAL]
numSequences ***
[SEQ_0]
key ***
numWayPoints ***
wp_0 (poss (10 ...)) (vels (20 ...)) (tols (30 ...)) (thres
(1 2 3 4 5)) (tmo 10.0)
wp_1 *** ...
[SEQ_1]
...
// the "poss", "vels" and "tols" keys specify 9 joints
// positions, velocities and tolerances whereas the "thres"
// key specifies 5 fingers thresholds used for contact detection
// The "tols" key serves to detect the end motion condition.
// The "tmo" key specifies the timeout beyond which the motion
// is considered to be finished.
Note
A port called /<local>/<part>/detectGrasp:i is open to acquire data provided by icub_graspDetector module.

Reimplemented in iCub::action::ActionPrimitivesLayer2.

Definition at line 582 of file actionPrimitives.cpp.

◆ postReachCallback()

void ActionPrimitives::postReachCallback ( )
protectedvirtual

Reimplemented in iCub::action::ActionPrimitivesLayer2.

Definition at line 816 of file actionPrimitives.cpp.

◆ printMessage()

void ActionPrimitives::printMessage ( const int  logtype,
const char *  format,
  ... 
) const
protectedvirtual

Definition at line 371 of file actionPrimitives.cpp.

◆ pushAction() [1/7]

bool ActionPrimitives::pushAction ( const std::deque< ActionPrimitivesWayPoint > &  wayPoints,
ActionPrimitivesCallback clb = NULL 
)
virtual

Insert in the actions queue a trajectory in the operational space parametrized in terms of waypoints.

Parameters
wayPointsthe list of waypoints that will be used to generate the trajectory.
Note
The first waypoint will act as the end-point of the path whose starting point is the current position of robot end-effector.
Parameters
clbaction callback that is executed when the action ends; none by default.
Returns
true/false on success/fail.

Definition at line 1038 of file actionPrimitives.cpp.

◆ pushAction() [2/7]

bool ActionPrimitives::pushAction ( const std::deque< ActionPrimitivesWayPoint > &  wayPoints,
const std::string handSeqKey,
ActionPrimitivesCallback clb = NULL 
)
virtual

Insert in the actions queue a combination of hand and arm trajectory in the operational space parametrized in terms of waypoints.

Parameters
wayPointsthe list of waypoints that will be used to generate the trajectory.
Note
The first waypoint will act as the end-point of the path whose starting point is the current position of robot end-effector.
Parameters
handSeqKeythe hand sequence key.
clbaction callback that is executed when the action ends; none by default.
Returns
true/false on success/fail.

Definition at line 1065 of file actionPrimitives.cpp.

◆ pushAction() [3/7]

bool ActionPrimitives::pushAction ( const std::string handSeqKey,
ActionPrimitivesCallback clb = NULL 
)
virtual

Insert a hand-primitive action in the actions queue.

Parameters
handSeqKeythe hand sequence key.
clbaction callback that is executed when the action ends; none by default.
Returns
true/false on success/fail.

Definition at line 1002 of file actionPrimitives.cpp.

◆ pushAction() [4/7]

virtual bool iCub::action::ActionPrimitives::pushAction ( const yarp::sig::Vector &  x,
const double  execTime = ACTIONPRIM_DISABLE_EXECTIME,
ActionPrimitivesCallback clb = NULL 
)
virtual

Insert the arm-primitive action reach for target in the actions queue.

Parameters
xthe 3-d target position [m].
execTimethe arm action execution time [s] (to be specified iff different from default value).
clbaction callback that is executed when the action ends; none by default.
Returns
true/false on success/fail.

◆ pushAction() [5/7]

virtual bool iCub::action::ActionPrimitives::pushAction ( const yarp::sig::Vector &  x,
const std::string handSeqKey,
const double  execTime = ACTIONPRIM_DISABLE_EXECTIME,
ActionPrimitivesCallback clb = NULL 
)
virtual

Insert a combination of arm and hand primitive actions in the actions queue.

Parameters
xthe 3-d target position [m].
handSeqKeythe hand sequence key.
execTimethe arm action execution time [s] (to be specified iff different from default value).
clbaction callback that is executed when the action ends; none by default.
Returns
true/false on success/fail.
Note
Some examples:

the call pushAction(x,"close_hand") pushes the combined action of reachPosition(x) and hand "close_hand" sequence into the queue; the action will be executed as soon as all the previous items in the queue will have been served.

◆ pushAction() [6/7]

virtual bool iCub::action::ActionPrimitives::pushAction ( const yarp::sig::Vector &  x,
const yarp::sig::Vector &  o,
const double  execTime = ACTIONPRIM_DISABLE_EXECTIME,
ActionPrimitivesCallback clb = NULL 
)
virtual

Insert the arm-primitive action reach for target in the actions queue.

Parameters
xthe 3-d target position [m].
othe 4-d hand orientation used while reaching (given in axis-angle representation: ax ay az angle in rad).
execTimethe arm action execution time [s] (to be specified iff different from default value).
clbaction callback that is executed when the action ends; none by default.
Returns
true/false on success/fail.

◆ pushAction() [7/7]

virtual bool iCub::action::ActionPrimitives::pushAction ( const yarp::sig::Vector &  x,
const yarp::sig::Vector &  o,
const std::string handSeqKey,
const double  execTime = ACTIONPRIM_DISABLE_EXECTIME,
ActionPrimitivesCallback clb = NULL 
)
virtual

Insert a combination of arm and hand primitive actions in the actions queue.

Parameters
xthe 3-d target position [m].
othe 4-d hand orientation used while reaching (given in axis-angle representation: ax ay az angle in rad).
handSeqKeythe hand sequence key.
execTimethe arm action execution time [s] (to be specified iff different from default value).
clbaction callback that is executed when the action ends; none by default.
Returns
true/false on success/fail.
Note
Some examples:

the call pushAction(x,o,"close_hand") pushes the combined action of reachPose(x,o) and hand "close_hand" sequence into the queue; the action will be executed as soon as all the previous items in the queue will have been served.

◆ pushWaitState()

bool ActionPrimitives::pushWaitState ( const double  tmo,
ActionPrimitivesCallback clb = NULL 
)
virtual

Insert a wait state in the actions queue.

Parameters
tmois the wait timeout [s].
clbcallback that is executed when the timeout expires; none by default.
Returns
true/false on success/fail.

Definition at line 1123 of file actionPrimitives.cpp.

◆ reachPose()

bool ActionPrimitives::reachPose ( const yarp::sig::Vector &  x,
const yarp::sig::Vector &  o,
const double  execTime = ACTIONPRIM_DISABLE_EXECTIME 
)
virtual

Immediately update the current reaching target (without affecting the actions queue) or initiate a new reach (if the actions queue is empty).

Parameters
xthe 3-d target position [m].
othe 4-d hand orientation used while reaching (given in axis-angle representation: ax ay az angle in rad).
execTimethe arm action execution time [s] (to be specified iff different from default value).
Returns
true/false on success/fail.
Note
The intended use is for tracking moving targets.

Definition at line 1147 of file actionPrimitives.cpp.

◆ reachPosition()

bool ActionPrimitives::reachPosition ( const yarp::sig::Vector &  x,
const double  execTime = ACTIONPRIM_DISABLE_EXECTIME 
)
virtual

Immediately update the current reaching target (without affecting the actions queue) or initiate a new reach (if the actions queue is empty).

Parameters
xthe 3-d target position [m].
execTimethe arm action execution time [s] (to be specified iff different from default value).
Returns
true/false on success/fail.
Note
The intended use is for tracking moving targets.

Definition at line 1176 of file actionPrimitives.cpp.

◆ removeHandSeq()

bool ActionPrimitives::removeHandSeq ( const std::string handSeqKey)
virtual

Remove an already existing hand motion sequence.

Parameters
handSeqKeythe hand sequence key.
Returns
true/false on success/fail.

Definition at line 1660 of file actionPrimitives.cpp.

◆ run()

void ActionPrimitives::run ( )
protectedvirtual

Reimplemented in iCub::action::ActionPrimitivesLayer2.

Definition at line 1270 of file actionPrimitives.cpp.

◆ setDefaultExecTime()

bool ActionPrimitives::setDefaultExecTime ( const double  execTime)
virtual

Set the default arm movement execution time.

Parameters
execTimeexecution time given in seconds.
Returns
true/false on success/failure.

Definition at line 1894 of file actionPrimitives.cpp.

◆ setTorsoJoints()

bool ActionPrimitives::setTorsoJoints ( const yarp::sig::Vector &  torso)
virtual

Change the control status of torso joints.

Parameters
torsothe vector containing the control status of torso joints.
See also
setDOF
Returns
true/false on success/fail.
Note
Unlike the arm, the torso is a part that can be shared, therefore the enabling/disabling of its joints must be properly notified.

Definition at line 1832 of file actionPrimitives.cpp.

◆ setTrackingMode()

bool ActionPrimitives::setTrackingMode ( const bool  f)
virtual

Set the task space controller in tracking or non-tracking mode.

Parameters
ftrue for tracking mode, false otherwise.
Note
In tracking mode the cartesian position is mantained on the reached target.
Returns
true/false on success/failure.

Definition at line 1914 of file actionPrimitives.cpp.

◆ stopControl()

bool ActionPrimitives::stopControl ( )
virtual

Stop any ongoing arm/hand movements.

Returns
true/false on success/fail.
Note
it empty out the actions queue.

Definition at line 1861 of file actionPrimitives.cpp.

◆ syncCheckInterrupt()

bool ActionPrimitives::syncCheckInterrupt ( const bool  disable = false)
virtual

Suddenly interrupt any blocking call that is pending on querying the action status.

Parameters
disabledisable the blocking feature for future calls with sync switch on; useful to allow a graceful stop of the application.
See also
syncCheckReinstate
Returns
true/false on success/fail.

Definition at line 2041 of file actionPrimitives.cpp.

◆ syncCheckReinstate()

bool ActionPrimitives::syncCheckReinstate ( )
virtual

Reinstate the blocking feature for future calls with sync switch on.

Returns
true/false on success/fail.

Definition at line 2058 of file actionPrimitives.cpp.

◆ unlockActions()

bool ActionPrimitives::unlockActions ( )
virtual

Enable the possibility to yield new actions.

Returns
true/false on success/fail.

Definition at line 850 of file actionPrimitives.cpp.

◆ wait()

bool ActionPrimitives::wait ( const Action action)
protectedvirtual

Definition at line 1401 of file actionPrimitives.cpp.

Friends And Related Function Documentation

◆ ArmWayPoints

friend class ArmWayPoints
friend

Definition at line 257 of file actionPrimitives.h.

Member Data Documentation

◆ actionClb

ActionPrimitivesCallback* iCub::action::ActionPrimitives::actionClb
protected

Definition at line 291 of file actionPrimitives.h.

◆ actionsQueue

iCub::action::ActionPrimitives::ActionsQueue iCub::action::ActionPrimitives::actionsQueue
protected

◆ actionWP

yarp::os::PeriodicThread* iCub::action::ActionPrimitives::actionWP
protected

Definition at line 292 of file actionPrimitives.h.

◆ armMoveDone

bool iCub::action::ActionPrimitives::armMoveDone
protected

Definition at line 216 of file actionPrimitives.h.

◆ armWaver

yarp::os::PeriodicThread* iCub::action::ActionPrimitives::armWaver
protected

Definition at line 209 of file actionPrimitives.h.

◆ cartCtrl

yarp::dev::ICartesianControl* iCub::action::ActionPrimitives::cartCtrl
protected

Definition at line 205 of file actionPrimitives.h.

◆ checkEnabled

bool iCub::action::ActionPrimitives::checkEnabled
protected

Definition at line 226 of file actionPrimitives.h.

◆ closed

bool iCub::action::ActionPrimitives::closed
protected

Definition at line 225 of file actionPrimitives.h.

◆ configured

bool iCub::action::ActionPrimitives::configured
protected

Definition at line 224 of file actionPrimitives.h.

◆ curGraspDetectionThres

yarp::sig::Vector iCub::action::ActionPrimitives::curGraspDetectionThres
protected

Definition at line 248 of file actionPrimitives.h.

◆ curHandFinalPoss

yarp::sig::Vector iCub::action::ActionPrimitives::curHandFinalPoss
protected

Definition at line 246 of file actionPrimitives.h.

◆ curHandTmo

double iCub::action::ActionPrimitives::curHandTmo
protected

Definition at line 249 of file actionPrimitives.h.

◆ curHandTols

yarp::sig::Vector iCub::action::ActionPrimitives::curHandTols
protected

Definition at line 247 of file actionPrimitives.h.

◆ cv_motionDoneEvent

std::condition_variable iCub::action::ActionPrimitives::cv_motionDoneEvent
protected

Definition at line 214 of file actionPrimitives.h.

◆ cv_motionStartEvent

std::condition_variable iCub::action::ActionPrimitives::cv_motionStartEvent
protected

Definition at line 212 of file actionPrimitives.h.

◆ default_exec_time

double iCub::action::ActionPrimitives::default_exec_time
protected

Definition at line 233 of file actionPrimitives.h.

◆ disableTorsoSw

yarp::sig::Vector iCub::action::ActionPrimitives::disableTorsoSw
protected

Definition at line 244 of file actionPrimitives.h.

◆ enableTorsoSw

yarp::sig::Vector iCub::action::ActionPrimitives::enableTorsoSw
protected

Definition at line 243 of file actionPrimitives.h.

◆ encCtrl

yarp::dev::IEncoders* iCub::action::ActionPrimitives::encCtrl
protected

Definition at line 203 of file actionPrimitives.h.

◆ fingerInPosition

std::deque<bool> iCub::action::ActionPrimitives::fingerInPosition
protected

Definition at line 222 of file actionPrimitives.h.

◆ fingers2JntsMap

std::multimap<int,int> iCub::action::ActionPrimitives::fingers2JntsMap
protected

Definition at line 255 of file actionPrimitives.h.

◆ fingersInPosition

bool iCub::action::ActionPrimitives::fingersInPosition
protected

Definition at line 221 of file actionPrimitives.h.

◆ fingersJnts

std::vector<int> iCub::action::ActionPrimitives::fingersJnts
protected

Definition at line 252 of file actionPrimitives.h.

◆ fingersJntsSet

std::set<int> iCub::action::ActionPrimitives::fingersJntsSet
protected

Definition at line 253 of file actionPrimitives.h.

◆ fingersMovingJntsSet

std::set<int> iCub::action::ActionPrimitives::fingersMovingJntsSet
protected

Definition at line 254 of file actionPrimitives.h.

◆ graspModel

perception::Model* iCub::action::ActionPrimitives::graspModel
protected

Definition at line 207 of file actionPrimitives.h.

◆ handMoveDone

bool iCub::action::ActionPrimitives::handMoveDone
protected

Definition at line 217 of file actionPrimitives.h.

◆ handSeqMap

std::map<std::string,std::deque<HandWayPoint> > iCub::action::ActionPrimitives::handSeqMap
protected

Definition at line 298 of file actionPrimitives.h.

◆ handSeqTerminator

bool iCub::action::ActionPrimitives::handSeqTerminator
protected

Definition at line 220 of file actionPrimitives.h.

◆ jHandMax

int iCub::action::ActionPrimitives::jHandMax
protected

Definition at line 241 of file actionPrimitives.h.

◆ jHandMin

int iCub::action::ActionPrimitives::jHandMin
protected

Definition at line 240 of file actionPrimitives.h.

◆ latchArmMoveDone

bool iCub::action::ActionPrimitives::latchArmMoveDone
protected

Definition at line 218 of file actionPrimitives.h.

◆ latchHandMoveDone

bool iCub::action::ActionPrimitives::latchHandMoveDone
protected

Definition at line 219 of file actionPrimitives.h.

◆ latchTimerHand

double iCub::action::ActionPrimitives::latchTimerHand
protected

Definition at line 250 of file actionPrimitives.h.

◆ latchTimerReach

double iCub::action::ActionPrimitives::latchTimerReach
protected

Definition at line 237 of file actionPrimitives.h.

◆ latchTimerReachLog

double iCub::action::ActionPrimitives::latchTimerReachLog
protected

Definition at line 238 of file actionPrimitives.h.

◆ latchTimerWait

double iCub::action::ActionPrimitives::latchTimerWait
protected

Definition at line 236 of file actionPrimitives.h.

◆ local

std::string iCub::action::ActionPrimitives::local
protected

Definition at line 197 of file actionPrimitives.h.

◆ locked

bool iCub::action::ActionPrimitives::locked
protected

Definition at line 230 of file actionPrimitives.h.

◆ modCtrl

yarp::dev::IControlMode* iCub::action::ActionPrimitives::modCtrl
protected

Definition at line 202 of file actionPrimitives.h.

◆ mtx

std::mutex iCub::action::ActionPrimitives::mtx
protected

Definition at line 210 of file actionPrimitives.h.

◆ mtx_motionDoneEvent

std::mutex iCub::action::ActionPrimitives::mtx_motionDoneEvent
protected

Definition at line 213 of file actionPrimitives.h.

◆ mtx_motionStartEvent

std::mutex iCub::action::ActionPrimitives::mtx_motionStartEvent
protected

Definition at line 211 of file actionPrimitives.h.

◆ part

std::string iCub::action::ActionPrimitives::part
protected

Definition at line 198 of file actionPrimitives.h.

◆ polyCart

yarp::dev::PolyDriver iCub::action::ActionPrimitives::polyCart
protected

Definition at line 201 of file actionPrimitives.h.

◆ polyHand

yarp::dev::PolyDriver iCub::action::ActionPrimitives::polyHand
protected

Definition at line 200 of file actionPrimitives.h.

◆ posCtrl

yarp::dev::IPositionControl* iCub::action::ActionPrimitives::posCtrl
protected

Definition at line 204 of file actionPrimitives.h.

◆ reachTmo

double iCub::action::ActionPrimitives::reachTmo
protected

Definition at line 235 of file actionPrimitives.h.

◆ reachTmoEnabled

bool iCub::action::ActionPrimitives::reachTmoEnabled
protected

Definition at line 229 of file actionPrimitives.h.

◆ robot

std::string iCub::action::ActionPrimitives::robot
protected

Definition at line 196 of file actionPrimitives.h.

◆ torsoActive

bool iCub::action::ActionPrimitives::torsoActive
protected

Definition at line 228 of file actionPrimitives.h.

◆ tracking_mode

bool iCub::action::ActionPrimitives::tracking_mode
protected

Definition at line 227 of file actionPrimitives.h.

◆ verbose

bool iCub::action::ActionPrimitives::verbose
protected

Definition at line 231 of file actionPrimitives.h.

◆ waitTmo

double iCub::action::ActionPrimitives::waitTmo
protected

Definition at line 234 of file actionPrimitives.h.


The documentation for this class was generated from the following files:
iCub::action::ActionPrimitives::checkActionsDone
virtual bool checkActionsDone(bool &f, const bool sync=false)
Check whether all the actions in queue are accomplished.
Definition: actionPrimitives.cpp:2001
iCub::action::ActionPrimitives::pushAction
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.
iCub::action::ActionPrimitives::checkActionOnGoing
virtual bool checkActionOnGoing(bool &f, const bool sync=false)
Check whether an action is still ongoing.
Definition: actionPrimitives.cpp:2021
f
f
Definition: compute_ekf_sym.m:22
A
A
Definition: sine.m:16