iCub-main
actionPrimitives.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
79 #ifndef __AFFACTIONPRIMITIVES_H__
80 #define __AFFACTIONPRIMITIVES_H__
81 
82 #include <mutex>
83 #include <condition_variable>
84 #include <string>
85 #include <vector>
86 #include <deque>
87 #include <set>
88 #include <map>
89 
90 #include <yarp/os/all.h>
91 #include <yarp/dev/all.h>
92 #include <yarp/sig/all.h>
93 
94 #include <iCub/perception/models.h>
95 
96 #define ACTIONPRIM_DISABLE_EXECTIME -1.0
97 
98 
99 namespace iCub
100 {
101 
102 namespace action
103 {
104 
112 {
113 public:
118 
122  virtual void exec() = 0;
123 };
124 
125 
133 {
137  yarp::sig::Vector x;
138 
143  yarp::sig::Vector o;
144 
149  bool oEnabled;
150 
156  double duration;
157 
163  double trajTime;
164 
169  double granularity;
170 
176 
181 };
182 
183 
193 class ActionPrimitives : protected yarp::os::PeriodicThread
194 {
195 protected:
196  std::string robot;
197  std::string local;
198  std::string part;
199 
200  yarp::dev::PolyDriver polyHand;
201  yarp::dev::PolyDriver polyCart;
202  yarp::dev::IControlMode *modCtrl;
203  yarp::dev::IEncoders *encCtrl;
204  yarp::dev::IPositionControl *posCtrl;
205  yarp::dev::ICartesianControl *cartCtrl;
206 
208 
209  yarp::os::PeriodicThread *armWaver;
210  std::mutex mtx;
212  std::condition_variable cv_motionStartEvent;
214  std::condition_variable cv_motionDoneEvent;
215 
222  std::deque<bool> fingerInPosition;
223 
225  bool closed;
230  bool locked;
231  bool verbose;
232 
234  double waitTmo;
235  double reachTmo;
239 
240  int jHandMin;
241  int jHandMax;
242 
243  yarp::sig::Vector enableTorsoSw;
244  yarp::sig::Vector disableTorsoSw;
245 
246  yarp::sig::Vector curHandFinalPoss;
247  yarp::sig::Vector curHandTols;
248  yarp::sig::Vector curGraspDetectionThres;
249  double curHandTmo;
251 
252  std::vector<int> fingersJnts;
253  std::set<int> fingersJntsSet;
254  std::set<int> fingersMovingJntsSet;
255  std::multimap<int,int> fingers2JntsMap;
256 
257  friend class ArmWayPoints;
258 
260  {
261  std::string tag;
262  yarp::sig::Vector poss;
263  yarp::sig::Vector vels;
264  yarp::sig::Vector tols;
265  yarp::sig::Vector thres;
266  double tmo;
267  };
268 
269  struct Action
270  {
271  // wait action
272  bool waitState;
273  double tmo;
274  // reach action
275  bool execArm;
276  yarp::sig::Vector x;
277  yarp::sig::Vector o;
278  double execTime;
279  bool oEnabled;
280  // hand action
281  bool execHand;
284  // reach way points action
286  yarp::os::PeriodicThread *wayPointsThr;
287  // action callback
289  };
290 
292  yarp::os::PeriodicThread *actionWP;
293  class ActionsQueue : public std::deque<Action>
294  {
295  public:
296  void clear();
298  std::map<std::string,std::deque<HandWayPoint> > handSeqMap;
299 
300  virtual void printMessage(const int logtype, const char *format, ...) const;
301 
302  virtual bool handleTorsoDOF(yarp::os::Property &opt, const std::string &key);
303  virtual void disableTorsoDof();
304  virtual void enableTorsoDof();
305  virtual bool configHandSeq(yarp::os::Property &opt);
306  virtual bool configGraspModel(yarp::os::Property &opt);
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,
313  ActionPrimitivesCallback *clb, const bool oEnabled);
314  virtual bool handCheckMotionDone(const int jnt);
315  virtual bool wait(const Action &action);
316  virtual bool cmdArm(const Action &action);
317  virtual bool cmdArmWP(const Action &action);
318  virtual bool cmdHand(const Action &action);
319  virtual bool isHandSeqEnded();
320  virtual void postReachCallback();
321 
322  virtual void init();
323  virtual bool execQueuedAction();
324  virtual bool execPendingHandSequences();
325  virtual void run();
326 
327 public:
332 
338  ActionPrimitives(yarp::os::Property &opt);
339 
345  virtual ~ActionPrimitives();
346 
442  virtual bool open(yarp::os::Property &opt);
443 
448  virtual bool isValid() const;
449 
453  virtual void close();
454 
475  virtual bool pushAction(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
476  const std::string &handSeqKey,
477  const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
478  ActionPrimitivesCallback *clb=NULL);
479 
498  virtual bool pushAction(const yarp::sig::Vector &x,
499  const std::string &handSeqKey,
500  const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
501  ActionPrimitivesCallback *clb=NULL);
502 
515  virtual bool pushAction(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
516  const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
517  ActionPrimitivesCallback *clb=NULL);
518 
529  virtual bool pushAction(const yarp::sig::Vector &x,
530  const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
531  ActionPrimitivesCallback *clb=NULL);
532 
540  virtual bool pushAction(const std::string &handSeqKey,
541  ActionPrimitivesCallback *clb=NULL);
542 
555  virtual bool pushAction(const std::deque<ActionPrimitivesWayPoint> &wayPoints,
556  ActionPrimitivesCallback *clb=NULL);
557 
572  virtual bool pushAction(const std::deque<ActionPrimitivesWayPoint> &wayPoints,
573  const std::string &handSeqKey, ActionPrimitivesCallback *clb=NULL);
574 
582  virtual bool pushWaitState(const double tmo, ActionPrimitivesCallback *clb=NULL);
583 
597  virtual bool reachPose(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
598  const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
599 
611  virtual bool reachPosition(const yarp::sig::Vector &x,
612  const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
613 
618  virtual bool clearActionsQueue();
619 
624  virtual bool lockActions();
625 
630  virtual bool unlockActions();
631 
636  virtual bool getActionsLockStatus() const;
637 
658  virtual bool addHandSeqWP(const std::string &handSeqKey, const yarp::sig::Vector &poss,
659  const yarp::sig::Vector &vels, const yarp::sig::Vector &tols,
660  const yarp::sig::Vector &thres, const double tmo);
661 
669  virtual bool addHandSequence(const std::string &handSeqKey,
670  const yarp::os::Bottle &sequence);
671 
677  virtual bool isValidHandSeq(const std::string &handSeqKey);
678 
684  virtual bool removeHandSeq(const std::string &handSeqKey);
685 
690  std::deque<std::string> getHandSeqList();
691 
698  virtual bool getHandSequence(const std::string &handSeqKey, yarp::os::Bottle &sequence);
699 
705  virtual bool areFingersMoving(bool &f);
706 
718  virtual bool areFingersInPosition(bool &f);
719 
731  virtual bool areFingersInPosition(std::deque<bool> &f);
732 
742  virtual bool getGraspModel(perception::Model *&model) const;
743 
754  virtual bool getCartesianIF(yarp::dev::ICartesianControl *&ctrl) const;
755 
767  virtual bool getTorsoJoints(yarp::sig::Vector &torso);
768 
780  virtual bool setTorsoJoints(const yarp::sig::Vector &torso);
781 
791  virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o) const;
792 
799  virtual bool stopControl();
800 
806  virtual bool setDefaultExecTime(const double execTime);
807 
812  virtual double getDefaultExecTime() const;
813 
822  virtual bool setTrackingMode(const bool f);
823 
828  virtual bool getTrackingMode() const;
829 
839  virtual bool enableArmWaving(const yarp::sig::Vector &restPos);
840 
845  virtual bool disableArmWaving();
846 
854  virtual bool enableReachingTimeout(const double tmo);
855 
860  virtual bool disableReachingTimeout();
861 
873  virtual bool checkActionsDone(bool &f, const bool sync=false);
874 
893  virtual bool checkActionOnGoing(bool &f, const bool sync=false);
894 
904  virtual bool syncCheckInterrupt(const bool disable=false);
905 
911  virtual bool syncCheckReinstate();
912 };
913 
914 
931 {
932 public:
937 
943  ActionPrimitivesLayer1(yarp::os::Property &opt) : ActionPrimitives(opt) { }
944 
968  virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
969  const yarp::sig::Vector &d);
970 
993  virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
994  const yarp::sig::Vector &d);
995 
1021  virtual bool tap(const yarp::sig::Vector &x1, const yarp::sig::Vector &o1,
1022  const yarp::sig::Vector &x2, const yarp::sig::Vector &o2,
1023  const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
1024 };
1025 
1026 
1027 // forward declaration
1028 class ActionPrimitivesLayer2;
1029 
1030 
1031 // callback for executing final grasp after contact
1033 {
1034 protected:
1036 
1037 public:
1039  action(_action) { }
1040 
1041  virtual void exec();
1042 };
1043 
1044 
1045 // callback for executing touch callback
1047 {
1048 protected:
1050 
1051 public:
1053  action(_action) { }
1054 
1055  virtual void exec();
1056 };
1057 
1058 
1070 {
1071 protected:
1076 
1078 
1081 
1082  yarp::os::BufferedPort<yarp::sig::Vector> wbdynPortIn;
1083 
1084  yarp::sig::Vector wrenchExternal;
1085 
1086  yarp::sig::Vector grasp_d2;
1087  yarp::sig::Vector grasp_o;
1088 
1089  yarp::sig::Vector encDataTorso;
1090  yarp::sig::Vector encDataArm;
1091 
1092  friend class liftAndGraspCallback;
1093  friend class touchCallback;
1094 
1095  virtual void init();
1096  virtual void postReachCallback();
1097  virtual void run();
1098 
1099 public:
1104 
1110  ActionPrimitivesLayer2(yarp::os::Property &opt);
1111 
1117  virtual ~ActionPrimitivesLayer2();
1118 
1145  virtual bool open(yarp::os::Property &opt);
1146 
1151  virtual bool isValid() const;
1152 
1156  virtual void close();
1157 
1173  virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
1174  const yarp::sig::Vector &d1, const yarp::sig::Vector &d2);
1175 
1187  virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
1188  const yarp::sig::Vector &d);
1189 
1201  virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
1202  const yarp::sig::Vector &d);
1203 
1210  virtual bool getExtWrench(yarp::sig::Vector &wrench) const;
1211 
1218  virtual bool getExtForceThres(double &thres) const;
1219 
1226  virtual bool setExtForceThres(const double thres);
1227 
1232  virtual bool enableContactDetection();
1233 
1238  virtual bool disableContactDetection();
1239 
1245  virtual bool isContactDetectionEnabled(bool &f) const;
1246 
1253  virtual bool checkContact(bool &f) const;
1254 };
1255 
1256 }
1257 
1258 }
1259 
1260 #endif
1261 
1262 
#define ACTIONPRIM_DISABLE_EXECTIME
Class for defining routines to be called when action is completed.
ActionPrimitivesCallback()
Default Constructor.
virtual void exec()=0
Defines the callback body to be called at the action end.
A derived class defining a first abstraction layer on top of actionPrimitives father class.
ActionPrimitivesLayer1()
Default 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).
virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
Grasp the given target (combined action).
ActionPrimitivesLayer1(yarp::os::Property &opt)
Constructor.
virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
Touch the given target (combined action).
A class that inherits from ActionPrimitivesLayer1 and integrates the force-torque sensing in order to...
virtual bool disableContactDetection()
Self-explaining :)
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 isContactDetectionEnabled(bool &f) const
Self-explaining :)
virtual bool open(yarp::os::Property &opt)
Configure the object.
virtual bool getExtForceThres(double &thres) const
Retrieve the current threshold on the external force used to stop the limb while reaching.
virtual bool isValid() const
Check if the object is initialized correctly.
virtual bool enableContactDetection()
Self-explaining :)
virtual bool getExtWrench(yarp::sig::Vector &wrench) const
Retrieve the current wrench on the end-effector.
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 checkContact(bool &f) const
Check whether the reaching has been stopped due to a contact with external objects.
virtual void close()
Deallocate the object.
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 setExtForceThres(const double thres)
Set the threshold on the external force used to stop the limb while reaching.
yarp::os::BufferedPort< yarp::sig::Vector > wbdynPortIn
ActionPrimitivesLayer2(yarp::os::Property &opt)
Constructor.
The base class defining actions.
virtual bool removeHandSeq(const std::string &handSeqKey)
Remove an already existing hand motion sequence.
std::multimap< int, int > fingers2JntsMap
ActionPrimitives(yarp::os::Property &opt)
Constructor.
virtual bool areFingersInPosition(bool &f)
Query if fingers are in position (cumulative response).
virtual bool lockActions()
Disable the possibility to yield any new action.
virtual bool syncCheckInterrupt(const bool disable=false)
Suddenly interrupt any blocking call that is pending on querying the action status.
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 cmdHand(const Action &action)
virtual bool isValid() const
Check if the object is initialized correctly.
std::condition_variable cv_motionDoneEvent
virtual bool enableReachingTimeout(const double tmo)
Enable timeout while reaching.
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 getTorsoJoints(yarp::sig::Vector &torso)
Return the control status of torso joints.
virtual bool wait(const Action &action)
virtual bool setTorsoJoints(const yarp::sig::Vector &torso)
Change the control status of torso joints.
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.
std::deque< bool > fingerInPosition
virtual bool handCheckMotionDone(const int jnt)
virtual bool setTrackingMode(const bool f)
Set the task space controller in tracking or non-tracking mode.
yarp::dev::PolyDriver polyHand
virtual bool disableArmWaving()
Disable the waving mode.
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 ~ActionPrimitives()
Destructor.
virtual bool unlockActions()
Enable the possibility to yield new actions.
virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o) const
Get the current arm pose.
virtual void close()
Deallocate the object.
iCub::action::ActionPrimitives::ActionsQueue actionsQueue
yarp::dev::IControlMode * modCtrl
ActionPrimitivesCallback * actionClb
virtual bool areFingersMoving(bool &f)
Query if fingers are moving.
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.
yarp::os::PeriodicThread * armWaver
virtual bool cmdArmWP(const Action &action)
virtual bool getHandSequence(const std::string &handSeqKey, yarp::os::Bottle &sequence)
Return a hand sequence.
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...
virtual bool stopControl()
Stop any ongoing arm/hand movements.
virtual bool setDefaultExecTime(const double execTime)
Set the default arm movement execution time.
virtual bool cmdArm(const Action &action)
virtual bool getTrackingMode() const
Get the current controller mode.
virtual bool checkActionOnGoing(bool &f, const bool sync=false)
Check whether an action is still ongoing.
ActionPrimitives()
Default Constructor.
std::map< std::string, std::deque< HandWayPoint > > handSeqMap
yarp::sig::Vector curGraspDetectionThres
virtual bool configHandSeq(yarp::os::Property &opt)
virtual double getDefaultExecTime() const
Get the current default arm movement execution time.
yarp::dev::IPositionControl * posCtrl
virtual bool configGraspModel(yarp::os::Property &opt)
virtual bool open(yarp::os::Property &opt)
Configure the object.
virtual bool getGraspModel(perception::Model *&model) const
Return the model used internally to detect external contacts.
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 void printMessage(const int logtype, const char *format,...) const
yarp::dev::PolyDriver polyCart
virtual bool getActionsLockStatus() const
Return the actions lock status.
yarp::dev::IEncoders * encCtrl
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 clearActionsQueue()
Empty the actions queue.
virtual bool getCartesianIF(yarp::dev::ICartesianControl *&ctrl) const
Return the cartesian interface used internally to control the limb.
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 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 disableReachingTimeout()
Disable timeout while reaching.
std::deque< std::string > getHandSeqList()
Return the list of available hand sequence keys.
virtual bool enableArmWaving(const yarp::sig::Vector &restPos)
Enable the waving mode that keeps on moving the arm around a predefined position.
virtual bool syncCheckReinstate()
Reinstate the blocking feature for future calls with sync switch on.
std::condition_variable cv_motionStartEvent
yarp::os::PeriodicThread * actionWP
virtual bool checkActionsDone(bool &f, const bool sync=false)
Check whether all the actions in queue are accomplished.
virtual bool handleTorsoDOF(yarp::os::Property &opt, const std::string &key)
virtual bool pushWaitState(const double tmo, ActionPrimitivesCallback *clb=NULL)
Insert a wait state in the actions queue.
yarp::dev::ICartesianControl * cartCtrl
liftAndGraspCallback(ActionPrimitivesLayer2 *_action)
virtual void exec()
Defines the callback body to be called at the action end.
virtual void exec()
Defines the callback body to be called at the action end.
ActionPrimitivesLayer2 * action
touchCallback(ActionPrimitivesLayer2 *_action)
An abstract class that provides basic methods for interfacing with the data acquisition.
Definition: models.h:62
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Struct for defining way points used for movements in the operational space.
ActionPrimitivesCallback * callback
Action callback that is executed when the waypoint is reached.
yarp::sig::Vector o
The 4x1 Vector specifying the orientation of the waypoint in axis-angle representation.
double granularity
The time granularity [s] used by the trajectory generator [s].
double trajTime
The arm execution time [s] accounting for the controller's responsivity.
yarp::sig::Vector x
The 3x1 Vector specifyng the position of the waypoint [m].
bool oEnabled
If this flag is set to true then orientation will be taken into account.
double duration
The time duration [s] to achieve the waypoint.
yarp::os::PeriodicThread * wayPointsThr