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

#include <actionPrimitives.h>

+ Inheritance diagram for iCub::action::ActionPrimitivesLayer2:

Public Member Functions

 ActionPrimitivesLayer2 ()
 Default Constructor. More...
 
 ActionPrimitivesLayer2 (yarp::os::Property &opt)
 Constructor. More...
 
virtual ~ActionPrimitivesLayer2 ()
 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 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. More...
 
virtual bool grasp (const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
 The usual grasp is still available. More...
 
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. More...
 
virtual bool getExtWrench (yarp::sig::Vector &wrench) const
 Retrieve the current wrench on the end-effector. More...
 
virtual bool getExtForceThres (double &thres) const
 Retrieve the current threshold on the external force used to stop the limb while reaching. More...
 
virtual bool setExtForceThres (const double thres)
 Set the threshold on the external force used to stop the limb while reaching. More...
 
virtual bool enableContactDetection ()
 Self-explaining :) More...
 
virtual bool disableContactDetection ()
 Self-explaining :) More...
 
virtual bool isContactDetectionEnabled (bool &f) const
 Self-explaining :) More...
 
virtual bool checkContact (bool &f) const
 Check whether the reaching has been stopped due to a contact with external objects. More...
 
- Public Member Functions inherited from iCub::action::ActionPrimitivesLayer1
 ActionPrimitivesLayer1 ()
 Default Constructor. More...
 
 ActionPrimitivesLayer1 (yarp::os::Property &opt)
 Constructor. More...
 
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). More...
 
- Public Member Functions inherited from iCub::action::ActionPrimitives
 ActionPrimitives ()
 Default Constructor. More...
 
 ActionPrimitives (yarp::os::Property &opt)
 Constructor. More...
 
virtual ~ActionPrimitives ()
 Destructor. 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 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
 
liftAndGraspCallbackexecLiftAndGrasp
 
touchCallbackexecTouch
 
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::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 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 1070 of file actionPrimitives.h.

Constructor & Destructor Documentation

◆ ActionPrimitivesLayer2() [1/2]

ActionPrimitivesLayer2::ActionPrimitivesLayer2 ( )

Default Constructor.

Definition at line 2153 of file actionPrimitives.cpp.

◆ ActionPrimitivesLayer2() [2/2]

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

Constructor.

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

◆ ~ActionPrimitivesLayer2()

ActionPrimitivesLayer2::~ActionPrimitivesLayer2 ( )
virtual

Destructor.

Note
it calls the close() method.

Definition at line 2463 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
fthe result of the check.
Returns
true/false on success/fail.

Definition at line 2450 of file actionPrimitives.cpp.

◆ close()

void ActionPrimitivesLayer2::close ( )
virtual

Deallocate the object.

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2290 of file actionPrimitives.cpp.

◆ disableContactDetection()

bool ActionPrimitivesLayer2::disableContactDetection ( )
virtual

Self-explaining :)

Returns
true/false on success/fail.

Definition at line 2424 of file actionPrimitives.cpp.

◆ enableContactDetection()

bool ActionPrimitivesLayer2::enableContactDetection ( )
virtual

Self-explaining :)

Returns
true/false on success/fail.

Definition at line 2411 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
threswhere to return the threshold.
Returns
true/false on success/fail.

Definition at line 2385 of file actionPrimitives.cpp.

◆ getExtWrench()

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

Retrieve the current wrench on the end-effector.

Parameters
wrencha vector containing the external forces/moments acting on the end-effector.
Returns
true/false on success/fail.

Definition at line 2372 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
xthe 3-d target position [m].
othe 4-d hand orientation used while reaching/grasping (given in axis-angle representation: ax ay az angle in rad).
dthe 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
xthe 3-d target position [m].
othe 4-d hand orientation used while reaching/grasping (given in axis-angle representation: ax ay az angle in rad).
d1the displacement [m] wrt the target position that identifies a location to be reached prior to grasping.
d2the 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 2170 of file actionPrimitives.cpp.

◆ isContactDetectionEnabled()

bool ActionPrimitivesLayer2::isContactDetectionEnabled ( bool &  f) const
virtual

Self-explaining :)

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

Definition at line 2437 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 2283 of file actionPrimitives.cpp.

◆ open()

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

Configure the object.

Parameters
optthe 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 2241 of file actionPrimitives.cpp.

◆ postReachCallback()

void ActionPrimitivesLayer2::postReachCallback ( )
protectedvirtual

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2183 of file actionPrimitives.cpp.

◆ run()

void ActionPrimitivesLayer2::run ( )
protectedvirtual

Reimplemented from iCub::action::ActionPrimitives.

Definition at line 2194 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
thresthe new threshold.
Returns
true/false on success/fail.

Definition at line 2398 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
xthe 3-d target position [m].
othe 4-d hand orientation used while reaching/touching (given in axis-angle representation: ax ay az angle in rad).
dthe 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 2353 of file actionPrimitives.cpp.

Friends And Related Function Documentation

◆ liftAndGraspCallback

friend class liftAndGraspCallback
friend

Definition at line 1093 of file actionPrimitives.h.

◆ touchCallback

friend class touchCallback
friend

Definition at line 1094 of file actionPrimitives.h.

Member Data Documentation

◆ configuredLayer2

bool iCub::action::ActionPrimitivesLayer2::configuredLayer2
protected

Definition at line 1074 of file actionPrimitives.h.

◆ contactDetected

bool iCub::action::ActionPrimitivesLayer2::contactDetected
protected

Definition at line 1076 of file actionPrimitives.h.

◆ contactDetectionOn

bool iCub::action::ActionPrimitivesLayer2::contactDetectionOn
protected

Definition at line 1075 of file actionPrimitives.h.

◆ encDataArm

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

Definition at line 1091 of file actionPrimitives.h.

◆ encDataTorso

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

Definition at line 1090 of file actionPrimitives.h.

◆ execLiftAndGrasp

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

Definition at line 1080 of file actionPrimitives.h.

◆ execTouch

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

Definition at line 1081 of file actionPrimitives.h.

◆ ext_force_thres

double iCub::action::ActionPrimitivesLayer2::ext_force_thres
protected

Definition at line 1078 of file actionPrimitives.h.

◆ grasp_d2

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

Definition at line 1087 of file actionPrimitives.h.

◆ grasp_o

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

Definition at line 1088 of file actionPrimitives.h.

◆ skipFatherPart

bool iCub::action::ActionPrimitivesLayer2::skipFatherPart
protected

Definition at line 1073 of file actionPrimitives.h.

◆ wbdynPortIn

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

Definition at line 1083 of file actionPrimitives.h.

◆ wrenchExternal

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

Definition at line 1085 of file actionPrimitives.h.


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