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:
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  {
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();
297  } actionsQueue;
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 
443  virtual bool open(yarp::os::Property &opt);
444 
449  virtual bool isValid() const;
450 
454  virtual void close();
455 
476  virtual bool pushAction(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
477  const std::string &handSeqKey,
478  const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
479  ActionPrimitivesCallback *clb=NULL);
480 
499  virtual bool pushAction(const yarp::sig::Vector &x,
500  const std::string &handSeqKey,
501  const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
502  ActionPrimitivesCallback *clb=NULL);
503 
516  virtual bool pushAction(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
517  const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
518  ActionPrimitivesCallback *clb=NULL);
519 
530  virtual bool pushAction(const yarp::sig::Vector &x,
531  const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
532  ActionPrimitivesCallback *clb=NULL);
533 
541  virtual bool pushAction(const std::string &handSeqKey,
542  ActionPrimitivesCallback *clb=NULL);
543 
556  virtual bool pushAction(const std::deque<ActionPrimitivesWayPoint> &wayPoints,
557  ActionPrimitivesCallback *clb=NULL);
558 
573  virtual bool pushAction(const std::deque<ActionPrimitivesWayPoint> &wayPoints,
574  const std::string &handSeqKey, ActionPrimitivesCallback *clb=NULL);
575 
583  virtual bool pushWaitState(const double tmo, ActionPrimitivesCallback *clb=NULL);
584 
598  virtual bool reachPose(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
599  const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
600 
612  virtual bool reachPosition(const yarp::sig::Vector &x,
613  const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
614 
619  virtual bool clearActionsQueue();
620 
625  virtual bool lockActions();
626 
631  virtual bool unlockActions();
632 
637  virtual bool getActionsLockStatus() const;
638 
659  virtual bool addHandSeqWP(const std::string &handSeqKey, const yarp::sig::Vector &poss,
660  const yarp::sig::Vector &vels, const yarp::sig::Vector &tols,
661  const yarp::sig::Vector &thres, const double tmo);
662 
670  virtual bool addHandSequence(const std::string &handSeqKey,
671  const yarp::os::Bottle &sequence);
672 
678  virtual bool isValidHandSeq(const std::string &handSeqKey);
679 
685  virtual bool removeHandSeq(const std::string &handSeqKey);
686 
691  std::deque<std::string> getHandSeqList();
692 
699  virtual bool getHandSequence(const std::string &handSeqKey, yarp::os::Bottle &sequence);
700 
706  virtual bool areFingersMoving(bool &f);
707 
719  virtual bool areFingersInPosition(bool &f);
720 
732  virtual bool areFingersInPosition(std::deque<bool> &f);
733 
743  virtual bool getGraspModel(perception::Model *&model) const;
744 
755  virtual bool getCartesianIF(yarp::dev::ICartesianControl *&ctrl) const;
756 
768  virtual bool getTorsoJoints(yarp::sig::Vector &torso);
769 
781  virtual bool setTorsoJoints(const yarp::sig::Vector &torso);
782 
792  virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o) const;
793 
800  virtual bool stopControl();
801 
807  virtual bool setDefaultExecTime(const double execTime);
808 
813  virtual double getDefaultExecTime() const;
814 
823  virtual bool setTrackingMode(const bool f);
824 
829  virtual bool getTrackingMode() const;
830 
840  virtual bool enableArmWaving(const yarp::sig::Vector &restPos);
841 
846  virtual bool disableArmWaving();
847 
855  virtual bool enableReachingTimeout(const double tmo);
856 
861  virtual bool disableReachingTimeout();
862 
874  virtual bool checkActionsDone(bool &f, const bool sync=false);
875 
894  virtual bool checkActionOnGoing(bool &f, const bool sync=false);
895 
905  virtual bool syncCheckInterrupt(const bool disable=false);
906 
912  virtual bool syncCheckReinstate();
913 };
914 
915 
932 {
933 public:
938 
944  ActionPrimitivesLayer1(yarp::os::Property &opt) : ActionPrimitives(opt) { }
945 
969  virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
970  const yarp::sig::Vector &d);
971 
994  virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
995  const yarp::sig::Vector &d);
996 
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,
1024  const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
1025 };
1026 
1027 
1028 // forward declaration
1029 class ActionPrimitivesLayer2;
1030 
1031 
1032 // callback for executing final grasp after contact
1034 {
1035 protected:
1037 
1038 public:
1040  action(_action) { }
1041 
1042  virtual void exec();
1043 };
1044 
1045 
1046 // callback for executing touch callback
1048 {
1049 protected:
1051 
1052 public:
1054  action(_action) { }
1055 
1056  virtual void exec();
1057 };
1058 
1059 
1071 {
1072 protected:
1077 
1079 
1082 
1083  yarp::os::BufferedPort<yarp::sig::Vector> wbdynPortIn;
1084 
1085  yarp::sig::Vector wrenchExternal;
1086 
1087  yarp::sig::Vector grasp_d2;
1088  yarp::sig::Vector grasp_o;
1089 
1090  yarp::sig::Vector encDataTorso;
1091  yarp::sig::Vector encDataArm;
1092 
1093  friend class liftAndGraspCallback;
1094  friend class touchCallback;
1095 
1096  virtual void init();
1097  virtual void postReachCallback();
1098  virtual void run();
1099 
1100 public:
1105 
1111  ActionPrimitivesLayer2(yarp::os::Property &opt);
1112 
1118  virtual ~ActionPrimitivesLayer2();
1119 
1146  virtual bool open(yarp::os::Property &opt);
1147 
1152  virtual bool isValid() const;
1153 
1157  virtual void close();
1158 
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);
1176 
1188  virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
1189  const yarp::sig::Vector &d);
1190 
1202  virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
1203  const yarp::sig::Vector &d);
1204 
1211  virtual bool getExtWrench(yarp::sig::Vector &wrench) const;
1212 
1219  virtual bool getExtForceThres(double &thres) const;
1220 
1227  virtual bool setExtForceThres(const double thres);
1228 
1233  virtual bool enableContactDetection();
1234 
1239  virtual bool disableContactDetection();
1240 
1246  virtual bool isContactDetectionEnabled(bool &f) const;
1247 
1254  virtual bool checkContact(bool &f) const;
1255 };
1256 
1257 }
1258 
1259 }
1260 
1261 #endif
1262 
1263 
iCub::action::ActionPrimitives::lockActions
virtual bool lockActions()
Disable the possibility to yield any new action.
Definition: actionPrimitives.cpp:837
iCub::action::ActionPrimitives::handSeqMap
std::map< std::string, std::deque< HandWayPoint > > handSeqMap
Definition: actionPrimitives.h:298
iCub::action::ActionPrimitives::run
virtual void run()
Definition: actionPrimitives.cpp:1270
iCub::action::ActionPrimitives::postReachCallback
virtual void postReachCallback()
Definition: actionPrimitives.cpp:816
iCub::action::ActionPrimitivesWayPoint::callback
ActionPrimitivesCallback * callback
Action callback that is executed when the waypoint is reached.
Definition: actionPrimitives.h:175
iCub::action::ActionPrimitives::fingersMovingJntsSet
std::set< int > fingersMovingJntsSet
Definition: actionPrimitives.h:254
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::action::ActionPrimitives::Action::execArm
bool execArm
Definition: actionPrimitives.h:275
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::polyHand
yarp::dev::PolyDriver polyHand
Definition: actionPrimitives.h:200
iCub::action::ActionPrimitives::curHandTols
yarp::sig::Vector curHandTols
Definition: actionPrimitives.h:247
iCub::action::ActionPrimitivesLayer2::disableContactDetection
virtual bool disableContactDetection()
Self-explaining :)
Definition: actionPrimitives.cpp:2424
iCub::action::ActionPrimitives::enableTorsoSw
yarp::sig::Vector enableTorsoSw
Definition: actionPrimitives.h:243
iCub::action::ActionPrimitives::locked
bool locked
Definition: actionPrimitives.h:230
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::_pushAction
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)
iCub::action::ActionPrimitives::getCartesianIF
virtual bool getCartesianIF(yarp::dev::ICartesianControl *&ctrl) const
Return the cartesian interface used internally to control the limb.
Definition: actionPrimitives.cpp:1806
iCub::action::ActionPrimitivesLayer2::encDataTorso
yarp::sig::Vector encDataTorso
Definition: actionPrimitives.h:1090
iCub::action::touchCallback
Definition: actionPrimitives.h:1047
iCub::action::ActionPrimitivesLayer2::checkContact
virtual bool checkContact(bool &f) const
Check whether the reaching has been stopped due to a contact with external objects.
Definition: actionPrimitives.cpp:2450
iCub::action::touchCallback::touchCallback
touchCallback(ActionPrimitivesLayer2 *_action)
Definition: actionPrimitives.h:1053
iCub::action::ActionPrimitives::reachPose
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...
Definition: actionPrimitives.cpp:1147
iCub::action::ActionPrimitives::latchTimerReach
double latchTimerReach
Definition: actionPrimitives.h:237
iCub::action::ActionPrimitives::armMoveDone
bool armMoveDone
Definition: actionPrimitives.h:216
iCub::action::ActionPrimitives
Definition: actionPrimitives.h:193
iCub::action::ActionPrimitives::addHandSeqWP
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...
Definition: actionPrimitives.cpp:1532
iCub::action::ActionPrimitives::actionsQueue
iCub::action::ActionPrimitives::ActionsQueue actionsQueue
iCub::action::ActionPrimitives::HandWayPoint::tag
std::string tag
Definition: actionPrimitives.h:261
iCub::action::ActionPrimitives::enableTorsoDof
virtual void enableTorsoDof()
Definition: actionPrimitives.cpp:1375
iCub::action::ActionPrimitives::Action::execTime
double execTime
Definition: actionPrimitives.h:278
iCub::action::ActionPrimitives::getHandSeqList
std::deque< std::string > getHandSeqList()
Return the list of available hand sequence keys.
Definition: actionPrimitives.cpp:1675
iCub::action::ActionPrimitives::HandWayPoint::poss
yarp::sig::Vector poss
Definition: actionPrimitives.h:262
iCub::action::ActionPrimitivesLayer2::grasp
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.
iCub::action::ActionPrimitivesLayer2::wbdynPortIn
yarp::os::BufferedPort< yarp::sig::Vector > wbdynPortIn
Definition: actionPrimitives.h:1083
x2
x2
Definition: compute_ekf_fast.m:9
iCub::action::ActionPrimitives::checkActionOnGoing
virtual bool checkActionOnGoing(bool &f, const bool sync=false)
Check whether an action is still ongoing.
Definition: actionPrimitives.cpp:2021
iCub::action::ActionPrimitives::Action::waitState
bool waitState
Definition: actionPrimitives.h:272
iCub::action::ActionPrimitives::fingerInPosition
std::deque< bool > fingerInPosition
Definition: actionPrimitives.h:222
iCub::action::ActionPrimitives::fingersJnts
std::vector< int > fingersJnts
Definition: actionPrimitives.h:252
iCub::action::ActionPrimitives::unlockActions
virtual bool unlockActions()
Enable the possibility to yield new actions.
Definition: actionPrimitives.cpp:850
iCub::action::ActionPrimitives::curGraspDetectionThres
yarp::sig::Vector curGraspDetectionThres
Definition: actionPrimitives.h:248
iCub::action::ActionPrimitives::HandWayPoint::tols
yarp::sig::Vector tols
Definition: actionPrimitives.h:264
iCub::action::ActionPrimitives::Action::wayPointsThr
yarp::os::PeriodicThread * wayPointsThr
Definition: actionPrimitives.h:286
iCub::action::ActionPrimitives::Action::handWP
HandWayPoint handWP
Definition: actionPrimitives.h:282
iCub::action::ActionPrimitives::cv_motionDoneEvent
std::condition_variable cv_motionDoneEvent
Definition: actionPrimitives.h:214
iCub::action::ActionPrimitives::default_exec_time
double default_exec_time
Definition: actionPrimitives.h:233
iCub::action::ActionPrimitivesLayer2::init
virtual void init()
Definition: actionPrimitives.cpp:2170
iCub::action::ActionPrimitives::Action::tmo
double tmo
Definition: actionPrimitives.h:273
iCub::action::ActionPrimitives::isValidHandSeq
virtual bool isValidHandSeq(const std::string &handSeqKey)
Check whether a sequence key is defined.
Definition: actionPrimitives.cpp:1648
iCub::action::ActionPrimitives::HandWayPoint::vels
yarp::sig::Vector vels
Definition: actionPrimitives.h:263
iCub::action::ActionPrimitivesWayPoint::granularity
double granularity
The time granularity [s] used by the trajectory generator [s].
Definition: actionPrimitives.h:169
iCub::action::ActionPrimitives::Action::clb
ActionPrimitivesCallback * clb
Definition: actionPrimitives.h:288
iCub::action::ActionPrimitivesLayer2::isValid
virtual bool isValid() const
Check if the object is initialized correctly.
Definition: actionPrimitives.cpp:2283
iCub::action::liftAndGraspCallback
Definition: actionPrimitives.h:1033
iCub::action::ActionPrimitives::getHandSequence
virtual bool getHandSequence(const std::string &handSeqKey, yarp::os::Bottle &sequence)
Return a hand sequence.
Definition: actionPrimitives.cpp:1688
iCub::action::ActionPrimitivesLayer2::close
virtual void close()
Deallocate the object.
Definition: actionPrimitives.cpp:2290
iCub::action::ActionPrimitives::mtx
std::mutex mtx
Definition: actionPrimitives.h:210
iCub::action::ActionPrimitivesLayer2::encDataArm
yarp::sig::Vector encDataArm
Definition: actionPrimitives.h:1091
iCub::action::ActionPrimitives::latchArmMoveDone
bool latchArmMoveDone
Definition: actionPrimitives.h:218
iCub::action::ActionPrimitivesCallback::ActionPrimitivesCallback
ActionPrimitivesCallback()
Default Constructor.
Definition: actionPrimitives.h:117
iCub::action::ActionPrimitives::~ActionPrimitives
virtual ~ActionPrimitives()
Destructor.
Definition: actionPrimitives.cpp:1357
iCub::action::ActionPrimitives::waitTmo
double waitTmo
Definition: actionPrimitives.h:234
iCub::action::liftAndGraspCallback::exec
virtual void exec()
Defines the callback body to be called at the action end.
Definition: actionPrimitives.cpp:2126
iCub::action::ActionPrimitives::Action::handSeqTerminator
bool handSeqTerminator
Definition: actionPrimitives.h:283
iCub::action::ActionPrimitives::disableReachingTimeout
virtual bool disableReachingTimeout()
Disable timeout while reaching.
Definition: actionPrimitives.cpp:1988
iCub::action::ActionPrimitives::isHandSeqEnded
virtual bool isHandSeqEnded()
Definition: actionPrimitives.cpp:746
iCub::action::ActionPrimitives::printMessage
virtual void printMessage(const int logtype, const char *format,...) const
Definition: actionPrimitives.cpp:371
iCub::action::ActionPrimitives::reachTmoEnabled
bool reachTmoEnabled
Definition: actionPrimitives.h:229
iCub::action::ActionPrimitivesLayer2::touch
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.
Definition: actionPrimitives.cpp:2353
iCub::action::ActionPrimitivesWayPoint::ActionPrimitivesWayPoint
ActionPrimitivesWayPoint()
Default Constructor.
Definition: actionPrimitives.cpp:294
iCub::action::ActionPrimitives::setDefaultExecTime
virtual bool setDefaultExecTime(const double execTime)
Set the default arm movement execution time.
Definition: actionPrimitives.cpp:1894
iCub::action::ActionPrimitives::disableArmWaving
virtual bool disableArmWaving()
Disable the waving mode.
Definition: actionPrimitives.cpp:1958
iCub::action::ActionPrimitivesLayer2::getExtWrench
virtual bool getExtWrench(yarp::sig::Vector &wrench) const
Retrieve the current wrench on the end-effector.
Definition: actionPrimitives.cpp:2372
iCub::action::ActionPrimitivesLayer2::grasp_o
yarp::sig::Vector grasp_o
Definition: actionPrimitives.h:1088
iCub::action::ActionPrimitives::Action::execWayPoints
bool execWayPoints
Definition: actionPrimitives.h:285
iCub::action::ActionPrimitivesLayer2::configuredLayer2
bool configuredLayer2
Definition: actionPrimitives.h:1074
iCub::action::ActionPrimitives::polyCart
yarp::dev::PolyDriver polyCart
Definition: actionPrimitives.h:201
iCub::action::ActionPrimitivesWayPoint::o
yarp::sig::Vector o
The 4x1 Vector specifying the orientation of the waypoint in axis-angle representation.
Definition: actionPrimitives.h:143
iCub::action::ActionPrimitives::actionClb
ActionPrimitivesCallback * actionClb
Definition: actionPrimitives.h:291
iCub::action::ActionPrimitives::latchTimerWait
double latchTimerWait
Definition: actionPrimitives.h:236
iCub::action::ActionPrimitivesLayer1
Definition: actionPrimitives.h:931
iCub::action::ActionPrimitives::configGraspModel
virtual bool configGraspModel(yarp::os::Property &opt)
Definition: actionPrimitives.cpp:529
iCub::action::ActionPrimitives::jHandMin
int jHandMin
Definition: actionPrimitives.h:240
iCub::action::ArmWayPoints
Definition: actionPrimitives.cpp:75
iCub::action::ActionPrimitivesLayer2::execLiftAndGrasp
liftAndGraspCallback * execLiftAndGrasp
Definition: actionPrimitives.h:1080
iCub::action::ActionPrimitives::jHandMax
int jHandMax
Definition: actionPrimitives.h:241
iCub::action::ActionPrimitivesLayer2::~ActionPrimitivesLayer2
virtual ~ActionPrimitivesLayer2()
Destructor.
Definition: actionPrimitives.cpp:2463
iCub::action::ActionPrimitives::Action::x
yarp::sig::Vector x
Definition: actionPrimitives.h:276
iCub::action::ActionPrimitives::getGraspModel
virtual bool getGraspModel(perception::Model *&model) const
Return the model used internally to detect external contacts.
Definition: actionPrimitives.cpp:1793
iCub::action::ActionPrimitivesCallback::exec
virtual void exec()=0
Defines the callback body to be called at the action end.
iCub::action::ActionPrimitives::encCtrl
yarp::dev::IEncoders * encCtrl
Definition: actionPrimitives.h:203
iCub::action::ActionPrimitives::latchTimerHand
double latchTimerHand
Definition: actionPrimitives.h:250
iCub::action::ActionPrimitives::setTrackingMode
virtual bool setTrackingMode(const bool f)
Set the task space controller in tracking or non-tracking mode.
Definition: actionPrimitives.cpp:1914
iCub::action::ActionPrimitives::areFingersInPosition
virtual bool areFingersInPosition(bool &f)
Query if fingers are in position (cumulative response).
Definition: actionPrimitives.cpp:1765
iCub::action::ActionPrimitives::closed
bool closed
Definition: actionPrimitives.h:225
iCub::action::ActionPrimitivesLayer1::ActionPrimitivesLayer1
ActionPrimitivesLayer1(yarp::os::Property &opt)
Constructor.
Definition: actionPrimitives.h:944
iCub::action::ActionPrimitives::Action::oEnabled
bool oEnabled
Definition: actionPrimitives.h:279
iCub::action::ActionPrimitives::ActionPrimitives
ActionPrimitives()
Default Constructor.
Definition: actionPrimitives.cpp:321
iCub::action::ActionPrimitives::isValid
virtual bool isValid() const
Check if the object is initialized correctly.
Definition: actionPrimitives.cpp:364
iCub::action::ActionPrimitivesLayer2::enableContactDetection
virtual bool enableContactDetection()
Self-explaining :)
Definition: actionPrimitives.cpp:2411
iCub::action::ActionPrimitives::getPose
virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o) const
Get the current arm pose.
Definition: actionPrimitives.cpp:1848
iCub::action::ActionPrimitives::handMoveDone
bool handMoveDone
Definition: actionPrimitives.h:217
iCub::action::ActionPrimitives::Action::execHand
bool execHand
Definition: actionPrimitives.h:281
ACTIONPRIM_DISABLE_EXECTIME
#define ACTIONPRIM_DISABLE_EXECTIME
Definition: actionPrimitives.h:96
iCub::action::ActionPrimitives::ActionsQueue
Definition: actionPrimitives.h:293
iCub::action::ActionPrimitives::handleTorsoDOF
virtual bool handleTorsoDOF(yarp::os::Property &opt, const std::string &key)
Definition: actionPrimitives.cpp:404
iCub::action::ActionPrimitives::open
virtual bool open(yarp::os::Property &opt)
Configure the object.
Definition: actionPrimitives.cpp:582
iCub::action::ActionPrimitivesLayer1::grasp
virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
Grasp the given target (combined action).
Definition: actionPrimitives.cpp:2071
iCub::action::ActionPrimitives::getDefaultExecTime
virtual double getDefaultExecTime() const
Get the current default arm movement execution time.
Definition: actionPrimitives.cpp:1907
iCub::action::ActionPrimitives::cmdArmWP
virtual bool cmdArmWP(const Action &action)
Definition: actionPrimitives.cpp:1466
x
x
Definition: compute_ekf_sym.m:21
iCub::action::ActionPrimitives::graspModel
perception::Model * graspModel
Definition: actionPrimitives.h:207
iCub::action::ActionPrimitives::cmdHand
virtual bool cmdHand(const Action &action)
Definition: actionPrimitives.cpp:1484
iCub::action::ActionPrimitives::syncCheckReinstate
virtual bool syncCheckReinstate()
Reinstate the blocking feature for future calls with sync switch on.
Definition: actionPrimitives.cpp:2058
iCub::action::ActionPrimitivesLayer2::postReachCallback
virtual void postReachCallback()
Definition: actionPrimitives.cpp:2183
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::action::ActionPrimitives::cv_motionStartEvent
std::condition_variable cv_motionStartEvent
Definition: actionPrimitives.h:212
iCub::action::ActionPrimitives::part
std::string part
Definition: actionPrimitives.h:198
iCub::action::ActionPrimitives::HandWayPoint::thres
yarp::sig::Vector thres
Definition: actionPrimitives.h:265
iCub::action::ActionPrimitives::syncCheckInterrupt
virtual bool syncCheckInterrupt(const bool disable=false)
Suddenly interrupt any blocking call that is pending on querying the action status.
Definition: actionPrimitives.cpp:2041
iCub::action::ActionPrimitivesWayPoint::oEnabled
bool oEnabled
If this flag is set to true then orientation will be taken into account.
Definition: actionPrimitives.h:149
iCub::action::ActionPrimitives::reachPosition
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...
Definition: actionPrimitives.cpp:1176
iCub::action::liftAndGraspCallback::liftAndGraspCallback
liftAndGraspCallback(ActionPrimitivesLayer2 *_action)
Definition: actionPrimitives.h:1039
models.h
iCub::action::ActionPrimitives::fingersJntsSet
std::set< int > fingersJntsSet
Definition: actionPrimitives.h:253
iCub::action::ActionPrimitivesWayPoint::duration
double duration
The time duration [s] to achieve the waypoint.
Definition: actionPrimitives.h:156
iCub::action::ActionPrimitivesLayer2
Definition: actionPrimitives.h:1070
iCub::action::ActionPrimitives::HandWayPoint::tmo
double tmo
Definition: actionPrimitives.h:266
iCub::action::ActionPrimitives::init
virtual void init()
Definition: actionPrimitives.cpp:338
iCub::action::ActionPrimitives::configured
bool configured
Definition: actionPrimitives.h:224
iCub::action::ActionPrimitives::local
std::string local
Definition: actionPrimitives.h:197
f
f
Definition: compute_ekf_sym.m:22
iCub::action::ActionPrimitives::checkEnabled
bool checkEnabled
Definition: actionPrimitives.h:226
iCub::action::liftAndGraspCallback::action
ActionPrimitivesLayer2 * action
Definition: actionPrimitives.h:1036
iCub::action::ActionPrimitives::close
virtual void close()
Deallocate the object.
Definition: actionPrimitives.cpp:701
iCub::action::ActionPrimitives::latchTimerReachLog
double latchTimerReachLog
Definition: actionPrimitives.h:238
iCub::action::ActionPrimitives::curHandTmo
double curHandTmo
Definition: actionPrimitives.h:249
iCub::action::ActionPrimitives::enableReachingTimeout
virtual bool enableReachingTimeout(const double tmo)
Enable timeout while reaching.
Definition: actionPrimitives.cpp:1973
iCub::action::ActionPrimitivesLayer2::execTouch
touchCallback * execTouch
Definition: actionPrimitives.h:1081
iCub::action::ActionPrimitivesLayer2::run
virtual void run()
Definition: actionPrimitives.cpp:2194
iCub::action::touchCallback::exec
virtual void exec()
Defines the callback body to be called at the action end.
Definition: actionPrimitives.cpp:2145
iCub::action::ActionPrimitivesLayer2::contactDetectionOn
bool contactDetectionOn
Definition: actionPrimitives.h:1075
iCub::action::ActionPrimitives::posCtrl
yarp::dev::IPositionControl * posCtrl
Definition: actionPrimitives.h:204
iCub::action::ActionPrimitives::removeHandSeq
virtual bool removeHandSeq(const std::string &handSeqKey)
Remove an already existing hand motion sequence.
Definition: actionPrimitives.cpp:1660
iCub::action::ActionPrimitives::setTorsoJoints
virtual bool setTorsoJoints(const yarp::sig::Vector &torso)
Change the control status of torso joints.
Definition: actionPrimitives.cpp:1832
iCub::action::ActionPrimitives::ActionsQueue::clear
void clear()
Definition: actionPrimitives.cpp:307
iCub::action::ActionPrimitives::pushWaitState
virtual bool pushWaitState(const double tmo, ActionPrimitivesCallback *clb=NULL)
Insert a wait state in the actions queue.
Definition: actionPrimitives.cpp:1123
iCub::action::ActionPrimitives::mtx_motionStartEvent
std::mutex mtx_motionStartEvent
Definition: actionPrimitives.h:211
x1
x1
Definition: compute_ekf_sym.m:28
iCub::action::ActionPrimitives::tracking_mode
bool tracking_mode
Definition: actionPrimitives.h:227
iCub::perception::Model
Definition: models.h:61
iCub::action::ActionPrimitives::Action
Definition: actionPrimitives.h:269
iCub::action::ActionPrimitives::areFingersMoving
virtual bool areFingersMoving(bool &f)
Query if fingers are moving.
Definition: actionPrimitives.cpp:1751
iCub::action::ActionPrimitives::reachTmo
double reachTmo
Definition: actionPrimitives.h:235
iCub::action::ActionPrimitives::curHandFinalPoss
yarp::sig::Vector curHandFinalPoss
Definition: actionPrimitives.h:246
iCub::action::ActionPrimitivesLayer2::isContactDetectionEnabled
virtual bool isContactDetectionEnabled(bool &f) const
Self-explaining :)
Definition: actionPrimitives.cpp:2437
iCub::action::ActionPrimitives::execQueuedAction
virtual bool execQueuedAction()
Definition: actionPrimitives.cpp:1204
iCub::action::ActionPrimitives::disableTorsoDof
virtual void disableTorsoDof()
Definition: actionPrimitives.cpp:1388
iCub::action::ActionPrimitivesLayer1::ActionPrimitivesLayer1
ActionPrimitivesLayer1()
Default Constructor.
Definition: actionPrimitives.h:937
iCub::action::ActionPrimitives::actionWP
yarp::os::PeriodicThread * actionWP
Definition: actionPrimitives.h:292
iCub::action::ActionPrimitives::armWaver
yarp::os::PeriodicThread * armWaver
Definition: actionPrimitives.h:209
iCub::action::ActionPrimitivesWayPoint::trajTime
double trajTime
The arm execution time [s] accounting for the controller's responsivity.
Definition: actionPrimitives.h:163
iCub::action::ActionPrimitives::configHandSeq
virtual bool configHandSeq(yarp::os::Property &opt)
Definition: actionPrimitives.cpp:465
iCub::action::ActionPrimitives::robot
std::string robot
Definition: actionPrimitives.h:196
iCub::action::ActionPrimitivesCallback
Definition: actionPrimitives.h:111
iCub::action::ActionPrimitives::verbose
bool verbose
Definition: actionPrimitives.h:231
iCub::action::ActionPrimitivesLayer2::ActionPrimitivesLayer2
ActionPrimitivesLayer2()
Default Constructor.
Definition: actionPrimitives.cpp:2153
iCub::action::ActionPrimitives::stopControl
virtual bool stopControl()
Stop any ongoing arm/hand movements.
Definition: actionPrimitives.cpp:1861
iCub::action::ActionPrimitivesLayer2::grasp_d2
yarp::sig::Vector grasp_d2
Definition: actionPrimitives.h:1087
iCub::action::ActionPrimitives::cartCtrl
yarp::dev::ICartesianControl * cartCtrl
Definition: actionPrimitives.h:205
iCub::action::ActionPrimitivesLayer2::open
virtual bool open(yarp::os::Property &opt)
Configure the object.
Definition: actionPrimitives.cpp:2241
iCub::action::ActionPrimitivesLayer2::setExtForceThres
virtual bool setExtForceThres(const double thres)
Set the threshold on the external force used to stop the limb while reaching.
Definition: actionPrimitives.cpp:2398
iCub::action::touchCallback::action
ActionPrimitivesLayer2 * action
Definition: actionPrimitives.h:1050
iCub::action::ActionPrimitives::handCheckMotionDone
virtual bool handCheckMotionDone(const int jnt)
Definition: actionPrimitives.cpp:1364
iCub::action::ActionPrimitives::fingers2JntsMap
std::multimap< int, int > fingers2JntsMap
Definition: actionPrimitives.h:255
iCub::action::ActionPrimitives::torsoActive
bool torsoActive
Definition: actionPrimitives.h:228
iCub::action::ActionPrimitivesLayer1::tap
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).
Definition: actionPrimitives.cpp:2106
iCub::action::ActionPrimitives::enableArmWaving
virtual bool enableArmWaving(const yarp::sig::Vector &restPos)
Enable the waving mode that keeps on moving the arm around a predefined position.
Definition: actionPrimitives.cpp:1939
iCub::action::ActionPrimitives::clearActionsQueue
virtual bool clearActionsQueue()
Empty the actions queue.
Definition: actionPrimitives.cpp:823
iCub::action::ActionPrimitives::wait
virtual bool wait(const Action &action)
Definition: actionPrimitives.cpp:1401
iCub::action::ActionPrimitives::cmdArm
virtual bool cmdArm(const Action &action)
Definition: actionPrimitives.cpp:1417
iCub::action::ActionPrimitives::addHandSequence
virtual bool addHandSequence(const std::string &handSeqKey, const yarp::os::Bottle &sequence)
Define an hand motion sequence from a configuration bottle.
Definition: actionPrimitives.cpp:1557
iCub::action::ActionPrimitives::disableTorsoSw
yarp::sig::Vector disableTorsoSw
Definition: actionPrimitives.h:244
iCub::action::ActionPrimitives::handSeqTerminator
bool handSeqTerminator
Definition: actionPrimitives.h:220
iCub::action::ActionPrimitivesLayer2::contactDetected
bool contactDetected
Definition: actionPrimitives.h:1076
iCub::action::ActionPrimitives::mtx_motionDoneEvent
std::mutex mtx_motionDoneEvent
Definition: actionPrimitives.h:213
iCub::action::ActionPrimitivesLayer2::getExtForceThres
virtual bool getExtForceThres(double &thres) const
Retrieve the current threshold on the external force used to stop the limb while reaching.
Definition: actionPrimitives.cpp:2385
iCub::action::ActionPrimitives::modCtrl
yarp::dev::IControlMode * modCtrl
Definition: actionPrimitives.h:202
iCub::action::ActionPrimitivesWayPoint
Definition: actionPrimitives.h:132
iCub::action::ActionPrimitives::HandWayPoint
Definition: actionPrimitives.h:259
iCub::action::ActionPrimitivesLayer2::skipFatherPart
bool skipFatherPart
Definition: actionPrimitives.h:1073
iCub::action::ActionPrimitives::getTrackingMode
virtual bool getTrackingMode() const
Get the current controller mode.
Definition: actionPrimitives.cpp:1932
iCub::action::ActionPrimitives::execPendingHandSequences
virtual bool execPendingHandSequences()
Definition: actionPrimitives.cpp:1245
iCub::action::ActionPrimitives::fingersInPosition
bool fingersInPosition
Definition: actionPrimitives.h:221
iCub::action::ActionPrimitives::Action::o
yarp::sig::Vector o
Definition: actionPrimitives.h:277
iCub::action::ActionPrimitivesLayer2::wrenchExternal
yarp::sig::Vector wrenchExternal
Definition: actionPrimitives.h:1085
iCub::action::ActionPrimitivesLayer1::touch
virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o, const yarp::sig::Vector &d)
Touch the given target (combined action).
Definition: actionPrimitives.cpp:2089
iCub::action::ActionPrimitivesLayer2::ext_force_thres
double ext_force_thres
Definition: actionPrimitives.h:1078
iCub::action::ActionPrimitives::getActionsLockStatus
virtual bool getActionsLockStatus() const
Return the actions lock status.
Definition: actionPrimitives.cpp:863
iCub::action::ActionPrimitives::getTorsoJoints
virtual bool getTorsoJoints(yarp::sig::Vector &torso)
Return the control status of torso joints.
Definition: actionPrimitives.cpp:1819
iCub::action::ActionPrimitivesWayPoint::x
yarp::sig::Vector x
The 3x1 Vector specifyng the position of the waypoint [m].
Definition: actionPrimitives.h:137
iCub::action::ActionPrimitives::latchHandMoveDone
bool latchHandMoveDone
Definition: actionPrimitives.h:219