iCub-main
MotorThread.h
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Carlo Ciliberto, Vadim Tikhanoff
4 * email: carlo.ciliberto@iit.it vadim.tikhanoff@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 
19 
20 #ifndef __MOTOR_THREAD__
21 #define __MOTOR_THREAD__
22 
23 #include <yarp/os/all.h>
24 #include <yarp/dev/all.h>
25 #include <yarp/sig/all.h>
26 #include <yarp/math/SVD.h>
27 #include <yarp/math/Math.h>
28 #include <iCub/ctrl/math.h>
29 #include <iCub/ctrl/pids.h>
32 
33 
34 #include <vector>
35 #include <limits>
36 
37 #include <iCub/utils.h>
38 
39 #define HEAD_MODE_IDLE 0
40 #define HEAD_MODE_GO_HOME 1
41 #define HEAD_MODE_TRACK_HAND 2
42 #define HEAD_MODE_TRACK_TEMP 3
43 #define HEAD_MODE_TRACK_CART 4
44 #define HEAD_MODE_TRACK_FIX 5
45 
46 #define ARM_MODE_IDLE 0
47 #define ARM_MODE_LEARN_ACTION 1
48 #define ARM_MODE_LEARN_KINOFF 2
49 #define ARM_MODE_FINE_REACHING 3
50 
51 #define GRASP_STATE_IDLE 0
52 #define GRASP_STATE_ABOVE 1
53 #define GRASP_STATE_SIDE 2
54 
55 #define ARM_HOMING_PERIOD 1.5 //[s]
56 
57 
58 #define S2C_HOMOGRAPHY yarp::os::createVocab32('h','o','m','o')
59 #define S2C_DISPARITY yarp::os::createVocab32('d','i','s','p')
60 #define S2C_NETWORK yarp::os::createVocab32('n','e','t','w')
61 
62 
63 using namespace std;
64 using namespace yarp::os;
65 using namespace yarp::sig;
66 using namespace yarp::dev;
67 using namespace yarp::math;
68 using namespace iCub::ctrl;
69 using namespace iCub::action;
70 using namespace iCub::perception;
71 
72 
73 struct Dragger
74 {
75  ICartesianControl *ctrl;
76 
78  double t0;
79  double samplingRate;
80  Vector x0;
81  int arm;
82  Bottle actions;
83  string actionName;
84  double min;
85  double max;
86 
88 
90  double damping;
91  double inertia;
92 
94  {
95  I=NULL;
96  }
97 
99  {
100  if(I!=NULL)
101  delete I;
102  }
103 
104  void init(Bottle &initializer, double thread_rate);
105 };
106 
107 
108 
109 class MotorThread: public PeriodicThread
110 {
111 private:
112  ResourceFinder &rf;
113 
114  StereoTarget &stereo_target;
115 
117 
118  PolyDriver *drv_head;
119  PolyDriver *drv_torso;
120  PolyDriver *drv_ctrl_gaze;
121  PolyDriver *drv_arm[2];
122  PolyDriver *drv_car_arm[2];
123 
124  IEncoders *enc_arm[2];
125  IEncoders *enc_head;
126  IEncoders *enc_torso;
127  IGazeControl *ctrl_gaze;
128 
129  IPositionControl *pos_torso;
130  IVelocityControl *vel_torso;
131  IControlMode *ctrl_mode_torso;
132  IInteractionMode *int_mode_torso;
133  IImpedanceControl *ctrl_impedance_torso;
134 
135  IControlMode *ctrl_mode_arm[2];
136  IPositionControl *pos_arm[2];
137  IInteractionMode *int_mode_arm[2];
138  IImpedanceControl *ctrl_impedance_arm[2];
139 
140  int gaze_context;
141 
142  bool gazeUnderControl;
143  bool status_impedance_on;
144 
145  bool stereo_track;
146  int dominant_eye;
147 
149 
150  ActionPrimitives *action[2];
151  int action_context[2];
152 
153  bool homeFixCartType;
154  Vector homeFix;
155  Vector reachAboveDisp;
156  Vector graspAboveRelief;
157  Vector pushAboveRelief;
158  double extForceThresh[2];
159  double default_exec_time;
160  double reachingTimeout;
161 
162  //tool
163  Vector takeToolPos[2];
164  Vector takeToolOrient[2];
165 
166  Vector homePos[2];
167  Vector homeOrient[2];
168  Vector reachSideDisp[2];
169  Vector reachAboveOrient[2];
170  Vector reachAboveCata[2];
171  Vector reachSideOrient[2];
172  Vector deployPos[2];
173  Vector deployOrient[2];
174  Vector drawNearPos[2];
175  Vector drawNearOrient[2];
176  Vector shiftPos[2];
177  Vector expectPos[2];
178  Vector expectOrient[2];
179 
180  //tactile perception
181  iCub::perception::Model *graspModel[2];
182  string graspFile[2];
183 
184  //stereo 2 cartesian mode
185  int modeS2C;
186  bool neuralNetworkAvailable;
187  RpcClient disparityPort;
188 
189  vector<Vector> pos_torsoes;
190  vector<Vector> handPoses;
191  vector<Vector> headPoses;
192 
193  Bottle bKinOffsets;
194  string kinematics_file;
195  Vector defaultKinematicOffset[2];
196  Vector currentKinematicOffset[2];
197  Vector table;
198  double go_up_distance;
199  double table_height;
200  double table_height_tolerance;
201 
202  int armInUse;
203  int head_mode;
204  int arm_mode;
205 
206  int grasp_state;
207 
208  bool waveing;
209  bool avoid_table;
210  bool closed;
211  bool interrupted;
212 
213  RpcClient wbdPort;
214 
215  //drag stuff
216  Port wrenchPort[2];
217 
218  string actions_path;
219  Dragger dragger;
220 
221  Vector track_cartesian_target;
222  double avoid_table_height[2];
223 
224  double random_pos_y;
225 
226  bool storeContext(const int arm);
227  bool restoreContext(const int arm);
228  bool deleteContext(const int arm);
229 
230  bool storeContext(ActionPrimitives *action);
231  bool restoreContext(ActionPrimitives *action);
232  bool deleteContext(ActionPrimitives *action);
233 
234  bool loadExplorationPoses(const string &file_name);
235  int checkArm(int arm);
236  int checkArm(int arm, Vector &xd, const bool applyOffset=true);
237  bool checkOptions(const Bottle &options, const string &parameter);
238  Vector eye2root(const Vector &out,bool forehead);
239  bool stereoToCartesianHomography(const Vector &stereo, Vector &xd);
240  bool stereoToCartesianDisparity(const Vector &stereo, Vector &xd);
241  bool stereoToCartesianNetwork(const Vector &stereo, Vector &xd);
242  Vector randomDeployOffset();
243  bool getGeneralOptions(Bottle &b);
244  bool loadKinematicOffsets();
245  bool saveKinematicOffsets();
246  bool getArmOptions(Bottle &b, const int &arm);
247  void goWithTorsoUpright(ActionPrimitives *action, const Vector &xin, const Vector &oin);
248  bool avoidTable(bool avoid);
249  void close();
250 
251 public:
252  MotorThread(ResourceFinder &_rf, Initializer *initializer)
253  :PeriodicThread(0.02),rf(_rf),stereo_target(initializer->stereo_target),opcPort(initializer->port_opc),
254  track_cartesian_target(3,0.0)
255  {
256  ctrl_gaze=NULL;
257  ctrl_mode_torso=NULL;
258  int_mode_torso=NULL;
259  drv_head=drv_torso=drv_ctrl_gaze=NULL;
260  drv_arm[LEFT]=drv_arm[RIGHT]=NULL;
261  drv_car_arm[LEFT]=drv_car_arm[RIGHT]=NULL;
262  ctrl_mode_arm[LEFT]=ctrl_mode_arm[RIGHT]=NULL;
263  int_mode_arm[LEFT]=int_mode_arm[RIGHT]=NULL;
264  action[LEFT]=action[RIGHT]=NULL;
265  closed=false;
266  }
267 
268  virtual bool threadInit();
269  virtual void run();
270  virtual void threadRelease();
271  virtual void onStop();
272 
274  {
275  if (wbdPort.getOutputCount()>0)
276  {
277  Bottle cmd,reply;
278  cmd.addString("calib");
279  cmd.addString("all");
280  wbdPort.write(cmd,reply);
281  Time::delay(0.3);
282  return true;
283  }
284  else
285  return false;
286  }
287 
288  void track(Bottle &options)
289  {
290  Bottle *bTarget=options.find("target").asList();
291  if (!gazeUnderControl)
292  {
293  ctrl_gaze->stopControl();
294  ctrl_gaze->restoreContext(gaze_context);
295 
296  if (checkOptions(options,"no_sacc"))
297  ctrl_gaze->setSaccadesMode(false);
298 
299  if (bTarget!=NULL)
300  head_mode=(bTarget->check("cartesian")?HEAD_MODE_TRACK_CART:HEAD_MODE_TRACK_TEMP);
301  else
302  head_mode=HEAD_MODE_TRACK_TEMP;
303 
304  gazeUnderControl=true;
305  }
306 
307  if (bTarget!=NULL)
308  if (Bottle *bCartesian=bTarget->find("cartesian").asList())
309  if ((size_t)bCartesian->size()==track_cartesian_target.length())
310  for (size_t i=0; i<track_cartesian_target.length(); i++)
311  track_cartesian_target[i]=bCartesian->get(i).asFloat64();
312  }
313 
314 
315  void lookAtHand(Bottle &options)
316  {
317  if(checkOptions(options,"no_sacc"))
318  ctrl_gaze->setSaccadesMode(false);
319  head_mode=HEAD_MODE_TRACK_HAND;
320  }
321 
322  void keepFixation(Bottle &options)
323  {
324  gazeUnderControl=true;
325  ctrl_gaze->setTrackingMode(true);
326  if(checkOptions(options,"no_sacc"))
327  ctrl_gaze->setSaccadesMode(false);
328  head_mode=HEAD_MODE_TRACK_FIX;
329  }
330 
331  void setGazeIdle()
332  {
333  head_mode=HEAD_MODE_IDLE;
334  ctrl_gaze->stopControl();
335  gazeUnderControl=false;
336  }
337 
338  bool setArmInUse(int arm)
339  {
340  if(action[arm]==NULL)
341  return false;
342 
343  armInUse=arm;
344  return true;
345  }
346 
347  bool setWaveing(bool _waveing)
348  {
349  waveing=_waveing;
350 
351  if(waveing)
352  {
353  if(action[RIGHT]!=NULL)
354  action[RIGHT]->enableArmWaving(homePos[RIGHT]);
355 
356  if(action[LEFT]!=NULL)
357  action[LEFT]->enableArmWaving(homePos[LEFT]);
358  }
359  else
360  {
361  if(action[RIGHT]!=NULL)
362  action[RIGHT]->disableArmWaving();
363  if(action[LEFT]!=NULL)
364  action[LEFT]->disableArmWaving();
365  }
366  return true;
367  }
368 
369  void update();
370  void interrupt();
371  void reinstate();
372 
373  //get the table height
374  bool getTableHeight(double *_table_height)
375  {
376  *_table_height=table_height-table_height_tolerance;
377  return true;
378  }
379 
380  //transforms stereo coordinates to cartesian space
381  bool stereoToCartesian(const Vector &stereo, Vector &xd);
382  bool targetToCartesian(Bottle *target, Vector &xd);
383 
384  // basic commands
385  bool goUp(Bottle &options, const double h=std::numeric_limits<double>::quiet_NaN());
386  bool goHome(Bottle &options);
387  bool reach(Bottle &options);
388  bool powerGrasp(Bottle &options);
389  bool push(Bottle &options);
390  bool point(Bottle &options);
391  bool point_far(Bottle &options);
392  bool look(Bottle &options);
393  bool hand(const Bottle &options, const string &type="", bool *holding=NULL);
394  bool grasp(const Bottle &options);
395  bool release(const Bottle &options);
396  bool deploy(Bottle &options);
397  bool drawNear(Bottle &options);
398  bool shiftAndGrasp(Bottle &options);
399  bool expect(Bottle &options);
400  bool give(Bottle &options);
401 
402  bool clearIt(Bottle &options);
403 
404  // explore
405  bool exploreTorso(Bottle &options);
406  bool exploreHand(Bottle &options);
407 
408  //tool functions
409  bool takeTool(Bottle &options);
410 
411  bool getHandImagePosition(Bottle &hand_image_pos);
412  bool isHolding(const Bottle &options);
413  bool calibTable(Bottle &options);
414  bool calibFingers(Bottle &options);
415 
416  bool startLearningModeAction(Bottle &options);
417  bool suspendLearningModeAction(Bottle &options);
418  bool imitateAction(Bottle &options);
419 
420  bool startLearningModeKinOffset(Bottle &options);
421  bool suspendLearningModeKinOffset(Bottle &options);
422 
423  bool changeElbowHeight(const int arm, const double height, const double weight);
424  bool changeExecTime(const int arm, const double execTime);
425 
426  bool setImpedance(bool turn_on);
427  bool setTorque(bool turn_on, int arm=ARM_IN_USE);
428 
429  int setStereoToCartesianMode(const int mode, Bottle &reply);
430  int setStereoToCartesianMode(const int mode);
431 
432  void setGraspState(bool side)
433  {
434  if(side)
435  grasp_state=GRASP_STATE_SIDE;
436  else
437  grasp_state=GRASP_STATE_ABOVE;
438  }
439 
440  void getStatus(Bottle &status);
441 };
442 
443 
444 #endif
445 
446 
#define HEAD_MODE_TRACK_TEMP
Definition: MotorThread.h:42
#define GRASP_STATE_ABOVE
Definition: MotorThread.h:52
#define HEAD_MODE_IDLE
Definition: MotorThread.h:39
#define HEAD_MODE_TRACK_HAND
Definition: MotorThread.h:41
#define HEAD_MODE_TRACK_CART
Definition: MotorThread.h:43
#define GRASP_STATE_SIDE
Definition: MotorThread.h:53
#define HEAD_MODE_TRACK_FIX
Definition: MotorThread.h:44
bool setArmInUse(int arm)
Definition: MotorThread.h:338
void keepFixation(Bottle &options)
Definition: MotorThread.h:322
void track(Bottle &options)
Definition: MotorThread.h:288
bool getTableHeight(double *_table_height)
Definition: MotorThread.h:374
void setGazeIdle()
Definition: MotorThread.h:331
void lookAtHand(Bottle &options)
Definition: MotorThread.h:315
MotorThread(ResourceFinder &_rf, Initializer *initializer)
Definition: MotorThread.h:252
bool wbdRecalibration()
Definition: MotorThread.h:273
bool setWaveing(bool _waveing)
Definition: MotorThread.h:347
void setGraspState(bool side)
Definition: MotorThread.h:432
The base class defining actions.
virtual bool disableArmWaving()
Disable the waving mode.
virtual bool enableArmWaving(const yarp::sig::Vector &restPos)
Enable the waving mode that keeps on moving the arm around a predefined position.
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:48
Feed-Forward 2 layers Neural Network with a tansig function for the hidden nodes and a purelin for th...
An abstract class that provides basic methods for interfacing with the data acquisition.
Definition: models.h:62
cmd
Definition: dataTypes.h:30
#define LEFT
Definition: utils.h:33
#define ARM_IN_USE
Definition: utils.h:35
#define RIGHT
Definition: utils.h:34
out
Definition: sine.m:8
bool using_impedance
Definition: MotorThread.h:87
Vector x0
Definition: MotorThread.h:80
string actionName
Definition: MotorThread.h:83
Bottle actions
Definition: MotorThread.h:82
iCub::ctrl::Integrator * I
Definition: MotorThread.h:89
int arm
Definition: MotorThread.h:81
double inertia
Definition: MotorThread.h:91
~Dragger()
Definition: MotorThread.h:98
double min
Definition: MotorThread.h:84
ICartesianControl * ctrl
Definition: MotorThread.h:75
double t0
Definition: MotorThread.h:78
double max
Definition: MotorThread.h:85
double samplingRate
Definition: MotorThread.h:79
double damping
Definition: MotorThread.h:90
double extForceThresh
Definition: MotorThread.h:77