20 #ifdef FARSA_USE_YARP_AND_ICUB
25 #include "worldsimconfig.h"
28 #include "wquaternion.h"
30 #include "yarpobject.h"
33 #include "phyicubhelpers.h"
43 #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
44 #pragma GCC diagnostic push
45 #pragma GCC diagnostic ignored "-Wunused-parameter"
47 #pragma GCC diagnostic ignored "-Wunused-parameter"
51 #include "yarp/dev/CartesianControl.h"
52 #include "iCub/iKin/iKinHlp.h"
53 #include "iCub/iKin/iKinSlv.h"
56 #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ > 5)
57 #pragma GCC diagnostic pop
59 #pragma GCC diagnostic warning "-Wunused-parameter"
77 class MultiMotorController;
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,
144 void doKinematicSimulation(
bool k,
bool clh =
false,
bool crh =
false);
150 return kinematicSimulation;
162 void configurePosture( QMap<int, real> jointSetup );
175 const QVector<PhyObject*>&
torso() {
201 return leftLegJointv;
206 return rightLegJointv;
227 return leftArmJointv;
243 return rightArmJointv;
248 return headNeckJointv;
276 return cartCtrlLeftArm;
280 return cartCtrlRightArm;
283 yarp::dev::IFrameGrabberImage* leftEyeFrameGrabber();
285 yarp::dev::IFrameGrabberImage* rightEyeFrameGrabber();
292 virtual void preUpdate();
297 virtual void postUpdate();
302 alwaysUpdateKinematicChains = enable;
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 );
327 bool isEnabledLeftLeg() {
328 return enabledLeftLeg;
330 bool isEnabledRightLeg() {
331 return enabledRightLeg;
333 bool isEnabledTorso() {
336 bool isEnabledHead() {
339 bool isEnabledCameras() {
340 return enabledCameras;
342 bool isEnabledLeftArm() {
343 return enabledLeftArm;
345 bool isEnabledRightArm() {
346 return enabledRightArm;
348 bool isBlockedTorso0() {
349 return blockedTorso0;
351 bool isEnabledLeftKinematicHand() {
352 return enabledLeftKinematicHand;
354 bool isEnabledRightKinematicHand() {
355 return enabledRightKinematicHand;
359 void enableLeftArmCartesianController();
361 void enableRightArmCartesianController();
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);
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 );
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 );
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 );
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 );
431 QVector<PhyObject*> leftLegv;
433 QVector<PhyObject*> rightLegv;
435 QVector<PhyObject*> torsov;
437 QVector<PhyObject*> leftArmv;
439 QVector<PhyObject*> rightArmv;
441 QVector<PhyObject*> headNeckv;
444 QVector<WMesh*> coversv;
447 QVector<PhyJoint*> leftLegJointv;
449 QVector<PhyJoint*> rightLegJointv;
451 QVector<PhyJoint*> torsoJointv;
453 QVector<PhyJoint*> leftArmJointv;
455 QVector<PhyJoint*> rightArmJointv;
457 QVector<PhyJoint*> headNeckJointv;
470 bool ignoreEyeSignals;
473 enum PhyObjectsGroupChains {
474 IndexChain, MiddleChain, RingChain, PinkyChain,
479 PhyObjectsGroup* leftHandGroup;
481 PhyObjectsGroup* rightHandGroup;
484 QVector<real> leftLegMasses;
486 QVector<real> rightLegMasses;
488 QVector<real> torsoMasses;
490 QVector<real> leftArmMasses;
492 QVector<real> rightArmMasses;
494 QVector<real> headNeckMasses;
497 MultiMotorController* torsoCtrl;
499 MultiMotorController* leftArmCtrl;
501 MultiMotorController* rightArmCtrl;
503 MultiMotorController* headNeckCtrl;
505 MultiMotorController* leftLegCtrl;
507 MultiMotorController* rightLegCtrl;
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;
530 bool enabledRightLeg;
533 bool enabledRightArm;
537 bool enabledRightKinematicHand;
538 bool enabledLeftKinematicHand;
540 const bool enabledServerControlBoards;
542 bool alwaysUpdateKinematicChains;
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;
555 bool kinematicSimulation;
559 bool collidingLeftHand;
563 bool collidingRightHand;
569 #endif // FARSA_USE_YARP_AND_ICUB