WorldSim - 1.2.0

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  #include "yarp/dev/CartesianControl.h"
51  #include "iCub/iKin/iKinHlp.h"
52  #include "iCub/iKin/iKinSlv.h"
53 #ifdef __GNUC__
54 #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
55 #pragma GCC diagnostic pop
56 #else
57 #pragma GCC diagnostic warning "-Wunused-parameter"
58 #endif
59 #endif
60 
61 // #ifdef __GNUC__
62 // #pragma GCC diagnostic ignored "-Wunused-parameter"
63 // #endif
64 // #include "yarp/dev/CartesianControl.h"
65 // #include "iCub/iKin/iKinHlp.h"
66 // #include "iCub/iKin/iKinSlv.h"
67 // #ifdef __GNUC__
68 // #pragma GCC diagnostic warning "-Wunused-parameter"
69 // #endif
70 
71 namespace farsa {
72  class PhyObject;
73  class PhyJoint;
74  class PhyDOF;
75  class MultiMotorController;
76  class WCamera;
77 
90  class FARSA_WSIM_API PhyiCub : public YarpObject {
91  Q_OBJECT
92 
93  public:
95  enum {
96  torso_yaw=0, torso_roll, torso_pitch,
97  left_shoulder_pitch, left_shoulder_roll, left_shoulder_yaw,
98  left_elbow, left_wrist_prosup, left_wrist_pitch,
99  left_wrist_yaw, left_hand_finger, left_thumb_oppose,
100  left_thumb_proximal, left_thumb_distal, left_index_proximal,
101  left_index_distal, left_middle_proximal, left_middle_distal, left_pinky,
102  right_shoulder_pitch, right_shoulder_roll, right_shoulder_yaw,
103  right_elbow, right_wrist_prosup, right_wrist_pitch,
104  right_wrist_yaw, right_hand_finger, right_thumb_oppose,
105  right_thumb_proximal, right_thumb_distal, right_index_proximal,
106  right_index_distal, right_middle_proximal, right_middle_distal, right_pinky,
107  neck_pitch, neck_roll, neck_yaw,
108  eyes_tilt, eyes_version, eyes_vergence,
109  left_hip_pitch, left_hip_roll, left_hip_yaw,
110  left_knee, left_ankle_pitch, left_ankle_roll,
111  right_hip_pitch, right_hip_roll, right_hip_yaw,
112  right_knee, right_ankle_pitch, right_ankle_roll,
113  numOfJoints
114  } jointNames;
115 
122  PhyiCub( World* world, QString name, const wMatrix& tm = wMatrix::identity(), bool createServerControlBoards=true );
123 
127  virtual ~PhyiCub();
128 
142  void doKinematicSimulation(bool k, bool clh = false, bool crh = false);
143 
146  bool isKinematic()
147  {
148  return kinematicSimulation;
149  }
150 
160  void configurePosture( QMap<int, real> jointSetup );
161 
163  const QVector<PhyObject*>& leftLeg() {
164  return leftLegv;
165  };
166 
168  const QVector<PhyObject*>& rightLeg() {
169  return rightLegv;
170  };
171 
173  const QVector<PhyObject*>& torso() {
174  return torsov;
175  };
176 
178  const QVector<PhyObject*>& leftArm() {
179  return leftArmv;
180  };
181 
183  const QVector<PhyObject*>& rightArm() {
184  return rightArmv;
185  };
186 
188  const QVector<PhyObject*>& headNeck() {
189  return headNeckv;
190  };
191 
193  const QVector<WMesh*>& covers() {
194  return coversv;
195  };
196 
198  const QVector<PhyJoint*>& leftLegJoints() {
199  return leftLegJointv;
200  };
201 
203  const QVector<PhyJoint*>& rightLegJoints() {
204  return rightLegJointv;
205  };
206 
208  const QVector<PhyJoint*>& torsoJoints() {
209  return torsoJointv;
210  };
211 
224  const QVector<PhyJoint*>& leftArmJoints() {
225  return leftArmJointv;
226  };
227 
240  const QVector<PhyJoint*>& rightArmJoints() {
241  return rightArmJointv;
242  };
243 
245  const QVector<PhyJoint*>& headNeckJoints() {
246  return headNeckJointv;
247  };
249  MultiMotorController* torsoController() {
250  return torsoCtrl;
251  };
253  MultiMotorController* leftArmController() {
254  return leftArmCtrl;
255  };
257  MultiMotorController* rightArmController() {
258  return rightArmCtrl;
259  };
261  MultiMotorController* headNeckController() {
262  return headNeckCtrl;
263  };
265  MultiMotorController* leftLegController() {
266  return leftLegCtrl;
267  };
269  MultiMotorController* rightLegController() {
270  return rightLegCtrl;
271  };
273  yarp::dev::ICartesianControl* leftArmCartesianController() {
274  return cartCtrlLeftArm;
275  };
277  yarp::dev::ICartesianControl* rightArmCartesianController() {
278  return cartCtrlRightArm;
279  };
281  yarp::dev::IFrameGrabberImage* leftEyeFrameGrabber();
283  yarp::dev::IFrameGrabberImage* rightEyeFrameGrabber();
284 
290  virtual void preUpdate();
295  virtual void postUpdate();
299  void forceKinematicChainsUpdate(bool enable) {
300  alwaysUpdateKinematicChains = enable;
301  }
302 
303  public slots:
305  void enableLeftLeg( bool );
307  void enableRightLeg( bool );
309  void enableTorso( bool );
311  void enableHead( bool );
313  void enableCameras( bool );
315  void enableLeftArm( bool );
317  void enableRightArm( bool );
319  void blockTorso0( bool );
321  void enableLeftKinematicHand( bool );
323  void enableRightKinematicHand( bool );
324 
325  bool isEnabledLeftLeg() {
326  return enabledLeftLeg;
327  };
328  bool isEnabledRightLeg() {
329  return enabledRightLeg;
330  };
331  bool isEnabledTorso() {
332  return enabledTorso;
333  };
334  bool isEnabledHead() {
335  return enabledHead;
336  };
337  bool isEnabledCameras() {
338  return enabledCameras;
339  };
340  bool isEnabledLeftArm() {
341  return enabledLeftArm;
342  };
343  bool isEnabledRightArm() {
344  return enabledRightArm;
345  };
346  bool isBlockedTorso0() {
347  return blockedTorso0;
348  };
349  bool isEnabledLeftKinematicHand() {
350  return enabledLeftKinematicHand;
351  };
352  bool isEnabledRightKinematicHand() {
353  return enabledRightKinematicHand;
354  };
355 
357  void enableLeftArmCartesianController();
359  void enableRightArmCartesianController();
360 
361  protected:
363  virtual void changedMatrix();
365  void setTorso0Matrix();
367  void enableObjectsAndLinks(bool enable, QVector<PhyObject*>& objects, QVector<PhyJoint*>& joints);
369  static real rightEyeFromVersionAndVergence(real version, real vergence);
371  static real leftEyeFromVersionAndVergence(real version, real vergence);
373  static real versionFromRightAndLeftEye(real left, real right);
375  static real vergenceFromRightAndLeftEye(real left, real right);
377  static real lowEyeLimitFromVersionAndVergenceLimits(real versionLow, real vergenceLow);
379  static real highEyeLimitFromVersionAndVergenceLimits(real versionHigh, real vergenceHigh);
380 
381  protected slots:
383  void versionChangedDesiredPosition( real wishpos );
385  void versionChangedDesiredVelocity( real wishvel );
387  void versionChangedPosition( real newpos );
389  void versionChangedVelocity( real newvel );
391  void versionChangedLimits( real lowlimit, real highlimit );
392 
394  void vergenceChangedDesiredPosition( real wishpos );
396  void vergenceChangedDesiredVelocity( real wishvel );
398  void vergenceChangedPosition( real newpos );
400  void vergenceChangedVelocity( real newvel );
402  void vergenceChangedLimits( real lowlimit, real highlimit );
403 
405  void rightEyeChangedDesiredPosition( real wishpos );
407  void rightEyeChangedDesiredVelocity( real wishvel );
409  void rightEyeChangedPosition( real newpos );
411  void rightEyeChangedVelocity( real newvel );
413  void rightEyeChangedLimits( real lowlimit, real highlimit );
414 
416  void leftEyeChangedDesiredPosition( real wishpos );
418  void leftEyeChangedDesiredVelocity( real wishvel );
420  void leftEyeChangedPosition( real newpos );
422  void leftEyeChangedVelocity( real newvel );
424  void leftEyeChangedLimits( real lowlimit, real highlimit );
425 
426  private:
427 
429  QVector<PhyObject*> leftLegv;
431  QVector<PhyObject*> rightLegv;
433  QVector<PhyObject*> torsov;
435  QVector<PhyObject*> leftArmv;
437  QVector<PhyObject*> rightArmv;
439  QVector<PhyObject*> headNeckv;
440 
442  QVector<WMesh*> coversv;
443 
445  QVector<PhyJoint*> leftLegJointv;
447  QVector<PhyJoint*> rightLegJointv;
449  QVector<PhyJoint*> torsoJointv;
451  QVector<PhyJoint*> leftArmJointv;
453  QVector<PhyJoint*> rightArmJointv;
455  QVector<PhyJoint*> headNeckJointv;
457  PhyDOF* versionDOF;
459  PhyDOF* vergenceDOF;
462  PhyDOF* rightEyeDOF;
465  PhyDOF* leftEyeDOF;
468  bool ignoreEyeSignals;
469 
471  enum PhyObjectsGroupChains {
472  IndexChain, MiddleChain, RingChain, PinkyChain,
473  ThumbChain, EyeChain
474  };
475 
477  PhyObjectsGroup* leftHandGroup;
479  PhyObjectsGroup* rightHandGroup;
480 
482  QVector<real> leftLegMasses;
484  QVector<real> rightLegMasses;
486  QVector<real> torsoMasses;
488  QVector<real> leftArmMasses;
490  QVector<real> rightArmMasses;
492  QVector<real> headNeckMasses;
493 
495  MultiMotorController* torsoCtrl;
497  MultiMotorController* leftArmCtrl;
499  MultiMotorController* rightArmCtrl;
501  MultiMotorController* headNeckCtrl;
503  MultiMotorController* leftLegCtrl;
505  MultiMotorController* rightLegCtrl;
506 
513  yarp::dev::PolyDriver* cartServLeftArm;
514  yarp::dev::ICartesianControl* cartCtrlLeftArm;
515  ::iCub::iKin::iCubArmCartesianSolver* cartSolvLeftArm;
516  yarp::os::RateThread* cartThreadLeftArm;
518  yarp::dev::PolyDriver* cartServRightArm;
519  yarp::dev::ICartesianControl* cartCtrlRightArm;
520  ::iCub::iKin::iCubArmCartesianSolver* cartSolvRightArm;
521 
523  WCamera* leftcam;
524  WCamera* rightcam;
525 
527  bool enabledLeftLeg;
528  bool enabledRightLeg;
529  bool enabledTorso;
530  bool enabledLeftArm;
531  bool enabledRightArm;
532  bool enabledHead;
533  bool enabledCameras;
534  bool blockedTorso0;
535  bool enabledRightKinematicHand; // If true the right hand is enabled in kinematic simulations, otherwise it is not
536  bool enabledLeftKinematicHand; // If true the left hand is enabled in kinematic simulations, otherwise it is not
537  // This can be selected only in the Constructor
538  const bool enabledServerControlBoards;
540  bool alwaysUpdateKinematicChains;
541 
543  KinematicLimb<iCub::iKin::iCubArm> kinRightArm;
544  KinematicLimb<iCub::iKin::iCubArm> kinLeftArm;
545  KinematicLimb<iCub::iKin::iCubLeg> kinRightLeg;
546  KinematicLimb<iCub::iKin::iCubLeg> kinLeftLeg;
547  KinematicLimb<iCub::iKin::iCubEye> kinRightEye;
548  KinematicLimb<iCub::iKin::iCubEye> kinLeftEye;
549 
553  bool kinematicSimulation;
557  bool collidingLeftHand;
561  bool collidingRightHand;
562  };
563 } // end namespace farsa
564 
565 #endif
566 
567 #endif // FARSA_USE_YARP_AND_ICUB