Go to the documentation of this file.
79 #ifndef __AFFACTIONPRIMITIVES_H__
80 #define __AFFACTIONPRIMITIVES_H__
83 #include <condition_variable>
90 #include <yarp/os/all.h>
91 #include <yarp/dev/all.h>
92 #include <yarp/sig/all.h>
96 #define ACTIONPRIM_DISABLE_EXECTIME -1.0
122 virtual void exec() = 0;
300 virtual void printMessage(
const int logtype,
const char *format, ...)
const;
307 virtual bool _pushAction(
const bool execArm,
const yarp::sig::Vector &
x,
308 const yarp::sig::Vector &o,
const double execTime,
309 const bool oEnabled,
const bool execHand,
const HandWayPoint &handWP,
311 virtual bool _pushAction(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
312 const std::string &handSeqKey,
const double execTime,
443 virtual bool open(yarp::os::Property &opt);
454 virtual void close();
476 virtual bool pushAction(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
499 virtual bool pushAction(
const yarp::sig::Vector &
x,
516 virtual bool pushAction(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
530 virtual bool pushAction(
const yarp::sig::Vector &
x,
556 virtual bool pushAction(
const std::deque<ActionPrimitivesWayPoint> &wayPoints,
573 virtual bool pushAction(
const std::deque<ActionPrimitivesWayPoint> &wayPoints,
598 virtual bool reachPose(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
660 const yarp::sig::Vector &vels,
const yarp::sig::Vector &tols,
661 const yarp::sig::Vector &thres,
const double tmo);
671 const yarp::os::Bottle &sequence);
755 virtual bool getCartesianIF(yarp::dev::ICartesianControl *&ctrl)
const;
792 virtual bool getPose(yarp::sig::Vector &
x, yarp::sig::Vector &o)
const;
969 virtual bool grasp(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
970 const yarp::sig::Vector &d);
994 virtual bool touch(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
995 const yarp::sig::Vector &d);
1022 virtual bool tap(
const yarp::sig::Vector &
x1,
const yarp::sig::Vector &o1,
1023 const yarp::sig::Vector &
x2,
const yarp::sig::Vector &o2,
1029 class ActionPrimitivesLayer2;
1042 virtual void exec();
1056 virtual void exec();
1096 virtual void init();
1146 virtual bool open(yarp::os::Property &opt);
1157 virtual void close();
1174 virtual bool grasp(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
1175 const yarp::sig::Vector &d1,
const yarp::sig::Vector &d2);
1188 virtual bool grasp(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
1189 const yarp::sig::Vector &d);
1202 virtual bool touch(
const yarp::sig::Vector &
x,
const yarp::sig::Vector &o,
1203 const yarp::sig::Vector &d);
1211 virtual bool getExtWrench(yarp::sig::Vector &wrench)
const;
virtual bool lockActions()
Disable the possibility to yield any new action.
std::map< std::string, std::deque< HandWayPoint > > handSeqMap
virtual void postReachCallback()
ActionPrimitivesCallback * callback
Action callback that is executed when the waypoint is reached.
std::set< int > fingersMovingJntsSet
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
virtual bool checkActionsDone(bool &f, const bool sync=false)
Check whether all the actions in queue are accomplished.
yarp::dev::PolyDriver polyHand
yarp::sig::Vector curHandTols
virtual bool disableContactDetection()
Self-explaining :)
yarp::sig::Vector enableTorsoSw
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 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 getCartesianIF(yarp::dev::ICartesianControl *&ctrl) const
Return the cartesian interface used internally to control the limb.
yarp::sig::Vector encDataTorso
virtual bool checkContact(bool &f) const
Check whether the reaching has been stopped due to a contact with external objects.
touchCallback(ActionPrimitivesLayer2 *_action)
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 ne...
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...
iCub::action::ActionPrimitives::ActionsQueue actionsQueue
virtual void enableTorsoDof()
std::deque< std::string > getHandSeqList()
Return the list of available hand sequence keys.
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.
yarp::os::BufferedPort< yarp::sig::Vector > wbdynPortIn
virtual bool checkActionOnGoing(bool &f, const bool sync=false)
Check whether an action is still ongoing.
std::deque< bool > fingerInPosition
std::vector< int > fingersJnts
virtual bool unlockActions()
Enable the possibility to yield new actions.
yarp::sig::Vector curGraspDetectionThres
yarp::os::PeriodicThread * wayPointsThr
std::condition_variable cv_motionDoneEvent
virtual bool isValidHandSeq(const std::string &handSeqKey)
Check whether a sequence key is defined.
double granularity
The time granularity [s] used by the trajectory generator [s].
ActionPrimitivesCallback * clb
virtual bool isValid() const
Check if the object is initialized correctly.
virtual bool getHandSequence(const std::string &handSeqKey, yarp::os::Bottle &sequence)
Return a hand sequence.
virtual void close()
Deallocate the object.
yarp::sig::Vector encDataArm
ActionPrimitivesCallback()
Default Constructor.
virtual ~ActionPrimitives()
Destructor.
virtual void exec()
Defines the callback body to be called at the action end.
virtual bool disableReachingTimeout()
Disable timeout while reaching.
virtual bool isHandSeqEnded()
virtual void printMessage(const int logtype, const char *format,...) const
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.
ActionPrimitivesWayPoint()
Default Constructor.
virtual bool setDefaultExecTime(const double execTime)
Set the default arm movement execution time.
virtual bool disableArmWaving()
Disable the waving mode.
virtual bool getExtWrench(yarp::sig::Vector &wrench) const
Retrieve the current wrench on the end-effector.
yarp::sig::Vector grasp_o
yarp::dev::PolyDriver polyCart
yarp::sig::Vector o
The 4x1 Vector specifying the orientation of the waypoint in axis-angle representation.
ActionPrimitivesCallback * actionClb
virtual bool configGraspModel(yarp::os::Property &opt)
liftAndGraspCallback * execLiftAndGrasp
virtual ~ActionPrimitivesLayer2()
Destructor.
virtual bool getGraspModel(perception::Model *&model) const
Return the model used internally to detect external contacts.
virtual void exec()=0
Defines the callback body to be called at the action end.
yarp::dev::IEncoders * encCtrl
virtual bool setTrackingMode(const bool f)
Set the task space controller in tracking or non-tracking mode.
virtual bool areFingersInPosition(bool &f)
Query if fingers are in position (cumulative response).
ActionPrimitivesLayer1(yarp::os::Property &opt)
Constructor.
ActionPrimitives()
Default Constructor.
virtual bool isValid() const
Check if the object is initialized correctly.
virtual bool enableContactDetection()
Self-explaining :)
virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o) const
Get the current arm pose.
#define ACTIONPRIM_DISABLE_EXECTIME
virtual bool handleTorsoDOF(yarp::os::Property &opt, const std::string &key)
virtual bool open(yarp::os::Property &opt)
Configure the object.
virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
Grasp the given target (combined action).
virtual double getDefaultExecTime() const
Get the current default arm movement execution time.
virtual bool cmdArmWP(const Action &action)
perception::Model * graspModel
virtual bool cmdHand(const Action &action)
virtual bool syncCheckReinstate()
Reinstate the blocking feature for future calls with sync switch on.
virtual void postReachCallback()
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
std::condition_variable cv_motionStartEvent
virtual bool syncCheckInterrupt(const bool disable=false)
Suddenly interrupt any blocking call that is pending on querying the action status.
bool oEnabled
If this flag is set to true then orientation will be taken into account.
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 ne...
liftAndGraspCallback(ActionPrimitivesLayer2 *_action)
std::set< int > fingersJntsSet
double duration
The time duration [s] to achieve the waypoint.
ActionPrimitivesLayer2 * action
virtual void close()
Deallocate the object.
double latchTimerReachLog
virtual bool enableReachingTimeout(const double tmo)
Enable timeout while reaching.
touchCallback * execTouch
virtual void exec()
Defines the callback body to be called at the action end.
yarp::dev::IPositionControl * posCtrl
virtual bool removeHandSeq(const std::string &handSeqKey)
Remove an already existing hand motion sequence.
virtual bool setTorsoJoints(const yarp::sig::Vector &torso)
Change the control status of torso joints.
virtual bool pushWaitState(const double tmo, ActionPrimitivesCallback *clb=NULL)
Insert a wait state in the actions queue.
std::mutex mtx_motionStartEvent
virtual bool areFingersMoving(bool &f)
Query if fingers are moving.
yarp::sig::Vector curHandFinalPoss
virtual bool isContactDetectionEnabled(bool &f) const
Self-explaining :)
virtual bool execQueuedAction()
virtual void disableTorsoDof()
ActionPrimitivesLayer1()
Default Constructor.
yarp::os::PeriodicThread * actionWP
yarp::os::PeriodicThread * armWaver
double trajTime
The arm execution time [s] accounting for the controller's responsivity.
virtual bool configHandSeq(yarp::os::Property &opt)
ActionPrimitivesLayer2()
Default Constructor.
virtual bool stopControl()
Stop any ongoing arm/hand movements.
yarp::sig::Vector grasp_d2
yarp::dev::ICartesianControl * cartCtrl
virtual bool open(yarp::os::Property &opt)
Configure the object.
virtual bool setExtForceThres(const double thres)
Set the threshold on the external force used to stop the limb while reaching.
ActionPrimitivesLayer2 * action
virtual bool handCheckMotionDone(const int jnt)
std::multimap< int, int > fingers2JntsMap
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).
virtual bool enableArmWaving(const yarp::sig::Vector &restPos)
Enable the waving mode that keeps on moving the arm around a predefined position.
virtual bool clearActionsQueue()
Empty the actions queue.
virtual bool wait(const Action &action)
virtual bool cmdArm(const Action &action)
virtual bool addHandSequence(const std::string &handSeqKey, const yarp::os::Bottle &sequence)
Define an hand motion sequence from a configuration bottle.
yarp::sig::Vector disableTorsoSw
std::mutex mtx_motionDoneEvent
virtual bool getExtForceThres(double &thres) const
Retrieve the current threshold on the external force used to stop the limb while reaching.
yarp::dev::IControlMode * modCtrl
virtual bool getTrackingMode() const
Get the current controller mode.
virtual bool execPendingHandSequences()
yarp::sig::Vector wrenchExternal
virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
Touch the given target (combined action).
virtual bool getActionsLockStatus() const
Return the actions lock status.
virtual bool getTorsoJoints(yarp::sig::Vector &torso)
Return the control status of torso joints.
yarp::sig::Vector x
The 3x1 Vector specifyng the position of the waypoint [m].