00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifdef FARSA_USE_YARP_AND_ICUB
00021
00022 #ifndef PHYICUB_H
00023 #define PHYICUB_H
00024
00025 #include "worldsimconfig.h"
00026 #include "wvector.h"
00027 #include "wmatrix.h"
00028 #include "wquaternion.h"
00029 #include "world.h"
00030 #include "yarpobject.h"
00031 #include "wmesh.h"
00032 #include "phyhinge.h"
00033 #include "phyicubhelpers.h"
00034 #include <QVector>
00035 #include <QSet>
00036 #include <QMap>
00037 #include <QString>
00038 #include <string>
00039 #include <typeinfo>
00040
00041
00042 #ifdef __GNUC__
00043 #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
00044 #pragma GCC diagnostic push
00045 #pragma GCC diagnostic ignored "-Wunused-parameter"
00046 #else
00047 #pragma GCC diagnostic ignored "-Wunused-parameter"
00048 #endif
00049 #endif
00050 #include "yarp/dev/CartesianControl.h"
00051 #include "iCub/iKin/iKinHlp.h"
00052 #include "iCub/iKin/iKinSlv.h"
00053 #ifdef __GNUC__
00054 #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
00055 #pragma GCC diagnostic pop
00056 #else
00057 #pragma GCC diagnostic warning "-Wunused-parameter"
00058 #endif
00059 #endif
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 namespace farsa {
00072 class PhyObject;
00073 class PhyJoint;
00074 class PhyDOF;
00075 class MultiMotorController;
00076 class WCamera;
00077
00090 class FARSA_WSIM_API PhyiCub : public YarpObject {
00091 Q_OBJECT
00092
00093 public:
00095 enum {
00096 torso_yaw=0, torso_roll, torso_pitch,
00097 left_shoulder_pitch, left_shoulder_roll, left_shoulder_yaw,
00098 left_elbow, left_wrist_prosup, left_wrist_pitch,
00099 left_wrist_yaw, left_hand_finger, left_thumb_oppose,
00100 left_thumb_proximal, left_thumb_distal, left_index_proximal,
00101 left_index_distal, left_middle_proximal, left_middle_distal, left_pinky,
00102 right_shoulder_pitch, right_shoulder_roll, right_shoulder_yaw,
00103 right_elbow, right_wrist_prosup, right_wrist_pitch,
00104 right_wrist_yaw, right_hand_finger, right_thumb_oppose,
00105 right_thumb_proximal, right_thumb_distal, right_index_proximal,
00106 right_index_distal, right_middle_proximal, right_middle_distal, right_pinky,
00107 neck_pitch, neck_roll, neck_yaw,
00108 eyes_tilt, eyes_version, eyes_vergence,
00109 left_hip_pitch, left_hip_roll, left_hip_yaw,
00110 left_knee, left_ankle_pitch, left_ankle_roll,
00111 right_hip_pitch, right_hip_roll, right_hip_yaw,
00112 right_knee, right_ankle_pitch, right_ankle_roll,
00113 numOfJoints
00114 } jointNames;
00115
00122 PhyiCub( World* world, QString name, const wMatrix& tm = wMatrix::identity(), bool createServerControlBoards=true );
00123
00127 virtual ~PhyiCub();
00128
00142 void doKinematicSimulation(bool k, bool clh = false, bool crh = false);
00143
00146 bool isKinematic()
00147 {
00148 return kinematicSimulation;
00149 }
00150
00160 void configurePosture( QMap<int, real> jointSetup );
00161
00163 const QVector<PhyObject*>& leftLeg() {
00164 return leftLegv;
00165 };
00166
00168 const QVector<PhyObject*>& rightLeg() {
00169 return rightLegv;
00170 };
00171
00173 const QVector<PhyObject*>& torso() {
00174 return torsov;
00175 };
00176
00178 const QVector<PhyObject*>& leftArm() {
00179 return leftArmv;
00180 };
00181
00183 const QVector<PhyObject*>& rightArm() {
00184 return rightArmv;
00185 };
00186
00188 const QVector<PhyObject*>& headNeck() {
00189 return headNeckv;
00190 };
00191
00193 const QVector<WMesh*>& covers() {
00194 return coversv;
00195 };
00196
00198 const QVector<PhyJoint*>& leftLegJoints() {
00199 return leftLegJointv;
00200 };
00201
00203 const QVector<PhyJoint*>& rightLegJoints() {
00204 return rightLegJointv;
00205 };
00206
00208 const QVector<PhyJoint*>& torsoJoints() {
00209 return torsoJointv;
00210 };
00211
00224 const QVector<PhyJoint*>& leftArmJoints() {
00225 return leftArmJointv;
00226 };
00227
00240 const QVector<PhyJoint*>& rightArmJoints() {
00241 return rightArmJointv;
00242 };
00243
00245 const QVector<PhyJoint*>& headNeckJoints() {
00246 return headNeckJointv;
00247 };
00249 MultiMotorController* torsoController() {
00250 return torsoCtrl;
00251 };
00253 MultiMotorController* leftArmController() {
00254 return leftArmCtrl;
00255 };
00257 MultiMotorController* rightArmController() {
00258 return rightArmCtrl;
00259 };
00261 MultiMotorController* headNeckController() {
00262 return headNeckCtrl;
00263 };
00265 MultiMotorController* leftLegController() {
00266 return leftLegCtrl;
00267 };
00269 MultiMotorController* rightLegController() {
00270 return rightLegCtrl;
00271 };
00273 yarp::dev::ICartesianControl* leftArmCartesianController() {
00274 return cartCtrlLeftArm;
00275 };
00277 yarp::dev::ICartesianControl* rightArmCartesianController() {
00278 return cartCtrlRightArm;
00279 };
00281 yarp::dev::IFrameGrabberImage* leftEyeFrameGrabber();
00283 yarp::dev::IFrameGrabberImage* rightEyeFrameGrabber();
00284
00290 virtual void preUpdate();
00295 virtual void postUpdate();
00299 void forceKinematicChainsUpdate(bool enable) {
00300 alwaysUpdateKinematicChains = enable;
00301 }
00302
00303 public slots:
00305 void enableLeftLeg( bool );
00307 void enableRightLeg( bool );
00309 void enableTorso( bool );
00311 void enableHead( bool );
00313 void enableCameras( bool );
00315 void enableLeftArm( bool );
00317 void enableRightArm( bool );
00319 void blockTorso0( bool );
00321 void enableLeftKinematicHand( bool );
00323 void enableRightKinematicHand( bool );
00324
00325 bool isEnabledLeftLeg() {
00326 return enabledLeftLeg;
00327 };
00328 bool isEnabledRightLeg() {
00329 return enabledRightLeg;
00330 };
00331 bool isEnabledTorso() {
00332 return enabledTorso;
00333 };
00334 bool isEnabledHead() {
00335 return enabledHead;
00336 };
00337 bool isEnabledCameras() {
00338 return enabledCameras;
00339 };
00340 bool isEnabledLeftArm() {
00341 return enabledLeftArm;
00342 };
00343 bool isEnabledRightArm() {
00344 return enabledRightArm;
00345 };
00346 bool isBlockedTorso0() {
00347 return blockedTorso0;
00348 };
00349 bool isEnabledLeftKinematicHand() {
00350 return enabledLeftKinematicHand;
00351 };
00352 bool isEnabledRightKinematicHand() {
00353 return enabledRightKinematicHand;
00354 };
00355
00357 void enableLeftArmCartesianController();
00359 void enableRightArmCartesianController();
00360
00361 protected:
00363 virtual void changedMatrix();
00365 void setTorso0Matrix();
00367 void enableObjectsAndLinks(bool enable, QVector<PhyObject*>& objects, QVector<PhyJoint*>& joints);
00369 static real rightEyeFromVersionAndVergence(real version, real vergence);
00371 static real leftEyeFromVersionAndVergence(real version, real vergence);
00373 static real versionFromRightAndLeftEye(real left, real right);
00375 static real vergenceFromRightAndLeftEye(real left, real right);
00377 static real lowEyeLimitFromVersionAndVergenceLimits(real versionLow, real vergenceLow);
00379 static real highEyeLimitFromVersionAndVergenceLimits(real versionHigh, real vergenceHigh);
00380
00381 protected slots:
00383 void versionChangedDesiredPosition( real wishpos );
00385 void versionChangedDesiredVelocity( real wishvel );
00387 void versionChangedPosition( real newpos );
00389 void versionChangedVelocity( real newvel );
00391 void versionChangedLimits( real lowlimit, real highlimit );
00392
00394 void vergenceChangedDesiredPosition( real wishpos );
00396 void vergenceChangedDesiredVelocity( real wishvel );
00398 void vergenceChangedPosition( real newpos );
00400 void vergenceChangedVelocity( real newvel );
00402 void vergenceChangedLimits( real lowlimit, real highlimit );
00403
00405 void rightEyeChangedDesiredPosition( real wishpos );
00407 void rightEyeChangedDesiredVelocity( real wishvel );
00409 void rightEyeChangedPosition( real newpos );
00411 void rightEyeChangedVelocity( real newvel );
00413 void rightEyeChangedLimits( real lowlimit, real highlimit );
00414
00416 void leftEyeChangedDesiredPosition( real wishpos );
00418 void leftEyeChangedDesiredVelocity( real wishvel );
00420 void leftEyeChangedPosition( real newpos );
00422 void leftEyeChangedVelocity( real newvel );
00424 void leftEyeChangedLimits( real lowlimit, real highlimit );
00425
00426 private:
00427
00429 QVector<PhyObject*> leftLegv;
00431 QVector<PhyObject*> rightLegv;
00433 QVector<PhyObject*> torsov;
00435 QVector<PhyObject*> leftArmv;
00437 QVector<PhyObject*> rightArmv;
00439 QVector<PhyObject*> headNeckv;
00440
00442 QVector<WMesh*> coversv;
00443
00445 QVector<PhyJoint*> leftLegJointv;
00447 QVector<PhyJoint*> rightLegJointv;
00449 QVector<PhyJoint*> torsoJointv;
00451 QVector<PhyJoint*> leftArmJointv;
00453 QVector<PhyJoint*> rightArmJointv;
00455 QVector<PhyJoint*> headNeckJointv;
00457 PhyDOF* versionDOF;
00459 PhyDOF* vergenceDOF;
00462 PhyDOF* rightEyeDOF;
00465 PhyDOF* leftEyeDOF;
00468 bool ignoreEyeSignals;
00469
00471 enum PhyObjectsGroupChains {
00472 IndexChain, MiddleChain, RingChain, PinkyChain,
00473 ThumbChain, EyeChain
00474 };
00475
00477 PhyObjectsGroup* leftHandGroup;
00479 PhyObjectsGroup* rightHandGroup;
00480
00482 QVector<real> leftLegMasses;
00484 QVector<real> rightLegMasses;
00486 QVector<real> torsoMasses;
00488 QVector<real> leftArmMasses;
00490 QVector<real> rightArmMasses;
00492 QVector<real> headNeckMasses;
00493
00495 MultiMotorController* torsoCtrl;
00497 MultiMotorController* leftArmCtrl;
00499 MultiMotorController* rightArmCtrl;
00501 MultiMotorController* headNeckCtrl;
00503 MultiMotorController* leftLegCtrl;
00505 MultiMotorController* rightLegCtrl;
00506
00513 yarp::dev::PolyDriver* cartServLeftArm;
00514 yarp::dev::ICartesianControl* cartCtrlLeftArm;
00515 ::iCub::iKin::iCubArmCartesianSolver* cartSolvLeftArm;
00516 yarp::os::RateThread* cartThreadLeftArm;
00518 yarp::dev::PolyDriver* cartServRightArm;
00519 yarp::dev::ICartesianControl* cartCtrlRightArm;
00520 ::iCub::iKin::iCubArmCartesianSolver* cartSolvRightArm;
00521
00523 WCamera* leftcam;
00524 WCamera* rightcam;
00525
00527 bool enabledLeftLeg;
00528 bool enabledRightLeg;
00529 bool enabledTorso;
00530 bool enabledLeftArm;
00531 bool enabledRightArm;
00532 bool enabledHead;
00533 bool enabledCameras;
00534 bool blockedTorso0;
00535 bool enabledRightKinematicHand;
00536 bool enabledLeftKinematicHand;
00537
00538 const bool enabledServerControlBoards;
00540 bool alwaysUpdateKinematicChains;
00541
00543 KinematicLimb<iCub::iKin::iCubArm> kinRightArm;
00544 KinematicLimb<iCub::iKin::iCubArm> kinLeftArm;
00545 KinematicLimb<iCub::iKin::iCubLeg> kinRightLeg;
00546 KinematicLimb<iCub::iKin::iCubLeg> kinLeftLeg;
00547 KinematicLimb<iCub::iKin::iCubEye> kinRightEye;
00548 KinematicLimb<iCub::iKin::iCubEye> kinLeftEye;
00549
00553 bool kinematicSimulation;
00557 bool collidingLeftHand;
00561 bool collidingRightHand;
00562 };
00563 }
00564
00565 #endif
00566
00567 #endif // FARSA_USE_YARP_AND_ICUB