WorldSim - 1.2.3

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phyicub.h
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #ifdef FARSA_USE_YARP_AND_ICUB
21 
22 #ifndef PHYICUB_H
23 #define PHYICUB_H
24 
25 #include "worldsimconfig.h"
26 #include "wvector.h"
27 #include "wmatrix.h"
28 #include "wquaternion.h"
29 #include "world.h"
30 #include "yarpobject.h"
31 #include "wmesh.h"
32 #include "phyhinge.h"
33 #include "phyicubhelpers.h"
34 #include <QVector>
35 #include <QSet>
36 #include <QMap>
37 #include <QString>
38 #include <string>
39 #include <typeinfo>
40 
41 // All this stuff is to get rid of annoying warnings...
42 #ifdef __GNUC__
43  #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
44  #pragma GCC diagnostic push
45  #pragma GCC diagnostic ignored "-Wunused-parameter"
46  #else
47  #pragma GCC diagnostic ignored "-Wunused-parameter"
48  #endif
49 #endif
50 
51 #include "yarp/dev/CartesianControl.h"
52 #include "iCub/iKin/iKinHlp.h"
53 #include "iCub/iKin/iKinSlv.h"
54 
55 #ifdef __GNUC__
56  #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
57  #pragma GCC diagnostic pop
58  #else
59  #pragma GCC diagnostic warning "-Wunused-parameter"
60  #endif
61 #endif
62 
63 // #ifdef __GNUC__
64 // #pragma GCC diagnostic ignored "-Wunused-parameter"
65 // #endif
66 // #include "yarp/dev/CartesianControl.h"
67 // #include "iCub/iKin/iKinHlp.h"
68 // #include "iCub/iKin/iKinSlv.h"
69 // #ifdef __GNUC__
70 // #pragma GCC diagnostic warning "-Wunused-parameter"
71 // #endif
72 
73 namespace farsa {
74  class PhyObject;
75  class PhyJoint;
76  class PhyDOF;
77  class MultiMotorController;
78  class WCamera;
79 
92  class FARSA_WSIM_API PhyiCub : public YarpObject {
93  Q_OBJECT
94 
95  public:
97  enum {
98  torso_yaw=0, torso_roll, torso_pitch,
99  left_shoulder_pitch, left_shoulder_roll, left_shoulder_yaw,
100  left_elbow, left_wrist_prosup, left_wrist_pitch,
101  left_wrist_yaw, left_hand_finger, left_thumb_oppose,
102  left_thumb_proximal, left_thumb_distal, left_index_proximal,
103  left_index_distal, left_middle_proximal, left_middle_distal, left_pinky,
104  right_shoulder_pitch, right_shoulder_roll, right_shoulder_yaw,
105  right_elbow, right_wrist_prosup, right_wrist_pitch,
106  right_wrist_yaw, right_hand_finger, right_thumb_oppose,
107  right_thumb_proximal, right_thumb_distal, right_index_proximal,
108  right_index_distal, right_middle_proximal, right_middle_distal, right_pinky,
109  neck_pitch, neck_roll, neck_yaw,
110  eyes_tilt, eyes_version, eyes_vergence,
111  left_hip_pitch, left_hip_roll, left_hip_yaw,
112  left_knee, left_ankle_pitch, left_ankle_roll,
113  right_hip_pitch, right_hip_roll, right_hip_yaw,
114  right_knee, right_ankle_pitch, right_ankle_roll,
115  numOfJoints
116  } jointNames;
117 
124  PhyiCub( World* world, QString name, const wMatrix& tm = wMatrix::identity(), bool createServerControlBoards=true );
125 
129  virtual ~PhyiCub();
130 
144  void doKinematicSimulation(bool k, bool clh = false, bool crh = false);
145 
148  bool isKinematic()
149  {
150  return kinematicSimulation;
151  }
152 
162  void configurePosture( QMap<int, real> jointSetup );
163 
165  const QVector<PhyObject*>& leftLeg() {
166  return leftLegv;
167  };
168 
170  const QVector<PhyObject*>& rightLeg() {
171  return rightLegv;
172  };
173 
175  const QVector<PhyObject*>& torso() {
176  return torsov;
177  };
178 
180  const QVector<PhyObject*>& leftArm() {
181  return leftArmv;
182  };
183 
185  const QVector<PhyObject*>& rightArm() {
186  return rightArmv;
187  };
188 
190  const QVector<PhyObject*>& headNeck() {
191  return headNeckv;
192  };
193 
195  const QVector<WMesh*>& covers() {
196  return coversv;
197  };
198 
200  const QVector<PhyJoint*>& leftLegJoints() {
201  return leftLegJointv;
202  };
203 
205  const QVector<PhyJoint*>& rightLegJoints() {
206  return rightLegJointv;
207  };
208 
210  const QVector<PhyJoint*>& torsoJoints() {
211  return torsoJointv;
212  };
213 
226  const QVector<PhyJoint*>& leftArmJoints() {
227  return leftArmJointv;
228  };
229 
242  const QVector<PhyJoint*>& rightArmJoints() {
243  return rightArmJointv;
244  };
245 
247  const QVector<PhyJoint*>& headNeckJoints() {
248  return headNeckJointv;
249  };
251  MultiMotorController* torsoController() {
252  return torsoCtrl;
253  };
255  MultiMotorController* leftArmController() {
256  return leftArmCtrl;
257  };
259  MultiMotorController* rightArmController() {
260  return rightArmCtrl;
261  };
263  MultiMotorController* headNeckController() {
264  return headNeckCtrl;
265  };
267  MultiMotorController* leftLegController() {
268  return leftLegCtrl;
269  };
271  MultiMotorController* rightLegController() {
272  return rightLegCtrl;
273  };
275  yarp::dev::ICartesianControl* leftArmCartesianController() {
276  return cartCtrlLeftArm;
277  };
279  yarp::dev::ICartesianControl* rightArmCartesianController() {
280  return cartCtrlRightArm;
281  };
283  yarp::dev::IFrameGrabberImage* leftEyeFrameGrabber();
285  yarp::dev::IFrameGrabberImage* rightEyeFrameGrabber();
286 
292  virtual void preUpdate();
297  virtual void postUpdate();
301  void forceKinematicChainsUpdate(bool enable) {
302  alwaysUpdateKinematicChains = enable;
303  }
304 
305  public slots:
307  void enableLeftLeg( bool );
309  void enableRightLeg( bool );
311  void enableTorso( bool );
313  void enableHead( bool );
315  void enableCameras( bool );
317  void enableLeftArm( bool );
319  void enableRightArm( bool );
321  void blockTorso0( bool );
323  void enableLeftKinematicHand( bool );
325  void enableRightKinematicHand( bool );
326 
327  bool isEnabledLeftLeg() {
328  return enabledLeftLeg;
329  };
330  bool isEnabledRightLeg() {
331  return enabledRightLeg;
332  };
333  bool isEnabledTorso() {
334  return enabledTorso;
335  };
336  bool isEnabledHead() {
337  return enabledHead;
338  };
339  bool isEnabledCameras() {
340  return enabledCameras;
341  };
342  bool isEnabledLeftArm() {
343  return enabledLeftArm;
344  };
345  bool isEnabledRightArm() {
346  return enabledRightArm;
347  };
348  bool isBlockedTorso0() {
349  return blockedTorso0;
350  };
351  bool isEnabledLeftKinematicHand() {
352  return enabledLeftKinematicHand;
353  };
354  bool isEnabledRightKinematicHand() {
355  return enabledRightKinematicHand;
356  };
357 
359  void enableLeftArmCartesianController();
361  void enableRightArmCartesianController();
362 
363  protected:
365  virtual void changedMatrix();
367  void setTorso0Matrix();
369  void enableObjectsAndLinks(bool enable, QVector<PhyObject*>& objects, QVector<PhyJoint*>& joints);
371  static real rightEyeFromVersionAndVergence(real version, real vergence);
373  static real leftEyeFromVersionAndVergence(real version, real vergence);
375  static real versionFromRightAndLeftEye(real left, real right);
377  static real vergenceFromRightAndLeftEye(real left, real right);
379  static real lowEyeLimitFromVersionAndVergenceLimits(real versionLow, real vergenceLow);
381  static real highEyeLimitFromVersionAndVergenceLimits(real versionHigh, real vergenceHigh);
382 
383  protected slots:
385  void versionChangedDesiredPosition( real wishpos );
387  void versionChangedDesiredVelocity( real wishvel );
389  void versionChangedPosition( real newpos );
391  void versionChangedVelocity( real newvel );
393  void versionChangedLimits( real lowlimit, real highlimit );
394 
396  void vergenceChangedDesiredPosition( real wishpos );
398  void vergenceChangedDesiredVelocity( real wishvel );
400  void vergenceChangedPosition( real newpos );
402  void vergenceChangedVelocity( real newvel );
404  void vergenceChangedLimits( real lowlimit, real highlimit );
405 
407  void rightEyeChangedDesiredPosition( real wishpos );
409  void rightEyeChangedDesiredVelocity( real wishvel );
411  void rightEyeChangedPosition( real newpos );
413  void rightEyeChangedVelocity( real newvel );
415  void rightEyeChangedLimits( real lowlimit, real highlimit );
416 
418  void leftEyeChangedDesiredPosition( real wishpos );
420  void leftEyeChangedDesiredVelocity( real wishvel );
422  void leftEyeChangedPosition( real newpos );
424  void leftEyeChangedVelocity( real newvel );
426  void leftEyeChangedLimits( real lowlimit, real highlimit );
427 
428  private:
429 
431  QVector<PhyObject*> leftLegv;
433  QVector<PhyObject*> rightLegv;
435  QVector<PhyObject*> torsov;
437  QVector<PhyObject*> leftArmv;
439  QVector<PhyObject*> rightArmv;
441  QVector<PhyObject*> headNeckv;
442 
444  QVector<WMesh*> coversv;
445 
447  QVector<PhyJoint*> leftLegJointv;
449  QVector<PhyJoint*> rightLegJointv;
451  QVector<PhyJoint*> torsoJointv;
453  QVector<PhyJoint*> leftArmJointv;
455  QVector<PhyJoint*> rightArmJointv;
457  QVector<PhyJoint*> headNeckJointv;
459  PhyDOF* versionDOF;
461  PhyDOF* vergenceDOF;
464  PhyDOF* rightEyeDOF;
467  PhyDOF* leftEyeDOF;
470  bool ignoreEyeSignals;
471 
473  enum PhyObjectsGroupChains {
474  IndexChain, MiddleChain, RingChain, PinkyChain,
475  ThumbChain, EyeChain
476  };
477 
479  PhyObjectsGroup* leftHandGroup;
481  PhyObjectsGroup* rightHandGroup;
482 
484  QVector<real> leftLegMasses;
486  QVector<real> rightLegMasses;
488  QVector<real> torsoMasses;
490  QVector<real> leftArmMasses;
492  QVector<real> rightArmMasses;
494  QVector<real> headNeckMasses;
495 
497  MultiMotorController* torsoCtrl;
499  MultiMotorController* leftArmCtrl;
501  MultiMotorController* rightArmCtrl;
503  MultiMotorController* headNeckCtrl;
505  MultiMotorController* leftLegCtrl;
507  MultiMotorController* rightLegCtrl;
508 
515  yarp::dev::PolyDriver* cartServLeftArm;
516  yarp::dev::ICartesianControl* cartCtrlLeftArm;
517  ::iCub::iKin::iCubArmCartesianSolver* cartSolvLeftArm;
518  yarp::os::RateThread* cartThreadLeftArm;
520  yarp::dev::PolyDriver* cartServRightArm;
521  yarp::dev::ICartesianControl* cartCtrlRightArm;
522  ::iCub::iKin::iCubArmCartesianSolver* cartSolvRightArm;
523 
525  WCamera* leftcam;
526  WCamera* rightcam;
527 
529  bool enabledLeftLeg;
530  bool enabledRightLeg;
531  bool enabledTorso;
532  bool enabledLeftArm;
533  bool enabledRightArm;
534  bool enabledHead;
535  bool enabledCameras;
536  bool blockedTorso0;
537  bool enabledRightKinematicHand; // If true the right hand is enabled in kinematic simulations, otherwise it is not
538  bool enabledLeftKinematicHand; // If true the left hand is enabled in kinematic simulations, otherwise it is not
539  // This can be selected only in the Constructor
540  const bool enabledServerControlBoards;
542  bool alwaysUpdateKinematicChains;
543 
545  KinematicLimb<iCub::iKin::iCubArm> kinRightArm;
546  KinematicLimb<iCub::iKin::iCubArm> kinLeftArm;
547  KinematicLimb<iCub::iKin::iCubLeg> kinRightLeg;
548  KinematicLimb<iCub::iKin::iCubLeg> kinLeftLeg;
549  KinematicLimb<iCub::iKin::iCubEye> kinRightEye;
550  KinematicLimb<iCub::iKin::iCubEye> kinLeftEye;
551 
555  bool kinematicSimulation;
559  bool collidingLeftHand;
563  bool collidingRightHand;
564  };
565 } // end namespace farsa
566 
567 #endif
568 
569 #endif // FARSA_USE_YARP_AND_ICUB