WorldSim - 0.6.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
File List
00001 /********************************************************************************
00002  *  WorldSim -- library for robot simulations                                   *
00003  *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
00004  *                                                                              *
00005  *  This program is free software; you can redistribute it and/or modify        *
00006  *  it under the terms of the GNU General Public License as published by        *
00007  *  the Free Software Foundation; either version 2 of the License, or           *
00008  *  (at your option) any later version.                                         *
00009  *                                                                              *
00010  *  This program is distributed in the hope that it will be useful,             *
00011  *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
00012  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
00013  *  GNU General Public License for more details.                                *
00014  *                                                                              *
00015  *  You should have received a copy of the GNU General Public License           *
00016  *  along with this program; if not, write to the Free Software                 *
00017  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
00018  ********************************************************************************/
00019 
00020 #ifdef FARSA_USE_YARP_AND_ICUB
00021 
00022 #include "phyicub.h"
00023 #include "phybox.h"
00024 #include "phycylinder.h"
00025 #include "physphere.h"
00026 #include "phyfixed.h"
00027 #include "phyhinge.h"
00028 #include "phyuniversal.h"
00029 #include "phycompoundobject.h"
00030 #include "motorcontrollers.h"
00031 #include "wcamera.h"
00032 
00033 #ifdef __GNUC__
00034 #pragma GCC diagnostic ignored "-Wunused-parameter"
00035 #endif
00036     #include "yarp/os/ResourceFinder.h"
00037     #include "yarp/os/Bottle.h"
00038     #include "yarp/os/Property.h"
00039     #include "yarp/dev/PolyDriverList.h"
00040     #include "yarp/dev/Wrapper.h"
00041     #include "yarp/os/RateThread.h"
00042     #include "yarp/os/Network.h"
00043 #ifdef __GNUC__
00044 #pragma GCC diagnostic warning "-Wunused-parameter"
00045 #endif
00046 
00047 using namespace yarp::os;
00048 using namespace yarp::dev;
00049 using namespace iCub::iKin;
00050 
00051 #include <iostream>
00052 #include <QFile>
00053 
00054 namespace farsa {
00055 #ifdef __GNUC__
00056     #warning AD ELIO IL ROBOT DINAMICO BALLA MOLTO (DOPO QUALCHE STEP, CIRCA 350)
00057 #endif
00058     PhyiCub::PhyiCub(World* world, QString name, const wMatrix& torso0_tm, bool createServerControlBoards) :
00059         YarpObject(world, name, torso0_tm),
00060         leftLegv(),
00061         rightLegv(),
00062         torsov(),
00063         leftArmv(),
00064         rightArmv(),
00065         headNeckv(),
00066         coversv(),
00067         leftLegJointv(),
00068         rightLegJointv(),
00069         torsoJointv(),
00070         leftArmJointv(),
00071         rightArmJointv(),
00072         headNeckJointv(),
00073         versionDOF(NULL),
00074         vergenceDOF(NULL),
00075         rightEyeDOF(NULL),
00076         leftEyeDOF(NULL),
00077         ignoreEyeSignals(false),
00078         leftHandGroup(NULL),
00079         rightHandGroup(NULL),
00080         leftLegMasses(),
00081         rightLegMasses(),
00082         torsoMasses(),
00083         leftArmMasses(),
00084         rightArmMasses(),
00085         headNeckMasses(),
00086         torsoCtrl(NULL),
00087         leftArmCtrl(NULL),
00088         rightArmCtrl(NULL),
00089         headNeckCtrl(NULL),
00090         leftLegCtrl(NULL),
00091         rightLegCtrl(NULL),
00092         cartServLeftArm(NULL),
00093         cartCtrlLeftArm(NULL),
00094         cartSolvLeftArm(NULL),
00095         cartThreadLeftArm(NULL),
00096         cartServRightArm(NULL),
00097         cartCtrlRightArm(NULL),
00098         cartSolvRightArm(NULL),
00099         leftcam(NULL),
00100         rightcam(NULL),
00101         enabledLeftLeg(true),
00102         enabledRightLeg(true),
00103         enabledTorso(true),
00104         enabledLeftArm(true),
00105         enabledRightArm(true),
00106         enabledHead(true),
00107         enabledCameras(false),
00108         blockedTorso0(false),
00109         enabledRightKinematicHand(true),
00110         enabledLeftKinematicHand(true),
00111         enabledServerControlBoards(createServerControlBoards),
00112         alwaysUpdateKinematicChains(false),
00113         kinRightArm("right"),
00114         kinLeftArm("left"),
00115         kinRightLeg("right"),
00116         kinLeftLeg("left"),
00117         kinRightEye("right"),
00118         kinLeftEye("left"),
00119         kinematicSimulation(false)
00120     {
00121         // Removing constraints when building, so to avoid problems with the 0 position of joints (e.g.
00122         // the elbow lower limit is 5.5 degrees, but when building we need solids to be at 0 angle).
00123         // They will be re-enabled at the end of the constructor
00124         kinRightArm.setAllConstraints(false);
00125         kinLeftArm.setAllConstraints(false);
00126         kinRightLeg.setAllConstraints(false);
00127         kinLeftLeg.setAllConstraints(false);
00128         kinRightEye.setAllConstraints(false);
00129         kinLeftEye.setAllConstraints(false);
00130 
00131         // First of all setting buddies for all links
00132         kinRightArm.getLinkInfo(0).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftArm.getLinkInfo(0)) << &(kinRightEye.getLinkInfo(0)) << &(kinLeftEye.getLinkInfo(0)));
00133         kinRightArm.getLinkInfo(1).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftArm.getLinkInfo(1)) << &(kinRightEye.getLinkInfo(1)) << &(kinLeftEye.getLinkInfo(1)));
00134         kinRightArm.getLinkInfo(2).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftArm.getLinkInfo(2)) << &(kinRightEye.getLinkInfo(2)) << &(kinLeftEye.getLinkInfo(2)));
00135         kinRightEye.getLinkInfo(3).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftEye.getLinkInfo(3)));
00136         kinRightEye.getLinkInfo(4).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftEye.getLinkInfo(4)));
00137         kinRightEye.getLinkInfo(5).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftEye.getLinkInfo(5)));
00138         kinRightEye.getLinkInfo(6).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftEye.getLinkInfo(6)));
00139 
00140         // Initializing the kinematic chains so that all joints are at 0.0. This is needed to make
00141         // sure that when we will create joints, solids positions will automatically be the 0 position.
00142         for (unsigned int i = 0; i < kinRightEye.getN(); i++) {
00143             kinRightEye.setAng(i, 0.0);
00144         }
00145         for (unsigned int i = 0; i < kinLeftEye.getN(); i++) {
00146             kinLeftEye.setAng(i, 0.0);
00147         }
00148         for (unsigned int i = 0; i < kinRightLeg.getN(); i++) {
00149             kinRightLeg.setAng(i, 0.0);
00150         }
00151         for (unsigned int i = 0; i < kinLeftLeg.getN(); i++) {
00152             kinLeftLeg.setAng(i, 0.0);
00153         }
00154         for (unsigned int i = 0; i < kinRightArm.getN(); i++) {
00155             kinRightArm.setAng(i, 0.0);
00156         }
00157         for (unsigned int i = 0; i < kinLeftArm.getN(); i++) {
00158             kinLeftArm.setAng(i, 0.0);
00159         }
00160 
00161         // Before creating objects, setting the world matrix (i.e. the iCub transformation matrix)
00162         // to all kinematic chains and updating them. tm is the transformation matrix for the whole
00163         // iCub
00164         kinRightArm.setWorldMatrix(&tm);
00165         kinLeftArm.setWorldMatrix(&tm);
00166         kinRightLeg.setWorldMatrix(&tm);
00167         kinLeftLeg.setWorldMatrix(&tm);
00168         kinRightEye.setWorldMatrix(&tm);
00169         kinLeftEye.setWorldMatrix(&tm);
00170         kinRightArm.updateInformationFromiKin();
00171         kinLeftArm.updateInformationFromiKin();
00172         kinRightLeg.updateInformationFromiKin();
00173         kinLeftLeg.updateInformationFromiKin();
00174         kinRightEye.updateInformationFromiKin();
00175         kinLeftEye.updateInformationFromiKin();
00176 
00177         // Now we can start to create objects and joints and associate them with links in the kinematic
00178         // chains.
00179 
00180         //--------------------------------------
00181         //--- TORSO creation: object & joints
00182         //--------------------------------------
00183         {
00184             // Creating objects
00185             PhyBox* torso0 = new PhyBox( 0.11443f, 0.064f, 0.0470f, world, name+":torso0" );
00186             torso0->setMass( 0.20297f );
00187             torsov << torso0;
00188 
00189             PhyBox* torso1 = new PhyBox( 0.127f, 0.176f, 0.063f, world, name+":torso1" );
00190             torso1->setMass( 3.623f );
00191             torsov << torso1;
00192 
00193             PhyCylinder* torso2 = new PhyCylinder( 0.031f, 0.097f, world, name+":torso2" );
00194             torso2->setMass( 0.91179f );
00195             torsov << torso2;
00196 
00197             QVector<PhyObject*> torsoCentre;
00198             PhyCylinder* torso3a = new PhyCylinder( 0.04f, 0.0274f, world, name+":torso3a" );
00199             torso3a->setMass( 0.45165f );
00200             torso3a->setMatrix(wMatrix::identity());
00201             torsoCentre << torso3a;
00202             PhyBox* torso3b = new PhyBox( 0.1169f, 0.076f, 0.118f, world, name+":torso3b" ); // sideX was 0.109f
00203             torso3b->setMass( 1.8388f );
00204             torso3b->setMatrix(wMatrix(wQuaternion(wVector(1.0, 0.0, 0.0), -PI_GRECO * 0.0833f), wVector((torso3a->height() + torso3b->sideX()) / 2.0f, +0.038f, 0.0)));
00205             torsoCentre << torso3b;
00206             PhyBox* torso3c = new PhyBox( 0.1169f, 0.076f, 0.118f, world, name+":torso3c" ); // sideX was 0.109f
00207             torso3c->setMass( 1.8388f );
00208             torso3c->setMatrix(wMatrix(wQuaternion(wVector(1.0, 0.0, 0.0), +PI_GRECO * 0.0833f), wVector((torso3a->height() + torso3c->sideX()) / 2.0, -0.038f, 0.0)));
00209             torsoCentre << torso3c;
00210             PhyCompoundObject* torso3 = new PhyCompoundObject( torsoCentre, world, name+":torso3" );
00211             torsov << torso3;
00212 
00213             // Setting rotation/position of the left leg parts. This is automatically done when
00214             // objects are associated with kinematic links, we only need to specify the transformation
00215             // of objects relative to the frame of reference of the corresponding kinematic link.
00216             // As torso is part of more than one kinematic chain, we associate objects with links only
00217             // in the right arm chain (other chains are always aligned)
00218             setTorso0Matrix(); // Torso0 is not in a chain, this function sets its matrix
00219             // Setting torso0 as the root object for all chains
00220             kinRightArm.setRootObject(torso0);
00221             kinLeftArm.setRootObject(torso0);
00222             kinRightLeg.setRootObject(torso0);
00223             kinLeftLeg.setRootObject(torso0);
00224             kinRightEye.setRootObject(torso0);
00225             kinLeftEye.setRootObject(torso0);
00226 
00227             wMatrix mtr;
00228             wVector rotAx = kinRightArm.getLinkInfo(0).getRotAxis();
00229             real rotAng = PI_GRECO / 4.0f;
00230             wVector transl = kinRightArm.getLinkInfo(0).getRotCenterWObject();
00231             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of 45° around rotation axis
00232             kinRightArm.getLinkInfo(0).setWObject(torso1, mtr);
00233 
00234             rotAx = kinRightArm.getLinkInfo(1).getRotAxis() * wVector(1.0, 0.0, 0.0);
00235             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00236             transl = wVector(0.0, 0.0, 0.0);
00237             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of 90° around y axis
00238             kinRightArm.getLinkInfo(1).setWObject(torso2, mtr);
00239 
00240             // Here we need two rotations...
00241             rotAx = kinRightArm.getLinkInfo(2).getRotAxis() * wVector(1.0, 0.0, 0.0);
00242             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00243             transl = kinRightArm.getLinkInfo(2).getRotCenterWObject() - wVector(0.0, torso2->radius() + torso3a->height() / 2.0, 0.0);
00244             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of 90° around y axis
00245             rotAx = wVector(1.0, 0.0, 0.0);
00246             rotAng = PI_GRECO * 0.5f - PI_GRECO * 0.0833f;
00247             transl = wVector(0.0, 0.0, 0.0);
00248             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl) * mtr; // rotation around x axis
00249             kinRightArm.getLinkInfo(2).setWObject(torso3, mtr);
00250 
00251             //--- TORSO joints
00252             torsoJointv.resize(0);
00253             PhyJoint* j1 = kinRightArm.createJoint(2);
00254             torsoJointv << j1;
00255 
00256             PhyJoint* j2 = kinRightArm.createJoint(1);
00257             torsoJointv << j2;
00258 
00259             PhyJoint* j3 = kinRightArm.createJoint(0);
00260             torsoJointv << j3;
00261         }
00262 
00263         //---------------------------------------
00264         //--- LEFT LEG creation: objects & joints
00265         //---------------------------------------
00266         {
00267             // Creating objects
00268             PhyCylinder* leftLeg0 = new PhyCylinder( 0.038f, 0.013f, world, name+":leftLeg0" );
00269             leftLeg0->setMass( 0.32708f );
00270             leftLegv << leftLeg0;
00271 
00272             PhyCylinder* leftLeg1 = new PhyCylinder( 0.031f, 0.075f, world, name+":leftLeg1" );
00273             leftLeg1->setMass( 0.85061f );
00274             leftLegv << leftLeg1;
00275 
00276             const wMatrix y90 = wMatrix::yaw( PI_GRECO * 0.5f );
00277             QVector<PhyObject*> legsComp;
00278             PhyCylinder* leftLeg2a = new PhyCylinder( 0.0315f, 0.077f, world, name+":leftLeg2a" );
00279             leftLeg2a->setMatrix( y90 );
00280             leftLeg2a->setPosition( 0.112f, 0.0, 0.0 );
00281             leftLeg2a->setMass( 0.79206f );
00282             legsComp << leftLeg2a;
00283             PhyCylinder* leftLeg2b = new PhyCylinder( 0.034f, 0.224f, world, name+":leftLeg2b" );
00284             leftLeg2b->setMass( 1.5304f );
00285             legsComp << leftLeg2b;
00286             PhyCompoundObject* leftLeg2 = new PhyCompoundObject( legsComp, world, name+":leftLeg2" );
00287             leftLegv << leftLeg2;
00288 
00289             legsComp.clear();
00290             PhyCylinder* leftLeg3a = new PhyCylinder( 0.0245f, 0.063f, world, name+":leftLeg3a" );
00291             leftLeg3a->setMatrix( y90 );
00292             leftLeg3a->setPosition( -0.1065f, 0.0, 0.0 );
00293             leftLeg3a->setMass( 0.14801f );
00294             legsComp << leftLeg3a;
00295             PhyCylinder* leftLeg3b = new PhyCylinder( 0.0315f, 0.213f, world, name+":leftLeg3b" );
00296             leftLeg3b->setMass( 0.95262f );
00297             legsComp << leftLeg3b;
00298             PhyCompoundObject* leftLeg3 = new PhyCompoundObject( legsComp, world, name+":leftLeg3" );
00299             leftLegv << leftLeg3;
00300 
00301             PhyCylinder* leftLeg4 = new PhyCylinder( 0.027f, 0.095f, world, name+":leftLeg4" );
00302             leftLeg4->setMass( 0.59285f );
00303             leftLegv << leftLeg4;
00304 
00305             PhyBox* leftLeg5 = new PhyBox( 0.13f, 0.054f, 0.004f, world, name+":leftLeg5" );
00306             leftLeg5->setMass( 0.08185f );
00307             leftLegv << leftLeg5;
00308 
00309             // Setting rotation/position of the left leg parts. This is automatically done when
00310             // objects are associated with kinematic links, we only need to specify the transformation
00311             // of objects relative to the frame of reference of the corresponding kinematic link.
00312             wMatrix mtr;
00313             wVector rotAx = kinLeftLeg.getLinkInfo(0).getRotAxis() * wVector(1.0, 0.0, 0.0);
00314             real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00315             wVector transl = wVector(0.0, -(leftLeg1->radius() + (leftLeg0->height() / 2.0)), 0.0);
00316             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00317             kinLeftLeg.getLinkInfo(0).setWObject(leftLeg0, mtr);
00318 
00319             rotAx = kinLeftLeg.getLinkInfo(1).getRotAxis() * wVector(1.0, 0.0, 0.0);
00320             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00321             transl = wVector(0.0, 0.0, 0.0);
00322             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00323             kinLeftLeg.getLinkInfo(1).setWObject(leftLeg1, mtr);
00324 
00325             rotAx = kinLeftLeg.getLinkInfo(2).getRotAxis() * wVector(1.0, 0.0, 0.0);
00326             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00327             transl = wVector(0.0, (leftLeg2b->height() / 2.0), 0.0);
00328             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00329             kinLeftLeg.getLinkInfo(2).setWObject(leftLeg2, mtr);
00330 
00331             mtr = wMatrix::identity();
00332             mtr.w_pos = wVector((leftLeg3b->height() / 2.0), 0.0, 0.0, 1.0);
00333             kinLeftLeg.getLinkInfo(3).setWObject(leftLeg3, mtr);
00334 
00335             rotAx = kinLeftLeg.getLinkInfo(4).getRotAxis();
00336             rotAng = -PI_GRECO * 0.5f;
00337             transl = wVector(0.0, 0.0, 0.0105f); // I don't know how to calculate 0.0105 using object dimensions
00338             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around z axis
00339             kinLeftLeg.getLinkInfo(4).setWObject(leftLeg4, mtr);
00340 
00341             rotAx = kinLeftLeg.getLinkInfo(5).getRotAxis() * wVector(1.0, 0.0, 0.0);
00342             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00343             transl = wVector(leftLeg5->sideZ() / 2.0, 0.0, 0.0235f); // I don't know how to calculate 0.0235 using object dimensions
00344             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00345             kinLeftLeg.getLinkInfo(5).setWObject(leftLeg5, mtr);
00346 
00347             // Creating joints. Axis are inverted or not to have positive angles in the right direction
00348             // (perhaps I have to do this because we create the joint with the link n+1 as the parent
00349             // object of the joint instead of link n most of the time...)
00350             leftLegJointv.resize(0);
00351             PhyJoint* j1 = kinLeftLeg.createJoint(0);
00352             leftLegJointv << j1;
00353 
00354             PhyJoint* j2 = kinLeftLeg.createJoint(1);
00355             leftLegJointv << j2;
00356 
00357             PhyJoint* j3 = kinLeftLeg.createJoint(2, false);
00358             leftLegJointv << j3;
00359 
00360             PhyJoint* j4 = kinLeftLeg.createJoint(3);
00361             leftLegJointv << j4;
00362 
00363             PhyJoint* j5 = kinLeftLeg.createJoint(4);
00364             leftLegJointv << j5;
00365 
00366             PhyJoint* j6 = kinLeftLeg.createJoint(5);
00367             leftLegJointv << j6;
00368         }
00369 
00370         //---------------------------------------
00371         //--- RIGHT LEG creation: objects & joints
00372         //---------------------------------------
00373         {
00374             // Creating objects
00375             PhyCylinder* rightLeg0 = new PhyCylinder( 0.038f, 0.013f, world, name+":rightLeg0" );
00376             rightLeg0->setMass( 0.32708f );
00377             rightLegv << rightLeg0;
00378 
00379             PhyCylinder* rightLeg1 = new PhyCylinder( 0.031f, 0.075f, world, name+":rightLeg1" );
00380             rightLeg1->setMass( 0.85061f );
00381             rightLegv << rightLeg1;
00382 
00383             const wMatrix y90 = wMatrix::yaw( PI_GRECO * 0.5f );
00384             QVector<PhyObject*> legsComp;
00385             PhyCylinder* rightLeg2a = new PhyCylinder( 0.0315f, 0.077f, world, name+":rightLeg2a" );
00386             rightLeg2a->setMatrix( y90 );
00387             rightLeg2a->setPosition( 0.112f, 0.0, 0.0 );
00388             rightLeg2a->setMass( 0.79206f );
00389             legsComp << rightLeg2a;
00390             PhyCylinder* rightLeg2b = new PhyCylinder( 0.034f, 0.224f, world, name+":rightLeg2b" );
00391             rightLeg2b->setMass( 1.5304f );
00392             legsComp << rightLeg2b;
00393             PhyCompoundObject* rightLeg2 = new PhyCompoundObject( legsComp, world, name+":rightLeg2" );
00394             rightLegv << rightLeg2;
00395 
00396             legsComp.clear();
00397             PhyCylinder* rightLeg3a = new PhyCylinder( 0.0245f, 0.063f, world, name+":rightLeg3a" );
00398             rightLeg3a->setMatrix( y90 );
00399             rightLeg3a->setPosition( -0.1065f, 0.0, 0.0 );
00400             rightLeg3a->setMass( 0.14801f );
00401             legsComp << rightLeg3a;
00402             PhyCylinder* rightLeg3b = new PhyCylinder( 0.0315f, 0.213f, world, name+":rightLeg3b" );
00403             rightLeg3b->setMass( 0.95262f );
00404             legsComp << rightLeg3b;
00405             PhyCompoundObject* rightLeg3 = new PhyCompoundObject( legsComp, world, name+":rightLeg3" );
00406             rightLegv << rightLeg3;
00407 
00408             PhyCylinder* rightLeg4 = new PhyCylinder( 0.027f, 0.095f, world, name+":rightLeg4" );
00409             rightLeg4->setMass( 0.59285f );
00410             rightLegv << rightLeg4;
00411 
00412             PhyBox* rightLeg5 = new PhyBox( 0.13f, 0.054f, 0.004f, world, name+":rightLeg5" );
00413             rightLeg5->setMass( 0.08185f );
00414             rightLegv << rightLeg5;
00415 
00416             // Setting rotation/position of the right leg parts. This is automatically done when
00417             // objects are associated with kinematic links, we only need to specify the transformation
00418             // of objects relative to the frame of reference of the corresponding kinematic link.
00419             wMatrix mtr;
00420             wVector rotAx = wVector(1.0, 0.0, 0.0) * kinRightLeg.getLinkInfo(0).getRotAxis();
00421             real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00422             wVector transl = wVector(0.0, -(rightLeg1->radius() + (rightLeg0->height() / 2.0)), 0.0);
00423             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00424             kinRightLeg.getLinkInfo(0).setWObject(rightLeg0, mtr);
00425 
00426             rotAx = wVector(1.0, 0.0, 0.0) * kinRightLeg.getLinkInfo(1).getRotAxis();
00427             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00428             transl = wVector(0.0, 0.0, 0.0);
00429             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00430             kinRightLeg.getLinkInfo(1).setWObject(rightLeg1, mtr);
00431 
00432             rotAx = wVector(1.0, 0.0, 0.0) * kinRightLeg.getLinkInfo(2).getRotAxis();
00433             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00434             transl = wVector(0.0, (rightLeg2b->height() / 2.0), 0.0);
00435             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00436             kinRightLeg.getLinkInfo(2).setWObject(rightLeg2, mtr);
00437 
00438             mtr = wMatrix::identity();
00439             mtr.w_pos = wVector((rightLeg3b->height() / 2.0), 0.0, 0.0, 1.0);
00440             kinRightLeg.getLinkInfo(3).setWObject(rightLeg3, mtr);
00441 
00442             rotAx = kinRightLeg.getLinkInfo(4).getRotAxis();
00443             rotAng = PI_GRECO * 0.5f;
00444             transl = wVector(0.0, 0.0, -0.0105f); // I don't know how to calculate 0.0105 using object dimensions
00445             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around z axis
00446             kinRightLeg.getLinkInfo(4).setWObject(rightLeg4, mtr);
00447 
00448             rotAx = wVector(1.0, 0.0, 0.0) * kinRightLeg.getLinkInfo(5).getRotAxis();
00449             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00450             transl = wVector(rightLeg5->sideZ() / 2.0f, 0.0, 0.0235f); // I don't know how to calculate 0.0235 using object dimensions
00451             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00452             kinRightLeg.getLinkInfo(5).setWObject(rightLeg5, mtr);
00453 
00454             // Creating joints. Axis are inverted or not to have positive angles in the right direction
00455             // (perhaps I have to do this because we create the joint with the link n+1 as the parent
00456             // object of the joint instead of link n most of the time...)
00457             rightLegJointv.resize(0);
00458             PhyJoint* j1 = kinRightLeg.createJoint(0);
00459             rightLegJointv << j1;
00460 
00461             PhyJoint* j2 = kinRightLeg.createJoint(1);
00462             rightLegJointv << j2;
00463 
00464             PhyJoint* j3 = kinRightLeg.createJoint(2);
00465             rightLegJointv << j3;
00466 
00467             PhyJoint* j4 = kinRightLeg.createJoint(3);
00468             rightLegJointv << j4;
00469 
00470             PhyJoint* j5 = kinRightLeg.createJoint(4);
00471             rightLegJointv << j5;
00472 
00473             PhyJoint* j6 = kinRightLeg.createJoint(5);
00474             rightLegJointv << j6;
00475         }
00476 
00477         //---------------------------------------
00478         //--- LEFT ARM creation: object & joints
00479         //---------------------------------------
00480         {
00481             // Creating objects
00482             PhyCylinder* leftArm0 = new PhyCylinder( 0.031f, 0.011f, world, name+":leftArm0" );
00483             leftArm0->setMass( 0.48278f );
00484             leftArmv << leftArm0;
00485 
00486             PhyCylinder* leftArm1 = new PhyCylinder( 0.03f, 0.059f, world, name+":leftArm1" );
00487             leftArm1->setMass( 0.20779f );
00488             leftArmv << leftArm1;
00489 
00490             PhyCylinder* leftArm2 = new PhyCylinder( 0.026f, 0.156f, world, name+":leftArm2" );
00491             leftArm2->setMass( 1.1584f );
00492             leftArmv << leftArm2;
00493 
00494 #ifdef __GNUC__
00495     #warning SAREBBE MEGLIO SE INVECE DELLA SFERA CI FOSSE UN CILINDRO CON L'ASSE DI ROTAZIONE SULL'ASSE DEL GIUNTO (IL SEGMENTO SUCCESSIVO SAREBBE POI DISASSATO)
00496 #endif
00497             PhySphere* leftArm3 = new PhySphere( 0.024f /*was 0.01*/, world, name+":leftArm3" );
00498             leftArm3->setMass( 0.050798f );
00499             leftArmv << leftArm3;
00500 
00501             PhyCylinder* leftArm4 = new PhyCylinder( 0.02f, 0.14f, world, name+":leftArm4" );
00502             leftArm4->setMass( 0.48774f );
00503             leftArmv << leftArm4;
00504 
00505             //--- this object isn't into Vadim's code, and I invented dimension and mass
00506             //--- It was introduced because Universal joint doesn't have the same frame as
00507             //--- specified to DH notation, so I created two hinge joints
00508             //--- Actually the real wrist has two hinge joints arranged as I did, but
00509             //--- I don't know which mass to set at intermediate joint
00510             PhyCylinder* leftArm5 = new PhyCylinder( 0.007f, 0.04f, world, name+":leftArm5" );
00511             leftArm5->setMass( 0.05f );
00512             leftArmv << leftArm5;
00513 
00514             //--- left hand creation
00515             //------------- structure respect to index inside leftArmv
00516             //              ----------------------
00517             //  26   25  24|  23    Palm 6      |
00518             //  ===|===|===|=====|              |
00519             //   thumb     |                    |
00520             //             -----------------------
00521             //      index 7 ||  8 ||  9 || 10 ||   pinky
00522             //              --    --    --    --
00523             //              ||    ||    ||    ||   pinky
00524             //           11 || 12 || 13 || 14 ||   pinky
00525             //              ||    ||    ||    ||   pinky
00526             //              --    --    --    --
00527             //           15 || 16 || 17 || 18 ||   pinky
00528             //              ||    ||    ||    ||   pinky
00529             //              --    --    --    --
00530             //           19 || 20 || 21 || 22 ||   pinky
00531             //              ||    ||    ||    ||   pinky
00532             //              --    --    --    --
00533             //---------------------- the same indices are valid also for right hand
00534             // This dimension is computed so that the frame of reference of the hand ends on the edge between
00535             // the inner palm and the face to which four fingers are attached and the palm is tangent to the
00536             // leftArm5 solid. The dimensions of leftArmPalm6 from original simulator code were
00537             // (0.069, 0.065, 0.024)
00538             const real leftArmPalm6X = fabs(kinLeftArm.getLinkInfo(9).getAxis() % kinLeftArm.getLinkInfo(9).getYarpMatrixAswMatrix().x_ax) - leftArm5->radius();
00539             PhyBox* leftArmPalm6 = new PhyBox( leftArmPalm6X, 0.065f, 0.018f, world, name+":leftArmPalm6" );
00540             leftArmPalm6->setMass( 0.19099f );
00541             leftArmv << leftArmPalm6;
00542             // Creating the hand group and adding leftArmPalm6 as the head object. Other objects will be added
00543             // only after their position has been set
00544             leftHandGroup = new PhyObjectsGroup(leftArmPalm6, world, name+":leftHandGroup");
00545             leftHandGroup->setOwner(this, false);
00546 
00547 #ifdef __GNUC__
00548     #warning ============= AREN T FINGER PIECES A BIT TOO HEAVY? =============
00549 #endif
00550             PhyCylinder* leftArmIndex7 = new PhyCylinder( 0.0065f,0.012f, world, name+":leftArmIndex7" );
00551             leftArmIndex7->setMass( 0.2f );
00552             leftArmv << leftArmIndex7;
00553 
00554             PhyCylinder* leftArmMiddle8 = new PhyCylinder( 0.0065f,0.012f, world, name+":leftArmMiddle8" );
00555             leftArmMiddle8->setMass( 0.2f );
00556             leftArmv << leftArmMiddle8;
00557 
00558             PhyCylinder* leftArmRing9 = new PhyCylinder( 0.0065f,0.012f, world, name+":leftArmRing9" );
00559             leftArmRing9->setMass( 0.2f );
00560             leftArmv << leftArmRing9;
00561 
00562             PhyCylinder* leftArmPinky10 = new PhyCylinder( 0.0065f,0.012f, world, name+":leftArmPinky10" );
00563             leftArmPinky10->setMass( 0.2f );
00564             leftArmv << leftArmPinky10;
00565 
00566             PhyCylinder* leftArmIndex11 = new PhyCylinder( 0.0065f,0.026f, world, name+":leftArmIndex11" );
00567             leftArmIndex11->setMass( 0.2f );
00568             leftArmv << leftArmIndex11;
00569 
00570             PhyCylinder* leftArmMiddle12 = new PhyCylinder( 0.0065f,0.028f, world, name+":leftArmMiddle12" );
00571             leftArmMiddle12->setMass( 0.2f );
00572             leftArmv << leftArmMiddle12;
00573 
00574             PhyCylinder* leftArmRing13 = new PhyCylinder( 0.0065f,0.026f, world, name+":leftArmRing13" );
00575             leftArmRing13->setMass( 0.2f );
00576             leftArmv << leftArmRing13;
00577 
00578             PhyCylinder* leftArmPinky14 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmPinky14" );
00579             leftArmPinky14->setMass( 0.2f );
00580             leftArmv << leftArmPinky14;
00581 
00582             PhyCylinder* leftArmIndex15 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmIndex15" );
00583             leftArmIndex15->setMass( 0.2f );
00584             leftArmv << leftArmIndex15;
00585 
00586             PhyCylinder* leftArmMiddle16 = new PhyCylinder( 0.0065f,0.024f, world, name+":leftArmMiddle16" );
00587             leftArmMiddle16->setMass( 0.2f );
00588             leftArmv << leftArmMiddle16;
00589 
00590             PhyCylinder* leftArmRing17 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmRing17" );
00591             leftArmRing17->setMass( 0.2f );
00592             leftArmv << leftArmRing17;
00593 
00594             PhyCylinder* leftArmPinky18 = new PhyCylinder( 0.0065f,0.019f, world, name+":leftArmPinky18" );
00595             leftArmPinky18->setMass( 0.2f );
00596             leftArmv << leftArmPinky18;
00597 
00598             PhyCylinder* leftArmIndex19 = new PhyCylinder( 0.0065f,0.02f, world, name+":leftArmIndex19" );
00599             leftArmIndex19->setMass( 0.2f );
00600             leftArmv << leftArmIndex19;
00601 
00602             PhyCylinder* leftArmMiddle20 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmMiddle20" );
00603             leftArmMiddle20->setMass( 0.2f );
00604             leftArmv << leftArmMiddle20;
00605 
00606             PhyCylinder* leftArmRing21 = new PhyCylinder( 0.0065f,0.02f, world, name+":leftArmRing21" );
00607             leftArmRing21->setMass( 0.2f );
00608             leftArmv << leftArmRing21;
00609 
00610             PhyCylinder* leftArmPinky22 = new PhyCylinder( 0.0065f,0.02f, world, name+":leftArmPinky22" );
00611             leftArmPinky22->setMass( 0.2f );
00612             leftArmv << leftArmPinky22;
00613 
00614             PhyCylinder* leftArmThumb23 = new PhyCylinder( 0.0065f,0.026f, world, name+":leftArmThumb23" );
00615             leftArmThumb23->setMass( 0.2f );
00616             leftArmv << leftArmThumb23;
00617 
00618             PhyCylinder* leftArmThumb24 = new PhyCylinder( 0.0065f,0.026f, world, name+":leftArmThumb24" );
00619             leftArmThumb24->setMass( 0.2f );
00620             leftArmv << leftArmThumb24;
00621 
00622             PhyCylinder* leftArmThumb25 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmThumb25" );
00623             leftArmThumb25->setMass( 0.2f );
00624             leftArmv << leftArmThumb25;
00625 
00626             PhyCylinder* leftArmThumb26 = new PhyCylinder( 0.0065f,0.016f, world, name+":leftArmThumb26" );
00627             leftArmThumb26->setMass( 0.2f );
00628             leftArmv << leftArmThumb26;
00629 
00630             // Setting rotation/position of the left arm parts. This is automatically done when
00631             // objects are associated with kinematic links, we only need to specify the transformation
00632             // of objects relative to the frame of reference of the corresponding kinematic link.
00633             wMatrix mtr;
00634             wVector rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(3).getRotAxis();
00635             real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00636             wVector transl = wVector(0.0, leftArm0->height() / 2.0 + leftArm1->radius(), 0.0);
00637             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00638             kinLeftArm.getLinkInfo(3).setWObject(leftArm0, mtr);
00639 
00640             rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(4).getRotAxis();
00641             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00642             transl = wVector(0.0, 0.0, 0.0);
00643             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00644             kinLeftArm.getLinkInfo(4).setWObject(leftArm1, mtr);
00645 
00646             rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(5).getRotAxis();
00647             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00648             transl = wVector(0.0, leftArm2->height() / 2.0, 0.0);
00649             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00650             kinLeftArm.getLinkInfo(5).setWObject(leftArm2, mtr);
00651 
00652             mtr = wMatrix::identity();
00653             kinLeftArm.getLinkInfo(6).setWObject(leftArm3, mtr);
00654 
00655             rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(7).getRotAxis();
00656             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00657             transl = wVector(0.0, -leftArm4->height() / 2.0, 0.0);
00658             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00659             kinLeftArm.getLinkInfo(7).setWObject(leftArm4, mtr);
00660 
00661             rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(8).getRotAxis();
00662             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
00663             transl = wVector(0.0, 0.0, 0.0);
00664             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
00665             kinLeftArm.getLinkInfo(8).setWObject(leftArm5, mtr);
00666 
00667             mtr = wMatrix::identity();
00668             mtr.w_pos = wVector(-leftArmPalm6->sideX() / 2.0, 0.0, leftArmPalm6->sideZ() / 2.0, 1.0);
00669             kinLeftArm.getLinkInfo(9).setWObject(leftHandGroup, mtr);
00670 
00671             // Now positioning fingers. We cannot rely on iKin as fingers are not part of any kinematic chain.
00672             // We always start from the leftArmPalm6 matrix
00673             // --- positioning of index finger
00674             mtr = leftHandGroup->matrix();
00675             mtr.w_pos += mtr.rotateVector(wVector((leftArmPalm6->sideX() + leftArmIndex7->height()) / 2.0f, -0.02275f, 0.0));
00676             leftArmIndex7->setMatrix(mtr);
00677             leftHandGroup->addObject(leftArmIndex7, IndexChain);
00678             mtr.w_pos += mtr.rotateVector(wVector((leftArmIndex7->height() + leftArmIndex11->height()) / 2.0, 0.0, 0.0));
00679             leftArmIndex11->setMatrix(mtr);
00680             leftHandGroup->addObject(leftArmIndex11, IndexChain);
00681             mtr.w_pos += mtr.rotateVector(wVector((leftArmIndex11->height() + leftArmIndex15->height()) / 2.0, 0.0, 0.0));
00682             leftArmIndex15->setMatrix(mtr);
00683             leftHandGroup->addObject(leftArmIndex15, IndexChain);
00684             mtr.w_pos += mtr.rotateVector(wVector((leftArmIndex15->height() + leftArmIndex19->height()) / 2.0, 0.0, 0.0));
00685             leftArmIndex19->setMatrix(mtr);
00686             leftHandGroup->addObject(leftArmIndex19, IndexChain);
00687 
00688             // --- positioning of middle finger
00689             mtr = leftHandGroup->matrix();
00690             mtr.w_pos += mtr.rotateVector(wVector((leftArmPalm6->sideX() + leftArmMiddle8->height()) / 2.0, -0.0065f, 0.0));
00691             leftArmMiddle8->setMatrix(mtr);
00692             leftHandGroup->addObject(leftArmMiddle8, MiddleChain);
00693             mtr.w_pos += mtr.rotateVector(wVector((leftArmMiddle8->height() + leftArmMiddle12->height()) / 2.0, 0.0, 0.0));
00694             leftArmMiddle12->setMatrix(mtr);
00695             leftHandGroup->addObject(leftArmMiddle12, MiddleChain);
00696             mtr.w_pos += mtr.rotateVector(wVector((leftArmMiddle12->height() + leftArmMiddle16->height()) / 2.0, 0.0, 0.0));
00697             leftArmMiddle16->setMatrix(mtr);
00698             leftHandGroup->addObject(leftArmMiddle16, MiddleChain);
00699             mtr.w_pos += mtr.rotateVector(wVector((leftArmMiddle16->height() + leftArmMiddle20->height()) / 2.0, 0.0, 0.0));
00700             leftArmMiddle20->setMatrix(mtr);
00701             leftHandGroup->addObject(leftArmMiddle20, MiddleChain);
00702 
00703             // --- positioning of ring finger
00704             mtr = leftHandGroup->matrix();
00705             mtr.w_pos += mtr.rotateVector(wVector((leftArmPalm6->sideX() + leftArmRing9->height()) / 2.0, 0.00975f, 0.0));
00706             leftArmRing9->setMatrix(mtr);
00707             leftHandGroup->addObject(leftArmRing9, RingChain);
00708             mtr.w_pos += mtr.rotateVector(wVector((leftArmRing9->height() + leftArmRing13->height()) / 2.0, 0.0, 0.0));
00709             leftArmRing13->setMatrix(mtr);
00710             leftHandGroup->addObject(leftArmRing13, RingChain);
00711             mtr.w_pos += mtr.rotateVector(wVector((leftArmRing13->height() + leftArmRing17->height()) / 2.0, 0.0, 0.0));
00712             leftArmRing17->setMatrix(mtr);
00713             leftHandGroup->addObject(leftArmRing17, RingChain);
00714             mtr.w_pos += mtr.rotateVector(wVector((leftArmRing17->height() + leftArmRing21->height()) / 2.0, 0.0, 0.0));
00715             leftArmRing21->setMatrix(mtr);
00716             leftHandGroup->addObject(leftArmRing21, RingChain);
00717 
00718             // --- positioning of pinky
00719             mtr = leftHandGroup->matrix();
00720             mtr.w_pos += mtr.rotateVector(wVector((leftArmPalm6->sideX() + leftArmPinky10->height()) / 2.0, 0.026f, 0.0));
00721             leftArmPinky10->setMatrix(mtr);
00722             leftHandGroup->addObject(leftArmPinky10, PinkyChain);
00723             mtr.w_pos += mtr.rotateVector(wVector((leftArmPinky10->height() + leftArmPinky14->height()) / 2.0, 0.0, 0.0));
00724             leftArmPinky14->setMatrix(mtr);
00725             leftHandGroup->addObject(leftArmPinky14, PinkyChain);
00726             mtr.w_pos += mtr.rotateVector(wVector((leftArmPinky14->height() + leftArmPinky18->height()) / 2.0, 0.0, 0.0));
00727             leftArmPinky18->setMatrix(mtr);
00728             leftHandGroup->addObject(leftArmPinky18, PinkyChain);
00729             mtr.w_pos += mtr.rotateVector(wVector((leftArmPinky18->height() + leftArmPinky22->height()) / 2.0, 0.0, 0.0));
00730             leftArmPinky22->setMatrix(mtr);
00731             leftHandGroup->addObject(leftArmPinky22, PinkyChain);
00732 
00733             // --- positioning of thumb
00734             mtr = leftHandGroup->matrix().rotateAround(leftArmPalm6->matrix().z_ax, leftArmPalm6->matrix().w_pos, -PI_GRECO * 0.5f);
00735             mtr.w_pos += mtr.rotateVector(wVector(0.015f + leftArmThumb23->height() / 2.0, leftArmPalm6->sideY() / 2.0 - 0.035f, -leftArmPalm6->sideZ() / 2.0));
00736             leftArmThumb23->setMatrix(mtr);
00737             leftHandGroup->addObject(leftArmThumb23, ThumbChain);
00738             mtr.w_pos += mtr.rotateVector(wVector((leftArmThumb23->height() + leftArmThumb24->height()) / 2.0, 0.0, 0.0));
00739             leftArmThumb24->setMatrix(mtr);
00740             leftHandGroup->addObject(leftArmThumb24, ThumbChain);
00741             mtr.w_pos += mtr.rotateVector(wVector((leftArmThumb24->height() + leftArmThumb25->height()) / 2.0, 0.0, 0.0));
00742             leftArmThumb25->setMatrix(mtr);
00743             leftHandGroup->addObject(leftArmThumb25, ThumbChain);
00744             mtr.w_pos += mtr.rotateVector(wVector((leftArmThumb25->height() + leftArmThumb26->height()) / 2.0, 0.0, 0.0));
00745             leftArmThumb26->setMatrix(mtr);
00746             leftHandGroup->addObject(leftArmThumb26, ThumbChain);
00747 
00748             // Creating joints. Axis are inverted or not to have positive angles in the right direction
00749             // (perhaps I have to do this because we create the joint with the link n+1 as the parent
00750             // object of the joint instead of link n most of the time...)
00751             leftArmJointv.resize(0);
00752             PhyJoint* j1 = kinLeftArm.createJoint(3);
00753             leftArmJointv << j1;
00754 
00755             PhyJoint* j2 = kinLeftArm.createJoint(4);
00756             leftArmJointv << j2;
00757 
00758             PhyJoint* j3 = kinLeftArm.createJoint(5);
00759             leftArmJointv << j3;
00760 
00761             PhyJoint* j4 = kinLeftArm.createJoint(6);
00762             leftArmJointv << j4;
00763 
00764             PhyJoint* j5 = kinLeftArm.createJoint(7);
00765             leftArmJointv << j5;
00766 
00767             PhyJoint* j6 = kinLeftArm.createJoint(8);
00768             leftArmJointv << j6;
00769 
00770             PhyJoint* j7 = kinLeftArm.createJoint(9);
00771             leftArmJointv << j7;
00772 
00773             // As above, for fingers we cannot rely on iKin
00774             PhyHinge* j8 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-leftArmIndex7->height() / 2.0, 0.0, 0.0), leftArmIndex7, leftArmPalm6);
00775             leftArmJointv << j8;
00776             leftHandGroup->addJointDOF(j8, 0, IndexChain);
00777 
00778             PhyHinge* j9 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-leftArmMiddle8->height() / 2.0, 0.0, 0.0), leftArmMiddle8, leftArmPalm6);
00779             leftArmJointv << j9;
00780             leftHandGroup->addJointDOF(j9, 0, MiddleChain);
00781 
00782             PhyHinge* j10 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-leftArmRing9->height() / 2.0, 0.0, 0.0), leftArmRing9, leftArmPalm6);
00783             leftArmJointv << j10;
00784             leftHandGroup->addJointDOF(j10, 0, RingChain);
00785 
00786             PhyHinge* j11 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-leftArmPinky10->height() / 2.0, 0.0, 0.0), leftArmPinky10, leftArmPalm6);
00787             leftArmJointv << j11;
00788             leftHandGroup->addJointDOF(j11, 0, PinkyChain);
00789 
00790             PhyHinge* j12 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmIndex11->height() / 2.0, 0.0, 0.0), leftArmIndex11, leftArmIndex7);
00791             leftArmJointv << j12;
00792             leftHandGroup->addJointDOF(j12, 0, IndexChain);
00793 
00794             PhyHinge* j13 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmIndex15->height() / 2.0, 0.0, 0.0), leftArmIndex15, leftArmIndex11);
00795             leftArmJointv << j13;
00796             leftHandGroup->addJointDOF(j13, 0, IndexChain);
00797 
00798             PhyHinge* j14 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmIndex19->height() / 2.0, 0.0, 0.0), leftArmIndex19, leftArmIndex15);
00799             leftArmJointv << j14;
00800             leftHandGroup->addJointDOF(j14, 0, IndexChain);
00801 
00802             PhyHinge* j15 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmMiddle12->height() / 2.0, 0.0, 0.0), leftArmMiddle12, leftArmMiddle8);
00803             leftArmJointv << j15;
00804             leftHandGroup->addJointDOF(j15, 0, MiddleChain);
00805 
00806             PhyHinge* j16 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmMiddle16->height() / 2.0, 0.0, 0.0), leftArmMiddle16, leftArmMiddle12);
00807             leftArmJointv << j16;
00808             leftHandGroup->addJointDOF(j16, 0, MiddleChain);
00809 
00810             PhyHinge* j17 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmMiddle20->height() / 2.0, 0.0, 0.0), leftArmMiddle20, leftArmMiddle16);
00811             leftArmJointv << j17;
00812             leftHandGroup->addJointDOF(j17, 0, MiddleChain);
00813 
00814             PhyHinge* j18 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmRing13->height() / 2.0, 0.0, 0.0), leftArmRing13, leftArmRing9);
00815             leftArmJointv << j18;
00816             leftHandGroup->addJointDOF(j18, 0, RingChain);
00817 
00818             PhyHinge* j19 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmRing17->height() / 2.0, 0.0, 0.0), leftArmRing17, leftArmRing13);
00819             leftArmJointv << j19;
00820             leftHandGroup->addJointDOF(j19, 0, RingChain);
00821 
00822             PhyHinge* j20 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmRing21->height() / 2.0, 0.0, 0.0), leftArmRing21, leftArmRing17);
00823             leftArmJointv << j20;
00824             leftHandGroup->addJointDOF(j20, 0, RingChain);
00825 
00826             PhyHinge* j21 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmPinky14->height() / 2.0, 0.0, 0.0), leftArmPinky14, leftArmPinky10);
00827             leftArmJointv << j21;
00828             leftHandGroup->addJointDOF(j21, 0, PinkyChain);
00829 
00830             PhyHinge* j22 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmPinky18->height() / 2.0, 0.0, 0.0), leftArmPinky18, leftArmPinky14);
00831             leftArmJointv << j22;
00832             leftHandGroup->addJointDOF(j22, 0, PinkyChain);
00833 
00834             PhyHinge* j23 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmPinky22->height() / 2.0, 0.0, 0.0), leftArmPinky22, leftArmPinky18);
00835             leftArmJointv << j23;
00836             leftHandGroup->addJointDOF(j23, 0, PinkyChain);
00837 
00838             PhyHinge* j24 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmThumb23->height() / 2.0, 0.0, 0.0), leftArmThumb23, leftArmPalm6);
00839             leftArmJointv << j24;
00840             leftHandGroup->addJointDOF(j24, 0, ThumbChain);
00841 
00842             PhyHinge* j25 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-leftArmThumb24->height() / 2.0, 0.0, 0.0), leftArmThumb24, leftArmThumb23);
00843             leftArmJointv << j25;
00844             leftHandGroup->addJointDOF(j25, 0, ThumbChain);
00845 
00846             PhyHinge* j26 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-leftArmThumb25->height() / 2.0, 0.0, 0.0), leftArmThumb25, leftArmThumb24);
00847             leftArmJointv << j26;
00848             leftHandGroup->addJointDOF(j26, 0, ThumbChain);
00849 
00850             PhyHinge* j27 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-leftArmThumb26->height() / 2.0, 0.0, 0.0), leftArmThumb26, leftArmThumb25);
00851             leftArmJointv << j27;
00852             leftHandGroup->addJointDOF(j27, 0, ThumbChain);
00853 
00854             //--- Coupling Left Hand joints (Opening)
00855             connect( leftArmJointv[7]->dofs()[0], SIGNAL( changedPosition( real ) ),
00856                     leftArmJointv[9]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00857             connect( leftArmJointv[7]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00858                     leftArmJointv[9]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00859             connect( leftArmJointv[7]->dofs()[0], SIGNAL( changedPosition( real ) ),
00860                     leftArmJointv[10]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00861             connect( leftArmJointv[7]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00862                     leftArmJointv[10]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00863             //--- Coupling (last two phalanges P2-P3 of fingers)
00864             connect( leftArmJointv[12]->dofs()[0], SIGNAL( changedPosition( real ) ),
00865                     leftArmJointv[13]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00866             connect( leftArmJointv[12]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00867                     leftArmJointv[13]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00868             connect( leftArmJointv[15]->dofs()[0], SIGNAL( changedPosition( real ) ),
00869                     leftArmJointv[16]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00870             connect( leftArmJointv[15]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00871                     leftArmJointv[16]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00872             connect( leftArmJointv[25]->dofs()[0], SIGNAL( changedPosition( real ) ),
00873                     leftArmJointv[26]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00874             connect( leftArmJointv[25]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00875                     leftArmJointv[26]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00876             //--- Coupling the ring-pinky phalanges
00877             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
00878                     leftArmJointv[18]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00879             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00880                     leftArmJointv[18]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00881             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
00882                     leftArmJointv[19]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00883             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00884                     leftArmJointv[19]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00885             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
00886                     leftArmJointv[20]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00887             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00888                     leftArmJointv[20]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00889             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
00890                     leftArmJointv[21]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00891             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00892                     leftArmJointv[21]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00893             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
00894                     leftArmJointv[22]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
00895             connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
00896                     leftArmJointv[22]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
00897         }
00898 
00899         //---------------------------------------
00900         //--- RIGHT ARM creation: object & joints
00901         //---------------------------------------
00902         {
00903             // Creating objects
00904             PhyCylinder* rightArm0 = new PhyCylinder( 0.031f, 0.011f, world, name+":rightArm0" );
00905             rightArm0->setMass( 0.48278f );
00906             rightArmv << rightArm0;
00907 
00908             PhyCylinder* rightArm1 = new PhyCylinder( 0.03f, 0.059f, world, name+":rightArm1" );
00909             rightArm1->setMass( 0.20779f );
00910             rightArmv << rightArm1;
00911 
00912             PhyCylinder* rightArm2 = new PhyCylinder( 0.026f, 0.156f, world, name+":rightArm2" );
00913             rightArm2->setMass( 1.1584f );
00914             rightArmv << rightArm2;
00915 
00916 #ifdef __GNUC__
00917     #warning SAREBBE MEGLIO SE INVECE DELLA SFERA CI FOSSE UN CILINDRO CON L'ASSE DI ROTAZIONE SULL'ASSE DEL GIUNTO (IL SEGMENTO SUCCESSIVO SAREBBE POI DISASSATO)
00918 #endif
00919             PhySphere* rightArm3 = new PhySphere( 0.024f /*was 0.01f*/, world, name+":rightArm3" );
00920             rightArm3->setMass( 0.050798f );
00921             rightArmv << rightArm3;
00922 
00923             PhyCylinder* rightArm4 = new PhyCylinder( 0.02f, 0.14f, world, name+":rightArm4" );
00924             rightArm4->setMass( 0.48774f );
00925             rightArmv << rightArm4;
00926 
00927             //--- this object isn't into Vadim's code, and I invented dimension and mass
00928             //--- It was introduced because Universal joint doesn't have the same frame as
00929             //--- specified to DH notation, so I created two hinge joints
00930             PhyCylinder* rightArm5 = new PhyCylinder( 0.007f, 0.04f, world, name+":rightArm5" );
00931             rightArm5->setMass( 0.05f );
00932             rightArmv << rightArm5;
00933 
00934             //--- right hand creation (see left hand for a description of hand structure)
00935             // This dimension is computed so that the frame of reference of the hand ends on the edge between
00936             // the inner palm and the face to which four fingers are attached and the palm is tangent to the
00937             // rightArm5 solid. The dimensions of rightArmPalm6 from original simulator code were
00938             // (0.069, 0.065, 0.024)
00939             const real rightArmPalm6X = fabs(kinRightArm.getLinkInfo(9).getAxis() % kinRightArm.getLinkInfo(9).getYarpMatrixAswMatrix().x_ax) - rightArm5->radius();
00940             PhyBox* rightArmPalm6 = new PhyBox( rightArmPalm6X, 0.065f, 0.018f, world, name+":rightArmPalm6" );
00941             rightArmPalm6->setMass( 0.19099f );
00942             rightArmv << rightArmPalm6;
00943             // Creating the hand group and adding rightArmPalm6 as the head object. Other objects will be added
00944             // only after their position has been set
00945             rightHandGroup = new PhyObjectsGroup(rightArmPalm6, world, name+":rightHandGroup");
00946             rightHandGroup->setOwner(this, false);
00947 
00948 #ifdef __GNUC__
00949     #warning ============= AREN T FINGER PIECES A BIT TOO HEAVY? =============
00950 #endif
00951             PhyCylinder* rightArmIndex7 = new PhyCylinder( 0.0065f,0.012f, world, name+":rightArmIndex7" );
00952             rightArmIndex7->setMass( 0.2f );
00953             rightArmv << rightArmIndex7;
00954 
00955             PhyCylinder* rightArmMiddle8 = new PhyCylinder( 0.0065f,0.012f, world, name+":rightArmMiddle8" );
00956             rightArmMiddle8->setMass( 0.2f );
00957             rightArmv << rightArmMiddle8;
00958 
00959             PhyCylinder* rightArmRing9 = new PhyCylinder( 0.0065f,0.012f, world, name+":rightArmRing9" );
00960             rightArmRing9->setMass( 0.2f );
00961             rightArmv << rightArmRing9;
00962 
00963             PhyCylinder* rightArmPinky10 = new PhyCylinder( 0.0065f,0.012f, world, name+":rightArmPinky10" );
00964             rightArmPinky10->setMass( 0.2f );
00965             rightArmv << rightArmPinky10;
00966 
00967             PhyCylinder* rightArmIndex11 = new PhyCylinder( 0.0065f,0.026f, world, name+":rightArmIndex11" );
00968             rightArmIndex11->setMass( 0.2f );
00969             rightArmv << rightArmIndex11;
00970 
00971             PhyCylinder* rightArmMiddle12 = new PhyCylinder( 0.0065f,0.028f, world, name+":rightArmMiddle12" );
00972             rightArmMiddle12->setMass( 0.2f );
00973             rightArmv << rightArmMiddle12;
00974 
00975             PhyCylinder* rightArmRing13 = new PhyCylinder( 0.0065f,0.026f, world, name+":rightArmRing13" );
00976             rightArmRing13->setMass( 0.2f );
00977             rightArmv << rightArmRing13;
00978 
00979             PhyCylinder* rightArmPinky14 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmPinky14" );
00980             rightArmPinky14->setMass( 0.2f );
00981             rightArmv << rightArmPinky14;
00982 
00983             PhyCylinder* rightArmIndex15 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmIndex15" );
00984             rightArmIndex15->setMass( 0.2f );
00985             rightArmv << rightArmIndex15;
00986 
00987             PhyCylinder* rightArmMiddle16 = new PhyCylinder( 0.0065f,0.024f, world, name+":rightArmMiddle16" );
00988             rightArmMiddle16->setMass( 0.2f );
00989             rightArmv << rightArmMiddle16;
00990 
00991             PhyCylinder* rightArmRing17 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmRing17" );
00992             rightArmRing17->setMass( 0.2f );
00993             rightArmv << rightArmRing17;
00994 
00995             PhyCylinder* rightArmPinky18 = new PhyCylinder( 0.0065f,0.019f, world, name+":rightArmPinky18" );
00996             rightArmPinky18->setMass( 0.2f );
00997             rightArmv << rightArmPinky18;
00998 
00999             PhyCylinder* rightArmIndex19 = new PhyCylinder( 0.0065f,0.02f, world, name+":rightArmIndex19" );
01000             rightArmIndex19->setMass( 0.2f );
01001             rightArmv << rightArmIndex19;
01002 
01003             PhyCylinder* rightArmMiddle20 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmMiddle20" );
01004             rightArmMiddle20->setMass( 0.2f );
01005             rightArmv << rightArmMiddle20;
01006 
01007             PhyCylinder* rightArmRing21 = new PhyCylinder( 0.0065f,0.02f, world, name+":rightArmRing21" );
01008             rightArmRing21->setMass( 0.2f );
01009             rightArmv << rightArmRing21;
01010 
01011             PhyCylinder* rightArmPinky22 = new PhyCylinder( 0.0065f,0.02f, world, name+":rightArmPinky22" );
01012             rightArmPinky22->setMass( 0.2f );
01013             rightArmv << rightArmPinky22;
01014 
01015             PhyCylinder* rightArmThumb23 = new PhyCylinder( 0.0065f,0.026f, world, name+":rightArmThumb23" );
01016             rightArmThumb23->setMass( 0.2f );
01017             rightArmv << rightArmThumb23;
01018 
01019             PhyCylinder* rightArmThumb24 = new PhyCylinder( 0.0065f,0.026f, world, name+":rightArmThumb24" );
01020             rightArmThumb24->setMass( 0.2f );
01021             rightArmv << rightArmThumb24;
01022 
01023             PhyCylinder* rightArmThumb25 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmThumb25" );
01024             rightArmThumb25->setMass( 0.2f );
01025             rightArmv << rightArmThumb25;
01026 
01027             PhyCylinder* rightArmThumb26 = new PhyCylinder( 0.0065f,0.016f, world, name+":rightArmThumb26" );
01028             rightArmThumb26->setMass( 0.2f );
01029             rightArmv << rightArmThumb26;
01030 
01031             // Setting rotation/position of the right arm parts. This is automatically done when
01032             // objects are associated with kinematic links, we only need to specify the transformation
01033             // of objects relative to the frame of reference of the corresponding kinematic link.
01034             wMatrix mtr;
01035             wVector rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(3).getRotAxis();
01036             real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01037             wVector transl = wVector(0.0, rightArm0->height() / 2.0 + rightArm1->radius(), 0.0);
01038             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
01039             kinRightArm.getLinkInfo(3).setWObject(rightArm0, mtr);
01040 
01041             rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(4).getRotAxis();
01042             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01043             transl = wVector(0.0, 0.0, 0.0);
01044             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
01045             kinRightArm.getLinkInfo(4).setWObject(rightArm1, mtr);
01046 
01047             rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(5).getRotAxis();
01048             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01049             transl = wVector(0.0, -rightArm2->height() / 2.0, 0.0);
01050             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
01051             kinRightArm.getLinkInfo(5).setWObject(rightArm2, mtr);
01052 
01053             mtr = wMatrix::identity();
01054             kinRightArm.getLinkInfo(6).setWObject(rightArm3, mtr);
01055 
01056             rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(7).getRotAxis();
01057             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01058             transl = wVector(0.0, rightArm4->height() / 2.0, 0.0);
01059             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
01060             kinRightArm.getLinkInfo(7).setWObject(rightArm4, mtr);
01061 
01062             rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(8).getRotAxis();
01063             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01064             transl = wVector(0.0, 0.0, 0.0);
01065             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
01066             kinRightArm.getLinkInfo(8).setWObject(rightArm5, mtr);
01067 
01068             mtr = wMatrix::identity();
01069             mtr.w_pos = wVector(-rightArmPalm6->sideX() / 2.0, 0.0, -rightArmPalm6->sideZ() / 2.0, 1.0);
01070             kinRightArm.getLinkInfo(9).setWObject(rightHandGroup, mtr);
01071 
01072             // Now positioning fingers. We cannot rely on iKin as fingers are not part of any kinematic chain.
01073             // We always start from the rightArmPalm6 matrix
01074             // --- positioning of index finger
01075             mtr = rightArmPalm6->matrix();
01076             mtr.w_pos += mtr.rotateVector(wVector((rightArmPalm6->sideX() + rightArmIndex7->height()) / 2.0f, -0.02275f, 0.0));
01077             rightArmIndex7->setMatrix(mtr);
01078             rightHandGroup->addObject(rightArmIndex7, IndexChain);
01079             mtr.w_pos += mtr.rotateVector(wVector((rightArmIndex7->height() + rightArmIndex11->height()) / 2.0, 0.0, 0.0));
01080             rightArmIndex11->setMatrix(mtr);
01081             rightHandGroup->addObject(rightArmIndex11, IndexChain);
01082             mtr.w_pos += mtr.rotateVector(wVector((rightArmIndex11->height() + rightArmIndex15->height()) / 2.0, 0.0, 0.0));
01083             rightArmIndex15->setMatrix(mtr);
01084             rightHandGroup->addObject(rightArmIndex15, IndexChain);
01085             mtr.w_pos += mtr.rotateVector(wVector((rightArmIndex15->height() + rightArmIndex19->height()) / 2.0, 0.0, 0.0));
01086             rightArmIndex19->setMatrix(mtr);
01087             rightHandGroup->addObject(rightArmIndex19, IndexChain);
01088 
01089             // --- positioning of middle finger
01090             mtr = rightArmPalm6->matrix();
01091             mtr.w_pos += mtr.rotateVector(wVector((rightArmPalm6->sideX() + rightArmMiddle8->height()) / 2.0, -0.0065f, 0.0));
01092             rightArmMiddle8->setMatrix(mtr);
01093             rightHandGroup->addObject(rightArmMiddle8, MiddleChain);
01094             mtr.w_pos += mtr.rotateVector(wVector((rightArmMiddle8->height() + rightArmMiddle12->height()) / 2.0, 0.0, 0.0));
01095             rightArmMiddle12->setMatrix(mtr);
01096             rightHandGroup->addObject(rightArmMiddle12, MiddleChain);
01097             mtr.w_pos += mtr.rotateVector(wVector((rightArmMiddle12->height() + rightArmMiddle16->height()) / 2.0, 0.0, 0.0));
01098             rightArmMiddle16->setMatrix(mtr);
01099             rightHandGroup->addObject(rightArmMiddle16, MiddleChain);
01100             mtr.w_pos += mtr.rotateVector(wVector((rightArmMiddle16->height() + rightArmMiddle20->height()) / 2.0, 0.0, 0.0));
01101             rightArmMiddle20->setMatrix(mtr);
01102             rightHandGroup->addObject(rightArmMiddle20, MiddleChain);
01103 
01104             // --- positioning of ring finger
01105             mtr = rightArmPalm6->matrix();
01106             mtr.w_pos += mtr.rotateVector(wVector((rightArmPalm6->sideX() + rightArmRing9->height()) / 2.0, 0.00975f, 0.0));
01107             rightArmRing9->setMatrix(mtr);
01108             rightHandGroup->addObject(rightArmRing9, RingChain);
01109             mtr.w_pos += mtr.rotateVector(wVector((rightArmRing9->height() + rightArmRing13->height()) / 2.0, 0.0, 0.0));
01110             rightArmRing13->setMatrix(mtr);
01111             rightHandGroup->addObject(rightArmRing13, RingChain);
01112             mtr.w_pos += mtr.rotateVector(wVector((rightArmRing13->height() + rightArmRing17->height()) / 2.0, 0.0, 0.0));
01113             rightArmRing17->setMatrix(mtr);
01114             rightHandGroup->addObject(rightArmRing17, RingChain);
01115             mtr.w_pos += mtr.rotateVector(wVector((rightArmRing17->height() + rightArmRing21->height()) / 2.0, 0.0, 0.0));
01116             rightArmRing21->setMatrix(mtr);
01117             rightHandGroup->addObject(rightArmRing21, RingChain);
01118 
01119             // --- positioning of pinky
01120             mtr = rightArmPalm6->matrix();
01121             mtr.w_pos += mtr.rotateVector(wVector((rightArmPalm6->sideX() + rightArmPinky10->height()) / 2.0, 0.026f, 0.0));
01122             rightArmPinky10->setMatrix(mtr);
01123             rightHandGroup->addObject(rightArmPinky10, PinkyChain);
01124             mtr.w_pos += mtr.rotateVector(wVector((rightArmPinky10->height() + rightArmPinky14->height()) / 2.0, 0.0, 0.0));
01125             rightArmPinky14->setMatrix(mtr);
01126             rightHandGroup->addObject(rightArmPinky14, PinkyChain);
01127             mtr.w_pos += mtr.rotateVector(wVector((rightArmPinky14->height() + rightArmPinky18->height()) / 2.0, 0.0, 0.0));
01128             rightArmPinky18->setMatrix(mtr);
01129             rightHandGroup->addObject(rightArmPinky18, PinkyChain);
01130             mtr.w_pos += mtr.rotateVector(wVector((rightArmPinky18->height() + rightArmPinky22->height()) / 2.0, 0.0, 0.0));
01131             rightArmPinky22->setMatrix(mtr);
01132             rightHandGroup->addObject(rightArmPinky22, PinkyChain);
01133 
01134             // --- positioning of thumb
01135             mtr = rightArmPalm6->matrix().rotateAround(rightArmPalm6->matrix().z_ax, rightArmPalm6->matrix().w_pos, -PI_GRECO * 0.5f);
01136             mtr.w_pos += mtr.rotateVector(wVector(0.015 + rightArmThumb23->height() / 2.0, rightArmPalm6->sideY() / 2.0 - 0.035, rightArmPalm6->sideZ() / 2.0));
01137             rightArmThumb23->setMatrix(mtr);
01138             rightHandGroup->addObject(rightArmThumb23, ThumbChain);
01139             mtr.w_pos += mtr.rotateVector(wVector((rightArmThumb23->height() + rightArmThumb24->height()) / 2.0, 0.0, 0.0));
01140             rightArmThumb24->setMatrix(mtr);
01141             rightHandGroup->addObject(rightArmThumb24, ThumbChain);
01142             mtr.w_pos += mtr.rotateVector(wVector((rightArmThumb24->height() + rightArmThumb25->height()) / 2.0, 0.0, 0.0));
01143             rightArmThumb25->setMatrix(mtr);
01144             rightHandGroup->addObject(rightArmThumb25, ThumbChain);
01145             mtr.w_pos += mtr.rotateVector(wVector((rightArmThumb25->height() + rightArmThumb26->height()) / 2.0, 0.0, 0.0));
01146             rightArmThumb26->setMatrix(mtr);
01147             rightHandGroup->addObject(rightArmThumb26, ThumbChain);
01148 
01149             // Creating joints. Axis are inverted or not to have positive angles in the right direction
01150             // (perhaps I have to do this because we create the joint with the link n+1 as the parent
01151             // object of the joint instead of link n most of the time...)
01152             rightArmJointv.resize(0);
01153             PhyJoint* j1 = kinRightArm.createJoint(3);
01154             rightArmJointv << j1;
01155 
01156             PhyJoint* j2 = kinRightArm.createJoint(4);
01157             rightArmJointv << j2;
01158 
01159             PhyJoint* j3 = kinRightArm.createJoint(5);
01160             rightArmJointv << j3;
01161 
01162             PhyJoint* j4 = kinRightArm.createJoint(6);
01163             rightArmJointv << j4;
01164 
01165             PhyJoint* j5 = kinRightArm.createJoint(7);
01166             rightArmJointv << j5;
01167 
01168             PhyJoint* j6 = kinRightArm.createJoint(8);
01169             rightArmJointv << j6;
01170 
01171             PhyJoint* j7 = kinRightArm.createJoint(9);
01172             rightArmJointv << j7;
01173 
01174             // As above, for fingers we cannot rely on iKin
01175             PhyHinge* j8 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-rightArmIndex7->height() / 2.0, 0.0, 0.0), rightArmIndex7, rightArmPalm6);
01176             rightArmJointv << j8;
01177             rightHandGroup->addJointDOF(j8, 0, IndexChain);
01178 
01179             PhyHinge* j9 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-rightArmMiddle8->height() / 2.0, 0.0, 0.0), rightArmMiddle8, rightArmPalm6);
01180             rightArmJointv << j9;
01181             rightHandGroup->addJointDOF(j9, 0, MiddleChain);
01182 
01183             PhyHinge* j10 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-rightArmRing9->height() / 2.0, 0.0, 0.0), rightArmRing9, rightArmPalm6);
01184             rightArmJointv << j10;
01185             rightHandGroup->addJointDOF(j10, 0, RingChain);
01186 
01187             PhyHinge* j11 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-rightArmPinky10->height() / 2.0, 0.0, 0.0), rightArmPinky10, rightArmPalm6);
01188             rightArmJointv << j11;
01189             rightHandGroup->addJointDOF(j11, 0, PinkyChain);
01190 
01191             PhyHinge* j12 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmIndex11->height() / 2.0, 0.0, 0.0), rightArmIndex11, rightArmIndex7);
01192             rightArmJointv << j12;
01193             rightHandGroup->addJointDOF(j12, 0, IndexChain);
01194 
01195             PhyHinge* j13 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmIndex15->height() / 2.0, 0.0, 0.0), rightArmIndex15, rightArmIndex11);
01196             rightArmJointv << j13;
01197             rightHandGroup->addJointDOF(j13, 0, IndexChain);
01198 
01199             PhyHinge* j14 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmIndex19->height() / 2.0, 0.0, 0.0), rightArmIndex19, rightArmIndex15);
01200             rightArmJointv << j14;
01201             rightHandGroup->addJointDOF(j14, 0, IndexChain);
01202 
01203             PhyHinge* j15 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmMiddle12->height() / 2.0, 0.0, 0.0), rightArmMiddle12, rightArmMiddle8);
01204             rightArmJointv << j15;
01205             rightHandGroup->addJointDOF(j15, 0, MiddleChain);
01206 
01207             PhyHinge* j16 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmMiddle16->height() / 2.0, 0.0, 0.0), rightArmMiddle16, rightArmMiddle12);
01208             rightArmJointv << j16;
01209             rightHandGroup->addJointDOF(j16, 0, MiddleChain);
01210 
01211             PhyHinge* j17 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmMiddle20->height() / 2.0, 0.0, 0.0), rightArmMiddle20, rightArmMiddle16);
01212             rightArmJointv << j17;
01213             rightHandGroup->addJointDOF(j17, 0, MiddleChain);
01214 
01215             PhyHinge* j18 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmRing13->height() / 2.0, 0.0, 0.0), rightArmRing13, rightArmRing9);
01216             rightArmJointv << j18;
01217             rightHandGroup->addJointDOF(j18, 0, RingChain);
01218 
01219             PhyHinge* j19 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmRing17->height() / 2.0, 0.0, 0.0), rightArmRing17, rightArmRing13);
01220             rightArmJointv << j19;
01221             rightHandGroup->addJointDOF(j19, 0, RingChain);
01222 
01223             PhyHinge* j20 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmRing21->height() / 2.0, 0.0, 0.0), rightArmRing21, rightArmRing17);
01224             rightArmJointv << j20;
01225             rightHandGroup->addJointDOF(j20, 0, RingChain);
01226 
01227             PhyHinge* j21 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmPinky14->height() / 2.0, 0.0, 0.0), rightArmPinky14, rightArmPinky10);
01228             rightArmJointv << j21;
01229             rightHandGroup->addJointDOF(j21, 0, PinkyChain);
01230 
01231             PhyHinge* j22 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmPinky18->height() / 2.0, 0.0, 0.0), rightArmPinky18, rightArmPinky14);
01232             rightArmJointv << j22;
01233             rightHandGroup->addJointDOF(j22, 0, PinkyChain);
01234 
01235             PhyHinge* j23 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmPinky22->height() / 2.0, 0.0, 0.0), rightArmPinky22, rightArmPinky18);
01236             rightArmJointv << j23;
01237             rightHandGroup->addJointDOF(j23, 0, PinkyChain);
01238 
01239             PhyHinge* j24 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmThumb23->height() / 2.0, 0.0, 0.0), rightArmThumb23, rightArmPalm6);
01240             rightArmJointv << j24;
01241             rightHandGroup->addJointDOF(j24, 0, ThumbChain);
01242 
01243             PhyHinge* j25 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-rightArmThumb24->height() / 2.0, 0.0, 0.0), rightArmThumb24, rightArmThumb23);
01244             rightArmJointv << j25;
01245             rightHandGroup->addJointDOF(j25, 0, ThumbChain);
01246 
01247             PhyHinge* j26 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-rightArmThumb25->height() / 2.0, 0.0, 0.0), rightArmThumb25, rightArmThumb24);
01248             rightArmJointv << j26;
01249             rightHandGroup->addJointDOF(j26, 0, ThumbChain);
01250 
01251             PhyHinge* j27 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-rightArmThumb26->height() / 2.0, 0.0, 0.0), rightArmThumb26, rightArmThumb25);
01252             rightArmJointv << j27;
01253             rightHandGroup->addJointDOF(j27, 0, ThumbChain);
01254 
01255             //--- Coupling Right Hand joints (Opening)
01256             connect( rightArmJointv[7]->dofs()[0], SIGNAL( changedPosition( real ) ),
01257                     rightArmJointv[9]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01258             connect( rightArmJointv[7]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01259                     rightArmJointv[9]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01260             connect( rightArmJointv[7]->dofs()[0], SIGNAL( changedPosition( real ) ),
01261                     rightArmJointv[10]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01262             connect( rightArmJointv[7]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01263                     rightArmJointv[10]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01264             //--- Coupling (last two phalanges P2-P3 of fingers)
01265             connect( rightArmJointv[12]->dofs()[0], SIGNAL( changedPosition( real ) ),
01266                     rightArmJointv[13]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01267             connect( rightArmJointv[12]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01268                     rightArmJointv[13]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01269             connect( rightArmJointv[15]->dofs()[0], SIGNAL( changedPosition( real ) ),
01270                     rightArmJointv[16]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01271             connect( rightArmJointv[15]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01272                     rightArmJointv[16]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01273             connect( rightArmJointv[25]->dofs()[0], SIGNAL( changedPosition( real ) ),
01274                     rightArmJointv[26]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01275             connect( rightArmJointv[25]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01276                     rightArmJointv[26]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01277             //--- Coupling the ring-pinky phalanges
01278             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
01279                     rightArmJointv[18]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01280             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01281                     rightArmJointv[18]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01282             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
01283                     rightArmJointv[19]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01284             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01285                     rightArmJointv[19]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01286             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
01287                     rightArmJointv[20]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01288             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01289                     rightArmJointv[20]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01290             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
01291                     rightArmJointv[21]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01292             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01293                     rightArmJointv[21]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01294             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
01295                     rightArmJointv[22]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
01296             connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
01297                     rightArmJointv[22]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
01298         }
01299 
01300         //---------------------------------------
01301         //--- HEAD-NECK creation: object & joints
01302         //---------------------------------------
01303         {
01304             // Creating objects
01305             PhyCylinder* headNeck0 = new PhyCylinder( 0.018f, 0.077f, world, name+":headNeck0" ); // Radius was 0.015
01306             headNeck0->setMass( 0.28252f );
01307             headNeckv << headNeck0;
01308 
01309             PhyCylinder* headNeck1 = new PhyCylinder( 0.015f, 0.077f, world, name+":headNeck1" );
01310             headNeck1->setMass( 0.1f );
01311             headNeckv << headNeck1;
01312 
01313             //--- creating a compound object composing the head-eyes
01314             const wMatrix y90 = wMatrix::yaw( PI_GRECO * 0.5f );
01315             QVector<PhyObject*> headObj;
01316             PhyCylinder* headNeck2a = new PhyCylinder( 0.015f,0.06f, world, name+":headNeck2a" );
01317             headNeck2a->setMatrix( y90 );
01318             headNeck2a->setMass( 0.13913f );
01319             headObj << headNeck2a;
01320             PhyBox* headNeck2b = new PhyBox( 0.052f,0.104f, 0.002f, world, name+":headNeck2b" );
01321             headNeck2b->setMass( 0.1562f );
01322             headObj << headNeck2b;
01323             PhyBox* headNeck2c = new PhyBox( 0.052f,0.002f, 0.093f, world, name+":headNeck2c" );
01324             headNeck2c->setMass( 0.1562f );
01325             headObj << headNeck2c;
01326             PhyBox* headNeck2d = new PhyBox( 0.052f,0.002f, 0.093f, world, name+":headNeck2d" );
01327             headNeck2d->setMass( 0.1562f );
01328             headObj << headNeck2d;
01329             PhyBox* headNeck2e = new PhyBox( 0.032f, 0.104f, 0.002f, world, name+":headNeck2e" );
01330             headNeck2e->setMass( 0.01f );
01331             headObj << headNeck2e;
01332             PhyBox* headNeck2f = new PhyBox( 0.025f, 0.011f, 0.026f, world, name+":headNeck2f" );
01333             headNeck2f->setMass( 0.0278f );
01334             headObj << headNeck2f;
01335             PhyBox* headNeck2g = new PhyBox( 0.012f, 0.011f, 0.051f, world, name+":headNeck2g" );
01336             headNeck2g->setMatrix( wMatrix::yaw( PI_GRECO*0.2f ) );
01337             headNeck2g->setMass( 0.0278f );
01338             headObj << headNeck2g;
01339             PhyBox* headNeck2h = new PhyBox( 0.012f, 0.02f, 0.022f, world, name+":headNeck2h" );
01340             headNeck2h->setMass( 0.0278f );
01341             headObj << headNeck2h;
01342             PhyCylinder* headNeck2i = new PhyCylinder( 0.006f,0.030f, world, name+":headNeck2i" );
01343             headNeck2i->setMass( 0.11f );
01344             headObj << headNeck2i;
01345             PhyCylinder* headNeck2j = new PhyCylinder( 0.006f,0.05f, world, name+":headNeck2j" );
01346             headNeck2j->setMatrix( y90 );
01347             headNeck2j->setMass( 0.0387f );
01348             headObj << headNeck2j;
01349             PhyCylinder* headNeck2l = new PhyCylinder( 0.006f,0.030f, world, name+":headNeck2l" );
01350             headNeck2l->setMass( 0.0234f );
01351             headObj << headNeck2l;
01352             PhyCylinder* headNeck2m = new PhyCylinder( 0.006f,0.05f, world, name+":headNeck2m" );
01353             headNeck2m->setMatrix( y90 );
01354             headNeck2m->setMass( 0.0387f );
01355             headObj << headNeck2m;
01356             headNeck2a->setPosition( wVector( 0.00,     0.0,   0.0 ) );
01357             headNeck2b->setPosition( wVector( -0.0125f,  0.0,   -0.011f ) );
01358             headNeck2c->setPosition( wVector( -0.0125f,  0.052f, 0.0355f ) );
01359             headNeck2d->setPosition( wVector( -0.0125f, -0.052f, 0.0355f ) );
01360             headNeck2e->setPosition( wVector( -0.0125f,  0.0,   0.0355f ) );
01361             headNeck2f->setPosition( wVector( 0.0275f,   0.0,   -0.01f ) );
01362             headNeck2g->setPosition( wVector( 0.05f,     0.0,   0.001f ) );
01363             headNeck2h->setPosition( wVector( 0.064f,    0.0,   0.028f ) );
01364             headNeck2i->setPosition( wVector( 0.049f,  +0.034f, 0.052f ) ); // wVector(0.049,  +0.034, 0.037) );
01365             headNeck2j->setPosition( wVector( 0.034f,  +0.034f, 0.037f ) );
01366             headNeck2l->setPosition( wVector( 0.049f,  -0.034f, 0.052f ) ); // wVector(0.049,  -0.034, 0.037) );
01367             headNeck2m->setPosition( wVector( 0.034f,  -0.034f, 0.037f ) );
01368             PhyCompoundObject* headNeck2 = new PhyCompoundObject( headObj, world, name+":headNeck2" );
01369             headNeckv << headNeck2;
01370 
01371             PhyCylinder* headNeckEye3 = new PhyCylinder( 0.002f,0.068f, world, name+":headNeckEye3" );
01372             headNeckEye3->setMass( 0.0059678f );
01373             headNeckv << headNeckEye3;
01374 
01375             PhySphere* headNeckEye4 = new PhySphere( 0.020f, world, name+":headNeckEye4" );
01376             headNeckEye4->setMass( 0.0234f );
01377             headNeckv << headNeckEye4;
01378 
01379             PhySphere* headNeckEye5 = new PhySphere( 0.020f, world, name+":headNeckEye5" );
01380             headNeckEye5->setMass( 0.0234f );
01381             headNeckv << headNeckEye5;
01382 
01383             // Setting rotation/position of the right arm parts. This is automatically done when
01384             // objects are associated with kinematic links, we only need to specify the transformation
01385             // of objects relative to the frame of reference of the corresponding kinematic link.
01386             // As the first four parts belong to the kinematic chains of both eyes, we associate them
01387             // with links only in the right eye chain (the other chain is always aligned)
01388             wMatrix mtr;
01389             wVector rotAx = wVector(1.0, 0.0, 0.0) * kinRightEye.getLinkInfo(3).getRotAxis();
01390             real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01391             wVector transl = kinRightEye.getLinkInfo(3).getRotCenterWObject();
01392             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
01393             kinRightEye.getLinkInfo(3).setWObject(headNeck0, mtr);
01394 
01395             rotAx = wVector(1.0, 0.0, 0.0) * kinRightEye.getLinkInfo(4).getRotAxis();
01396             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01397             transl = kinRightEye.getLinkInfo(4).getRotCenterWObject();
01398             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
01399             kinRightEye.getLinkInfo(4).setWObject(headNeck1, mtr);
01400 
01401             rotAx = wVector(0.0, 0.0, 1.0) * kinRightEye.getLinkInfo(5).getRotAxis(); // Using z instead of x because the object has already been rotated by y90
01402             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01403             transl = kinRightEye.getLinkInfo(5).getRotCenterWObject() + wVector(0.0, -headNeck2a->height() / 2.0, 0.0);
01404             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl);
01405             // Now rotating by PI_GRECO around the rotation axis
01406             rotAx = wVector(0.0, 0.0, 1.0);
01407             rotAng = PI_GRECO;
01408             transl = wVector(0.0, 0.0, 0.0);
01409             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl) * mtr;
01410             kinRightEye.getLinkInfo(5).setWObject(headNeck2, mtr);
01411 
01412             rotAx = wVector(1.0, 0.0, 0.0) * kinRightEye.getLinkInfo(6).getRotAxis();
01413             rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
01414             transl = kinRightEye.getLinkInfo(6).getRotCenterWObject();
01415             mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
01416             kinRightEye.getLinkInfo(6).setWObject(headNeckEye3, mtr);
01417 
01418             mtr = wMatrix::roll(PI_GRECO / 2.0f);
01419             kinRightEye.getLinkInfo(7).setWObject(headNeckEye4, mtr);
01420 
01421             mtr = wMatrix::roll(PI_GRECO / 2.0f);
01422             kinLeftEye.getLinkInfo(7).setWObject(headNeckEye5, mtr);
01423 
01424             //--- Eyes Cameras
01425             leftcam = NULL; //new WCamera( world, name+"/cam/left", headNeck()[7], 200, 200 );
01426             rightcam = NULL; //new WCamera( world, name+"/cam/right", headNeck()[5], 200, 200 );
01427 
01428             // Creating joints. Axis are inverted or not to have positive angles in the right direction
01429             // (perhaps I have to do this because we create the joint with the link n+1 as the parent
01430             // object of the joint instead of link n most of the time...)
01431             headNeckJointv.resize(0);
01432             PhyJoint* j1 = kinRightEye.createJoint(3);
01433             headNeckJointv << j1;
01434 
01435             PhyJoint* j2 = kinRightEye.createJoint(4, false);
01436             headNeckJointv << j2;
01437 
01438             PhyJoint* j3 = kinRightEye.createJoint(5);
01439             headNeckJointv << j3;
01440 
01441             PhyJoint* j4 = kinRightEye.createJoint(6);
01442             headNeckJointv << j4;
01443 
01444             PhyJoint* j5 = kinRightEye.createJoint(7);
01445             headNeckJointv << j5;
01446             rightEyeDOF = j5->dofs()[0];
01447 
01448             PhyJoint* j6 = kinLeftEye.createJoint(7);
01449             headNeckJointv << j6;
01450             leftEyeDOF = j6->dofs()[0];
01451 
01452             // Creating the virtual DOF which we use to control eye vergence and version. These haven't got valid axes
01453             versionDOF = new PhyDOF(NULL, wVector(0.0, 0.0, 0.0), wVector(0.0, 0.0, 0.0), false);
01454             vergenceDOF = new PhyDOF(NULL, wVector(0.0, 0.0, 0.0), wVector(0.0, 0.0, 0.0), false);
01455 
01456             // Connecting virtual and real dofs to our slots to synchronize them
01457             connect(versionDOF, SIGNAL(changedDesiredPosition(real)), this, SLOT(versionChangedDesiredPosition(real)));
01458             connect(versionDOF, SIGNAL(changedDesiredVelocity(real)), this, SLOT(versionChangedDesiredVelocity(real)));
01459             connect(versionDOF, SIGNAL(changedPosition(real)), this, SLOT(versionChangedPosition(real)));
01460             connect(versionDOF, SIGNAL(changedVelocity(real)), this, SLOT(versionChangedVelocity(real)));
01461             connect(versionDOF, SIGNAL(changedLimits(real, real)), this, SLOT(versionChangedLimits(real, real)));
01462 
01463             connect(vergenceDOF, SIGNAL(changedDesiredPosition(real)), this, SLOT(vergenceChangedDesiredPosition(real)));
01464             connect(vergenceDOF, SIGNAL(changedDesiredVelocity(real)), this, SLOT(vergenceChangedDesiredVelocity(real)));
01465             connect(vergenceDOF, SIGNAL(changedPosition(real)), this, SLOT(vergenceChangedPosition(real)));
01466             connect(vergenceDOF, SIGNAL(changedVelocity(real)), this, SLOT(vergenceChangedVelocity(real)));
01467             connect(vergenceDOF, SIGNAL(changedLimits(real, real)), this, SLOT(vergenceChangedLimits(real, real)));
01468 
01469             connect(rightEyeDOF, SIGNAL(changedDesiredPosition(real)), this, SLOT(rightEyeChangedDesiredPosition(real)));
01470             connect(rightEyeDOF, SIGNAL(changedDesiredVelocity(real)), this, SLOT(rightEyeChangedDesiredVelocity(real)));
01471             connect(rightEyeDOF, SIGNAL(changedPosition(real)), this, SLOT(rightEyeChangedPosition(real)));
01472             connect(rightEyeDOF, SIGNAL(changedVelocity(real)), this, SLOT(rightEyeChangedVelocity(real)));
01473             connect(rightEyeDOF, SIGNAL(changedLimits(real, real)), this, SLOT(rightEyeChangedLimits(real, real)));
01474 
01475             connect(leftEyeDOF, SIGNAL(changedDesiredPosition(real)), this, SLOT(leftEyeChangedDesiredPosition(real)));
01476             connect(leftEyeDOF, SIGNAL(changedDesiredVelocity(real)), this, SLOT(leftEyeChangedDesiredVelocity(real)));
01477             connect(leftEyeDOF, SIGNAL(changedPosition(real)), this, SLOT(leftEyeChangedPosition(real)));
01478             connect(leftEyeDOF, SIGNAL(changedVelocity(real)), this, SLOT(leftEyeChangedVelocity(real)));
01479             connect(leftEyeDOF, SIGNAL(changedLimits(real, real)), this, SLOT(leftEyeChangedLimits(real, real)));
01480         }
01481 
01482         // Now that we have created all pieces and joints, we can re-enable limits
01483         kinRightArm.setAllConstraints(true);
01484         kinLeftArm.setAllConstraints(true);
01485         kinRightLeg.setAllConstraints(true);
01486         kinLeftLeg.setAllConstraints(true);
01487         kinRightEye.setAllConstraints(true);
01488         kinLeftEye.setAllConstraints(true);
01489 
01490         //--- create an iCubMaterial and disable all contact among iCub parts
01491         world->materials().createMaterial( "iCubMaterial" );
01492         world->materials().enableCollision( "iCubMaterial", "iCubMaterial", false );
01493         world->materials().setGravityForce( "iCubMaterial", 0 );
01494         foreach( PhyObject* obj, leftLegv ) {
01495             obj->setMaterial( "iCubMaterial" );
01496         }
01497         foreach( PhyObject* obj, rightLegv ) {
01498             obj->setMaterial( "iCubMaterial" );
01499         }
01500         foreach( PhyObject* obj, torsov ) {
01501             obj->setMaterial( "iCubMaterial" );
01502         }
01503         foreach( PhyObject* obj, leftArmv ) {
01504             obj->setMaterial( "iCubMaterial" );
01505         }
01506         foreach( PhyObject* obj, rightArmv ) {
01507             obj->setMaterial( "iCubMaterial" );
01508         }
01509         foreach( PhyObject* obj, headNeckv ) {
01510             obj->setMaterial( "iCubMaterial" );
01511         }
01512 
01513         //---------------------------------------
01514         //--- MOTOR CONTROLLERS creation
01515         //---------------------------------------
01516 
01517         // Getting joint limits. The robot whose configuration is loaded
01518         // is the one indicated by the environmental variable ICUB_ROBOTNAME
01519         ResourceFinder rf;
01520         rf.configure( "ICUB_ROOT", 0, NULL );
01521         rf.setVerbose( false );
01522         // base path template for getting default iCub files included into FARSA
01523 #ifdef FARSA_WIN
01524         QString phyiCubConfTmpl = qApp->applicationDirPath() + "/../conf/worldsim/phyicub/%1";
01525 #else
01526         QString phyiCubConfTmpl = qApp->applicationDirPath() + "/../share/FARSA/conf/worldsim/phyicub/%1";
01527 #endif
01528         Property prop;
01529         if ( QFile::exists(rf.findFile("icub_head_torso.ini").c_str()) ) {
01530             prop.fromConfigFile( rf.findFile("icub_head_torso.ini"), true );
01531         } else {
01532             prop.fromConfigFile( phyiCubConfTmpl.arg("icub_head_torso.ini").toAscii().data() );
01533         }
01534         Bottle lstM = prop.findGroup("LIMITS").findGroup("Max");
01535         Bottle lstm = prop.findGroup("LIMITS").findGroup("Min");
01536         QVector<double> headMax(6), headMin(6), torsoMax(3), torsoMin(3);
01537         for( int i=0; i<6; i++ ) {
01538             headMax[i] = lstM.get(i+1).asDouble();
01539             headMin[i] = lstm.get(i+1).asDouble();
01540         }
01541         for( int i=6; i<9; i++ ) {
01542             torsoMax[i-6] = lstM.get(i+1).asDouble();
01543             torsoMin[i-6] = lstm.get(i+1).asDouble();
01544         }
01545         if ( QFile::exists(rf.findFile("icub_left_leg.ini").c_str()) ) {
01546             prop.fromConfigFile( rf.findFile("icub_left_leg.ini"), true );
01547         } else {
01548             prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_leg.ini").toAscii().data() );
01549         }
01550         lstM = prop.findGroup("LIMITS").findGroup("Max");
01551         lstm = prop.findGroup("LIMITS").findGroup("Min");
01552         QVector<double> leftLegMax(6), leftLegMin(6);
01553         for( int i=0; i<6; i++ ) {
01554             leftLegMax[i] = lstM.get(i+1).asDouble();
01555             leftLegMin[i] = lstm.get(i+1).asDouble();
01556         }
01557         if ( QFile::exists(rf.findFile("icub_right_leg.ini").c_str()) ) {
01558             prop.fromConfigFile( rf.findFile("icub_right_leg.ini"), true );
01559         } else {
01560             prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_leg.ini").toAscii().data() );
01561         }
01562         lstM = prop.findGroup("LIMITS").findGroup("Max");
01563         lstm = prop.findGroup("LIMITS").findGroup("Min");
01564         QVector<double> rightLegMax(6), rightLegMin(6);
01565         for( int i=0; i<6; i++ ) {
01566             rightLegMax[i] = lstM.get(i+1).asDouble();
01567             rightLegMin[i] = lstm.get(i+1).asDouble();
01568         }
01569         if ( QFile::exists(rf.findFile("icub_left_arm.ini").c_str()) ) {
01570             prop.fromConfigFile( rf.findFile("icub_left_arm.ini"), true );
01571         } else {
01572             prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_arm.ini").toAscii().data() );
01573         }
01574         lstM = prop.findGroup("LIMITS").findGroup("Max");
01575         lstm = prop.findGroup("LIMITS").findGroup("Min");
01576         QVector<double> leftArmMax(16), leftArmMin(16);
01577         //--- first 8 are in icub_left_arm.ini,
01578         //    second 8 in icub_left_hand.ini
01579         for( int i=0; i<8; i++ ) {
01580             leftArmMax[i] = lstM.get(i+1).asDouble();
01581             leftArmMin[i] = lstm.get(i+1).asDouble();
01582         }
01583         if ( QFile::exists(rf.findFile("icub_left_hand.ini").c_str()) ) {
01584             prop.fromConfigFile( rf.findFile("icub_left_hand.ini"), true );
01585         } else {
01586             prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_hand.ini").toAscii().data() );
01587         }
01588         lstM = prop.findGroup("LIMITS").findGroup("Max");
01589         lstm = prop.findGroup("LIMITS").findGroup("Min");
01590         for( int i=0; i<8; i++ ) {
01591             leftArmMax[8+i] = lstM.get(i+1).asDouble();
01592             leftArmMin[8+i] = lstm.get(i+1).asDouble();
01593         }
01594         if ( QFile::exists(rf.findFile("icub_right_arm.ini").c_str()) ) {
01595             prop.fromConfigFile( rf.findFile("icub_right_arm.ini"), true );
01596         } else {
01597             prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_arm.ini").toAscii().data() );
01598         }
01599         lstM = prop.findGroup("LIMITS").findGroup("Max");
01600         lstm = prop.findGroup("LIMITS").findGroup("Min");
01601         QVector<double> rightArmMax(16), rightArmMin(16);
01602         //--- first 8 are in icub_right_arm.ini,
01603         //    second 8 in icub_right_hand.ini
01604         for( int i=0; i<8; i++ ) {
01605             rightArmMax[i] = lstM.get(i+1).asDouble();
01606             rightArmMin[i] = lstm.get(i+1).asDouble();
01607         }
01608         if ( QFile::exists(rf.findFile("icub_right_hand.ini").c_str()) ) {
01609             prop.fromConfigFile( rf.findFile("icub_right_hand.ini"), true );
01610         } else {
01611             prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_hand.ini").toAscii().data() );
01612         }
01613         lstM = prop.findGroup("LIMITS").findGroup("Max");
01614         lstm = prop.findGroup("LIMITS").findGroup("Min");
01615         for( int i=0; i<8; i++ ) {
01616             rightArmMax[8+i] = lstM.get(i+1).asDouble();
01617             rightArmMin[8+i] = lstm.get(i+1).asDouble();
01618         }
01619 
01620         // Here we set limits both for the motor controllers and the kinematic chain
01621         QVector<PhyDOF*> motors;
01622         //--- number of controlled DOF of joints
01623         int counter = 0;
01624         //--- create MotorController for TORSO
01625         motors.clear();
01626         for( int i=0; i<torsoJointv.size(); i++ ) {
01627             motors << torsoJointv[i]->dofs()[0];
01628             motors[i]->setLimits( toRad(torsoMin[i]), toRad(torsoMax[i]) );
01629             motors[i]->enableLimits();
01630 
01631             // Setting limits for iKin chain. We don't use "i" because joint in the
01632             // torso are in inverse order (I really can't figure out why...)
01633             kinRightArm.getLinkInfo(torsoJointv.size() - i - 1).setiKinLinkLimits(toRad(torsoMin[i]), toRad(torsoMax[i]));
01634         }
01635         torsoCtrl = new MultiMotorController( motors, world );
01636         torsoCtrl->setOwner(this, false);
01637         if ( enabledServerControlBoards ) {
01638             registerServerControlBoard( torsoCtrl, "torso" );
01639         }
01640         counter += motors.size();
01641 
01642         //--- create MotorController for LEFT ARM
01643         motors.clear();
01644         motors << leftArmJointv[0]->dofs()[0];
01645         motors << leftArmJointv[1]->dofs()[0];
01646         motors << leftArmJointv[2]->dofs()[0];
01647         motors << leftArmJointv[3]->dofs()[0];
01648         motors << leftArmJointv[4]->dofs()[0];
01649         motors << leftArmJointv[5]->dofs()[0];
01650         motors << leftArmJointv[6]->dofs()[0];
01651         motors << leftArmJointv[7]->dofs()[0];
01652         motors << leftArmJointv[23]->dofs()[0];
01653         motors << leftArmJointv[24]->dofs()[0];
01654         motors << leftArmJointv[25]->dofs()[0];
01655         motors << leftArmJointv[11]->dofs()[0];
01656         motors << leftArmJointv[12]->dofs()[0];
01657         motors << leftArmJointv[14]->dofs()[0];
01658         motors << leftArmJointv[15]->dofs()[0];
01659         motors << leftArmJointv[17]->dofs()[0];
01660         leftArmCtrl = new MultiMotorController( motors, world );
01661         //--- setting limits
01662         for( unsigned int i=0; i<16; i++ ) {
01663             if ( i<7 ) {
01664                 //--- only for the first seven joints the raw limits correspond to public limits
01665                 leftArmCtrl->setLimitsRaw( i, leftArmMin[i], leftArmMax[i] );
01666             }
01667             leftArmCtrl->setLimits( i, leftArmMin[i], leftArmMax[i] );
01668             leftArmCtrl->setEnableLimitsRaw( i, true );
01669             // Setting limits for iKin chain
01670             if ((i + 3) < kinLeftArm.getN()) {
01671                 kinLeftArm.getLinkInfo(i + 3).setiKinLinkLimits(toRad(leftArmMin[i]), toRad(leftArmMax[i]));
01672             }
01673         }
01674         //--- the joints of the hand has different angular movement respect from what reported by robotMotorGUI
01675         //    This mismatch is implemented using setLimitsRaw of MultiMotorController that will change the
01676         //    real movement of the joint, but will not change what reported on the robotMotorGUI
01677         leftArmCtrl->setLimitsRaw( 7, -15, 0 );
01678         leftArmCtrl->setLimitsRaw( 8, 10, 90 );
01679         leftArmCtrl->setLimitsRaw( 9, 0, 90 );
01680         leftArmCtrl->setLimitsRaw( 10, 0, 90 );
01681         leftArmCtrl->setLimitsRaw( 11, 0, 90 );
01682         leftArmCtrl->setLimitsRaw( 12, 0, 90 );
01683         leftArmCtrl->setLimitsRaw( 13, 0, 90 );
01684         leftArmCtrl->setLimitsRaw( 14, 0, 90 );
01685         leftArmCtrl->setLimitsRaw( 15, 0, 90 );
01686         leftArmCtrl->setOwner(this, false);
01687         if ( enabledServerControlBoards ) {
01688             registerServerControlBoard( leftArmCtrl, "left_arm" );
01689         }
01690         counter += motors.size();
01691 
01692         //--- create MotorController for RIGHT ARM
01693         motors.clear();
01694         motors << rightArmJointv[0]->dofs()[0];
01695         motors << rightArmJointv[1]->dofs()[0];
01696         motors << rightArmJointv[2]->dofs()[0];
01697         motors << rightArmJointv[3]->dofs()[0];
01698         motors << rightArmJointv[4]->dofs()[0];
01699         motors << rightArmJointv[5]->dofs()[0];
01700         motors << rightArmJointv[6]->dofs()[0];
01701         motors << rightArmJointv[7]->dofs()[0];
01702         motors << rightArmJointv[23]->dofs()[0];
01703         motors << rightArmJointv[24]->dofs()[0];
01704         motors << rightArmJointv[25]->dofs()[0];
01705         motors << rightArmJointv[11]->dofs()[0];
01706         motors << rightArmJointv[12]->dofs()[0];
01707         motors << rightArmJointv[14]->dofs()[0];
01708         motors << rightArmJointv[15]->dofs()[0];
01709         motors << rightArmJointv[17]->dofs()[0];
01710         rightArmCtrl = new MultiMotorController( motors, world );
01711         //--- setting limits
01712         for( unsigned int i=0; i<16; i++ ) {
01713             if ( i<7 ) {
01714                 //--- only for the first seven joints the raw limits correspond to public limits
01715                 rightArmCtrl->setLimitsRaw( i, rightArmMin[i], rightArmMax[i] );
01716             }
01717             rightArmCtrl->setLimits( i, rightArmMin[i], rightArmMax[i] );
01718             rightArmCtrl->setEnableLimitsRaw( i, true );
01719             // Setting limits for iKin chain
01720             if ((i + 3) < kinRightArm.getN()) {
01721                 kinRightArm.getLinkInfo(i + 3).setiKinLinkLimits(toRad(rightArmMin[i]), toRad(rightArmMax[i]));
01722             }
01723         }
01724         //--- the joints of the hand has different angular movement respect from what reported by robotMotorGUI
01725         //    This mismatch is implemented using setLimitsRaw of MultiMotorController that will change the
01726         //    real movement of the joint, but will not change what reported on the robotMotorGUI
01727         rightArmCtrl->setLimitsRaw( 7, -15, 0 );
01728         rightArmCtrl->setLimitsRaw( 8, 10, 90 );
01729         rightArmCtrl->setLimitsRaw( 9, 0, 90 );
01730         rightArmCtrl->setLimitsRaw( 10, 0, 90 );
01731         rightArmCtrl->setLimitsRaw( 11, 0, 90 );
01732         rightArmCtrl->setLimitsRaw( 12, 0, 90 );
01733         rightArmCtrl->setLimitsRaw( 13, 0, 90 );
01734         rightArmCtrl->setLimitsRaw( 14, 0, 90 );
01735         rightArmCtrl->setLimitsRaw( 15, 0, 90 );
01736         rightArmCtrl->setOwner(this, false);
01737         if ( enabledServerControlBoards ) {
01738             registerServerControlBoard( rightArmCtrl, "right_arm" );
01739         }
01740         counter += motors.size();
01741 
01742         //--- create MotorController for HEAD-NECK
01743         motors.clear();
01744         motors << headNeckJointv[0]->dofs()[0];
01745         motors << headNeckJointv[1]->dofs()[0];
01746         motors << headNeckJointv[2]->dofs()[0];
01747         motors << headNeckJointv[3]->dofs()[0];
01748         motors << versionDOF;
01749         motors << vergenceDOF;
01750         for( int i=0; i<6; i++ ) {
01751             motors[i]->setLimits( toRad(headMin[i]), toRad(headMax[i]) );
01752             motors[i]->enableLimits();
01753 
01754             if ((i + 3) < 7) {
01755                 kinRightEye.getLinkInfo(i + 3).setiKinLinkLimits(toRad(headMin[i]), toRad(headMax[i]));
01756             }
01757         }
01758         // Setting limits on the last joint of right and left eye kinematic chain. They are computed from the limits
01759         // for version and vergence (4 is the limit for version, 5 for vergence)
01760         const real minEye = max(headMin[4] + headMin[5], -179.99999);
01761         const real maxEye = min(headMax[4] + headMax[5], +179.99999);
01762         headNeckJointv[4]->dofs()[0]->setLimits( toRad(minEye), toRad(maxEye) );
01763         headNeckJointv[4]->dofs()[0]->enableLimits();
01764         headNeckJointv[5]->dofs()[0]->setLimits( toRad(minEye), toRad(maxEye) );
01765         headNeckJointv[5]->dofs()[0]->enableLimits();
01766         kinRightEye.getLinkInfo(7).setiKinLinkLimits(toRad(minEye), toRad(maxEye));
01767         kinLeftEye.getLinkInfo(7).setiKinLinkLimits(toRad(minEye), toRad(maxEye));
01768         // Creating controller for head
01769         headNeckCtrl = new MultiMotorController( motors, world );
01770         headNeckCtrl->setOwner(this, false);
01771         if ( enabledServerControlBoards ) {
01772             registerServerControlBoard( headNeckCtrl, "head" );
01773         }
01774         counter += motors.size();
01775 
01776         //--- create MotorController for LEFT LEG
01777         motors.clear();
01778         for( int i=0; i<leftLegJointv.size(); i++ ) {
01779             motors << leftLegJointv[i]->dofs()[0];
01780             motors[i]->setLimits( toRad(leftLegMin[i]), toRad(leftLegMax[i]) );
01781             motors[i]->enableLimits();
01782 
01783             // Setting limits for iKin chain
01784             kinLeftLeg.getLinkInfo(i).setiKinLinkLimits(toRad(leftLegMin[i]), toRad(leftLegMax[i]));
01785         }
01786         leftLegCtrl = new MultiMotorController( motors, world );
01787         leftLegCtrl->setOwner(this, false);
01788         if ( enabledServerControlBoards ) {
01789             registerServerControlBoard( leftLegCtrl, "left_leg" );
01790         }
01791         counter += motors.size();
01792 
01793         //--- create MotorController for RIGHT LEG
01794         motors.clear();
01795         for( int i=0; i<rightLegJointv.size(); i++ ) {
01796             motors << rightLegJointv[i]->dofs()[0];
01797             motors[i]->setLimits( toRad(rightLegMin[i]), toRad(rightLegMax[i]) );
01798             motors[i]->enableLimits();
01799 
01800             // Setting limits for iKin chain
01801             kinRightLeg.getLinkInfo(i).setiKinLinkLimits(toRad(rightLegMin[i]), toRad(rightLegMax[i]));
01802         }
01803         rightLegCtrl = new MultiMotorController( motors, world );
01804         rightLegCtrl->setOwner(this, false);
01805         if ( enabledServerControlBoards ) {
01806             registerServerControlBoard( rightLegCtrl, "right_leg" );
01807         }
01808         counter += motors.size();
01809 
01810         //--- Creating the COVERS
01811         wMatrix matrix = wMatrix::yaw( PI_GRECO * 0.5f ) * wMatrix::pitch( PI_GRECO*0.5f );
01812         // Previous vector values: wVector(0.0, 0.034 /*0.037*/, 0.0)
01813         matrix.w_pos = matrix.rotateVector( wVector(0.0, 0.05f, -0.013f) );
01814         WMesh* mesh = new WMesh( world, name+":faceCover", matrix );
01815         mesh->loadMS3DModel( ":/covers/head.ms3d" );
01816         mesh->setTexture( "icubFace" );
01817         mesh->attachTo( headNeckv[2] );
01818         mesh->setOwner(this, false);
01819         coversv << mesh;
01820 
01821         //--- counting the number of effective DOF of joints created
01822         int counter2= 0;
01823         foreach( PhyJoint* joint, leftLegJointv ) {
01824             counter2 += joint->numDofs();
01825             joint->setOwner(this, false);
01826         }
01827         foreach( PhyJoint* joint, rightLegJointv ) {
01828             counter2 += joint->numDofs();
01829             joint->setOwner(this, false);
01830         }
01831         foreach( PhyJoint* joint, torsoJointv ) {
01832             counter2 += joint->numDofs();
01833             joint->setOwner(this, false);
01834         }
01835         foreach( PhyJoint* joint, leftArmJointv ) {
01836             counter2 += joint->numDofs();
01837             joint->setOwner(this, false);
01838         }
01839         foreach( PhyJoint* joint, rightArmJointv ) {
01840             counter2 += joint->numDofs();
01841             joint->setOwner(this, false);
01842         }
01843         foreach( PhyJoint* joint, headNeckJointv ) {
01844             counter2 += joint->numDofs();
01845             joint->setOwner(this, false);
01846         }
01847         //qDebug( "Total Joints: %d - %d", counter, counter2 );
01848 
01849         //--- calculate the total mass of the robot
01850         //--- and setting up masses vectors
01851         //--- This is required for enable/disable operations
01852         //--- Also Setting the Texture
01853         real mass = 0.0;
01854         leftLegMasses.resize(0);
01855         foreach( PhyObject* obj, leftLegv ) {
01856             mass += obj->mass();
01857             leftLegMasses << obj->mass();
01858             obj->setTexture( "icub" );
01859             obj->setOwner(this, false);
01860         }
01861         rightLegMasses.resize(0);
01862         foreach( PhyObject* obj, rightLegv ) {
01863             mass += obj->mass();
01864             rightLegMasses << obj->mass();
01865             obj->setTexture( "icub" );
01866             obj->setOwner(this, false);
01867         }
01868         torsoMasses.resize(0);
01869         foreach( PhyObject* obj, torsov ) {
01870             mass += obj->mass();
01871             torsoMasses << obj->mass();
01872             obj->setTexture( "icub" );
01873             obj->setOwner(this, false);
01874         }
01875         leftArmMasses.resize(0);
01876         foreach( PhyObject* obj, leftArmv ) {
01877             mass += obj->mass();
01878             leftArmMasses << obj->mass();
01879             obj->setTexture( "icub" );
01880             obj->setOwner(this, false);
01881         }
01882         rightArmMasses.resize(0);
01883         foreach( PhyObject* obj, rightArmv ) {
01884             mass += obj->mass();
01885             rightArmMasses << obj->mass();
01886             obj->setTexture( "icub" );
01887             obj->setOwner(this, false);
01888         }
01889         headNeckMasses.resize(0);
01890         foreach( PhyObject* obj, headNeckv ) {
01891             mass += obj->mass();
01892             headNeckMasses << obj->mass();
01893             obj->setTexture( "icub" );
01894             obj->setOwner(this, false);
01895         }
01896         headNeckv[4]->setTexture( "blueye" );
01897         headNeckv[5]->setTexture( "blueye" );
01898         //qDebug( "Total Mass: %g", mass );
01899 
01900         setTexture( "icub" );
01901         world->pushObject( this );
01902     }
01903 
01904     PhyiCub::~PhyiCub() {
01905         if ( cartCtrlLeftArm ) {
01906             //delete cartCtrlLeftArm;
01907             delete cartServLeftArm;
01908             delete cartSolvLeftArm;
01909         }
01910         if ( cartCtrlRightArm ) {
01911             //delete cartCtrlRightArm;
01912             delete cartServRightArm;
01913             delete cartSolvRightArm;
01914         }
01915         if ( enabledServerControlBoards ) {
01916             //--- stops the serverControlBoards
01917             QStringList servers;
01918             servers << "torso" << "left_arm" << "right_arm" << "head" << "left_leg" << "right_leg";
01919             foreach( QString srv, servers ) {
01920                 //--- this call also delete all MultiMotorControllers attached to ServerControlBoards
01921                 removeServerControlBoard( srv );
01922             }
01923         } else {
01924             //--- when there aren't ServerControlBoards enabled, it is necessary to destroy MultiMotorControllers
01925             delete torsoCtrl;
01926             delete leftArmCtrl;
01927             delete rightArmCtrl;
01928             delete headNeckCtrl;
01929             delete leftLegCtrl;
01930             delete rightLegCtrl;
01931         }
01932         if ( !leftcam ) {
01933             delete leftcam;
01934             delete rightcam;
01935         }
01936         for( int i=0; i<coversv.size(); i++ ) {
01937             delete (coversv[i]);
01938         }
01939         foreach( PhyJoint* joint, leftLegJointv ) {
01940             delete joint;
01941         }
01942         foreach( PhyJoint* joint, rightLegJointv ) {
01943             delete joint;
01944         }
01945         foreach( PhyJoint* joint, torsoJointv ) {
01946             delete joint;
01947         }
01948         foreach( PhyJoint* joint, leftArmJointv ) {
01949             delete joint;
01950         }
01951         foreach( PhyJoint* joint, rightArmJointv ) {
01952             delete joint;
01953         }
01954         foreach( PhyJoint* joint, headNeckJointv ) {
01955             delete joint;
01956         }
01957         foreach( PhyObject* obj, leftLegv ) {
01958             delete obj;
01959         }
01960         foreach( PhyObject* obj, rightLegv ) {
01961             delete obj;
01962         }
01963         foreach( PhyObject* obj, torsov ) {
01964             delete obj;
01965         }
01966         foreach( PhyObject* obj, leftArmv ) {
01967             delete obj;
01968         }
01969         foreach( PhyObject* obj, rightArmv ) {
01970             delete obj;
01971         }
01972         foreach( PhyObject* obj, headNeckv ) {
01973             delete obj;
01974         }
01975         delete leftHandGroup;
01976         delete rightHandGroup;
01977         delete vergenceDOF;
01978         delete versionDOF;
01979     }
01980 
01981     void PhyiCub::doKinematicSimulation(bool k, bool clh, bool crh)
01982     {
01983         kinematicSimulation = k;
01984         collidingLeftHand = clh;
01985         collidingRightHand = crh;
01986 
01987         if (kinematicSimulation) {
01988             // First disabling all joints...
01989             for (int i = 0; i < leftLegJointv.size(); i++) {
01990                 leftLegJointv[i]->enable(false);
01991             }
01992             for (int i = 0; i < rightLegJointv.size(); i++) {
01993                 rightLegJointv[i]->enable(false);
01994             }
01995             for (int i = 0; i < torsoJointv.size(); i++) {
01996                 torsoJointv[i]->enable(false);
01997             }
01998             for (int i = 0; i < leftArmJointv.size(); i++) {
01999                 if (collidingLeftHand && (i >= 6)) {
02000                     leftArmJointv[i]->enable(true);
02001                 } else {
02002                     leftArmJointv[i]->enable(false);
02003                 }
02004             }
02005             for (int i = 0; i < rightArmJointv.size(); i++) {
02006                 if (collidingRightHand && (i >= 6)) {
02007                     rightArmJointv[i]->enable(true);
02008                 } else {
02009                     rightArmJointv[i]->enable(false);
02010                 }
02011             }
02012             for (int i = 0; i < headNeckJointv.size(); i++) {
02013                 headNeckJointv[i]->enable(false);
02014             }
02015 
02016             // ... then setting all objects to kinematic behaviour
02017             for (int i = 0; i < leftLegv.size(); i++) {
02018                 leftLegv[i]->setKinematic(true);
02019             }
02020             for (int i = 0; i < rightLegv.size(); i++) {
02021                 rightLegv[i]->setKinematic(true);
02022             }
02023             for (int i = 0; i < torsov.size(); i++) {
02024                 torsov[i]->setKinematic(true);
02025             }
02026             for (int i = 0; i < leftArmv.size(); i++) {
02027                 if (collidingLeftHand) {
02028                     if (i < 5) {
02029                         leftArmv[i]->setKinematic(true, false);
02030                     } else if (i == 5) {
02031                         leftArmv[i]->setKinematic(true, true);
02032                     } else {
02033                         leftArmv[i]->setKinematic(false);
02034                     }
02035                 } else {
02036                     leftArmv[i]->setKinematic(true, false);
02037                 }
02038             }
02039             for (int i = 0; i < rightArmv.size(); i++) {
02040                 if (collidingRightHand) {
02041                     if (i < 5) {
02042                         rightArmv[i]->setKinematic(true, false);
02043                     } else if (i == 5) {
02044                         rightArmv[i]->setKinematic(true, true);
02045                     } else {
02046                         rightArmv[i]->setKinematic(false);
02047                     }
02048                 } else {
02049                     rightArmv[i]->setKinematic(true, false);
02050                 }
02051             }
02052             for (int i = 0; i < headNeckv.size(); i++) {
02053                 headNeckv[i]->setKinematic(true);
02054             }
02055         } else {
02056             // First setting all objects to dynamic behaviour...
02057             for (int i = 0; i < leftLegv.size(); i++) {
02058                 leftLegv[i]->setKinematic( !enabledLeftLeg );
02059             }
02060             for (int i = 0; i < rightLegv.size(); i++) {
02061                 rightLegv[i]->setKinematic( !enabledRightLeg );
02062             }
02063             for (int i = 0; i < torsov.size(); i++) {
02064                 torsov[i]->setKinematic( !enabledTorso );
02065             }
02066             for (int i = 0; i < leftArmv.size(); i++) {
02067                 leftArmv[i]->setKinematic( !enabledLeftArm );
02068             }
02069             for (int i = 0; i < rightArmv.size(); i++) {
02070                 rightArmv[i]->setKinematic( !enabledRightArm );
02071             }
02072             for (int i = 0; i < headNeckv.size(); i++) {
02073                 headNeckv[i]->setKinematic( !enabledHead );
02074             }
02075 
02076             // ... then enabling all joints (if the corresponding part is enabled)
02077             for (int i = 0; i < leftLegJointv.size(); i++) {
02078                 leftLegJointv[i]->enable(enabledLeftLeg);
02079                 if (enabledLeftLeg) {
02080                     leftLegJointv[i]->updateJointInfo();
02081                 }
02082             }
02083             for (int i = 0; i < rightLegJointv.size(); i++) {
02084                 rightLegJointv[i]->enable(enabledRightLeg);
02085                 if (enabledRightLeg) {
02086                     rightLegJointv[i]->updateJointInfo();
02087                 }
02088             }
02089             for (int i = 0; i < torsoJointv.size(); i++) {
02090                 torsoJointv[i]->enable(enabledTorso);
02091                 if (enabledTorso) {
02092                     torsoJointv[i]->updateJointInfo();
02093                 }
02094             }
02095             for (int i = 0; i < leftArmJointv.size(); i++) {
02096                 leftArmJointv[i]->enable(enabledLeftArm);
02097                 if (enabledLeftArm) {
02098                     leftArmJointv[i]->updateJointInfo();
02099                 }
02100             }
02101             for (int i = 0; i < rightArmJointv.size(); i++) {
02102                 rightArmJointv[i]->enable(enabledRightArm);
02103                 if (enabledRightArm) {
02104                     rightArmJointv[i]->updateJointInfo();
02105                 }
02106             }
02107             for (int i = 0; i < headNeckJointv.size(); i++) {
02108                 headNeckJointv[i]->enable(enabledHead);
02109                 if (enabledHead) {
02110                     headNeckJointv[i]->updateJointInfo();
02111                 }
02112             }
02113         }
02114     }
02115 
02116     void PhyiCub::configurePosture( QMap<int, real> jointSetup )
02117     {
02118 #ifdef __GNUC__
02119     #warning QUI ESPLODE (MATRICI DIVENTANO INVALIDE) SE CHIAMATA MOLTE VOLTE CON GLI STESSI ANGOLI. INOLTRE SUCCEDE MOLTO SPESSO CHE ESPLODA SE SIAMO IN CINEMATICO E LE MANI SONO ABILITATE (MENTRE SE LE MANI SONO DISABILITATE SUCCEDE MOLTO PIÙ DI RADO). PROVARE A RIPRODURRE QUESTA COSA PER CAPIRE DOVE STA IL PROBLEMA (CONTROLLARE SE QUESTO PROBLEMA C È ANCORA)
02120 #endif
02121 #ifdef __GNUC__
02122     #warning PROBLEMA DI GIANLUCA: IN UNA SIMULAZIONE DINAMICA, LA PRIMA CONFIGURE POSTURE FUNZIONA BENE (GLI ANGOLI LETTI DAGLI ENCODER IMMEDIATAMENTE DOPO LA CHIAMATA A QUESTA FUNZIONE SONO ESATTAMENTE QUELLI RICHIESTI), MENTRE LE SUCCESSIVE SONO MENO PRECISE. TRA CHIAMATE SUCCESSIVE A QUESTA FUNZIONE IL ROBOT VIENE MOSSO IN DINAMICA. FORSE O GIUNTI NON SONO RESETTATI BENE?
02123 #endif
02124         // First of all storing the new relative positions of objects in objects groups
02125         leftHandGroup->updateRelativePositions();
02126         rightHandGroup->updateRelativePositions();
02127 
02128         // Setting new angles in the kinematic chains
02129         for (QMap<int, real>::const_iterator it = jointSetup.begin(); it != jointSetup.end(); it++) {
02130             // Big switch to set new postures in the kinematic chains. Joints which are not in
02131             // a kinematic chain are checked below, after these have been set in the new position
02132             switch (it.key()) {
02133                 case torso_yaw:
02134                     kinRightArm.getLinkInfo(2).setLinkAngle(toRad(it.value()));
02135                     break;
02136                 case torso_roll:
02137                     kinRightArm.getLinkInfo(1).setLinkAngle(toRad(it.value()));
02138                     break;
02139                 case torso_pitch:
02140                     kinRightArm.getLinkInfo(0).setLinkAngle(toRad(it.value()));
02141                     break;
02142                 case left_shoulder_pitch:
02143                     kinLeftArm.getLinkInfo(3).setLinkAngle(toRad(it.value()));
02144                     break;
02145                 case left_shoulder_roll:
02146                     kinLeftArm.getLinkInfo(4).setLinkAngle(toRad(it.value()));
02147                     break;
02148                 case left_shoulder_yaw:
02149                     kinLeftArm.getLinkInfo(5).setLinkAngle(toRad(it.value()));
02150                     break;
02151                 case left_elbow:
02152                     kinLeftArm.getLinkInfo(6).setLinkAngle(toRad(it.value()));
02153                     break;
02154                 case left_wrist_prosup:
02155                     kinLeftArm.getLinkInfo(7).setLinkAngle(toRad(it.value()));
02156                     break;
02157                 case left_wrist_pitch:
02158                     kinLeftArm.getLinkInfo(8).setLinkAngle(toRad(it.value()));
02159                     break;
02160                 case left_wrist_yaw:
02161                     kinLeftArm.getLinkInfo(9).setLinkAngle(toRad(it.value()));
02162                     break;
02163                 case right_shoulder_pitch:
02164                     kinRightArm.getLinkInfo(3).setLinkAngle(toRad(it.value()));
02165                     break;
02166                 case right_shoulder_roll:
02167                     kinRightArm.getLinkInfo(4).setLinkAngle(toRad(it.value()));
02168                     break;
02169                 case right_shoulder_yaw:
02170                     kinRightArm.getLinkInfo(5).setLinkAngle(toRad(it.value()));
02171                     break;
02172                 case right_elbow:
02173                     kinRightArm.getLinkInfo(6).setLinkAngle(toRad(it.value()));
02174                     break;
02175                 case right_wrist_prosup:
02176                     kinRightArm.getLinkInfo(7).setLinkAngle(toRad(it.value()));
02177                     break;
02178                 case right_wrist_pitch:
02179                     kinRightArm.getLinkInfo(8).setLinkAngle(toRad(it.value()));
02180                     break;
02181                 case right_wrist_yaw:
02182                     kinRightArm.getLinkInfo(9).setLinkAngle(toRad(it.value()));
02183                     break;
02184                 case neck_pitch:
02185                     kinRightEye.getLinkInfo(3).setLinkAngle(toRad(it.value()));
02186                     break;
02187                 case neck_roll:
02188                     kinRightEye.getLinkInfo(4).setLinkAngle(toRad(it.value()));
02189                     break;
02190                 case neck_yaw:
02191                     kinRightEye.getLinkInfo(5).setLinkAngle(toRad(it.value()));
02192                     break;
02193                 case eyes_tilt:
02194                     kinRightEye.getLinkInfo(6).setLinkAngle(toRad(it.value()));
02195                     break;
02196                 case eyes_version: case eyes_vergence:
02197                     {
02198                         // Here we need to use both vergence and version. We use the current angles
02199                         // unless they are in the posture map
02200                         real versionAngle;
02201                         if (jointSetup. contains(eyes_version)) {
02202                             versionAngle = toRad(jointSetup[eyes_version]);
02203                         } else {
02204                             versionAngle = versionDOF->position();
02205                         }
02206                         real vergenceAngle;
02207                         if (jointSetup. contains(eyes_vergence)) {
02208                             vergenceAngle = toRad(jointSetup[eyes_vergence]);
02209                         } else {
02210                             vergenceAngle = vergenceDOF->position();
02211                         }
02212 
02213                         // Now computing the new positions for the right and left eye
02214                         const real rightEyeAngle = rightEyeFromVersionAndVergence(versionAngle, vergenceAngle);
02215                         const real leftEyeAngle = leftEyeFromVersionAndVergence(versionAngle, vergenceAngle);
02216 
02217                         // Finally setting angles in both the right and left kinemati link
02218                         kinRightEye.getLinkInfo(7).setLinkAngle(rightEyeAngle);
02219                         kinLeftEye.getLinkInfo(7).setLinkAngle(leftEyeAngle);
02220                     }
02221                     break;
02222                 case left_hip_pitch:
02223                     kinLeftLeg.getLinkInfo(0).setLinkAngle(toRad(it.value()));
02224                     break;
02225                 case left_hip_roll:
02226                     kinLeftLeg.getLinkInfo(1).setLinkAngle(toRad(it.value()));
02227                     break;
02228                 case left_hip_yaw:
02229                     kinLeftLeg.getLinkInfo(2).setLinkAngle(toRad(it.value()));
02230                     break;
02231                 case left_knee:
02232                     kinLeftLeg.getLinkInfo(3).setLinkAngle(toRad(it.value()));
02233                     break;
02234                 case left_ankle_pitch:
02235                     kinLeftLeg.getLinkInfo(4).setLinkAngle(toRad(it.value()));
02236                     break;
02237                 case left_ankle_roll:
02238                     kinLeftLeg.getLinkInfo(5).setLinkAngle(toRad(it.value()));
02239                     break;
02240                 case right_hip_pitch:
02241                     kinRightLeg.getLinkInfo(0).setLinkAngle(toRad(it.value()));
02242                     break;
02243                 case right_hip_roll:
02244                     kinRightLeg.getLinkInfo(1).setLinkAngle(toRad(it.value()));
02245                     break;
02246                 case right_hip_yaw:
02247                     kinRightLeg.getLinkInfo(2).setLinkAngle(toRad(it.value()));
02248                     break;
02249                 case right_knee:
02250                     kinRightLeg.getLinkInfo(3).setLinkAngle(toRad(it.value()));
02251                     break;
02252                 case right_ankle_pitch:
02253                     kinRightLeg.getLinkInfo(4).setLinkAngle(toRad(it.value()));
02254                     break;
02255                 case right_ankle_roll:
02256                     kinRightLeg.getLinkInfo(5).setLinkAngle(toRad(it.value()));
02257                     break;
02258                 default:
02259                     break;
02260             }
02261         }
02262 
02263         // We do this on all chains, perhaps we could just update those that have changed...
02264         kinRightArm.updateMatrixFromiKin();
02265         kinLeftArm.updateMatrixFromiKin();
02266         kinRightLeg.updateMatrixFromiKin();
02267         kinLeftLeg.updateMatrixFromiKin();
02268         kinRightEye.updateMatrixFromiKin();
02269         kinLeftEye.updateMatrixFromiKin();
02270 
02271         // Also updating object groups
02272         rightHandGroup->resetObjectPositions();
02273         leftHandGroup->resetObjectPositions();
02274 
02275         // Updating all joints (see comment above, perhaps we shouldn't update all)
02276         foreach( PhyJoint* joint, leftLegJointv ) {
02277             joint->updateJointInfo();
02278         }
02279         foreach( PhyJoint* joint, rightLegJointv ) {
02280             joint->updateJointInfo();
02281         }
02282         foreach( PhyJoint* joint, torsoJointv ) {
02283             joint->updateJointInfo();
02284         }
02285         foreach( PhyJoint* joint, leftArmJointv ) {
02286             joint->updateJointInfo();
02287         }
02288         foreach( PhyJoint* joint, rightArmJointv ) {
02289             joint->updateJointInfo();
02290         }
02291         foreach( PhyJoint* joint, headNeckJointv ) {
02292             joint->updateJointInfo();
02293         }
02294 
02295         // Setting new angles for objects which don't belong to any kinematic chain. Joints and objects
02296         // matrices are updated when they are moved
02297         for (QMap<int, real>::const_iterator it = jointSetup.begin(); it != jointSetup.end(); it++) {
02298             switch (it.key()) {
02299                 case left_hand_finger:
02300                     leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 0);
02301                     leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 0);
02302                     leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 0);
02303                     break;
02304                 case left_thumb_oppose:
02305                     leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 0);
02306                     break;
02307                 case left_thumb_proximal:
02308                     leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 1);
02309                     break;
02310                 case left_thumb_distal:
02311                     leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 2);
02312                     leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 3);
02313                     break;
02314                 case left_index_proximal:
02315                     leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 1);
02316                     break;
02317                 case left_index_distal:
02318                     leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 2);
02319                     leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 3);
02320                     break;
02321                 case left_middle_proximal:
02322                     leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 1);
02323                     break;
02324                 case left_middle_distal:
02325                     leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 2);
02326                     leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 3);
02327                     break;
02328                 case left_pinky:
02329                     leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 1);
02330                     leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 2);
02331                     leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 3);
02332                     leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 1);
02333                     leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 2);
02334                     leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 3);
02335                     break;
02336                 case right_hand_finger:
02337                     rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 0);
02338                     rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 0);
02339                     rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 0);
02340                     break;
02341                 case right_thumb_oppose:
02342                     rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 0);
02343                     break;
02344                 case right_thumb_proximal:
02345                     rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 1);
02346                     break;
02347                 case right_thumb_distal:
02348                     rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 2);
02349                     rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 3);
02350                     break;
02351                 case right_index_proximal:
02352                     rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 1);
02353                     break;
02354                 case right_index_distal:
02355                     rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 2);
02356                     rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 3);
02357                     break;
02358                 case right_middle_proximal:
02359                     rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 1);
02360                     break;
02361                 case right_middle_distal:
02362                     rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 2);
02363                     rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 3);
02364                     break;
02365                 case right_pinky:
02366                     rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 1);
02367                     rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 2);
02368                     rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 3);
02369                     rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 1);
02370                     rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 2);
02371                     rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 3);
02372                     break;
02373                 default:
02374                     break;
02375             }
02376         }
02377 
02378         // Stops all Motor Controllers
02379         torsoCtrl->stop();
02380         leftArmCtrl->stop();
02381         rightArmCtrl->stop();
02382         headNeckCtrl->stop();
02383         leftLegCtrl->stop();
02384         rightLegCtrl->stop();
02385     }
02386 
02387     yarp::dev::IFrameGrabberImage* PhyiCub::leftEyeFrameGrabber()
02388     {
02389         return (leftcam == NULL) ? NULL : leftcam->getFrameGrabber();
02390     }
02391 
02392     yarp::dev::IFrameGrabberImage* PhyiCub::rightEyeFrameGrabber()
02393     {
02394         return (rightcam == NULL) ? NULL : rightcam->getFrameGrabber();
02395     }
02396 
02397     void PhyiCub::preUpdate()
02398     {
02399         // First of all updating motors
02400         if (torsoCtrl->isEnabled()) {
02401             torsoCtrl->update();
02402         }
02403         if (leftArmCtrl->isEnabled()) {
02404             leftArmCtrl->update();
02405         }
02406         if (rightArmCtrl->isEnabled()) {
02407             rightArmCtrl->update();
02408         }
02409         if (headNeckCtrl->isEnabled()) {
02410             headNeckCtrl->update();
02411         }
02412         if (leftLegCtrl->isEnabled()) {
02413             leftLegCtrl->update();
02414         }
02415         if (rightLegCtrl->isEnabled()) {
02416             rightLegCtrl->update();
02417         }
02418 
02419         // advance the cartesian servers
02420         //if ( cartThreadLeftArm ) {
02421         //  cartThreadLeftArm->run();
02422         //}
02423 
02424         // Updating all enabled chains if in kinematic simulation (or updating all chains, even disabled ones
02425         // if alwaysUpdateKinematicChains is true)
02426         if (kinematicSimulation) {
02427             bool torsoUpdated = false;
02428             int lastLinkRight = -1;
02429             int lastLinkLeft = -1;
02430             if (enabledRightArm || alwaysUpdateKinematicChains) {
02431                 if (enabledRightArm && collidingRightHand) {
02432                     lastLinkRight = 8; // This kinematic chain also comprise torso, that's why we have to use this number
02433                 }
02434                 kinRightArm.updateMatrixFromiKin(0, lastLinkRight);
02435                 torsoUpdated = true;
02436             }
02437             if (enabledLeftArm || alwaysUpdateKinematicChains) {
02438                 if (enabledLeftArm && collidingLeftHand) {
02439                     lastLinkLeft = 8; // This kinematic chain also comprise torso, that's why we have to use this number
02440                 }
02441                 kinLeftArm.updateMatrixFromiKin(0, lastLinkLeft);
02442                 torsoUpdated = true;
02443             }
02444             if (enabledRightLeg || alwaysUpdateKinematicChains) {
02445                 kinRightLeg.updateMatrixFromiKin();
02446             }
02447             if (enabledLeftLeg || alwaysUpdateKinematicChains) {
02448                 kinLeftLeg.updateMatrixFromiKin();
02449             }
02450             if (enabledHead || alwaysUpdateKinematicChains) {
02451                 kinRightEye.updateMatrixFromiKin();
02452                 kinLeftEye.updateMatrixFromiKin();
02453                 torsoUpdated = true;
02454             }
02455             // No need to check alwaysUpdateKinematicChains here, if it is true
02456             // the torso has already been updated
02457             if (!torsoUpdated && enabledTorso) {
02458                 kinRightArm.updateMatrixFromiKin(0, lastLinkRight);
02459             }
02460 
02461             // Also updating object groups
02462             if (!collidingRightHand && (enabledRightArm || alwaysUpdateKinematicChains)) {
02463                 rightHandGroup->resetObjectPositions();
02464                 if (enabledRightKinematicHand) {
02465                     rightHandGroup->updateFromDOF();
02466                 }
02467             }
02468             if (!collidingLeftHand && (enabledLeftArm || alwaysUpdateKinematicChains)) {
02469                 leftHandGroup->resetObjectPositions();
02470                 if (enabledLeftKinematicHand) {
02471                     leftHandGroup->updateFromDOF();
02472                 }
02473             }
02474         }
02475     }
02476 
02477     void PhyiCub::postUpdate()
02478     {
02479         // Here we just have to update disabled kinematic chains if alwaysUpdateKinematicChains
02480         // is true and this is a dynamical simulation
02481         if (alwaysUpdateKinematicChains && !kinematicSimulation) {
02482             int upperBodyUpdateStartingChainLink = 3;
02483             if (!enabledTorso) {
02484                 // If torso is disabled we update it when an arm or the head is updated (if
02485                 // nothing is updated, then the torso is not updated as well)
02486                 upperBodyUpdateStartingChainLink = 0;
02487             }
02488             if (!enabledRightArm) {
02489                 kinRightArm.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
02490             }
02491             if (!enabledLeftArm) {
02492                 kinLeftArm.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
02493             }
02494             if (!enabledRightLeg) {
02495                 kinRightLeg.updateMatrixFromiKin();
02496             }
02497             if (!enabledLeftLeg) {
02498                 kinLeftLeg.updateMatrixFromiKin();
02499             }
02500             if (!enabledHead) {
02501                 kinRightEye.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
02502                 kinLeftEye.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
02503             }
02504         }
02505     }
02506 
02507     void PhyiCub::enableLeftLeg( bool b ) {
02508         if ( enabledLeftLeg == b ) return;
02509         enabledLeftLeg = b;
02510         if ( b ) {
02511             enableObjectsAndLinks( true, leftLegv, leftLegJointv );
02512             leftLegCtrl->setEnabled( true );
02513         } else {
02514             leftLegCtrl->setEnabled( false );
02515             enableObjectsAndLinks( false, leftLegv, leftLegJointv );
02516         }
02517     }
02518 
02519     void PhyiCub::enableRightLeg( bool b ) {
02520         if ( enabledRightLeg == b ) return;
02521         enabledRightLeg = b;
02522         if ( b ) {
02523             enableObjectsAndLinks( true, rightLegv, rightLegJointv );
02524             rightLegCtrl->setEnabled( true );
02525         } else {
02526             rightLegCtrl->setEnabled( false );
02527             enableObjectsAndLinks( false, rightLegv, rightLegJointv );
02528         }
02529     }
02530 
02531     void PhyiCub::enableTorso( bool b ) {
02532         // This function is slightly different from enable functions of other robot parts
02533         // because the torso objects in dynamic simulations cannot be set to kinematic (as
02534         // done in enableObjectsAndLinks): doing so causes the first joint of arms and
02535         // head not to work correctly
02536         if ( enabledTorso == b ) return;
02537         enabledTorso = b;
02538         if ( b ) {
02539             for (int i = 0; i < torsov.size(); i++) {
02540                 torsov[i]->setStatic(false);
02541             }
02542             if (!kinematicSimulation) {
02543                 for (int i = 0; i < torsoJointv.size(); i++) {
02544                     torsoJointv[i]->enable(true);
02545                     torsoJointv[i]->updateJointInfo();
02546                 }
02547             }
02548             torsoCtrl->setEnabled( true );
02549         } else {
02550             torsoCtrl->setEnabled( false );
02551             if (!kinematicSimulation) {
02552                 for (int i = 0; i < torsoJointv.size(); i++) {
02553                     torsoJointv[i]->enable(false);
02554                 }
02555             }
02556             for (int i = 0; i < torsov.size(); i++) {
02557                 torsov[i]->setStatic(true);
02558             }
02559         }
02560     }
02561 
02562     void PhyiCub::enableHead( bool b ) {
02563         if ( enabledHead == b ) return;
02564         enabledHead = b;
02565         if ( b ) {
02566             enableObjectsAndLinks( true, headNeckv, headNeckJointv );
02567             headNeckCtrl->setEnabled( true );
02568         } else {
02569             headNeckCtrl->setEnabled( false );
02570             enableObjectsAndLinks( false, headNeckv, headNeckJointv );
02571         }
02572     }
02573 
02574     void PhyiCub::enableCameras( bool b ) {
02575         if ( enabledCameras == b ) return;
02576         enabledCameras = b;
02577         if ( b && !leftcam ) {
02578             leftcam = new WCamera( world(), name()+"/cam/left", headNeckv[5], 640, 480 );
02579             rightcam = new WCamera( world(), name()+"/cam/right", headNeckv[4], 640, 480 );
02580         }
02581         if ( !b && leftcam ) {
02582             delete leftcam;
02583             delete rightcam;
02584             leftcam = 0;
02585             rightcam = 0;
02586         }
02587     }
02588 
02589     void PhyiCub::enableLeftArm( bool b ) {
02590         if ( enabledLeftArm == b ) return;
02591         enabledLeftArm = b;
02592         if ( b ) {
02593             enableObjectsAndLinks( true, leftArmv, leftArmJointv );
02594             leftArmCtrl->setEnabled( true );
02595         } else {
02596             leftArmCtrl->setEnabled( false );
02597             enableObjectsAndLinks( false, leftArmv, leftArmJointv );
02598         }
02599     }
02600 
02601     void PhyiCub::enableRightArm( bool b ) {
02602         if ( enabledRightArm == b ) return;
02603         enabledRightArm = b;
02604         if ( b ) {
02605             enableObjectsAndLinks( true, rightArmv, rightArmJointv );
02606             rightArmCtrl->setEnabled( true );
02607         } else {
02608             rightArmCtrl->setEnabled( false );
02609             enableObjectsAndLinks( false, rightArmv, rightArmJointv );
02610         }
02611     }
02612 
02613     void PhyiCub::blockTorso0( bool b ) {
02614         if ( blockedTorso0 == b ) return;
02615         blockedTorso0 = b;
02616         if ( b ) {
02617             torsov[0]->setMass( 0 );
02618         } else {
02619             torsov[0]->setMass( torsoMasses[0] );
02620         }
02621     }
02622 
02623     void PhyiCub::enableLeftKinematicHand( bool b )
02624     {
02625         enabledLeftKinematicHand = b;
02626     }
02627 
02628     void PhyiCub::enableRightKinematicHand( bool b )
02629     {
02630         enabledRightKinematicHand = b;
02631     }
02632 
02633     YARP_DECLARE_PLUGINS( icubmod )
02634     void PhyiCub::enableLeftArmCartesianController() {
02635         YARP_REGISTER_PLUGINS( icubmod );
02636         //--- Using directly the iKin library
02637         // declare the on-line arm solver called "solver"
02638         cartSolvLeftArm = new iCubArmCartesianSolver(
02639             QString( "%1/%2/cartesianSolver/left_arm" )
02640                 .arg( world()->name() )
02641                 .arg( name() )
02642                 .toAscii().data()
02643             );
02644         Property options( QString("\
02645             (robot %1/%2) \
02646             (type left) \
02647             (pose full) \
02648             (dof (1 1 1 1 1 1 1 1 1 1) ) \
02649             (verbosity off)")
02650             .arg( world()->name() )
02651             .arg( name() )
02652             .toAscii().data() );
02653         cartSolvLeftArm->open( options );
02654         //--- create the CartesianController and export its interface
02655         //    the idea is to create a cartesiancontrollerserver and
02656         //    using the IMultipleWrapper attaching the PolyDriver created above
02657         //    for the arms to the corresponding cartesiancontrollerserver and
02658         //    finally getting the ICartesianInterface from cartesiancontrollerserver
02659         Property optServerLeftArm( QString( "\
02660             (device cartesiancontrollerserver) \
02661             (GENERAL (ControllerName %1/%2/cartesianController/left_arm) \
02662             (ControllerPeriod 10) \
02663             (SolverNameToConnect %1/%2/cartesianSolver/left_arm) \
02664             (KinematicPart arm) \
02665             (KinematicType left) \
02666             (NumberOfDrivers 2)) \
02667             (DRIVER_0 (Key torso) (JointsOrder reversed)) \
02668             (DRIVER_1 (Key left_arm) (JointsOrder direct))" )
02669             .arg( world()->name() )
02670             .arg( name() )
02671             .toAscii().data() );
02672         cartServLeftArm = new PolyDriver();
02673         if ( !cartServLeftArm->open( optServerLeftArm ) ) {
02674             //--- non posso aggiungere l'interfaccia cartesiana, peccato
02675         } else {
02676             //--- ok, wrappo
02677             PolyDriverList listL;
02678             listL.push( polydriver("torso"), "torso" );
02679             listL.push( polydriver("left_arm"), "left_arm" );
02680             IMultipleWrapper *wrapperL;
02681             cartServLeftArm->view( wrapperL );
02682             if ( !wrapperL->attachAll(listL) ) {
02683                 //--- questo e' un errore !! :-S
02684                 qDebug() << "Errore nell'attaccare i PolyDriver al Server Cartesiano";
02685             }
02686             //--- esporto ICartesionInterface
02687             cartServLeftArm->view( cartCtrlLeftArm );
02688             //--- suspend the rateThread and update the cartesianController in sync with the simulator
02689             //cartServLeftArm->view( cartThreadLeftArm );
02690             //cartThreadLeftArm->suspend();
02691         }
02692     }
02693 
02694     void PhyiCub::enableRightArmCartesianController() {
02695         YARP_REGISTER_PLUGINS( icubmod );
02696         //--- Using directly the iKin library
02697         // declare the on-line arm solver called "solver"
02698         cartSolvRightArm = new iCubArmCartesianSolver(
02699             QString( "%1/%2/cartesianSolver/right_arm" )
02700                 .arg( world()->name() )
02701                 .arg( name() )
02702                 .toAscii().data()
02703             );
02704         Property options( QString("\
02705             (robot %1/%2) \
02706             (type right) \
02707             (pose full) \
02708             (verbosity off)")
02709             .arg( world()->name() )
02710             .arg( name() )
02711             .toAscii().data() );
02712         cartSolvRightArm->open( options );
02713         //--- create the CartesianController and export its interface
02714         //    the idea is to create a cartesiancontrollerserver and
02715         //    using the IMultipleWrapper attaching the PolyDriver created above
02716         //    for the arms to the corresponding cartesiancontrollerserver and
02717         //    finally getting the ICartesianInterface from cartesiancontrollerserver
02718         Property optServerRightArm( QString( "\
02719             (device cartesiancontrollerserver) \
02720             (GENERAL (ControllerName %1/%2/cartesianController/right_arm) \
02721             (ControllerPeriod 10) \
02722             (SolverNameToConnect %1/%2/cartesianSolver/right_arm) \
02723             (KinematicPart arm) \
02724             (KinematicType right) \
02725             (NumberOfDrivers 2)) \
02726             (DRIVER_0 (Key torso) (JointsOrder reversed)) \
02727             (DRIVER_1 (Key right_arm) (JointsOrder direct))" )
02728             .arg( world()->name() )
02729             .arg( name() )
02730             .toAscii().data() );
02731         cartServRightArm = new PolyDriver();
02732         if ( !cartServRightArm->open( optServerRightArm ) ) {
02733             //--- non posso aggiungere l'interfaccia cartesiana, peccato
02734         } else {
02735             //--- ok, wrappo
02736             PolyDriverList listL;
02737             listL.push( polydriver("torso"), "torso" );
02738             listL.push( polydriver("right_arm"), "right_arm" );
02739             IMultipleWrapper *wrapperL;
02740             cartServRightArm->view( wrapperL );
02741             if ( !wrapperL->attachAll(listL) ) {
02742                 //--- questo e' un errore !! :-S
02743                 qDebug() << "Errore nell'attaccare i PolyDriver al Server Cartesiano";
02744             }
02745             //--- esporto ICartesionInterface
02746             cartServRightArm->view( cartCtrlRightArm );
02747         }
02748     }
02749 
02750     void PhyiCub::changedMatrix() {
02751         // Resetting the matrix for torso0
02752         setTorso0Matrix();
02753 
02754         // Updating all kinematic chains
02755         kinRightArm.updateMatrixFromiKin();
02756         kinLeftArm.updateMatrixFromiKin();
02757         kinRightLeg.updateMatrixFromiKin();
02758         kinLeftLeg.updateMatrixFromiKin();
02759         kinRightEye.updateMatrixFromiKin();
02760         kinLeftEye.updateMatrixFromiKin();
02761 
02762         // Also updating object groups
02763         rightHandGroup->resetObjectPositions();
02764         leftHandGroup->resetObjectPositions();
02765 
02766         // Finally updating all joints
02767         foreach( PhyJoint* joint, leftLegJointv ) {
02768             joint->updateJointInfo();
02769         }
02770         foreach( PhyJoint* joint, rightLegJointv ) {
02771             joint->updateJointInfo();
02772         }
02773         foreach( PhyJoint* joint, torsoJointv ) {
02774             joint->updateJointInfo();
02775         }
02776         foreach( PhyJoint* joint, leftArmJointv ) {
02777             joint->updateJointInfo();
02778         }
02779         foreach( PhyJoint* joint, rightArmJointv ) {
02780             joint->updateJointInfo();
02781         }
02782         foreach( PhyJoint* joint, headNeckJointv ) {
02783             joint->updateJointInfo();
02784         }
02785     }
02786 
02787     void PhyiCub::setTorso0Matrix()
02788     {
02789         // Computing matrix for torso0. We start from H0 for the right arm
02790         // kinematic chain (its the same matrix as that of all the other chains
02791         // comprising the torso)
02792         wMatrix torso0Matrix;
02793         convertYarpMatrixToWorldMatrix(kinRightArm.getH0(), torso0Matrix);
02794         torso0Matrix = torso0Matrix * tm;
02795         const real torso0Height = (dynamic_cast<PhyBox*>(torsov[0]))->sideX();
02796         // Now we translate the torso0 object so that it touches the legs solids
02797         // (we use Z translation of the legs chain H0 - remember that YARP matrices
02798         // are transposed with respect to wMatrix)
02799         torso0Matrix.w_pos += wVector(0.0, 0.0, kinRightLeg.getH0()(2, 3) + torso0Height / 4.0);
02800         torsov[0]->setMatrix(torso0Matrix);
02801     }
02802 
02803     void PhyiCub::enableObjectsAndLinks(bool enable, QVector<PhyObject*>& objects, QVector<PhyJoint*>& joints)
02804     {
02805         // The order (objects then joints or vice-versa) is important, that's the reason for the if.
02806         // If we are doing a kinematic simulation we don't do anything on joints
02807         if (enable) {
02808             for (int i = 0; i < objects.size(); i++) {
02809                 objects[i]->setKinematic(false);
02810             }
02811             if (!kinematicSimulation) {
02812                 for (int i = 0; i < joints.size(); i++) {
02813                     joints[i]->enable(true);
02814                     joints[i]->updateJointInfo();
02815                 }
02816             }
02817         } else {
02818             if (!kinematicSimulation) {
02819                 for (int i = 0; i < joints.size(); i++) {
02820                     joints[i]->enable(false);
02821                 }
02822             }
02823             for (int i = 0; i < objects.size(); i++) {
02824                 objects[i]->setKinematic(true);
02825             }
02826         }
02827     }
02828 
02829     real PhyiCub::rightEyeFromVersionAndVergence(real version, real vergence)
02830     {
02831         return -version - vergence;
02832     }
02833 
02834     real PhyiCub::leftEyeFromVersionAndVergence(real version, real vergence)
02835     {
02836         return -version + vergence;
02837     }
02838 
02839     real PhyiCub::versionFromRightAndLeftEye(real right, real left)
02840     {
02841         return (-left - right) / 2.0;
02842     }
02843 
02844     real PhyiCub::vergenceFromRightAndLeftEye(real right, real left)
02845     {
02846         return (left - right) / 2.0;
02847     }
02848 
02849     real PhyiCub::lowEyeLimitFromVersionAndVergenceLimits(real versionLow, real vergenceLow)
02850     {
02851         const real lowEyeLimit = versionLow + vergenceLow;
02852 
02853         return (lowEyeLimit < -PI_GRECO) ? -(PI_GRECO - 0.000001) : lowEyeLimit;
02854     }
02855 
02856     real PhyiCub::highEyeLimitFromVersionAndVergenceLimits(real versionHigh, real vergenceHigh)
02857     {
02858         const real highEyeLimit = versionHigh + vergenceHigh;
02859         
02860         return (highEyeLimit > PI_GRECO) ? (PI_GRECO - 0.000001) : highEyeLimit;
02861     }
02862 
02863 #ifdef __GNUC__
02864     #warning I IGNORE CHANGES IN FORCE AND STIFFNESS OF THE VIRTUAL DOF BECAUSE I DON T KNOW HOW TO MAP THEM TO DOFS OF THE REAL JOINTS AND VICE-VERSA
02865 #endif
02866 
02867     void PhyiCub::versionChangedDesiredPosition( real wishpos )
02868     {
02869         if (ignoreEyeSignals) {
02870             return;
02871         }
02872 
02873         const real curVergence = vergenceDOF->desiredPosition();
02874         const real rightEyeAngle = rightEyeFromVersionAndVergence(wishpos, curVergence);
02875         const real leftEyeAngle = leftEyeFromVersionAndVergence(wishpos, curVergence);
02876 
02877         ignoreEyeSignals = true;
02878             rightEyeDOF->setDesiredPosition(rightEyeAngle);
02879             leftEyeDOF->setDesiredPosition(leftEyeAngle);
02880         ignoreEyeSignals = false;
02881     }
02882 
02883     void PhyiCub::versionChangedDesiredVelocity( real wishvel )
02884     {
02885         if (ignoreEyeSignals) {
02886             return;
02887         }
02888 
02889         const real curVergenceVelocity = vergenceDOF->desiredVelocity();
02890         const real rightEyeVelocity = rightEyeFromVersionAndVergence(wishvel, curVergenceVelocity);
02891         const real leftEyeVelocity = leftEyeFromVersionAndVergence(wishvel, curVergenceVelocity);
02892 
02893         ignoreEyeSignals = true;
02894             rightEyeDOF->setDesiredVelocity(rightEyeVelocity);
02895             leftEyeDOF->setDesiredVelocity(leftEyeVelocity);
02896         ignoreEyeSignals = false;
02897     }
02898 
02899     void PhyiCub::versionChangedPosition( real newpos )
02900     {
02901         if (ignoreEyeSignals) {
02902             return;
02903         }
02904 
02905         const real curVergence = vergenceDOF->position();
02906         const real rightEyeAngle = rightEyeFromVersionAndVergence(newpos, curVergence);
02907         const real leftEyeAngle = leftEyeFromVersionAndVergence(newpos, curVergence);
02908 
02909         ignoreEyeSignals = true;
02910             rightEyeDOF->setPosition(rightEyeAngle);
02911             leftEyeDOF->setPosition(leftEyeAngle);
02912         ignoreEyeSignals = false;
02913     }
02914 
02915     void PhyiCub::versionChangedVelocity( real newvel )
02916     {
02917         if (ignoreEyeSignals) {
02918             return;
02919         }
02920 
02921         const real curVergenceVelocity = vergenceDOF->velocity();
02922         const real rightEyeVelocity = rightEyeFromVersionAndVergence(newvel, curVergenceVelocity);
02923         const real leftEyeVelocity = leftEyeFromVersionAndVergence(newvel, curVergenceVelocity);
02924 
02925         ignoreEyeSignals = true;
02926             rightEyeDOF->setVelocity(rightEyeVelocity);
02927             leftEyeDOF->setVelocity(leftEyeVelocity);
02928         ignoreEyeSignals = false;
02929     }
02930 
02931     void PhyiCub::versionChangedLimits( real lowlimit, real highlimit )
02932     {
02933         if (ignoreEyeSignals) {
02934             return;
02935         }
02936 
02937         real curVergenceLow, curVergenceHigh;
02938         vergenceDOF->limits(curVergenceLow, curVergenceHigh);
02939         const real eyeLow = lowEyeLimitFromVersionAndVergenceLimits(lowlimit, curVergenceLow);
02940         const real eyeHigh = highEyeLimitFromVersionAndVergenceLimits(highlimit, curVergenceHigh);
02941 
02942         ignoreEyeSignals = true;
02943             rightEyeDOF->setLimits(eyeLow, eyeHigh);
02944             leftEyeDOF->setLimits(eyeLow, eyeHigh);
02945         ignoreEyeSignals = false;
02946     }
02947 
02948     void PhyiCub::vergenceChangedDesiredPosition( real wishpos )
02949     {
02950         if (ignoreEyeSignals) {
02951             return;
02952         }
02953 
02954         const real curVersion = versionDOF->desiredPosition();
02955         const real rightEyeAngle = rightEyeFromVersionAndVergence(curVersion, wishpos);
02956         const real leftEyeAngle = leftEyeFromVersionAndVergence(curVersion, wishpos);
02957 
02958         ignoreEyeSignals = true;
02959             rightEyeDOF->setDesiredPosition(rightEyeAngle);
02960             leftEyeDOF->setDesiredPosition(leftEyeAngle);
02961         ignoreEyeSignals = false;
02962     }
02963 
02964     void PhyiCub::vergenceChangedDesiredVelocity( real wishvel )
02965     {
02966         if (ignoreEyeSignals) {
02967             return;
02968         }
02969 
02970         const real curVersionVelocity = versionDOF->desiredVelocity();
02971         const real rightEyeVelocity = rightEyeFromVersionAndVergence(curVersionVelocity, wishvel);
02972         const real leftEyeVelocity = leftEyeFromVersionAndVergence(curVersionVelocity, wishvel);
02973 
02974         ignoreEyeSignals = true;
02975             rightEyeDOF->setDesiredVelocity(rightEyeVelocity);
02976             leftEyeDOF->setDesiredVelocity(leftEyeVelocity);
02977         ignoreEyeSignals = false;
02978     }
02979 
02980     void PhyiCub::vergenceChangedPosition( real newpos )
02981     {
02982         if (ignoreEyeSignals) {
02983             return;
02984         }
02985 
02986         const real curVersion = versionDOF->position();
02987         const real rightEyeAngle = rightEyeFromVersionAndVergence(curVersion, newpos);
02988         const real leftEyeAngle = leftEyeFromVersionAndVergence(curVersion, newpos);
02989 
02990         ignoreEyeSignals = true;
02991             rightEyeDOF->setPosition(rightEyeAngle);
02992             leftEyeDOF->setPosition(leftEyeAngle);
02993         ignoreEyeSignals = false;
02994     }
02995 
02996     void PhyiCub::vergenceChangedVelocity( real newvel )
02997     {
02998         if (ignoreEyeSignals) {
02999             return;
03000         }
03001 
03002         const real curVersionVelocity = versionDOF->velocity();
03003         const real rightEyeVelocity = rightEyeFromVersionAndVergence(curVersionVelocity, newvel);
03004         const real leftEyeVelocity = leftEyeFromVersionAndVergence(curVersionVelocity, newvel);
03005 
03006         ignoreEyeSignals = true;
03007             rightEyeDOF->setVelocity(rightEyeVelocity);
03008             leftEyeDOF->setVelocity(leftEyeVelocity);
03009         ignoreEyeSignals = false;
03010     }
03011 
03012     void PhyiCub::vergenceChangedLimits( real lowlimit, real highlimit )
03013     {
03014         if (ignoreEyeSignals) {
03015             return;
03016         }
03017 
03018         real curVersionLow, curVersionHigh;
03019         vergenceDOF->limits(curVersionLow, curVersionHigh);
03020         const real eyeLow = lowEyeLimitFromVersionAndVergenceLimits(curVersionLow, lowlimit);
03021         const real eyeHigh = highEyeLimitFromVersionAndVergenceLimits(curVersionHigh, highlimit);
03022 
03023         ignoreEyeSignals = true;
03024             rightEyeDOF->setLimits(eyeLow, eyeHigh);
03025             leftEyeDOF->setLimits(eyeLow, eyeHigh);
03026         ignoreEyeSignals = false;
03027     }
03028 
03029     void PhyiCub::rightEyeChangedDesiredPosition( real wishpos )
03030     {
03031         if (ignoreEyeSignals) {
03032             return;
03033         }
03034 
03035         const real curLeftEyeAngle = leftEyeDOF->position();
03036         const real versionAngle = versionFromRightAndLeftEye(wishpos, curLeftEyeAngle);
03037         const real vergenceAngle = vergenceFromRightAndLeftEye(wishpos, curLeftEyeAngle);
03038 
03039         ignoreEyeSignals = true;
03040             versionDOF->setDesiredPosition(versionAngle);
03041             vergenceDOF->setDesiredPosition(vergenceAngle);
03042         ignoreEyeSignals = false;
03043     }
03044 
03045     void PhyiCub::rightEyeChangedDesiredVelocity( real wishvel )
03046     {
03047         if (ignoreEyeSignals) {
03048             return;
03049         }
03050 
03051         const real curLeftEyeVelocity = leftEyeDOF->position();
03052         const real versionVelocity = versionFromRightAndLeftEye(wishvel, curLeftEyeVelocity);
03053         const real vergenceVelocity = vergenceFromRightAndLeftEye(wishvel, curLeftEyeVelocity);
03054 
03055         ignoreEyeSignals = true;
03056             versionDOF->setDesiredVelocity(versionVelocity);
03057             vergenceDOF->setDesiredVelocity(vergenceVelocity);
03058         ignoreEyeSignals = false;
03059     }
03060 
03061     void PhyiCub::rightEyeChangedPosition( real newpos )
03062     {
03063         if (ignoreEyeSignals) {
03064             return;
03065         }
03066 
03067         const real curLeftEyeAngle = leftEyeDOF->position();
03068         const real versionAngle = versionFromRightAndLeftEye(newpos, curLeftEyeAngle);
03069         const real vergenceAngle = vergenceFromRightAndLeftEye(newpos, curLeftEyeAngle);
03070 
03071         ignoreEyeSignals = true;
03072             versionDOF->setPosition(versionAngle);
03073             vergenceDOF->setPosition(vergenceAngle);
03074         ignoreEyeSignals = false;
03075     }
03076 
03077     void PhyiCub::rightEyeChangedVelocity( real newvel )
03078     {
03079         if (ignoreEyeSignals) {
03080             return;
03081         }
03082 
03083         const real curLeftEyeVelocity = leftEyeDOF->position();
03084         const real versionVelocity = versionFromRightAndLeftEye(newvel, curLeftEyeVelocity);
03085         const real vergenceVelocity = vergenceFromRightAndLeftEye(newvel, curLeftEyeVelocity);
03086 
03087         ignoreEyeSignals = true;
03088             versionDOF->setVelocity(versionVelocity);
03089             vergenceDOF->setVelocity(vergenceVelocity);
03090         ignoreEyeSignals = false;
03091     }
03092 
03093     void PhyiCub::rightEyeChangedLimits( real , real )
03094     {
03095         // This is not implemented as we expose vergence and version, so we should never change limtis
03096         // for one eye only. Moreover implementing this is not trivial because, given the left and right
03097         // eye limits, there is not a unique possibility for vergence and version
03098     }
03099 
03100     void PhyiCub::leftEyeChangedDesiredPosition( real wishpos )
03101     {
03102         if (ignoreEyeSignals) {
03103             return;
03104         }
03105 
03106         const real curRightEyeAngle = rightEyeDOF->position();
03107         const real versionAngle = versionFromRightAndLeftEye(curRightEyeAngle, wishpos);
03108         const real vergenceAngle = vergenceFromRightAndLeftEye(curRightEyeAngle, wishpos);
03109 
03110         ignoreEyeSignals = true;
03111             versionDOF->setDesiredPosition(versionAngle);
03112             vergenceDOF->setDesiredPosition(vergenceAngle);
03113         ignoreEyeSignals = false;
03114     }
03115 
03116     void PhyiCub::leftEyeChangedDesiredVelocity( real wishvel )
03117     {
03118         if (ignoreEyeSignals) {
03119             return;
03120         }
03121 
03122         const real curRightEyeVelocity = rightEyeDOF->position();
03123         const real versionVelocity = versionFromRightAndLeftEye(curRightEyeVelocity, wishvel);
03124         const real vergenceVelocity = vergenceFromRightAndLeftEye(curRightEyeVelocity, wishvel);
03125 
03126         ignoreEyeSignals = true;
03127             versionDOF->setDesiredVelocity(versionVelocity);
03128             vergenceDOF->setDesiredVelocity(vergenceVelocity);
03129         ignoreEyeSignals = false;
03130     }
03131 
03132     void PhyiCub::leftEyeChangedPosition( real newpos )
03133     {
03134         if (ignoreEyeSignals) {
03135             return;
03136         }
03137 
03138         const real curRightEyeAngle = rightEyeDOF->position();
03139         const real versionAngle = versionFromRightAndLeftEye(curRightEyeAngle, newpos);
03140         const real vergenceAngle = vergenceFromRightAndLeftEye(curRightEyeAngle, newpos);
03141 
03142         ignoreEyeSignals = true;
03143             versionDOF->setPosition(versionAngle);
03144             vergenceDOF->setPosition(vergenceAngle);
03145         ignoreEyeSignals = false;
03146     }
03147 
03148     void PhyiCub::leftEyeChangedVelocity( real newvel )
03149     {
03150         if (ignoreEyeSignals) {
03151             return;
03152         }
03153 
03154         const real curRightEyeVelocity = rightEyeDOF->position();
03155         const real versionVelocity = versionFromRightAndLeftEye(curRightEyeVelocity, newvel);
03156         const real vergenceVelocity = vergenceFromRightAndLeftEye(curRightEyeVelocity, newvel);
03157 
03158         ignoreEyeSignals = true;
03159             versionDOF->setVelocity(versionVelocity);
03160             vergenceDOF->setVelocity(vergenceVelocity);
03161         ignoreEyeSignals = false;
03162     }
03163 
03164     void PhyiCub::leftEyeChangedLimits( real, real )
03165     {
03166         // This is not implemented as we expose vergence and version, so we should never change limtis
03167         // for one eye only. Moreover implementing this is not trivial because, given the left and right
03168         // eye limits, there is not a unique possibility for vergence and version
03169     }
03170 } // end namespace farsa
03171 
03172 #endif // FARSA_USE_YARP_AND_ICUB
WorldSim - 0.6.0

worldsim/src/phyicub.cpp
