WorldSim - 1.2.3

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phyicub.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #ifdef FARSA_USE_YARP_AND_ICUB
21 
22 #include "phyicub.h"
23 #include "phybox.h"
24 #include "phycylinder.h"
25 #include "physphere.h"
26 #include "phyfixed.h"
27 #include "phyhinge.h"
28 #include "phyuniversal.h"
29 #include "phycompoundobject.h"
30 #include "motorcontrollers.h"
31 #include "wcamera.h"
32 
33 #ifdef __GNUC__
34 #pragma GCC diagnostic ignored "-Wunused-parameter"
35 #endif
36  #include "yarp/os/ResourceFinder.h"
37  #include "yarp/os/Bottle.h"
38  #include "yarp/os/Property.h"
39  #include "yarp/dev/PolyDriverList.h"
40  #include "yarp/dev/Wrapper.h"
41  #include "yarp/os/RateThread.h"
42  #include "yarp/os/Network.h"
43 #ifdef __GNUC__
44 #pragma GCC diagnostic warning "-Wunused-parameter"
45 #endif
46 
47 using namespace yarp::os;
48 using namespace yarp::dev;
49 using namespace iCub::iKin;
50 
51 #include <iostream>
52 #include <QFile>
53 
54 namespace farsa {
55 #ifdef __GNUC__
56  #warning AD ELIO IL ROBOT DINAMICO BALLA MOLTO (DOPO QUALCHE STEP, CIRCA 350)
57 #endif
58  PhyiCub::PhyiCub(World* world, QString name, const wMatrix& torso0_tm, bool createServerControlBoards) :
59  YarpObject(world, name, torso0_tm),
60  leftLegv(),
61  rightLegv(),
62  torsov(),
63  leftArmv(),
64  rightArmv(),
65  headNeckv(),
66  coversv(),
67  leftLegJointv(),
68  rightLegJointv(),
69  torsoJointv(),
70  leftArmJointv(),
71  rightArmJointv(),
72  headNeckJointv(),
73  versionDOF(NULL),
74  vergenceDOF(NULL),
75  rightEyeDOF(NULL),
76  leftEyeDOF(NULL),
77  ignoreEyeSignals(false),
78  leftHandGroup(NULL),
79  rightHandGroup(NULL),
80  leftLegMasses(),
81  rightLegMasses(),
82  torsoMasses(),
83  leftArmMasses(),
84  rightArmMasses(),
85  headNeckMasses(),
86  torsoCtrl(NULL),
87  leftArmCtrl(NULL),
88  rightArmCtrl(NULL),
89  headNeckCtrl(NULL),
90  leftLegCtrl(NULL),
91  rightLegCtrl(NULL),
92  cartServLeftArm(NULL),
93  cartCtrlLeftArm(NULL),
94  cartSolvLeftArm(NULL),
95  cartThreadLeftArm(NULL),
96  cartServRightArm(NULL),
97  cartCtrlRightArm(NULL),
98  cartSolvRightArm(NULL),
99  leftcam(NULL),
100  rightcam(NULL),
101  enabledLeftLeg(true),
102  enabledRightLeg(true),
103  enabledTorso(true),
104  enabledLeftArm(true),
105  enabledRightArm(true),
106  enabledHead(true),
107  enabledCameras(false),
108  blockedTorso0(false),
109  enabledRightKinematicHand(true),
110  enabledLeftKinematicHand(true),
111  enabledServerControlBoards(createServerControlBoards),
112  alwaysUpdateKinematicChains(false),
113  kinRightArm("right"),
114  kinLeftArm("left"),
115  kinRightLeg("right"),
116  kinLeftLeg("left"),
117  kinRightEye("right"),
118  kinLeftEye("left"),
119  kinematicSimulation(false)
120  {
121  // Removing constraints when building, so to avoid problems with the 0 position of joints (e.g.
122  // the elbow lower limit is 5.5 degrees, but when building we need solids to be at 0 angle).
123  // They will be re-enabled at the end of the constructor
124  kinRightArm.setAllConstraints(false);
125  kinLeftArm.setAllConstraints(false);
126  kinRightLeg.setAllConstraints(false);
127  kinLeftLeg.setAllConstraints(false);
128  kinRightEye.setAllConstraints(false);
129  kinLeftEye.setAllConstraints(false);
130 
131  // First of all setting buddies for all links
132  kinRightArm.getLinkInfo(0).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftArm.getLinkInfo(0)) << &(kinRightEye.getLinkInfo(0)) << &(kinLeftEye.getLinkInfo(0)));
133  kinRightArm.getLinkInfo(1).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftArm.getLinkInfo(1)) << &(kinRightEye.getLinkInfo(1)) << &(kinLeftEye.getLinkInfo(1)));
134  kinRightArm.getLinkInfo(2).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftArm.getLinkInfo(2)) << &(kinRightEye.getLinkInfo(2)) << &(kinLeftEye.getLinkInfo(2)));
135  kinRightEye.getLinkInfo(3).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftEye.getLinkInfo(3)));
136  kinRightEye.getLinkInfo(4).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftEye.getLinkInfo(4)));
137  kinRightEye.getLinkInfo(5).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftEye.getLinkInfo(5)));
138  kinRightEye.getLinkInfo(6).setBuddies(QSet<KinematicLinkInfo*>() << &(kinLeftEye.getLinkInfo(6)));
139 
140  // Initializing the kinematic chains so that all joints are at 0.0. This is needed to make
141  // sure that when we will create joints, solids positions will automatically be the 0 position.
142  for (unsigned int i = 0; i < kinRightEye.getN(); i++) {
143  kinRightEye.setAng(i, 0.0);
144  }
145  for (unsigned int i = 0; i < kinLeftEye.getN(); i++) {
146  kinLeftEye.setAng(i, 0.0);
147  }
148  for (unsigned int i = 0; i < kinRightLeg.getN(); i++) {
149  kinRightLeg.setAng(i, 0.0);
150  }
151  for (unsigned int i = 0; i < kinLeftLeg.getN(); i++) {
152  kinLeftLeg.setAng(i, 0.0);
153  }
154  for (unsigned int i = 0; i < kinRightArm.getN(); i++) {
155  kinRightArm.setAng(i, 0.0);
156  }
157  for (unsigned int i = 0; i < kinLeftArm.getN(); i++) {
158  kinLeftArm.setAng(i, 0.0);
159  }
160 
161  // Before creating objects, setting the world matrix (i.e. the iCub transformation matrix)
162  // to all kinematic chains and updating them. tm is the transformation matrix for the whole
163  // iCub
164  kinRightArm.setWorldMatrix(&tm);
165  kinLeftArm.setWorldMatrix(&tm);
166  kinRightLeg.setWorldMatrix(&tm);
167  kinLeftLeg.setWorldMatrix(&tm);
168  kinRightEye.setWorldMatrix(&tm);
169  kinLeftEye.setWorldMatrix(&tm);
170  kinRightArm.updateInformationFromiKin();
171  kinLeftArm.updateInformationFromiKin();
172  kinRightLeg.updateInformationFromiKin();
173  kinLeftLeg.updateInformationFromiKin();
174  kinRightEye.updateInformationFromiKin();
175  kinLeftEye.updateInformationFromiKin();
176 
177  // Now we can start to create objects and joints and associate them with links in the kinematic
178  // chains.
179 
180  //--------------------------------------
181  //--- TORSO creation: object & joints
182  //--------------------------------------
183  {
184  // Creating objects
185  PhyBox* torso0 = new PhyBox( 0.11443f, 0.064f, 0.0470f, world, name+":torso0" );
186  torso0->setMass( 0.20297f );
187  torsov << torso0;
188 
189  PhyBox* torso1 = new PhyBox( 0.127f, 0.176f, 0.063f, world, name+":torso1" );
190  torso1->setMass( 3.623f );
191  torsov << torso1;
192 
193  PhyCylinder* torso2 = new PhyCylinder( 0.031f, 0.097f, world, name+":torso2" );
194  torso2->setMass( 0.91179f );
195  torsov << torso2;
196 
197  QVector<PhyObject*> torsoCentre;
198  PhyCylinder* torso3a = new PhyCylinder( 0.04f, 0.0274f, world, name+":torso3a" );
199  torso3a->setMass( 0.45165f );
200  torso3a->setMatrix(wMatrix::identity());
201  torsoCentre << torso3a;
202  PhyBox* torso3b = new PhyBox( 0.1169f, 0.076f, 0.118f, world, name+":torso3b" ); // sideX was 0.109f
203  torso3b->setMass( 1.8388f );
204  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)));
205  torsoCentre << torso3b;
206  PhyBox* torso3c = new PhyBox( 0.1169f, 0.076f, 0.118f, world, name+":torso3c" ); // sideX was 0.109f
207  torso3c->setMass( 1.8388f );
208  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)));
209  torsoCentre << torso3c;
210  PhyCompoundObject* torso3 = new PhyCompoundObject( torsoCentre, world, name+":torso3" );
211  torsov << torso3;
212 
213  // Setting rotation/position of the left leg parts. This is automatically done when
214  // objects are associated with kinematic links, we only need to specify the transformation
215  // of objects relative to the frame of reference of the corresponding kinematic link.
216  // As torso is part of more than one kinematic chain, we associate objects with links only
217  // in the right arm chain (other chains are always aligned)
218  setTorso0Matrix(); // Torso0 is not in a chain, this function sets its matrix
219  // Setting torso0 as the root object for all chains
220  kinRightArm.setRootObject(torso0);
221  kinLeftArm.setRootObject(torso0);
222  kinRightLeg.setRootObject(torso0);
223  kinLeftLeg.setRootObject(torso0);
224  kinRightEye.setRootObject(torso0);
225  kinLeftEye.setRootObject(torso0);
226 
227  wMatrix mtr;
228  wVector rotAx = kinRightArm.getLinkInfo(0).getRotAxis();
229  real rotAng = PI_GRECO / 4.0f;
230  wVector transl = kinRightArm.getLinkInfo(0).getRotCenterWObject();
231  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of 45° around rotation axis
232  kinRightArm.getLinkInfo(0).setWObject(torso1, mtr);
233 
234  rotAx = kinRightArm.getLinkInfo(1).getRotAxis() * wVector(1.0, 0.0, 0.0);
235  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
236  transl = wVector(0.0, 0.0, 0.0);
237  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of 90° around y axis
238  kinRightArm.getLinkInfo(1).setWObject(torso2, mtr);
239 
240  // Here we need two rotations...
241  rotAx = kinRightArm.getLinkInfo(2).getRotAxis() * wVector(1.0, 0.0, 0.0);
242  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
243  transl = kinRightArm.getLinkInfo(2).getRotCenterWObject() - wVector(0.0, torso2->radius() + torso3a->height() / 2.0, 0.0);
244  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of 90° around y axis
245  rotAx = wVector(1.0, 0.0, 0.0);
246  rotAng = PI_GRECO * 0.5f - PI_GRECO * 0.0833f;
247  transl = wVector(0.0, 0.0, 0.0);
248  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl) * mtr; // rotation around x axis
249  kinRightArm.getLinkInfo(2).setWObject(torso3, mtr);
250 
251  //--- TORSO joints
252  torsoJointv.resize(0);
253  PhyJoint* j1 = kinRightArm.createJoint(2);
254  torsoJointv << j1;
255 
256  PhyJoint* j2 = kinRightArm.createJoint(1);
257  torsoJointv << j2;
258 
259  PhyJoint* j3 = kinRightArm.createJoint(0);
260  torsoJointv << j3;
261  }
262 
263  //---------------------------------------
264  //--- LEFT LEG creation: objects & joints
265  //---------------------------------------
266  {
267  // Creating objects
268  PhyCylinder* leftLeg0 = new PhyCylinder( 0.038f, 0.013f, world, name+":leftLeg0" );
269  leftLeg0->setMass( 0.32708f );
270  leftLegv << leftLeg0;
271 
272  PhyCylinder* leftLeg1 = new PhyCylinder( 0.031f, 0.075f, world, name+":leftLeg1" );
273  leftLeg1->setMass( 0.85061f );
274  leftLegv << leftLeg1;
275 
276  const wMatrix y90 = wMatrix::yaw( PI_GRECO * 0.5f );
277  QVector<PhyObject*> legsComp;
278  PhyCylinder* leftLeg2a = new PhyCylinder( 0.0315f, 0.077f, world, name+":leftLeg2a" );
279  leftLeg2a->setMatrix( y90 );
280  leftLeg2a->setPosition( 0.112f, 0.0, 0.0 );
281  leftLeg2a->setMass( 0.79206f );
282  legsComp << leftLeg2a;
283  PhyCylinder* leftLeg2b = new PhyCylinder( 0.034f, 0.224f, world, name+":leftLeg2b" );
284  leftLeg2b->setMass( 1.5304f );
285  legsComp << leftLeg2b;
286  PhyCompoundObject* leftLeg2 = new PhyCompoundObject( legsComp, world, name+":leftLeg2" );
287  leftLegv << leftLeg2;
288 
289  legsComp.clear();
290  PhyCylinder* leftLeg3a = new PhyCylinder( 0.0245f, 0.063f, world, name+":leftLeg3a" );
291  leftLeg3a->setMatrix( y90 );
292  leftLeg3a->setPosition( -0.1065f, 0.0, 0.0 );
293  leftLeg3a->setMass( 0.14801f );
294  legsComp << leftLeg3a;
295  PhyCylinder* leftLeg3b = new PhyCylinder( 0.0315f, 0.213f, world, name+":leftLeg3b" );
296  leftLeg3b->setMass( 0.95262f );
297  legsComp << leftLeg3b;
298  PhyCompoundObject* leftLeg3 = new PhyCompoundObject( legsComp, world, name+":leftLeg3" );
299  leftLegv << leftLeg3;
300 
301  PhyCylinder* leftLeg4 = new PhyCylinder( 0.027f, 0.095f, world, name+":leftLeg4" );
302  leftLeg4->setMass( 0.59285f );
303  leftLegv << leftLeg4;
304 
305  PhyBox* leftLeg5 = new PhyBox( 0.13f, 0.054f, 0.004f, world, name+":leftLeg5" );
306  leftLeg5->setMass( 0.08185f );
307  leftLegv << leftLeg5;
308 
309  // Setting rotation/position of the left leg parts. This is automatically done when
310  // objects are associated with kinematic links, we only need to specify the transformation
311  // of objects relative to the frame of reference of the corresponding kinematic link.
312  wMatrix mtr;
313  wVector rotAx = kinLeftLeg.getLinkInfo(0).getRotAxis() * wVector(1.0, 0.0, 0.0);
314  real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
315  wVector transl = wVector(0.0, -(leftLeg1->radius() + (leftLeg0->height() / 2.0)), 0.0);
316  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
317  kinLeftLeg.getLinkInfo(0).setWObject(leftLeg0, mtr);
318 
319  rotAx = kinLeftLeg.getLinkInfo(1).getRotAxis() * wVector(1.0, 0.0, 0.0);
320  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
321  transl = wVector(0.0, 0.0, 0.0);
322  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
323  kinLeftLeg.getLinkInfo(1).setWObject(leftLeg1, mtr);
324 
325  rotAx = kinLeftLeg.getLinkInfo(2).getRotAxis() * wVector(1.0, 0.0, 0.0);
326  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
327  transl = wVector(0.0, (leftLeg2b->height() / 2.0), 0.0);
328  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
329  kinLeftLeg.getLinkInfo(2).setWObject(leftLeg2, mtr);
330 
331  mtr = wMatrix::identity();
332  mtr.w_pos = wVector((leftLeg3b->height() / 2.0), 0.0, 0.0, 1.0);
333  kinLeftLeg.getLinkInfo(3).setWObject(leftLeg3, mtr);
334 
335  rotAx = kinLeftLeg.getLinkInfo(4).getRotAxis();
336  rotAng = -PI_GRECO * 0.5f;
337  transl = wVector(0.0, 0.0, 0.0105f); // I don't know how to calculate 0.0105 using object dimensions
338  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around z axis
339  kinLeftLeg.getLinkInfo(4).setWObject(leftLeg4, mtr);
340 
341  rotAx = kinLeftLeg.getLinkInfo(5).getRotAxis() * wVector(1.0, 0.0, 0.0);
342  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
343  transl = wVector(leftLeg5->sideZ() / 2.0, 0.0, 0.0235f); // I don't know how to calculate 0.0235 using object dimensions
344  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
345  kinLeftLeg.getLinkInfo(5).setWObject(leftLeg5, mtr);
346 
347  // Creating joints. Axis are inverted or not to have positive angles in the right direction
348  // (perhaps I have to do this because we create the joint with the link n+1 as the parent
349  // object of the joint instead of link n most of the time...)
350  leftLegJointv.resize(0);
351  PhyJoint* j1 = kinLeftLeg.createJoint(0);
352  leftLegJointv << j1;
353 
354  PhyJoint* j2 = kinLeftLeg.createJoint(1);
355  leftLegJointv << j2;
356 
357  PhyJoint* j3 = kinLeftLeg.createJoint(2, false);
358  leftLegJointv << j3;
359 
360  PhyJoint* j4 = kinLeftLeg.createJoint(3);
361  leftLegJointv << j4;
362 
363  PhyJoint* j5 = kinLeftLeg.createJoint(4);
364  leftLegJointv << j5;
365 
366  PhyJoint* j6 = kinLeftLeg.createJoint(5);
367  leftLegJointv << j6;
368  }
369 
370  //---------------------------------------
371  //--- RIGHT LEG creation: objects & joints
372  //---------------------------------------
373  {
374  // Creating objects
375  PhyCylinder* rightLeg0 = new PhyCylinder( 0.038f, 0.013f, world, name+":rightLeg0" );
376  rightLeg0->setMass( 0.32708f );
377  rightLegv << rightLeg0;
378 
379  PhyCylinder* rightLeg1 = new PhyCylinder( 0.031f, 0.075f, world, name+":rightLeg1" );
380  rightLeg1->setMass( 0.85061f );
381  rightLegv << rightLeg1;
382 
383  const wMatrix y90 = wMatrix::yaw( PI_GRECO * 0.5f );
384  QVector<PhyObject*> legsComp;
385  PhyCylinder* rightLeg2a = new PhyCylinder( 0.0315f, 0.077f, world, name+":rightLeg2a" );
386  rightLeg2a->setMatrix( y90 );
387  rightLeg2a->setPosition( 0.112f, 0.0, 0.0 );
388  rightLeg2a->setMass( 0.79206f );
389  legsComp << rightLeg2a;
390  PhyCylinder* rightLeg2b = new PhyCylinder( 0.034f, 0.224f, world, name+":rightLeg2b" );
391  rightLeg2b->setMass( 1.5304f );
392  legsComp << rightLeg2b;
393  PhyCompoundObject* rightLeg2 = new PhyCompoundObject( legsComp, world, name+":rightLeg2" );
394  rightLegv << rightLeg2;
395 
396  legsComp.clear();
397  PhyCylinder* rightLeg3a = new PhyCylinder( 0.0245f, 0.063f, world, name+":rightLeg3a" );
398  rightLeg3a->setMatrix( y90 );
399  rightLeg3a->setPosition( -0.1065f, 0.0, 0.0 );
400  rightLeg3a->setMass( 0.14801f );
401  legsComp << rightLeg3a;
402  PhyCylinder* rightLeg3b = new PhyCylinder( 0.0315f, 0.213f, world, name+":rightLeg3b" );
403  rightLeg3b->setMass( 0.95262f );
404  legsComp << rightLeg3b;
405  PhyCompoundObject* rightLeg3 = new PhyCompoundObject( legsComp, world, name+":rightLeg3" );
406  rightLegv << rightLeg3;
407 
408  PhyCylinder* rightLeg4 = new PhyCylinder( 0.027f, 0.095f, world, name+":rightLeg4" );
409  rightLeg4->setMass( 0.59285f );
410  rightLegv << rightLeg4;
411 
412  PhyBox* rightLeg5 = new PhyBox( 0.13f, 0.054f, 0.004f, world, name+":rightLeg5" );
413  rightLeg5->setMass( 0.08185f );
414  rightLegv << rightLeg5;
415 
416  // Setting rotation/position of the right leg parts. This is automatically done when
417  // objects are associated with kinematic links, we only need to specify the transformation
418  // of objects relative to the frame of reference of the corresponding kinematic link.
419  wMatrix mtr;
420  wVector rotAx = wVector(1.0, 0.0, 0.0) * kinRightLeg.getLinkInfo(0).getRotAxis();
421  real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
422  wVector transl = wVector(0.0, -(rightLeg1->radius() + (rightLeg0->height() / 2.0)), 0.0);
423  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
424  kinRightLeg.getLinkInfo(0).setWObject(rightLeg0, mtr);
425 
426  rotAx = wVector(1.0, 0.0, 0.0) * kinRightLeg.getLinkInfo(1).getRotAxis();
427  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
428  transl = wVector(0.0, 0.0, 0.0);
429  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
430  kinRightLeg.getLinkInfo(1).setWObject(rightLeg1, mtr);
431 
432  rotAx = wVector(1.0, 0.0, 0.0) * kinRightLeg.getLinkInfo(2).getRotAxis();
433  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
434  transl = wVector(0.0, (rightLeg2b->height() / 2.0), 0.0);
435  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
436  kinRightLeg.getLinkInfo(2).setWObject(rightLeg2, mtr);
437 
438  mtr = wMatrix::identity();
439  mtr.w_pos = wVector((rightLeg3b->height() / 2.0), 0.0, 0.0, 1.0);
440  kinRightLeg.getLinkInfo(3).setWObject(rightLeg3, mtr);
441 
442  rotAx = kinRightLeg.getLinkInfo(4).getRotAxis();
443  rotAng = PI_GRECO * 0.5f;
444  transl = wVector(0.0, 0.0, -0.0105f); // I don't know how to calculate 0.0105 using object dimensions
445  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around z axis
446  kinRightLeg.getLinkInfo(4).setWObject(rightLeg4, mtr);
447 
448  rotAx = wVector(1.0, 0.0, 0.0) * kinRightLeg.getLinkInfo(5).getRotAxis();
449  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
450  transl = wVector(rightLeg5->sideZ() / 2.0f, 0.0, 0.0235f); // I don't know how to calculate 0.0235 using object dimensions
451  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
452  kinRightLeg.getLinkInfo(5).setWObject(rightLeg5, mtr);
453 
454  // Creating joints. Axis are inverted or not to have positive angles in the right direction
455  // (perhaps I have to do this because we create the joint with the link n+1 as the parent
456  // object of the joint instead of link n most of the time...)
457  rightLegJointv.resize(0);
458  PhyJoint* j1 = kinRightLeg.createJoint(0);
459  rightLegJointv << j1;
460 
461  PhyJoint* j2 = kinRightLeg.createJoint(1);
462  rightLegJointv << j2;
463 
464  PhyJoint* j3 = kinRightLeg.createJoint(2);
465  rightLegJointv << j3;
466 
467  PhyJoint* j4 = kinRightLeg.createJoint(3);
468  rightLegJointv << j4;
469 
470  PhyJoint* j5 = kinRightLeg.createJoint(4);
471  rightLegJointv << j5;
472 
473  PhyJoint* j6 = kinRightLeg.createJoint(5);
474  rightLegJointv << j6;
475  }
476 
477  //---------------------------------------
478  //--- LEFT ARM creation: object & joints
479  //---------------------------------------
480  {
481  // Creating objects
482  PhyCylinder* leftArm0 = new PhyCylinder( 0.031f, 0.011f, world, name+":leftArm0" );
483  leftArm0->setMass( 0.48278f );
484  leftArmv << leftArm0;
485 
486  PhyCylinder* leftArm1 = new PhyCylinder( 0.03f, 0.059f, world, name+":leftArm1" );
487  leftArm1->setMass( 0.20779f );
488  leftArmv << leftArm1;
489 
490  PhyCylinder* leftArm2 = new PhyCylinder( 0.026f, 0.156f, world, name+":leftArm2" );
491  leftArm2->setMass( 1.1584f );
492  leftArmv << leftArm2;
493 
494 #ifdef __GNUC__
495  #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)
496 #endif
497  PhySphere* leftArm3 = new PhySphere( 0.024f /*was 0.01*/, world, name+":leftArm3" );
498  leftArm3->setMass( 0.050798f );
499  leftArmv << leftArm3;
500 
501  PhyCylinder* leftArm4 = new PhyCylinder( 0.02f, 0.14f, world, name+":leftArm4" );
502  leftArm4->setMass( 0.48774f );
503  leftArmv << leftArm4;
504 
505  //--- this object isn't into Vadim's code, and I invented dimension and mass
506  //--- It was introduced because Universal joint doesn't have the same frame as
507  //--- specified to DH notation, so I created two hinge joints
508  //--- Actually the real wrist has two hinge joints arranged as I did, but
509  //--- I don't know which mass to set at intermediate joint
510  PhyCylinder* leftArm5 = new PhyCylinder( 0.007f, 0.04f, world, name+":leftArm5" );
511  leftArm5->setMass( 0.05f );
512  leftArmv << leftArm5;
513 
514  //--- left hand creation
515  //------------- structure respect to index inside leftArmv
516  // ----------------------
517  // 26 25 24| 23 Palm 6 |
518  // ===|===|===|=====| |
519  // thumb | |
520  // -----------------------
521  // index 7 || 8 || 9 || 10 || pinky
522  // -- -- -- --
523  // || || || || pinky
524  // 11 || 12 || 13 || 14 || pinky
525  // || || || || pinky
526  // -- -- -- --
527  // 15 || 16 || 17 || 18 || pinky
528  // || || || || pinky
529  // -- -- -- --
530  // 19 || 20 || 21 || 22 || pinky
531  // || || || || pinky
532  // -- -- -- --
533  //---------------------- the same indices are valid also for right hand
534  // This dimension is computed so that the frame of reference of the hand ends on the edge between
535  // the inner palm and the face to which four fingers are attached and the palm is tangent to the
536  // leftArm5 solid. The dimensions of leftArmPalm6 from original simulator code were
537  // (0.069, 0.065, 0.024)
538  const real leftArmPalm6X = fabs(kinLeftArm.getLinkInfo(9).getAxis() % kinLeftArm.getLinkInfo(9).getYarpMatrixAswMatrix().x_ax) - leftArm5->radius();
539  PhyBox* leftArmPalm6 = new PhyBox( leftArmPalm6X, 0.065f, 0.018f, world, name+":leftArmPalm6" );
540  leftArmPalm6->setMass( 0.19099f );
541  leftArmv << leftArmPalm6;
542  // Creating the hand group and adding leftArmPalm6 as the head object. Other objects will be added
543  // only after their position has been set
544  leftHandGroup = new PhyObjectsGroup(leftArmPalm6, world, name+":leftHandGroup");
545  leftHandGroup->setOwner(this, false);
546 
547 #ifdef __GNUC__
548  #warning ============= AREN T FINGER PIECES A BIT TOO HEAVY? =============
549 #endif
550  PhyCylinder* leftArmIndex7 = new PhyCylinder( 0.0065f,0.012f, world, name+":leftArmIndex7" );
551  leftArmIndex7->setMass( 0.2f );
552  leftArmv << leftArmIndex7;
553 
554  PhyCylinder* leftArmMiddle8 = new PhyCylinder( 0.0065f,0.012f, world, name+":leftArmMiddle8" );
555  leftArmMiddle8->setMass( 0.2f );
556  leftArmv << leftArmMiddle8;
557 
558  PhyCylinder* leftArmRing9 = new PhyCylinder( 0.0065f,0.012f, world, name+":leftArmRing9" );
559  leftArmRing9->setMass( 0.2f );
560  leftArmv << leftArmRing9;
561 
562  PhyCylinder* leftArmPinky10 = new PhyCylinder( 0.0065f,0.012f, world, name+":leftArmPinky10" );
563  leftArmPinky10->setMass( 0.2f );
564  leftArmv << leftArmPinky10;
565 
566  PhyCylinder* leftArmIndex11 = new PhyCylinder( 0.0065f,0.026f, world, name+":leftArmIndex11" );
567  leftArmIndex11->setMass( 0.2f );
568  leftArmv << leftArmIndex11;
569 
570  PhyCylinder* leftArmMiddle12 = new PhyCylinder( 0.0065f,0.028f, world, name+":leftArmMiddle12" );
571  leftArmMiddle12->setMass( 0.2f );
572  leftArmv << leftArmMiddle12;
573 
574  PhyCylinder* leftArmRing13 = new PhyCylinder( 0.0065f,0.026f, world, name+":leftArmRing13" );
575  leftArmRing13->setMass( 0.2f );
576  leftArmv << leftArmRing13;
577 
578  PhyCylinder* leftArmPinky14 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmPinky14" );
579  leftArmPinky14->setMass( 0.2f );
580  leftArmv << leftArmPinky14;
581 
582  PhyCylinder* leftArmIndex15 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmIndex15" );
583  leftArmIndex15->setMass( 0.2f );
584  leftArmv << leftArmIndex15;
585 
586  PhyCylinder* leftArmMiddle16 = new PhyCylinder( 0.0065f,0.024f, world, name+":leftArmMiddle16" );
587  leftArmMiddle16->setMass( 0.2f );
588  leftArmv << leftArmMiddle16;
589 
590  PhyCylinder* leftArmRing17 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmRing17" );
591  leftArmRing17->setMass( 0.2f );
592  leftArmv << leftArmRing17;
593 
594  PhyCylinder* leftArmPinky18 = new PhyCylinder( 0.0065f,0.019f, world, name+":leftArmPinky18" );
595  leftArmPinky18->setMass( 0.2f );
596  leftArmv << leftArmPinky18;
597 
598  PhyCylinder* leftArmIndex19 = new PhyCylinder( 0.0065f,0.02f, world, name+":leftArmIndex19" );
599  leftArmIndex19->setMass( 0.2f );
600  leftArmv << leftArmIndex19;
601 
602  PhyCylinder* leftArmMiddle20 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmMiddle20" );
603  leftArmMiddle20->setMass( 0.2f );
604  leftArmv << leftArmMiddle20;
605 
606  PhyCylinder* leftArmRing21 = new PhyCylinder( 0.0065f,0.02f, world, name+":leftArmRing21" );
607  leftArmRing21->setMass( 0.2f );
608  leftArmv << leftArmRing21;
609 
610  PhyCylinder* leftArmPinky22 = new PhyCylinder( 0.0065f,0.02f, world, name+":leftArmPinky22" );
611  leftArmPinky22->setMass( 0.2f );
612  leftArmv << leftArmPinky22;
613 
614  PhyCylinder* leftArmThumb23 = new PhyCylinder( 0.0065f,0.026f, world, name+":leftArmThumb23" );
615  leftArmThumb23->setMass( 0.2f );
616  leftArmv << leftArmThumb23;
617 
618  PhyCylinder* leftArmThumb24 = new PhyCylinder( 0.0065f,0.026f, world, name+":leftArmThumb24" );
619  leftArmThumb24->setMass( 0.2f );
620  leftArmv << leftArmThumb24;
621 
622  PhyCylinder* leftArmThumb25 = new PhyCylinder( 0.0065f,0.022f, world, name+":leftArmThumb25" );
623  leftArmThumb25->setMass( 0.2f );
624  leftArmv << leftArmThumb25;
625 
626  PhyCylinder* leftArmThumb26 = new PhyCylinder( 0.0065f,0.016f, world, name+":leftArmThumb26" );
627  leftArmThumb26->setMass( 0.2f );
628  leftArmv << leftArmThumb26;
629 
630  // Setting rotation/position of the left arm parts. This is automatically done when
631  // objects are associated with kinematic links, we only need to specify the transformation
632  // of objects relative to the frame of reference of the corresponding kinematic link.
633  wMatrix mtr;
634  wVector rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(3).getRotAxis();
635  real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
636  wVector transl = wVector(0.0, leftArm0->height() / 2.0 + leftArm1->radius(), 0.0);
637  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
638  kinLeftArm.getLinkInfo(3).setWObject(leftArm0, mtr);
639 
640  rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(4).getRotAxis();
641  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
642  transl = wVector(0.0, 0.0, 0.0);
643  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
644  kinLeftArm.getLinkInfo(4).setWObject(leftArm1, mtr);
645 
646  rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(5).getRotAxis();
647  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
648  transl = wVector(0.0, leftArm2->height() / 2.0, 0.0);
649  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
650  kinLeftArm.getLinkInfo(5).setWObject(leftArm2, mtr);
651 
652  mtr = wMatrix::identity();
653  kinLeftArm.getLinkInfo(6).setWObject(leftArm3, mtr);
654 
655  rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(7).getRotAxis();
656  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
657  transl = wVector(0.0, -leftArm4->height() / 2.0, 0.0);
658  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
659  kinLeftArm.getLinkInfo(7).setWObject(leftArm4, mtr);
660 
661  rotAx = wVector(1.0, 0.0, 0.0) * kinLeftArm.getLinkInfo(8).getRotAxis();
662  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
663  transl = wVector(0.0, 0.0, 0.0);
664  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
665  kinLeftArm.getLinkInfo(8).setWObject(leftArm5, mtr);
666 
667  mtr = wMatrix::identity();
668  mtr.w_pos = wVector(-leftArmPalm6->sideX() / 2.0, 0.0, leftArmPalm6->sideZ() / 2.0, 1.0);
669  kinLeftArm.getLinkInfo(9).setWObject(leftHandGroup, mtr);
670 
671  // Now positioning fingers. We cannot rely on iKin as fingers are not part of any kinematic chain.
672  // We always start from the leftArmPalm6 matrix
673  // --- positioning of index finger
674  mtr = leftHandGroup->matrix();
675  mtr.w_pos += mtr.rotateVector(wVector((leftArmPalm6->sideX() + leftArmIndex7->height()) / 2.0f, -0.02275f, 0.0));
676  leftArmIndex7->setMatrix(mtr);
677  leftHandGroup->addObject(leftArmIndex7, IndexChain);
678  mtr.w_pos += mtr.rotateVector(wVector((leftArmIndex7->height() + leftArmIndex11->height()) / 2.0, 0.0, 0.0));
679  leftArmIndex11->setMatrix(mtr);
680  leftHandGroup->addObject(leftArmIndex11, IndexChain);
681  mtr.w_pos += mtr.rotateVector(wVector((leftArmIndex11->height() + leftArmIndex15->height()) / 2.0, 0.0, 0.0));
682  leftArmIndex15->setMatrix(mtr);
683  leftHandGroup->addObject(leftArmIndex15, IndexChain);
684  mtr.w_pos += mtr.rotateVector(wVector((leftArmIndex15->height() + leftArmIndex19->height()) / 2.0, 0.0, 0.0));
685  leftArmIndex19->setMatrix(mtr);
686  leftHandGroup->addObject(leftArmIndex19, IndexChain);
687 
688  // --- positioning of middle finger
689  mtr = leftHandGroup->matrix();
690  mtr.w_pos += mtr.rotateVector(wVector((leftArmPalm6->sideX() + leftArmMiddle8->height()) / 2.0, -0.0065f, 0.0));
691  leftArmMiddle8->setMatrix(mtr);
692  leftHandGroup->addObject(leftArmMiddle8, MiddleChain);
693  mtr.w_pos += mtr.rotateVector(wVector((leftArmMiddle8->height() + leftArmMiddle12->height()) / 2.0, 0.0, 0.0));
694  leftArmMiddle12->setMatrix(mtr);
695  leftHandGroup->addObject(leftArmMiddle12, MiddleChain);
696  mtr.w_pos += mtr.rotateVector(wVector((leftArmMiddle12->height() + leftArmMiddle16->height()) / 2.0, 0.0, 0.0));
697  leftArmMiddle16->setMatrix(mtr);
698  leftHandGroup->addObject(leftArmMiddle16, MiddleChain);
699  mtr.w_pos += mtr.rotateVector(wVector((leftArmMiddle16->height() + leftArmMiddle20->height()) / 2.0, 0.0, 0.0));
700  leftArmMiddle20->setMatrix(mtr);
701  leftHandGroup->addObject(leftArmMiddle20, MiddleChain);
702 
703  // --- positioning of ring finger
704  mtr = leftHandGroup->matrix();
705  mtr.w_pos += mtr.rotateVector(wVector((leftArmPalm6->sideX() + leftArmRing9->height()) / 2.0, 0.00975f, 0.0));
706  leftArmRing9->setMatrix(mtr);
707  leftHandGroup->addObject(leftArmRing9, RingChain);
708  mtr.w_pos += mtr.rotateVector(wVector((leftArmRing9->height() + leftArmRing13->height()) / 2.0, 0.0, 0.0));
709  leftArmRing13->setMatrix(mtr);
710  leftHandGroup->addObject(leftArmRing13, RingChain);
711  mtr.w_pos += mtr.rotateVector(wVector((leftArmRing13->height() + leftArmRing17->height()) / 2.0, 0.0, 0.0));
712  leftArmRing17->setMatrix(mtr);
713  leftHandGroup->addObject(leftArmRing17, RingChain);
714  mtr.w_pos += mtr.rotateVector(wVector((leftArmRing17->height() + leftArmRing21->height()) / 2.0, 0.0, 0.0));
715  leftArmRing21->setMatrix(mtr);
716  leftHandGroup->addObject(leftArmRing21, RingChain);
717 
718  // --- positioning of pinky
719  mtr = leftHandGroup->matrix();
720  mtr.w_pos += mtr.rotateVector(wVector((leftArmPalm6->sideX() + leftArmPinky10->height()) / 2.0, 0.026f, 0.0));
721  leftArmPinky10->setMatrix(mtr);
722  leftHandGroup->addObject(leftArmPinky10, PinkyChain);
723  mtr.w_pos += mtr.rotateVector(wVector((leftArmPinky10->height() + leftArmPinky14->height()) / 2.0, 0.0, 0.0));
724  leftArmPinky14->setMatrix(mtr);
725  leftHandGroup->addObject(leftArmPinky14, PinkyChain);
726  mtr.w_pos += mtr.rotateVector(wVector((leftArmPinky14->height() + leftArmPinky18->height()) / 2.0, 0.0, 0.0));
727  leftArmPinky18->setMatrix(mtr);
728  leftHandGroup->addObject(leftArmPinky18, PinkyChain);
729  mtr.w_pos += mtr.rotateVector(wVector((leftArmPinky18->height() + leftArmPinky22->height()) / 2.0, 0.0, 0.0));
730  leftArmPinky22->setMatrix(mtr);
731  leftHandGroup->addObject(leftArmPinky22, PinkyChain);
732 
733  // --- positioning of thumb
734  mtr = leftHandGroup->matrix().rotateAround(leftArmPalm6->matrix().z_ax, leftArmPalm6->matrix().w_pos, -PI_GRECO * 0.5f);
735  mtr.w_pos += mtr.rotateVector(wVector(0.015f + leftArmThumb23->height() / 2.0, leftArmPalm6->sideY() / 2.0 - 0.035f, -leftArmPalm6->sideZ() / 2.0));
736  leftArmThumb23->setMatrix(mtr);
737  leftHandGroup->addObject(leftArmThumb23, ThumbChain);
738  mtr.w_pos += mtr.rotateVector(wVector((leftArmThumb23->height() + leftArmThumb24->height()) / 2.0, 0.0, 0.0));
739  leftArmThumb24->setMatrix(mtr);
740  leftHandGroup->addObject(leftArmThumb24, ThumbChain);
741  mtr.w_pos += mtr.rotateVector(wVector((leftArmThumb24->height() + leftArmThumb25->height()) / 2.0, 0.0, 0.0));
742  leftArmThumb25->setMatrix(mtr);
743  leftHandGroup->addObject(leftArmThumb25, ThumbChain);
744  mtr.w_pos += mtr.rotateVector(wVector((leftArmThumb25->height() + leftArmThumb26->height()) / 2.0, 0.0, 0.0));
745  leftArmThumb26->setMatrix(mtr);
746  leftHandGroup->addObject(leftArmThumb26, ThumbChain);
747 
748  // Creating joints. Axis are inverted or not to have positive angles in the right direction
749  // (perhaps I have to do this because we create the joint with the link n+1 as the parent
750  // object of the joint instead of link n most of the time...)
751  leftArmJointv.resize(0);
752  PhyJoint* j1 = kinLeftArm.createJoint(3);
753  leftArmJointv << j1;
754 
755  PhyJoint* j2 = kinLeftArm.createJoint(4);
756  leftArmJointv << j2;
757 
758  PhyJoint* j3 = kinLeftArm.createJoint(5);
759  leftArmJointv << j3;
760 
761  PhyJoint* j4 = kinLeftArm.createJoint(6);
762  leftArmJointv << j4;
763 
764  PhyJoint* j5 = kinLeftArm.createJoint(7);
765  leftArmJointv << j5;
766 
767  PhyJoint* j6 = kinLeftArm.createJoint(8);
768  leftArmJointv << j6;
769 
770  PhyJoint* j7 = kinLeftArm.createJoint(9);
771  leftArmJointv << j7;
772 
773  // As above, for fingers we cannot rely on iKin
774  PhyHinge* j8 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-leftArmIndex7->height() / 2.0, 0.0, 0.0), leftArmIndex7, leftArmPalm6);
775  leftArmJointv << j8;
776  leftHandGroup->addJointDOF(j8, 0, IndexChain);
777 
778  PhyHinge* j9 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-leftArmMiddle8->height() / 2.0, 0.0, 0.0), leftArmMiddle8, leftArmPalm6);
779  leftArmJointv << j9;
780  leftHandGroup->addJointDOF(j9, 0, MiddleChain);
781 
782  PhyHinge* j10 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-leftArmRing9->height() / 2.0, 0.0, 0.0), leftArmRing9, leftArmPalm6);
783  leftArmJointv << j10;
784  leftHandGroup->addJointDOF(j10, 0, RingChain);
785 
786  PhyHinge* j11 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-leftArmPinky10->height() / 2.0, 0.0, 0.0), leftArmPinky10, leftArmPalm6);
787  leftArmJointv << j11;
788  leftHandGroup->addJointDOF(j11, 0, PinkyChain);
789 
790  PhyHinge* j12 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmIndex11->height() / 2.0, 0.0, 0.0), leftArmIndex11, leftArmIndex7);
791  leftArmJointv << j12;
792  leftHandGroup->addJointDOF(j12, 0, IndexChain);
793 
794  PhyHinge* j13 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmIndex15->height() / 2.0, 0.0, 0.0), leftArmIndex15, leftArmIndex11);
795  leftArmJointv << j13;
796  leftHandGroup->addJointDOF(j13, 0, IndexChain);
797 
798  PhyHinge* j14 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmIndex19->height() / 2.0, 0.0, 0.0), leftArmIndex19, leftArmIndex15);
799  leftArmJointv << j14;
800  leftHandGroup->addJointDOF(j14, 0, IndexChain);
801 
802  PhyHinge* j15 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmMiddle12->height() / 2.0, 0.0, 0.0), leftArmMiddle12, leftArmMiddle8);
803  leftArmJointv << j15;
804  leftHandGroup->addJointDOF(j15, 0, MiddleChain);
805 
806  PhyHinge* j16 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmMiddle16->height() / 2.0, 0.0, 0.0), leftArmMiddle16, leftArmMiddle12);
807  leftArmJointv << j16;
808  leftHandGroup->addJointDOF(j16, 0, MiddleChain);
809 
810  PhyHinge* j17 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmMiddle20->height() / 2.0, 0.0, 0.0), leftArmMiddle20, leftArmMiddle16);
811  leftArmJointv << j17;
812  leftHandGroup->addJointDOF(j17, 0, MiddleChain);
813 
814  PhyHinge* j18 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmRing13->height() / 2.0, 0.0, 0.0), leftArmRing13, leftArmRing9);
815  leftArmJointv << j18;
816  leftHandGroup->addJointDOF(j18, 0, RingChain);
817 
818  PhyHinge* j19 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmRing17->height() / 2.0, 0.0, 0.0), leftArmRing17, leftArmRing13);
819  leftArmJointv << j19;
820  leftHandGroup->addJointDOF(j19, 0, RingChain);
821 
822  PhyHinge* j20 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmRing21->height() / 2.0, 0.0, 0.0), leftArmRing21, leftArmRing17);
823  leftArmJointv << j20;
824  leftHandGroup->addJointDOF(j20, 0, RingChain);
825 
826  PhyHinge* j21 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmPinky14->height() / 2.0, 0.0, 0.0), leftArmPinky14, leftArmPinky10);
827  leftArmJointv << j21;
828  leftHandGroup->addJointDOF(j21, 0, PinkyChain);
829 
830  PhyHinge* j22 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmPinky18->height() / 2.0, 0.0, 0.0), leftArmPinky18, leftArmPinky14);
831  leftArmJointv << j22;
832  leftHandGroup->addJointDOF(j22, 0, PinkyChain);
833 
834  PhyHinge* j23 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmPinky22->height() / 2.0, 0.0, 0.0), leftArmPinky22, leftArmPinky18);
835  leftArmJointv << j23;
836  leftHandGroup->addJointDOF(j23, 0, PinkyChain);
837 
838  PhyHinge* j24 = new PhyHinge(wVector(0.0, -1.0, 0.0), wVector(-leftArmThumb23->height() / 2.0, 0.0, 0.0), leftArmThumb23, leftArmPalm6);
839  leftArmJointv << j24;
840  leftHandGroup->addJointDOF(j24, 0, ThumbChain);
841 
842  PhyHinge* j25 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-leftArmThumb24->height() / 2.0, 0.0, 0.0), leftArmThumb24, leftArmThumb23);
843  leftArmJointv << j25;
844  leftHandGroup->addJointDOF(j25, 0, ThumbChain);
845 
846  PhyHinge* j26 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-leftArmThumb25->height() / 2.0, 0.0, 0.0), leftArmThumb25, leftArmThumb24);
847  leftArmJointv << j26;
848  leftHandGroup->addJointDOF(j26, 0, ThumbChain);
849 
850  PhyHinge* j27 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-leftArmThumb26->height() / 2.0, 0.0, 0.0), leftArmThumb26, leftArmThumb25);
851  leftArmJointv << j27;
852  leftHandGroup->addJointDOF(j27, 0, ThumbChain);
853 
854  //--- Coupling Left Hand joints (Opening)
855  connect( leftArmJointv[7]->dofs()[0], SIGNAL( changedPosition( real ) ),
856  leftArmJointv[9]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
857  connect( leftArmJointv[7]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
858  leftArmJointv[9]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
859  connect( leftArmJointv[7]->dofs()[0], SIGNAL( changedPosition( real ) ),
860  leftArmJointv[10]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
861  connect( leftArmJointv[7]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
862  leftArmJointv[10]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
863  //--- Coupling (last two phalanges P2-P3 of fingers)
864  connect( leftArmJointv[12]->dofs()[0], SIGNAL( changedPosition( real ) ),
865  leftArmJointv[13]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
866  connect( leftArmJointv[12]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
867  leftArmJointv[13]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
868  connect( leftArmJointv[15]->dofs()[0], SIGNAL( changedPosition( real ) ),
869  leftArmJointv[16]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
870  connect( leftArmJointv[15]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
871  leftArmJointv[16]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
872  connect( leftArmJointv[25]->dofs()[0], SIGNAL( changedPosition( real ) ),
873  leftArmJointv[26]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
874  connect( leftArmJointv[25]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
875  leftArmJointv[26]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
876  //--- Coupling the ring-pinky phalanges
877  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
878  leftArmJointv[18]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
879  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
880  leftArmJointv[18]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
881  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
882  leftArmJointv[19]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
883  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
884  leftArmJointv[19]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
885  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
886  leftArmJointv[20]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
887  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
888  leftArmJointv[20]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
889  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
890  leftArmJointv[21]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
891  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
892  leftArmJointv[21]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
893  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
894  leftArmJointv[22]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
895  connect( leftArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
896  leftArmJointv[22]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
897  }
898 
899  //---------------------------------------
900  //--- RIGHT ARM creation: object & joints
901  //---------------------------------------
902  {
903  // Creating objects
904  PhyCylinder* rightArm0 = new PhyCylinder( 0.031f, 0.011f, world, name+":rightArm0" );
905  rightArm0->setMass( 0.48278f );
906  rightArmv << rightArm0;
907 
908  PhyCylinder* rightArm1 = new PhyCylinder( 0.03f, 0.059f, world, name+":rightArm1" );
909  rightArm1->setMass( 0.20779f );
910  rightArmv << rightArm1;
911 
912  PhyCylinder* rightArm2 = new PhyCylinder( 0.026f, 0.156f, world, name+":rightArm2" );
913  rightArm2->setMass( 1.1584f );
914  rightArmv << rightArm2;
915 
916 #ifdef __GNUC__
917  #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)
918 #endif
919  PhySphere* rightArm3 = new PhySphere( 0.024f /*was 0.01f*/, world, name+":rightArm3" );
920  rightArm3->setMass( 0.050798f );
921  rightArmv << rightArm3;
922 
923  PhyCylinder* rightArm4 = new PhyCylinder( 0.02f, 0.14f, world, name+":rightArm4" );
924  rightArm4->setMass( 0.48774f );
925  rightArmv << rightArm4;
926 
927  //--- this object isn't into Vadim's code, and I invented dimension and mass
928  //--- It was introduced because Universal joint doesn't have the same frame as
929  //--- specified to DH notation, so I created two hinge joints
930  PhyCylinder* rightArm5 = new PhyCylinder( 0.007f, 0.04f, world, name+":rightArm5" );
931  rightArm5->setMass( 0.05f );
932  rightArmv << rightArm5;
933 
934  //--- right hand creation (see left hand for a description of hand structure)
935  // This dimension is computed so that the frame of reference of the hand ends on the edge between
936  // the inner palm and the face to which four fingers are attached and the palm is tangent to the
937  // rightArm5 solid. The dimensions of rightArmPalm6 from original simulator code were
938  // (0.069, 0.065, 0.024)
939  const real rightArmPalm6X = fabs(kinRightArm.getLinkInfo(9).getAxis() % kinRightArm.getLinkInfo(9).getYarpMatrixAswMatrix().x_ax) - rightArm5->radius();
940  PhyBox* rightArmPalm6 = new PhyBox( rightArmPalm6X, 0.065f, 0.018f, world, name+":rightArmPalm6" );
941  rightArmPalm6->setMass( 0.19099f );
942  rightArmv << rightArmPalm6;
943  // Creating the hand group and adding rightArmPalm6 as the head object. Other objects will be added
944  // only after their position has been set
945  rightHandGroup = new PhyObjectsGroup(rightArmPalm6, world, name+":rightHandGroup");
946  rightHandGroup->setOwner(this, false);
947 
948 #ifdef __GNUC__
949  #warning ============= AREN T FINGER PIECES A BIT TOO HEAVY? =============
950 #endif
951  PhyCylinder* rightArmIndex7 = new PhyCylinder( 0.0065f,0.012f, world, name+":rightArmIndex7" );
952  rightArmIndex7->setMass( 0.2f );
953  rightArmv << rightArmIndex7;
954 
955  PhyCylinder* rightArmMiddle8 = new PhyCylinder( 0.0065f,0.012f, world, name+":rightArmMiddle8" );
956  rightArmMiddle8->setMass( 0.2f );
957  rightArmv << rightArmMiddle8;
958 
959  PhyCylinder* rightArmRing9 = new PhyCylinder( 0.0065f,0.012f, world, name+":rightArmRing9" );
960  rightArmRing9->setMass( 0.2f );
961  rightArmv << rightArmRing9;
962 
963  PhyCylinder* rightArmPinky10 = new PhyCylinder( 0.0065f,0.012f, world, name+":rightArmPinky10" );
964  rightArmPinky10->setMass( 0.2f );
965  rightArmv << rightArmPinky10;
966 
967  PhyCylinder* rightArmIndex11 = new PhyCylinder( 0.0065f,0.026f, world, name+":rightArmIndex11" );
968  rightArmIndex11->setMass( 0.2f );
969  rightArmv << rightArmIndex11;
970 
971  PhyCylinder* rightArmMiddle12 = new PhyCylinder( 0.0065f,0.028f, world, name+":rightArmMiddle12" );
972  rightArmMiddle12->setMass( 0.2f );
973  rightArmv << rightArmMiddle12;
974 
975  PhyCylinder* rightArmRing13 = new PhyCylinder( 0.0065f,0.026f, world, name+":rightArmRing13" );
976  rightArmRing13->setMass( 0.2f );
977  rightArmv << rightArmRing13;
978 
979  PhyCylinder* rightArmPinky14 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmPinky14" );
980  rightArmPinky14->setMass( 0.2f );
981  rightArmv << rightArmPinky14;
982 
983  PhyCylinder* rightArmIndex15 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmIndex15" );
984  rightArmIndex15->setMass( 0.2f );
985  rightArmv << rightArmIndex15;
986 
987  PhyCylinder* rightArmMiddle16 = new PhyCylinder( 0.0065f,0.024f, world, name+":rightArmMiddle16" );
988  rightArmMiddle16->setMass( 0.2f );
989  rightArmv << rightArmMiddle16;
990 
991  PhyCylinder* rightArmRing17 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmRing17" );
992  rightArmRing17->setMass( 0.2f );
993  rightArmv << rightArmRing17;
994 
995  PhyCylinder* rightArmPinky18 = new PhyCylinder( 0.0065f,0.019f, world, name+":rightArmPinky18" );
996  rightArmPinky18->setMass( 0.2f );
997  rightArmv << rightArmPinky18;
998 
999  PhyCylinder* rightArmIndex19 = new PhyCylinder( 0.0065f,0.02f, world, name+":rightArmIndex19" );
1000  rightArmIndex19->setMass( 0.2f );
1001  rightArmv << rightArmIndex19;
1002 
1003  PhyCylinder* rightArmMiddle20 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmMiddle20" );
1004  rightArmMiddle20->setMass( 0.2f );
1005  rightArmv << rightArmMiddle20;
1006 
1007  PhyCylinder* rightArmRing21 = new PhyCylinder( 0.0065f,0.02f, world, name+":rightArmRing21" );
1008  rightArmRing21->setMass( 0.2f );
1009  rightArmv << rightArmRing21;
1010 
1011  PhyCylinder* rightArmPinky22 = new PhyCylinder( 0.0065f,0.02f, world, name+":rightArmPinky22" );
1012  rightArmPinky22->setMass( 0.2f );
1013  rightArmv << rightArmPinky22;
1014 
1015  PhyCylinder* rightArmThumb23 = new PhyCylinder( 0.0065f,0.026f, world, name+":rightArmThumb23" );
1016  rightArmThumb23->setMass( 0.2f );
1017  rightArmv << rightArmThumb23;
1018 
1019  PhyCylinder* rightArmThumb24 = new PhyCylinder( 0.0065f,0.026f, world, name+":rightArmThumb24" );
1020  rightArmThumb24->setMass( 0.2f );
1021  rightArmv << rightArmThumb24;
1022 
1023  PhyCylinder* rightArmThumb25 = new PhyCylinder( 0.0065f,0.022f, world, name+":rightArmThumb25" );
1024  rightArmThumb25->setMass( 0.2f );
1025  rightArmv << rightArmThumb25;
1026 
1027  PhyCylinder* rightArmThumb26 = new PhyCylinder( 0.0065f,0.016f, world, name+":rightArmThumb26" );
1028  rightArmThumb26->setMass( 0.2f );
1029  rightArmv << rightArmThumb26;
1030 
1031  // Setting rotation/position of the right arm parts. This is automatically done when
1032  // objects are associated with kinematic links, we only need to specify the transformation
1033  // of objects relative to the frame of reference of the corresponding kinematic link.
1034  wMatrix mtr;
1035  wVector rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(3).getRotAxis();
1036  real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1037  wVector transl = wVector(0.0, rightArm0->height() / 2.0 + rightArm1->radius(), 0.0);
1038  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
1039  kinRightArm.getLinkInfo(3).setWObject(rightArm0, mtr);
1040 
1041  rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(4).getRotAxis();
1042  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1043  transl = wVector(0.0, 0.0, 0.0);
1044  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
1045  kinRightArm.getLinkInfo(4).setWObject(rightArm1, mtr);
1046 
1047  rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(5).getRotAxis();
1048  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1049  transl = wVector(0.0, -rightArm2->height() / 2.0, 0.0);
1050  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
1051  kinRightArm.getLinkInfo(5).setWObject(rightArm2, mtr);
1052 
1053  mtr = wMatrix::identity();
1054  kinRightArm.getLinkInfo(6).setWObject(rightArm3, mtr);
1055 
1056  rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(7).getRotAxis();
1057  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1058  transl = wVector(0.0, rightArm4->height() / 2.0, 0.0);
1059  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
1060  kinRightArm.getLinkInfo(7).setWObject(rightArm4, mtr);
1061 
1062  rotAx = wVector(1.0, 0.0, 0.0) * kinRightArm.getLinkInfo(8).getRotAxis();
1063  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1064  transl = wVector(0.0, 0.0, 0.0);
1065  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
1066  kinRightArm.getLinkInfo(8).setWObject(rightArm5, mtr);
1067 
1068  mtr = wMatrix::identity();
1069  mtr.w_pos = wVector(-rightArmPalm6->sideX() / 2.0, 0.0, -rightArmPalm6->sideZ() / 2.0, 1.0);
1070  kinRightArm.getLinkInfo(9).setWObject(rightHandGroup, mtr);
1071 
1072  // Now positioning fingers. We cannot rely on iKin as fingers are not part of any kinematic chain.
1073  // We always start from the rightArmPalm6 matrix
1074  // --- positioning of index finger
1075  mtr = rightArmPalm6->matrix();
1076  mtr.w_pos += mtr.rotateVector(wVector((rightArmPalm6->sideX() + rightArmIndex7->height()) / 2.0f, -0.02275f, 0.0));
1077  rightArmIndex7->setMatrix(mtr);
1078  rightHandGroup->addObject(rightArmIndex7, IndexChain);
1079  mtr.w_pos += mtr.rotateVector(wVector((rightArmIndex7->height() + rightArmIndex11->height()) / 2.0, 0.0, 0.0));
1080  rightArmIndex11->setMatrix(mtr);
1081  rightHandGroup->addObject(rightArmIndex11, IndexChain);
1082  mtr.w_pos += mtr.rotateVector(wVector((rightArmIndex11->height() + rightArmIndex15->height()) / 2.0, 0.0, 0.0));
1083  rightArmIndex15->setMatrix(mtr);
1084  rightHandGroup->addObject(rightArmIndex15, IndexChain);
1085  mtr.w_pos += mtr.rotateVector(wVector((rightArmIndex15->height() + rightArmIndex19->height()) / 2.0, 0.0, 0.0));
1086  rightArmIndex19->setMatrix(mtr);
1087  rightHandGroup->addObject(rightArmIndex19, IndexChain);
1088 
1089  // --- positioning of middle finger
1090  mtr = rightArmPalm6->matrix();
1091  mtr.w_pos += mtr.rotateVector(wVector((rightArmPalm6->sideX() + rightArmMiddle8->height()) / 2.0, -0.0065f, 0.0));
1092  rightArmMiddle8->setMatrix(mtr);
1093  rightHandGroup->addObject(rightArmMiddle8, MiddleChain);
1094  mtr.w_pos += mtr.rotateVector(wVector((rightArmMiddle8->height() + rightArmMiddle12->height()) / 2.0, 0.0, 0.0));
1095  rightArmMiddle12->setMatrix(mtr);
1096  rightHandGroup->addObject(rightArmMiddle12, MiddleChain);
1097  mtr.w_pos += mtr.rotateVector(wVector((rightArmMiddle12->height() + rightArmMiddle16->height()) / 2.0, 0.0, 0.0));
1098  rightArmMiddle16->setMatrix(mtr);
1099  rightHandGroup->addObject(rightArmMiddle16, MiddleChain);
1100  mtr.w_pos += mtr.rotateVector(wVector((rightArmMiddle16->height() + rightArmMiddle20->height()) / 2.0, 0.0, 0.0));
1101  rightArmMiddle20->setMatrix(mtr);
1102  rightHandGroup->addObject(rightArmMiddle20, MiddleChain);
1103 
1104  // --- positioning of ring finger
1105  mtr = rightArmPalm6->matrix();
1106  mtr.w_pos += mtr.rotateVector(wVector((rightArmPalm6->sideX() + rightArmRing9->height()) / 2.0, 0.00975f, 0.0));
1107  rightArmRing9->setMatrix(mtr);
1108  rightHandGroup->addObject(rightArmRing9, RingChain);
1109  mtr.w_pos += mtr.rotateVector(wVector((rightArmRing9->height() + rightArmRing13->height()) / 2.0, 0.0, 0.0));
1110  rightArmRing13->setMatrix(mtr);
1111  rightHandGroup->addObject(rightArmRing13, RingChain);
1112  mtr.w_pos += mtr.rotateVector(wVector((rightArmRing13->height() + rightArmRing17->height()) / 2.0, 0.0, 0.0));
1113  rightArmRing17->setMatrix(mtr);
1114  rightHandGroup->addObject(rightArmRing17, RingChain);
1115  mtr.w_pos += mtr.rotateVector(wVector((rightArmRing17->height() + rightArmRing21->height()) / 2.0, 0.0, 0.0));
1116  rightArmRing21->setMatrix(mtr);
1117  rightHandGroup->addObject(rightArmRing21, RingChain);
1118 
1119  // --- positioning of pinky
1120  mtr = rightArmPalm6->matrix();
1121  mtr.w_pos += mtr.rotateVector(wVector((rightArmPalm6->sideX() + rightArmPinky10->height()) / 2.0, 0.026f, 0.0));
1122  rightArmPinky10->setMatrix(mtr);
1123  rightHandGroup->addObject(rightArmPinky10, PinkyChain);
1124  mtr.w_pos += mtr.rotateVector(wVector((rightArmPinky10->height() + rightArmPinky14->height()) / 2.0, 0.0, 0.0));
1125  rightArmPinky14->setMatrix(mtr);
1126  rightHandGroup->addObject(rightArmPinky14, PinkyChain);
1127  mtr.w_pos += mtr.rotateVector(wVector((rightArmPinky14->height() + rightArmPinky18->height()) / 2.0, 0.0, 0.0));
1128  rightArmPinky18->setMatrix(mtr);
1129  rightHandGroup->addObject(rightArmPinky18, PinkyChain);
1130  mtr.w_pos += mtr.rotateVector(wVector((rightArmPinky18->height() + rightArmPinky22->height()) / 2.0, 0.0, 0.0));
1131  rightArmPinky22->setMatrix(mtr);
1132  rightHandGroup->addObject(rightArmPinky22, PinkyChain);
1133 
1134  // --- positioning of thumb
1135  mtr = rightArmPalm6->matrix().rotateAround(rightArmPalm6->matrix().z_ax, rightArmPalm6->matrix().w_pos, -PI_GRECO * 0.5f);
1136  mtr.w_pos += mtr.rotateVector(wVector(0.015 + rightArmThumb23->height() / 2.0, rightArmPalm6->sideY() / 2.0 - 0.035, rightArmPalm6->sideZ() / 2.0));
1137  rightArmThumb23->setMatrix(mtr);
1138  rightHandGroup->addObject(rightArmThumb23, ThumbChain);
1139  mtr.w_pos += mtr.rotateVector(wVector((rightArmThumb23->height() + rightArmThumb24->height()) / 2.0, 0.0, 0.0));
1140  rightArmThumb24->setMatrix(mtr);
1141  rightHandGroup->addObject(rightArmThumb24, ThumbChain);
1142  mtr.w_pos += mtr.rotateVector(wVector((rightArmThumb24->height() + rightArmThumb25->height()) / 2.0, 0.0, 0.0));
1143  rightArmThumb25->setMatrix(mtr);
1144  rightHandGroup->addObject(rightArmThumb25, ThumbChain);
1145  mtr.w_pos += mtr.rotateVector(wVector((rightArmThumb25->height() + rightArmThumb26->height()) / 2.0, 0.0, 0.0));
1146  rightArmThumb26->setMatrix(mtr);
1147  rightHandGroup->addObject(rightArmThumb26, ThumbChain);
1148 
1149  // Creating joints. Axis are inverted or not to have positive angles in the right direction
1150  // (perhaps I have to do this because we create the joint with the link n+1 as the parent
1151  // object of the joint instead of link n most of the time...)
1152  rightArmJointv.resize(0);
1153  PhyJoint* j1 = kinRightArm.createJoint(3);
1154  rightArmJointv << j1;
1155 
1156  PhyJoint* j2 = kinRightArm.createJoint(4);
1157  rightArmJointv << j2;
1158 
1159  PhyJoint* j3 = kinRightArm.createJoint(5);
1160  rightArmJointv << j3;
1161 
1162  PhyJoint* j4 = kinRightArm.createJoint(6);
1163  rightArmJointv << j4;
1164 
1165  PhyJoint* j5 = kinRightArm.createJoint(7);
1166  rightArmJointv << j5;
1167 
1168  PhyJoint* j6 = kinRightArm.createJoint(8);
1169  rightArmJointv << j6;
1170 
1171  PhyJoint* j7 = kinRightArm.createJoint(9);
1172  rightArmJointv << j7;
1173 
1174  // As above, for fingers we cannot rely on iKin
1175  PhyHinge* j8 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-rightArmIndex7->height() / 2.0, 0.0, 0.0), rightArmIndex7, rightArmPalm6);
1176  rightArmJointv << j8;
1177  rightHandGroup->addJointDOF(j8, 0, IndexChain);
1178 
1179  PhyHinge* j9 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-rightArmMiddle8->height() / 2.0, 0.0, 0.0), rightArmMiddle8, rightArmPalm6);
1180  rightArmJointv << j9;
1181  rightHandGroup->addJointDOF(j9, 0, MiddleChain);
1182 
1183  PhyHinge* j10 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-rightArmRing9->height() / 2.0, 0.0, 0.0), rightArmRing9, rightArmPalm6);
1184  rightArmJointv << j10;
1185  rightHandGroup->addJointDOF(j10, 0, RingChain);
1186 
1187  PhyHinge* j11 = new PhyHinge(wVector(0.0, 0.0, 1.0), wVector(-rightArmPinky10->height() / 2.0, 0.0, 0.0), rightArmPinky10, rightArmPalm6);
1188  rightArmJointv << j11;
1189  rightHandGroup->addJointDOF(j11, 0, PinkyChain);
1190 
1191  PhyHinge* j12 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmIndex11->height() / 2.0, 0.0, 0.0), rightArmIndex11, rightArmIndex7);
1192  rightArmJointv << j12;
1193  rightHandGroup->addJointDOF(j12, 0, IndexChain);
1194 
1195  PhyHinge* j13 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmIndex15->height() / 2.0, 0.0, 0.0), rightArmIndex15, rightArmIndex11);
1196  rightArmJointv << j13;
1197  rightHandGroup->addJointDOF(j13, 0, IndexChain);
1198 
1199  PhyHinge* j14 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmIndex19->height() / 2.0, 0.0, 0.0), rightArmIndex19, rightArmIndex15);
1200  rightArmJointv << j14;
1201  rightHandGroup->addJointDOF(j14, 0, IndexChain);
1202 
1203  PhyHinge* j15 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmMiddle12->height() / 2.0, 0.0, 0.0), rightArmMiddle12, rightArmMiddle8);
1204  rightArmJointv << j15;
1205  rightHandGroup->addJointDOF(j15, 0, MiddleChain);
1206 
1207  PhyHinge* j16 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmMiddle16->height() / 2.0, 0.0, 0.0), rightArmMiddle16, rightArmMiddle12);
1208  rightArmJointv << j16;
1209  rightHandGroup->addJointDOF(j16, 0, MiddleChain);
1210 
1211  PhyHinge* j17 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmMiddle20->height() / 2.0, 0.0, 0.0), rightArmMiddle20, rightArmMiddle16);
1212  rightArmJointv << j17;
1213  rightHandGroup->addJointDOF(j17, 0, MiddleChain);
1214 
1215  PhyHinge* j18 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmRing13->height() / 2.0, 0.0, 0.0), rightArmRing13, rightArmRing9);
1216  rightArmJointv << j18;
1217  rightHandGroup->addJointDOF(j18, 0, RingChain);
1218 
1219  PhyHinge* j19 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmRing17->height() / 2.0, 0.0, 0.0), rightArmRing17, rightArmRing13);
1220  rightArmJointv << j19;
1221  rightHandGroup->addJointDOF(j19, 0, RingChain);
1222 
1223  PhyHinge* j20 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmRing21->height() / 2.0, 0.0, 0.0), rightArmRing21, rightArmRing17);
1224  rightArmJointv << j20;
1225  rightHandGroup->addJointDOF(j20, 0, RingChain);
1226 
1227  PhyHinge* j21 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmPinky14->height() / 2.0, 0.0, 0.0), rightArmPinky14, rightArmPinky10);
1228  rightArmJointv << j21;
1229  rightHandGroup->addJointDOF(j21, 0, PinkyChain);
1230 
1231  PhyHinge* j22 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmPinky18->height() / 2.0, 0.0, 0.0), rightArmPinky18, rightArmPinky14);
1232  rightArmJointv << j22;
1233  rightHandGroup->addJointDOF(j22, 0, PinkyChain);
1234 
1235  PhyHinge* j23 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmPinky22->height() / 2.0, 0.0, 0.0), rightArmPinky22, rightArmPinky18);
1236  rightArmJointv << j23;
1237  rightHandGroup->addJointDOF(j23, 0, PinkyChain);
1238 
1239  PhyHinge* j24 = new PhyHinge(wVector(0.0, 1.0, 0.0), wVector(-rightArmThumb23->height() / 2.0, 0.0, 0.0), rightArmThumb23, rightArmPalm6);
1240  rightArmJointv << j24;
1241  rightHandGroup->addJointDOF(j24, 0, ThumbChain);
1242 
1243  PhyHinge* j25 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-rightArmThumb24->height() / 2.0, 0.0, 0.0), rightArmThumb24, rightArmThumb23);
1244  rightArmJointv << j25;
1245  rightHandGroup->addJointDOF(j25, 0, ThumbChain);
1246 
1247  PhyHinge* j26 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-rightArmThumb25->height() / 2.0, 0.0, 0.0), rightArmThumb25, rightArmThumb24);
1248  rightArmJointv << j26;
1249  rightHandGroup->addJointDOF(j26, 0, ThumbChain);
1250 
1251  PhyHinge* j27 = new PhyHinge(wVector(0.0, 0.0, -1.0), wVector(-rightArmThumb26->height() / 2.0, 0.0, 0.0), rightArmThumb26, rightArmThumb25);
1252  rightArmJointv << j27;
1253  rightHandGroup->addJointDOF(j27, 0, ThumbChain);
1254 
1255  //--- Coupling Right Hand joints (Opening)
1256  connect( rightArmJointv[7]->dofs()[0], SIGNAL( changedPosition( real ) ),
1257  rightArmJointv[9]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1258  connect( rightArmJointv[7]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1259  rightArmJointv[9]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1260  connect( rightArmJointv[7]->dofs()[0], SIGNAL( changedPosition( real ) ),
1261  rightArmJointv[10]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1262  connect( rightArmJointv[7]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1263  rightArmJointv[10]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1264  //--- Coupling (last two phalanges P2-P3 of fingers)
1265  connect( rightArmJointv[12]->dofs()[0], SIGNAL( changedPosition( real ) ),
1266  rightArmJointv[13]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1267  connect( rightArmJointv[12]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1268  rightArmJointv[13]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1269  connect( rightArmJointv[15]->dofs()[0], SIGNAL( changedPosition( real ) ),
1270  rightArmJointv[16]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1271  connect( rightArmJointv[15]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1272  rightArmJointv[16]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1273  connect( rightArmJointv[25]->dofs()[0], SIGNAL( changedPosition( real ) ),
1274  rightArmJointv[26]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1275  connect( rightArmJointv[25]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1276  rightArmJointv[26]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1277  //--- Coupling the ring-pinky phalanges
1278  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
1279  rightArmJointv[18]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1280  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1281  rightArmJointv[18]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1282  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
1283  rightArmJointv[19]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1284  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1285  rightArmJointv[19]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1286  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
1287  rightArmJointv[20]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1288  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1289  rightArmJointv[20]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1290  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
1291  rightArmJointv[21]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1292  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1293  rightArmJointv[21]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1294  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedPosition( real ) ),
1295  rightArmJointv[22]->dofs()[0], SLOT( setDesiredPosition( real ) ), Qt::DirectConnection );
1296  connect( rightArmJointv[17]->dofs()[0], SIGNAL( changedLimits( real, real ) ),
1297  rightArmJointv[22]->dofs()[0], SLOT( setLimits( real, real ) ), Qt::DirectConnection );
1298  }
1299 
1300  //---------------------------------------
1301  //--- HEAD-NECK creation: object & joints
1302  //---------------------------------------
1303  {
1304  // Creating objects
1305  PhyCylinder* headNeck0 = new PhyCylinder( 0.018f, 0.077f, world, name+":headNeck0" ); // Radius was 0.015
1306  headNeck0->setMass( 0.28252f );
1307  headNeckv << headNeck0;
1308 
1309  PhyCylinder* headNeck1 = new PhyCylinder( 0.015f, 0.077f, world, name+":headNeck1" );
1310  headNeck1->setMass( 0.1f );
1311  headNeckv << headNeck1;
1312 
1313  //--- creating a compound object composing the head-eyes
1314  const wMatrix y90 = wMatrix::yaw( PI_GRECO * 0.5f );
1315  QVector<PhyObject*> headObj;
1316  PhyCylinder* headNeck2a = new PhyCylinder( 0.015f,0.06f, world, name+":headNeck2a" );
1317  headNeck2a->setMatrix( y90 );
1318  headNeck2a->setMass( 0.13913f );
1319  headObj << headNeck2a;
1320  PhyBox* headNeck2b = new PhyBox( 0.052f,0.104f, 0.002f, world, name+":headNeck2b" );
1321  headNeck2b->setMass( 0.1562f );
1322  headObj << headNeck2b;
1323  PhyBox* headNeck2c = new PhyBox( 0.052f,0.002f, 0.093f, world, name+":headNeck2c" );
1324  headNeck2c->setMass( 0.1562f );
1325  headObj << headNeck2c;
1326  PhyBox* headNeck2d = new PhyBox( 0.052f,0.002f, 0.093f, world, name+":headNeck2d" );
1327  headNeck2d->setMass( 0.1562f );
1328  headObj << headNeck2d;
1329  PhyBox* headNeck2e = new PhyBox( 0.032f, 0.104f, 0.002f, world, name+":headNeck2e" );
1330  headNeck2e->setMass( 0.01f );
1331  headObj << headNeck2e;
1332  PhyBox* headNeck2f = new PhyBox( 0.025f, 0.011f, 0.026f, world, name+":headNeck2f" );
1333  headNeck2f->setMass( 0.0278f );
1334  headObj << headNeck2f;
1335  PhyBox* headNeck2g = new PhyBox( 0.012f, 0.011f, 0.051f, world, name+":headNeck2g" );
1336  headNeck2g->setMatrix( wMatrix::yaw( PI_GRECO*0.2f ) );
1337  headNeck2g->setMass( 0.0278f );
1338  headObj << headNeck2g;
1339  PhyBox* headNeck2h = new PhyBox( 0.012f, 0.02f, 0.022f, world, name+":headNeck2h" );
1340  headNeck2h->setMass( 0.0278f );
1341  headObj << headNeck2h;
1342  PhyCylinder* headNeck2i = new PhyCylinder( 0.006f,0.030f, world, name+":headNeck2i" );
1343  headNeck2i->setMass( 0.11f );
1344  headObj << headNeck2i;
1345  PhyCylinder* headNeck2j = new PhyCylinder( 0.006f,0.05f, world, name+":headNeck2j" );
1346  headNeck2j->setMatrix( y90 );
1347  headNeck2j->setMass( 0.0387f );
1348  headObj << headNeck2j;
1349  PhyCylinder* headNeck2l = new PhyCylinder( 0.006f,0.030f, world, name+":headNeck2l" );
1350  headNeck2l->setMass( 0.0234f );
1351  headObj << headNeck2l;
1352  PhyCylinder* headNeck2m = new PhyCylinder( 0.006f,0.05f, world, name+":headNeck2m" );
1353  headNeck2m->setMatrix( y90 );
1354  headNeck2m->setMass( 0.0387f );
1355  headObj << headNeck2m;
1356  headNeck2a->setPosition( wVector( 0.00, 0.0, 0.0 ) );
1357  headNeck2b->setPosition( wVector( -0.0125f, 0.0, -0.011f ) );
1358  headNeck2c->setPosition( wVector( -0.0125f, 0.052f, 0.0355f ) );
1359  headNeck2d->setPosition( wVector( -0.0125f, -0.052f, 0.0355f ) );
1360  headNeck2e->setPosition( wVector( -0.0125f, 0.0, 0.0355f ) );
1361  headNeck2f->setPosition( wVector( 0.0275f, 0.0, -0.01f ) );
1362  headNeck2g->setPosition( wVector( 0.05f, 0.0, 0.001f ) );
1363  headNeck2h->setPosition( wVector( 0.064f, 0.0, 0.028f ) );
1364  headNeck2i->setPosition( wVector( 0.049f, +0.034f, 0.052f ) ); // wVector(0.049, +0.034, 0.037) );
1365  headNeck2j->setPosition( wVector( 0.034f, +0.034f, 0.037f ) );
1366  headNeck2l->setPosition( wVector( 0.049f, -0.034f, 0.052f ) ); // wVector(0.049, -0.034, 0.037) );
1367  headNeck2m->setPosition( wVector( 0.034f, -0.034f, 0.037f ) );
1368  PhyCompoundObject* headNeck2 = new PhyCompoundObject( headObj, world, name+":headNeck2" );
1369  headNeckv << headNeck2;
1370 
1371  PhyCylinder* headNeckEye3 = new PhyCylinder( 0.002f,0.068f, world, name+":headNeckEye3" );
1372  headNeckEye3->setMass( 0.0059678f );
1373  headNeckv << headNeckEye3;
1374 
1375  PhySphere* headNeckEye4 = new PhySphere( 0.020f, world, name+":headNeckEye4" );
1376  headNeckEye4->setMass( 0.0234f );
1377  headNeckv << headNeckEye4;
1378 
1379  PhySphere* headNeckEye5 = new PhySphere( 0.020f, world, name+":headNeckEye5" );
1380  headNeckEye5->setMass( 0.0234f );
1381  headNeckv << headNeckEye5;
1382 
1383  // Setting rotation/position of the right arm parts. This is automatically done when
1384  // objects are associated with kinematic links, we only need to specify the transformation
1385  // of objects relative to the frame of reference of the corresponding kinematic link.
1386  // As the first four parts belong to the kinematic chains of both eyes, we associate them
1387  // with links only in the right eye chain (the other chain is always aligned)
1388  wMatrix mtr;
1389  wVector rotAx = wVector(1.0, 0.0, 0.0) * kinRightEye.getLinkInfo(3).getRotAxis();
1390  real rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1391  wVector transl = kinRightEye.getLinkInfo(3).getRotCenterWObject();
1392  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
1393  kinRightEye.getLinkInfo(3).setWObject(headNeck0, mtr);
1394 
1395  rotAx = wVector(1.0, 0.0, 0.0) * kinRightEye.getLinkInfo(4).getRotAxis();
1396  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1397  transl = kinRightEye.getLinkInfo(4).getRotCenterWObject();
1398  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
1399  kinRightEye.getLinkInfo(4).setWObject(headNeck1, mtr);
1400 
1401  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
1402  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1403  transl = kinRightEye.getLinkInfo(5).getRotCenterWObject() + wVector(0.0, -headNeck2a->height() / 2.0, 0.0);
1404  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl);
1405  // Now rotating by PI_GRECO around the rotation axis
1406  rotAx = wVector(0.0, 0.0, 1.0);
1407  rotAng = PI_GRECO;
1408  transl = wVector(0.0, 0.0, 0.0);
1409  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl) * mtr;
1410  kinRightEye.getLinkInfo(5).setWObject(headNeck2, mtr);
1411 
1412  rotAx = wVector(1.0, 0.0, 0.0) * kinRightEye.getLinkInfo(6).getRotAxis();
1413  rotAng = asin( ramp( -1.0, +1.0, rotAx.norm() ) );
1414  transl = kinRightEye.getLinkInfo(6).getRotCenterWObject();
1415  mtr = wMatrix(wQuaternion(rotAx, rotAng), transl); // rotation of -90° around y axis
1416  kinRightEye.getLinkInfo(6).setWObject(headNeckEye3, mtr);
1417 
1418  mtr = wMatrix::roll(PI_GRECO / 2.0f);
1419  kinRightEye.getLinkInfo(7).setWObject(headNeckEye4, mtr);
1420 
1421  mtr = wMatrix::roll(PI_GRECO / 2.0f);
1422  kinLeftEye.getLinkInfo(7).setWObject(headNeckEye5, mtr);
1423 
1424  //--- Eyes Cameras
1425  leftcam = NULL; //new WCamera( world, name+"/cam/left", headNeck()[7], 200, 200 );
1426  rightcam = NULL; //new WCamera( world, name+"/cam/right", headNeck()[5], 200, 200 );
1427 
1428  // Creating joints. Axis are inverted or not to have positive angles in the right direction
1429  // (perhaps I have to do this because we create the joint with the link n+1 as the parent
1430  // object of the joint instead of link n most of the time...)
1431  headNeckJointv.resize(0);
1432  PhyJoint* j1 = kinRightEye.createJoint(3);
1433  headNeckJointv << j1;
1434 
1435  PhyJoint* j2 = kinRightEye.createJoint(4, false);
1436  headNeckJointv << j2;
1437 
1438  PhyJoint* j3 = kinRightEye.createJoint(5);
1439  headNeckJointv << j3;
1440 
1441  PhyJoint* j4 = kinRightEye.createJoint(6);
1442  headNeckJointv << j4;
1443 
1444  PhyJoint* j5 = kinRightEye.createJoint(7);
1445  headNeckJointv << j5;
1446  rightEyeDOF = j5->dofs()[0];
1447 
1448  PhyJoint* j6 = kinLeftEye.createJoint(7);
1449  headNeckJointv << j6;
1450  leftEyeDOF = j6->dofs()[0];
1451 
1452  // Creating the virtual DOF which we use to control eye vergence and version. These haven't got valid axes
1453  versionDOF = new PhyDOF(NULL, wVector(0.0, 0.0, 0.0), wVector(0.0, 0.0, 0.0), false);
1454  vergenceDOF = new PhyDOF(NULL, wVector(0.0, 0.0, 0.0), wVector(0.0, 0.0, 0.0), false);
1455 
1456  // Connecting virtual and real dofs to our slots to synchronize them
1457  connect(versionDOF, SIGNAL(changedDesiredPosition(real)), this, SLOT(versionChangedDesiredPosition(real)));
1458  connect(versionDOF, SIGNAL(changedDesiredVelocity(real)), this, SLOT(versionChangedDesiredVelocity(real)));
1459  connect(versionDOF, SIGNAL(changedPosition(real)), this, SLOT(versionChangedPosition(real)));
1460  connect(versionDOF, SIGNAL(changedVelocity(real)), this, SLOT(versionChangedVelocity(real)));
1461  connect(versionDOF, SIGNAL(changedLimits(real, real)), this, SLOT(versionChangedLimits(real, real)));
1462 
1463  connect(vergenceDOF, SIGNAL(changedDesiredPosition(real)), this, SLOT(vergenceChangedDesiredPosition(real)));
1464  connect(vergenceDOF, SIGNAL(changedDesiredVelocity(real)), this, SLOT(vergenceChangedDesiredVelocity(real)));
1465  connect(vergenceDOF, SIGNAL(changedPosition(real)), this, SLOT(vergenceChangedPosition(real)));
1466  connect(vergenceDOF, SIGNAL(changedVelocity(real)), this, SLOT(vergenceChangedVelocity(real)));
1467  connect(vergenceDOF, SIGNAL(changedLimits(real, real)), this, SLOT(vergenceChangedLimits(real, real)));
1468 
1469  connect(rightEyeDOF, SIGNAL(changedDesiredPosition(real)), this, SLOT(rightEyeChangedDesiredPosition(real)));
1470  connect(rightEyeDOF, SIGNAL(changedDesiredVelocity(real)), this, SLOT(rightEyeChangedDesiredVelocity(real)));
1471  connect(rightEyeDOF, SIGNAL(changedPosition(real)), this, SLOT(rightEyeChangedPosition(real)));
1472  connect(rightEyeDOF, SIGNAL(changedVelocity(real)), this, SLOT(rightEyeChangedVelocity(real)));
1473  connect(rightEyeDOF, SIGNAL(changedLimits(real, real)), this, SLOT(rightEyeChangedLimits(real, real)));
1474 
1475  connect(leftEyeDOF, SIGNAL(changedDesiredPosition(real)), this, SLOT(leftEyeChangedDesiredPosition(real)));
1476  connect(leftEyeDOF, SIGNAL(changedDesiredVelocity(real)), this, SLOT(leftEyeChangedDesiredVelocity(real)));
1477  connect(leftEyeDOF, SIGNAL(changedPosition(real)), this, SLOT(leftEyeChangedPosition(real)));
1478  connect(leftEyeDOF, SIGNAL(changedVelocity(real)), this, SLOT(leftEyeChangedVelocity(real)));
1479  connect(leftEyeDOF, SIGNAL(changedLimits(real, real)), this, SLOT(leftEyeChangedLimits(real, real)));
1480  }
1481 
1482  // Now that we have created all pieces and joints, we can re-enable limits
1483  kinRightArm.setAllConstraints(true);
1484  kinLeftArm.setAllConstraints(true);
1485  kinRightLeg.setAllConstraints(true);
1486  kinLeftLeg.setAllConstraints(true);
1487  kinRightEye.setAllConstraints(true);
1488  kinLeftEye.setAllConstraints(true);
1489 
1490  //--- create an iCubMaterial and disable all contact among iCub parts
1491  world->materials().createMaterial( "iCubMaterial" );
1492  world->materials().enableCollision( "iCubMaterial", "iCubMaterial", false );
1493  world->materials().setGravityForce( "iCubMaterial", 0 );
1494  foreach( PhyObject* obj, leftLegv ) {
1495  obj->setMaterial( "iCubMaterial" );
1496  }
1497  foreach( PhyObject* obj, rightLegv ) {
1498  obj->setMaterial( "iCubMaterial" );
1499  }
1500  foreach( PhyObject* obj, torsov ) {
1501  obj->setMaterial( "iCubMaterial" );
1502  }
1503  foreach( PhyObject* obj, leftArmv ) {
1504  obj->setMaterial( "iCubMaterial" );
1505  }
1506  foreach( PhyObject* obj, rightArmv ) {
1507  obj->setMaterial( "iCubMaterial" );
1508  }
1509  foreach( PhyObject* obj, headNeckv ) {
1510  obj->setMaterial( "iCubMaterial" );
1511  }
1512 
1513  //---------------------------------------
1514  //--- MOTOR CONTROLLERS creation
1515  //---------------------------------------
1516 
1517  // Getting joint limits. The robot whose configuration is loaded
1518  // is the one indicated by the environmental variable ICUB_ROBOTNAME
1519  ResourceFinder rf;
1520  rf.configure( "ICUB_ROOT", 0, NULL );
1521  rf.setVerbose( false );
1522  // base path template for getting default iCub files included into FARSA
1523 #ifdef FARSA_WIN
1524  QString phyiCubConfTmpl = qApp->applicationDirPath() + "/../conf/worldsim/phyicub/%1";
1525 #else
1526  QString phyiCubConfTmpl = qApp->applicationDirPath() + "/../share/FARSA/conf/worldsim/phyicub/%1";
1527 #endif
1528  // Cleaning path and using system separators for directories
1529  phyiCubConfTmpl = QDir::toNativeSeparators(QDir::cleanPath(phyiCubConfTmpl));
1530  Property prop;
1531  if ( QFile::exists(rf.findFile("icub_head_torso.ini").c_str()) ) {
1532  prop.fromConfigFile( rf.findFile("icub_head_torso.ini"), true );
1533  } else {
1534  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_head_torso.ini").toAscii().data() );
1535  }
1536  Bottle lstM = prop.findGroup("LIMITS").findGroup("Max");
1537  Bottle lstm = prop.findGroup("LIMITS").findGroup("Min");
1538  QVector<double> headMax(6), headMin(6), torsoMax(3), torsoMin(3);
1539  for( int i=0; i<6; i++ ) {
1540  headMax[i] = lstM.get(i+1).asDouble();
1541  headMin[i] = lstm.get(i+1).asDouble();
1542  }
1543  for( int i=6; i<9; i++ ) {
1544  torsoMax[i-6] = lstM.get(i+1).asDouble();
1545  torsoMin[i-6] = lstm.get(i+1).asDouble();
1546  }
1547  if ( QFile::exists(rf.findFile("icub_left_leg.ini").c_str()) ) {
1548  prop.fromConfigFile( rf.findFile("icub_left_leg.ini"), true );
1549  } else {
1550  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_leg.ini").toAscii().data() );
1551  }
1552  lstM = prop.findGroup("LIMITS").findGroup("Max");
1553  lstm = prop.findGroup("LIMITS").findGroup("Min");
1554  QVector<double> leftLegMax(6), leftLegMin(6);
1555  for( int i=0; i<6; i++ ) {
1556  leftLegMax[i] = lstM.get(i+1).asDouble();
1557  leftLegMin[i] = lstm.get(i+1).asDouble();
1558  }
1559  if ( QFile::exists(rf.findFile("icub_right_leg.ini").c_str()) ) {
1560  prop.fromConfigFile( rf.findFile("icub_right_leg.ini"), true );
1561  } else {
1562  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_leg.ini").toAscii().data() );
1563  }
1564  lstM = prop.findGroup("LIMITS").findGroup("Max");
1565  lstm = prop.findGroup("LIMITS").findGroup("Min");
1566  QVector<double> rightLegMax(6), rightLegMin(6);
1567  for( int i=0; i<6; i++ ) {
1568  rightLegMax[i] = lstM.get(i+1).asDouble();
1569  rightLegMin[i] = lstm.get(i+1).asDouble();
1570  }
1571  if ( QFile::exists(rf.findFile("icub_left_arm.ini").c_str()) ) {
1572  prop.fromConfigFile( rf.findFile("icub_left_arm.ini"), true );
1573  } else {
1574  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_arm.ini").toAscii().data() );
1575  }
1576  lstM = prop.findGroup("LIMITS").findGroup("Max");
1577  lstm = prop.findGroup("LIMITS").findGroup("Min");
1578  QVector<double> leftArmMax(16), leftArmMin(16);
1579  //--- first 8 are in icub_left_arm.ini,
1580  // second 8 in icub_left_hand.ini
1581  for( int i=0; i<8; i++ ) {
1582  leftArmMax[i] = lstM.get(i+1).asDouble();
1583  leftArmMin[i] = lstm.get(i+1).asDouble();
1584  }
1585  if ( QFile::exists(rf.findFile("icub_left_hand.ini").c_str()) ) {
1586  prop.fromConfigFile( rf.findFile("icub_left_hand.ini"), true );
1587  } else {
1588  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_hand.ini").toAscii().data() );
1589  }
1590  lstM = prop.findGroup("LIMITS").findGroup("Max");
1591  lstm = prop.findGroup("LIMITS").findGroup("Min");
1592  for( int i=0; i<8; i++ ) {
1593  leftArmMax[8+i] = lstM.get(i+1).asDouble();
1594  leftArmMin[8+i] = lstm.get(i+1).asDouble();
1595  }
1596  if ( QFile::exists(rf.findFile("icub_right_arm.ini").c_str()) ) {
1597  prop.fromConfigFile( rf.findFile("icub_right_arm.ini"), true );
1598  } else {
1599  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_arm.ini").toAscii().data() );
1600  }
1601  lstM = prop.findGroup("LIMITS").findGroup("Max");
1602  lstm = prop.findGroup("LIMITS").findGroup("Min");
1603  QVector<double> rightArmMax(16), rightArmMin(16);
1604  //--- first 8 are in icub_right_arm.ini,
1605  // second 8 in icub_right_hand.ini
1606  for( int i=0; i<8; i++ ) {
1607  rightArmMax[i] = lstM.get(i+1).asDouble();
1608  rightArmMin[i] = lstm.get(i+1).asDouble();
1609  }
1610  if ( QFile::exists(rf.findFile("icub_right_hand.ini").c_str()) ) {
1611  prop.fromConfigFile( rf.findFile("icub_right_hand.ini"), true );
1612  } else {
1613  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_hand.ini").toAscii().data() );
1614  }
1615  lstM = prop.findGroup("LIMITS").findGroup("Max");
1616  lstm = prop.findGroup("LIMITS").findGroup("Min");
1617  for( int i=0; i<8; i++ ) {
1618  rightArmMax[8+i] = lstM.get(i+1).asDouble();
1619  rightArmMin[8+i] = lstm.get(i+1).asDouble();
1620  }
1621 
1622  // Here we set limits both for the motor controllers and the kinematic chain
1623  QVector<PhyDOF*> motors;
1624  //--- number of controlled DOF of joints
1625  int counter = 0;
1626  //--- create MotorController for TORSO
1627  motors.clear();
1628  for( int i=0; i<torsoJointv.size(); i++ ) {
1629  motors << torsoJointv[i]->dofs()[0];
1630  motors[i]->setLimits( toRad(torsoMin[i]), toRad(torsoMax[i]) );
1631  motors[i]->enableLimits();
1632 
1633  // Setting limits for iKin chain. We don't use "i" because joint in the
1634  // torso are in inverse order (I really can't figure out why...)
1635  kinRightArm.getLinkInfo(torsoJointv.size() - i - 1).setiKinLinkLimits(toRad(torsoMin[i]), toRad(torsoMax[i]));
1636  }
1637  torsoCtrl = new MultiMotorController( motors, world );
1638  torsoCtrl->setOwner(this, false);
1639  if ( enabledServerControlBoards ) {
1640  registerServerControlBoard( torsoCtrl, "torso" );
1641  }
1642  counter += motors.size();
1643 
1644  //--- create MotorController for LEFT ARM
1645  motors.clear();
1646  motors << leftArmJointv[0]->dofs()[0];
1647  motors << leftArmJointv[1]->dofs()[0];
1648  motors << leftArmJointv[2]->dofs()[0];
1649  motors << leftArmJointv[3]->dofs()[0];
1650  motors << leftArmJointv[4]->dofs()[0];
1651  motors << leftArmJointv[5]->dofs()[0];
1652  motors << leftArmJointv[6]->dofs()[0];
1653  motors << leftArmJointv[7]->dofs()[0];
1654  motors << leftArmJointv[23]->dofs()[0];
1655  motors << leftArmJointv[24]->dofs()[0];
1656  motors << leftArmJointv[25]->dofs()[0];
1657  motors << leftArmJointv[11]->dofs()[0];
1658  motors << leftArmJointv[12]->dofs()[0];
1659  motors << leftArmJointv[14]->dofs()[0];
1660  motors << leftArmJointv[15]->dofs()[0];
1661  motors << leftArmJointv[17]->dofs()[0];
1662  leftArmCtrl = new MultiMotorController( motors, world );
1663  //--- setting limits
1664  for( unsigned int i=0; i<16; i++ ) {
1665  if ( i<7 ) {
1666  //--- only for the first seven joints the raw limits correspond to public limits
1667  leftArmCtrl->setLimitsRaw( i, leftArmMin[i], leftArmMax[i] );
1668  }
1669  leftArmCtrl->setLimits( i, leftArmMin[i], leftArmMax[i] );
1670  leftArmCtrl->setEnableLimitsRaw( i, true );
1671  // Setting limits for iKin chain
1672  if ((i + 3) < kinLeftArm.getN()) {
1673  kinLeftArm.getLinkInfo(i + 3).setiKinLinkLimits(toRad(leftArmMin[i]), toRad(leftArmMax[i]));
1674  }
1675  }
1676  //--- the joints of the hand has different angular movement respect from what reported by robotMotorGUI
1677  // This mismatch is implemented using setLimitsRaw of MultiMotorController that will change the
1678  // real movement of the joint, but will not change what reported on the robotMotorGUI
1679  leftArmCtrl->setLimitsRaw( 7, -15, 0 );
1680  leftArmCtrl->setLimitsRaw( 8, 10, 90 );
1681  leftArmCtrl->setLimitsRaw( 9, 0, 90 );
1682  leftArmCtrl->setLimitsRaw( 10, 0, 90 );
1683  leftArmCtrl->setLimitsRaw( 11, 0, 90 );
1684  leftArmCtrl->setLimitsRaw( 12, 0, 90 );
1685  leftArmCtrl->setLimitsRaw( 13, 0, 90 );
1686  leftArmCtrl->setLimitsRaw( 14, 0, 90 );
1687  leftArmCtrl->setLimitsRaw( 15, 0, 90 );
1688  leftArmCtrl->setOwner(this, false);
1689  if ( enabledServerControlBoards ) {
1690  registerServerControlBoard( leftArmCtrl, "left_arm" );
1691  }
1692  counter += motors.size();
1693 
1694  //--- create MotorController for RIGHT ARM
1695  motors.clear();
1696  motors << rightArmJointv[0]->dofs()[0];
1697  motors << rightArmJointv[1]->dofs()[0];
1698  motors << rightArmJointv[2]->dofs()[0];
1699  motors << rightArmJointv[3]->dofs()[0];
1700  motors << rightArmJointv[4]->dofs()[0];
1701  motors << rightArmJointv[5]->dofs()[0];
1702  motors << rightArmJointv[6]->dofs()[0];
1703  motors << rightArmJointv[7]->dofs()[0];
1704  motors << rightArmJointv[23]->dofs()[0];
1705  motors << rightArmJointv[24]->dofs()[0];
1706  motors << rightArmJointv[25]->dofs()[0];
1707  motors << rightArmJointv[11]->dofs()[0];
1708  motors << rightArmJointv[12]->dofs()[0];
1709  motors << rightArmJointv[14]->dofs()[0];
1710  motors << rightArmJointv[15]->dofs()[0];
1711  motors << rightArmJointv[17]->dofs()[0];
1712  rightArmCtrl = new MultiMotorController( motors, world );
1713  //--- setting limits
1714  for( unsigned int i=0; i<16; i++ ) {
1715  if ( i<7 ) {
1716  //--- only for the first seven joints the raw limits correspond to public limits
1717  rightArmCtrl->setLimitsRaw( i, rightArmMin[i], rightArmMax[i] );
1718  }
1719  rightArmCtrl->setLimits( i, rightArmMin[i], rightArmMax[i] );
1720  rightArmCtrl->setEnableLimitsRaw( i, true );
1721  // Setting limits for iKin chain
1722  if ((i + 3) < kinRightArm.getN()) {
1723  kinRightArm.getLinkInfo(i + 3).setiKinLinkLimits(toRad(rightArmMin[i]), toRad(rightArmMax[i]));
1724  }
1725  }
1726  //--- the joints of the hand has different angular movement respect from what reported by robotMotorGUI
1727  // This mismatch is implemented using setLimitsRaw of MultiMotorController that will change the
1728  // real movement of the joint, but will not change what reported on the robotMotorGUI
1729  rightArmCtrl->setLimitsRaw( 7, -15, 0 );
1730  rightArmCtrl->setLimitsRaw( 8, 10, 90 );
1731  rightArmCtrl->setLimitsRaw( 9, 0, 90 );
1732  rightArmCtrl->setLimitsRaw( 10, 0, 90 );
1733  rightArmCtrl->setLimitsRaw( 11, 0, 90 );
1734  rightArmCtrl->setLimitsRaw( 12, 0, 90 );
1735  rightArmCtrl->setLimitsRaw( 13, 0, 90 );
1736  rightArmCtrl->setLimitsRaw( 14, 0, 90 );
1737  rightArmCtrl->setLimitsRaw( 15, 0, 90 );
1738  rightArmCtrl->setOwner(this, false);
1739  if ( enabledServerControlBoards ) {
1740  registerServerControlBoard( rightArmCtrl, "right_arm" );
1741  }
1742  counter += motors.size();
1743 
1744  //--- create MotorController for HEAD-NECK
1745  motors.clear();
1746  motors << headNeckJointv[0]->dofs()[0];
1747  motors << headNeckJointv[1]->dofs()[0];
1748  motors << headNeckJointv[2]->dofs()[0];
1749  motors << headNeckJointv[3]->dofs()[0];
1750  motors << versionDOF;
1751  motors << vergenceDOF;
1752  for( int i=0; i<6; i++ ) {
1753  motors[i]->setLimits( toRad(headMin[i]), toRad(headMax[i]) );
1754  motors[i]->enableLimits();
1755 
1756  if ((i + 3) < 7) {
1757  kinRightEye.getLinkInfo(i + 3).setiKinLinkLimits(toRad(headMin[i]), toRad(headMax[i]));
1758  }
1759  }
1760  // Setting limits on the last joint of right and left eye kinematic chain. They are computed from the limits
1761  // for version and vergence (4 is the limit for version, 5 for vergence)
1762  const real minEye = max(headMin[4] + headMin[5], -179.99999);
1763  const real maxEye = min(headMax[4] + headMax[5], +179.99999);
1764  headNeckJointv[4]->dofs()[0]->setLimits( toRad(minEye), toRad(maxEye) );
1765  headNeckJointv[4]->dofs()[0]->enableLimits();
1766  headNeckJointv[5]->dofs()[0]->setLimits( toRad(minEye), toRad(maxEye) );
1767  headNeckJointv[5]->dofs()[0]->enableLimits();
1768  kinRightEye.getLinkInfo(7).setiKinLinkLimits(toRad(minEye), toRad(maxEye));
1769  kinLeftEye.getLinkInfo(7).setiKinLinkLimits(toRad(minEye), toRad(maxEye));
1770  // Creating controller for head
1771  headNeckCtrl = new MultiMotorController( motors, world );
1772  headNeckCtrl->setOwner(this, false);
1773  if ( enabledServerControlBoards ) {
1774  registerServerControlBoard( headNeckCtrl, "head" );
1775  }
1776  counter += motors.size();
1777 
1778  //--- create MotorController for LEFT LEG
1779  motors.clear();
1780  for( int i=0; i<leftLegJointv.size(); i++ ) {
1781  motors << leftLegJointv[i]->dofs()[0];
1782  motors[i]->setLimits( toRad(leftLegMin[i]), toRad(leftLegMax[i]) );
1783  motors[i]->enableLimits();
1784 
1785  // Setting limits for iKin chain
1786  kinLeftLeg.getLinkInfo(i).setiKinLinkLimits(toRad(leftLegMin[i]), toRad(leftLegMax[i]));
1787  }
1788  leftLegCtrl = new MultiMotorController( motors, world );
1789  leftLegCtrl->setOwner(this, false);
1790  if ( enabledServerControlBoards ) {
1791  registerServerControlBoard( leftLegCtrl, "left_leg" );
1792  }
1793  counter += motors.size();
1794 
1795  //--- create MotorController for RIGHT LEG
1796  motors.clear();
1797  for( int i=0; i<rightLegJointv.size(); i++ ) {
1798  motors << rightLegJointv[i]->dofs()[0];
1799  motors[i]->setLimits( toRad(rightLegMin[i]), toRad(rightLegMax[i]) );
1800  motors[i]->enableLimits();
1801 
1802  // Setting limits for iKin chain
1803  kinRightLeg.getLinkInfo(i).setiKinLinkLimits(toRad(rightLegMin[i]), toRad(rightLegMax[i]));
1804  }
1805  rightLegCtrl = new MultiMotorController( motors, world );
1806  rightLegCtrl->setOwner(this, false);
1807  if ( enabledServerControlBoards ) {
1808  registerServerControlBoard( rightLegCtrl, "right_leg" );
1809  }
1810  counter += motors.size();
1811 
1812  //--- Creating the COVERS
1813  wMatrix matrix = wMatrix::yaw( PI_GRECO * 0.5f ) * wMatrix::pitch( PI_GRECO*0.5f );
1814  // Previous vector values: wVector(0.0, 0.034 /*0.037*/, 0.0)
1815  matrix.w_pos = matrix.rotateVector( wVector(0.0, 0.05f, -0.013f) );
1816  WMesh* mesh = new WMesh( world, name+":faceCover", matrix );
1817  mesh->loadMS3DModel( ":/covers/head.ms3d" );
1818  mesh->setTexture( "icubFace" );
1819  mesh->attachTo( headNeckv[2] );
1820  mesh->setOwner(this, false);
1821  coversv << mesh;
1822 
1823  //--- counting the number of effective DOF of joints created
1824  int counter2= 0;
1825  foreach( PhyJoint* joint, leftLegJointv ) {
1826  counter2 += joint->numDofs();
1827  joint->setOwner(this, false);
1828  }
1829  foreach( PhyJoint* joint, rightLegJointv ) {
1830  counter2 += joint->numDofs();
1831  joint->setOwner(this, false);
1832  }
1833  foreach( PhyJoint* joint, torsoJointv ) {
1834  counter2 += joint->numDofs();
1835  joint->setOwner(this, false);
1836  }
1837  foreach( PhyJoint* joint, leftArmJointv ) {
1838  counter2 += joint->numDofs();
1839  joint->setOwner(this, false);
1840  }
1841  foreach( PhyJoint* joint, rightArmJointv ) {
1842  counter2 += joint->numDofs();
1843  joint->setOwner(this, false);
1844  }
1845  foreach( PhyJoint* joint, headNeckJointv ) {
1846  counter2 += joint->numDofs();
1847  joint->setOwner(this, false);
1848  }
1849  //qDebug( "Total Joints: %d - %d", counter, counter2 );
1850 
1851  //--- calculate the total mass of the robot
1852  //--- and setting up masses vectors
1853  //--- This is required for enable/disable operations
1854  //--- Also Setting the Texture
1855  real mass = 0.0;
1856  leftLegMasses.resize(0);
1857  foreach( PhyObject* obj, leftLegv ) {
1858  mass += obj->mass();
1859  leftLegMasses << obj->mass();
1860  obj->setTexture( "icub" );
1861  obj->setOwner(this, false);
1862  }
1863  rightLegMasses.resize(0);
1864  foreach( PhyObject* obj, rightLegv ) {
1865  mass += obj->mass();
1866  rightLegMasses << obj->mass();
1867  obj->setTexture( "icub" );
1868  obj->setOwner(this, false);
1869  }
1870  torsoMasses.resize(0);
1871  foreach( PhyObject* obj, torsov ) {
1872  mass += obj->mass();
1873  torsoMasses << obj->mass();
1874  obj->setTexture( "icub" );
1875  obj->setOwner(this, false);
1876  }
1877  leftArmMasses.resize(0);
1878  foreach( PhyObject* obj, leftArmv ) {
1879  mass += obj->mass();
1880  leftArmMasses << obj->mass();
1881  obj->setTexture( "icub" );
1882  obj->setOwner(this, false);
1883  }
1884  rightArmMasses.resize(0);
1885  foreach( PhyObject* obj, rightArmv ) {
1886  mass += obj->mass();
1887  rightArmMasses << obj->mass();
1888  obj->setTexture( "icub" );
1889  obj->setOwner(this, false);
1890  }
1891  headNeckMasses.resize(0);
1892  foreach( PhyObject* obj, headNeckv ) {
1893  mass += obj->mass();
1894  headNeckMasses << obj->mass();
1895  obj->setTexture( "icub" );
1896  obj->setOwner(this, false);
1897  }
1898  headNeckv[4]->setTexture( "blueye" );
1899  headNeckv[5]->setTexture( "blueye" );
1900  //qDebug( "Total Mass: %g", mass );
1901 
1902  setTexture( "icub" );
1903  world->pushObject( this );
1904  }
1905 
1906  PhyiCub::~PhyiCub() {
1907  if ( cartCtrlLeftArm ) {
1908  //delete cartCtrlLeftArm;
1909  delete cartServLeftArm;
1910  delete cartSolvLeftArm;
1911  }
1912  if ( cartCtrlRightArm ) {
1913  //delete cartCtrlRightArm;
1914  delete cartServRightArm;
1915  delete cartSolvRightArm;
1916  }
1917  if ( enabledServerControlBoards ) {
1918  //--- stops the serverControlBoards
1919  QStringList servers;
1920  servers << "torso" << "left_arm" << "right_arm" << "head" << "left_leg" << "right_leg";
1921  foreach( QString srv, servers ) {
1922  //--- this call also delete all MultiMotorControllers attached to ServerControlBoards
1923  removeServerControlBoard( srv );
1924  }
1925  } else {
1926  //--- when there aren't ServerControlBoards enabled, it is necessary to destroy MultiMotorControllers
1927  delete torsoCtrl;
1928  delete leftArmCtrl;
1929  delete rightArmCtrl;
1930  delete headNeckCtrl;
1931  delete leftLegCtrl;
1932  delete rightLegCtrl;
1933  }
1934  if ( !leftcam ) {
1935  delete leftcam;
1936  delete rightcam;
1937  }
1938  for( int i=0; i<coversv.size(); i++ ) {
1939  delete (coversv[i]);
1940  }
1941  foreach( PhyJoint* joint, leftLegJointv ) {
1942  delete joint;
1943  }
1944  foreach( PhyJoint* joint, rightLegJointv ) {
1945  delete joint;
1946  }
1947  foreach( PhyJoint* joint, torsoJointv ) {
1948  delete joint;
1949  }
1950  foreach( PhyJoint* joint, leftArmJointv ) {
1951  delete joint;
1952  }
1953  foreach( PhyJoint* joint, rightArmJointv ) {
1954  delete joint;
1955  }
1956  foreach( PhyJoint* joint, headNeckJointv ) {
1957  delete joint;
1958  }
1959  foreach( PhyObject* obj, leftLegv ) {
1960  delete obj;
1961  }
1962  foreach( PhyObject* obj, rightLegv ) {
1963  delete obj;
1964  }
1965  foreach( PhyObject* obj, torsov ) {
1966  delete obj;
1967  }
1968  foreach( PhyObject* obj, leftArmv ) {
1969  delete obj;
1970  }
1971  foreach( PhyObject* obj, rightArmv ) {
1972  delete obj;
1973  }
1974  foreach( PhyObject* obj, headNeckv ) {
1975  delete obj;
1976  }
1977  delete leftHandGroup;
1978  delete rightHandGroup;
1979  delete vergenceDOF;
1980  delete versionDOF;
1981  }
1982 
1983  void PhyiCub::doKinematicSimulation(bool k, bool clh, bool crh)
1984  {
1985  kinematicSimulation = k;
1986  collidingLeftHand = clh;
1987  collidingRightHand = crh;
1988 
1989  if (kinematicSimulation) {
1990  // First disabling all joints...
1991  for (int i = 0; i < leftLegJointv.size(); i++) {
1992  leftLegJointv[i]->enable(false);
1993  }
1994  for (int i = 0; i < rightLegJointv.size(); i++) {
1995  rightLegJointv[i]->enable(false);
1996  }
1997  for (int i = 0; i < torsoJointv.size(); i++) {
1998  torsoJointv[i]->enable(false);
1999  }
2000  for (int i = 0; i < leftArmJointv.size(); i++) {
2001  if (collidingLeftHand && (i >= 6)) {
2002  leftArmJointv[i]->enable(true);
2003  } else {
2004  leftArmJointv[i]->enable(false);
2005  }
2006  }
2007  for (int i = 0; i < rightArmJointv.size(); i++) {
2008  if (collidingRightHand && (i >= 6)) {
2009  rightArmJointv[i]->enable(true);
2010  } else {
2011  rightArmJointv[i]->enable(false);
2012  }
2013  }
2014  for (int i = 0; i < headNeckJointv.size(); i++) {
2015  headNeckJointv[i]->enable(false);
2016  }
2017 
2018  // ... then setting all objects to kinematic behaviour
2019  for (int i = 0; i < leftLegv.size(); i++) {
2020  leftLegv[i]->setKinematic(true);
2021  }
2022  for (int i = 0; i < rightLegv.size(); i++) {
2023  rightLegv[i]->setKinematic(true);
2024  }
2025  for (int i = 0; i < torsov.size(); i++) {
2026  torsov[i]->setKinematic(true);
2027  }
2028  for (int i = 0; i < leftArmv.size(); i++) {
2029  if (collidingLeftHand) {
2030  if (i < 5) {
2031  leftArmv[i]->setKinematic(true, false);
2032  } else if (i == 5) {
2033  leftArmv[i]->setKinematic(true, true);
2034  } else {
2035  leftArmv[i]->setKinematic(false);
2036  }
2037  } else {
2038  leftArmv[i]->setKinematic(true, false);
2039  }
2040  }
2041  for (int i = 0; i < rightArmv.size(); i++) {
2042  if (collidingRightHand) {
2043  if (i < 5) {
2044  rightArmv[i]->setKinematic(true, false);
2045  } else if (i == 5) {
2046  rightArmv[i]->setKinematic(true, true);
2047  } else {
2048  rightArmv[i]->setKinematic(false);
2049  }
2050  } else {
2051  rightArmv[i]->setKinematic(true, false);
2052  }
2053  }
2054  for (int i = 0; i < headNeckv.size(); i++) {
2055  headNeckv[i]->setKinematic(true);
2056  }
2057  } else {
2058  // First setting all objects to dynamic behaviour...
2059  for (int i = 0; i < leftLegv.size(); i++) {
2060  leftLegv[i]->setKinematic( !enabledLeftLeg );
2061  }
2062  for (int i = 0; i < rightLegv.size(); i++) {
2063  rightLegv[i]->setKinematic( !enabledRightLeg );
2064  }
2065  for (int i = 0; i < torsov.size(); i++) {
2066  torsov[i]->setKinematic( !enabledTorso );
2067  }
2068  for (int i = 0; i < leftArmv.size(); i++) {
2069  leftArmv[i]->setKinematic( !enabledLeftArm );
2070  }
2071  for (int i = 0; i < rightArmv.size(); i++) {
2072  rightArmv[i]->setKinematic( !enabledRightArm );
2073  }
2074  for (int i = 0; i < headNeckv.size(); i++) {
2075  headNeckv[i]->setKinematic( !enabledHead );
2076  }
2077 
2078  // ... then enabling all joints (if the corresponding part is enabled)
2079  for (int i = 0; i < leftLegJointv.size(); i++) {
2080  leftLegJointv[i]->enable(enabledLeftLeg);
2081  if (enabledLeftLeg) {
2082  leftLegJointv[i]->updateJointInfo();
2083  }
2084  }
2085  for (int i = 0; i < rightLegJointv.size(); i++) {
2086  rightLegJointv[i]->enable(enabledRightLeg);
2087  if (enabledRightLeg) {
2088  rightLegJointv[i]->updateJointInfo();
2089  }
2090  }
2091  for (int i = 0; i < torsoJointv.size(); i++) {
2092  torsoJointv[i]->enable(enabledTorso);
2093  if (enabledTorso) {
2094  torsoJointv[i]->updateJointInfo();
2095  }
2096  }
2097  for (int i = 0; i < leftArmJointv.size(); i++) {
2098  leftArmJointv[i]->enable(enabledLeftArm);
2099  if (enabledLeftArm) {
2100  leftArmJointv[i]->updateJointInfo();
2101  }
2102  }
2103  for (int i = 0; i < rightArmJointv.size(); i++) {
2104  rightArmJointv[i]->enable(enabledRightArm);
2105  if (enabledRightArm) {
2106  rightArmJointv[i]->updateJointInfo();
2107  }
2108  }
2109  for (int i = 0; i < headNeckJointv.size(); i++) {
2110  headNeckJointv[i]->enable(enabledHead);
2111  if (enabledHead) {
2112  headNeckJointv[i]->updateJointInfo();
2113  }
2114  }
2115  }
2116  }
2117 
2118  void PhyiCub::configurePosture( QMap<int, real> jointSetup )
2119  {
2120 #ifdef __GNUC__
2121  #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)
2122 #endif
2123 #ifdef __GNUC__
2124  #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?
2125 #endif
2126  // First of all storing the new relative positions of objects in objects groups
2127  leftHandGroup->updateRelativePositions();
2128  rightHandGroup->updateRelativePositions();
2129 
2130  // Setting new angles in the kinematic chains
2131  for (QMap<int, real>::const_iterator it = jointSetup.begin(); it != jointSetup.end(); it++) {
2132  // Big switch to set new postures in the kinematic chains. Joints which are not in
2133  // a kinematic chain are checked below, after these have been set in the new position
2134  switch (it.key()) {
2135  case torso_yaw:
2136  kinRightArm.getLinkInfo(2).setLinkAngle(toRad(it.value()));
2137  break;
2138  case torso_roll:
2139  kinRightArm.getLinkInfo(1).setLinkAngle(toRad(it.value()));
2140  break;
2141  case torso_pitch:
2142  kinRightArm.getLinkInfo(0).setLinkAngle(toRad(it.value()));
2143  break;
2144  case left_shoulder_pitch:
2145  kinLeftArm.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2146  break;
2147  case left_shoulder_roll:
2148  kinLeftArm.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2149  break;
2150  case left_shoulder_yaw:
2151  kinLeftArm.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2152  break;
2153  case left_elbow:
2154  kinLeftArm.getLinkInfo(6).setLinkAngle(toRad(it.value()));
2155  break;
2156  case left_wrist_prosup:
2157  kinLeftArm.getLinkInfo(7).setLinkAngle(toRad(it.value()));
2158  break;
2159  case left_wrist_pitch:
2160  kinLeftArm.getLinkInfo(8).setLinkAngle(toRad(it.value()));
2161  break;
2162  case left_wrist_yaw:
2163  kinLeftArm.getLinkInfo(9).setLinkAngle(toRad(it.value()));
2164  break;
2165  case right_shoulder_pitch:
2166  kinRightArm.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2167  break;
2168  case right_shoulder_roll:
2169  kinRightArm.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2170  break;
2171  case right_shoulder_yaw:
2172  kinRightArm.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2173  break;
2174  case right_elbow:
2175  kinRightArm.getLinkInfo(6).setLinkAngle(toRad(it.value()));
2176  break;
2177  case right_wrist_prosup:
2178  kinRightArm.getLinkInfo(7).setLinkAngle(toRad(it.value()));
2179  break;
2180  case right_wrist_pitch:
2181  kinRightArm.getLinkInfo(8).setLinkAngle(toRad(it.value()));
2182  break;
2183  case right_wrist_yaw:
2184  kinRightArm.getLinkInfo(9).setLinkAngle(toRad(it.value()));
2185  break;
2186  case neck_pitch:
2187  kinRightEye.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2188  break;
2189  case neck_roll:
2190  kinRightEye.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2191  break;
2192  case neck_yaw:
2193  kinRightEye.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2194  break;
2195  case eyes_tilt:
2196  kinRightEye.getLinkInfo(6).setLinkAngle(toRad(it.value()));
2197  break;
2198  case eyes_version: case eyes_vergence:
2199  {
2200  // Here we need to use both vergence and version. We use the current angles
2201  // unless they are in the posture map
2202  real versionAngle;
2203  if (jointSetup. contains(eyes_version)) {
2204  versionAngle = toRad(jointSetup[eyes_version]);
2205  } else {
2206  versionAngle = versionDOF->position();
2207  }
2208  real vergenceAngle;
2209  if (jointSetup. contains(eyes_vergence)) {
2210  vergenceAngle = toRad(jointSetup[eyes_vergence]);
2211  } else {
2212  vergenceAngle = vergenceDOF->position();
2213  }
2214 
2215  // Now computing the new positions for the right and left eye
2216  const real rightEyeAngle = rightEyeFromVersionAndVergence(versionAngle, vergenceAngle);
2217  const real leftEyeAngle = leftEyeFromVersionAndVergence(versionAngle, vergenceAngle);
2218 
2219  // Finally setting angles in both the right and left kinemati link
2220  kinRightEye.getLinkInfo(7).setLinkAngle(rightEyeAngle);
2221  kinLeftEye.getLinkInfo(7).setLinkAngle(leftEyeAngle);
2222  }
2223  break;
2224  case left_hip_pitch:
2225  kinLeftLeg.getLinkInfo(0).setLinkAngle(toRad(it.value()));
2226  break;
2227  case left_hip_roll:
2228  kinLeftLeg.getLinkInfo(1).setLinkAngle(toRad(it.value()));
2229  break;
2230  case left_hip_yaw:
2231  kinLeftLeg.getLinkInfo(2).setLinkAngle(toRad(it.value()));
2232  break;
2233  case left_knee:
2234  kinLeftLeg.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2235  break;
2236  case left_ankle_pitch:
2237  kinLeftLeg.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2238  break;
2239  case left_ankle_roll:
2240  kinLeftLeg.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2241  break;
2242  case right_hip_pitch:
2243  kinRightLeg.getLinkInfo(0).setLinkAngle(toRad(it.value()));
2244  break;
2245  case right_hip_roll:
2246  kinRightLeg.getLinkInfo(1).setLinkAngle(toRad(it.value()));
2247  break;
2248  case right_hip_yaw:
2249  kinRightLeg.getLinkInfo(2).setLinkAngle(toRad(it.value()));
2250  break;
2251  case right_knee:
2252  kinRightLeg.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2253  break;
2254  case right_ankle_pitch:
2255  kinRightLeg.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2256  break;
2257  case right_ankle_roll:
2258  kinRightLeg.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2259  break;
2260  default:
2261  break;
2262  }
2263  }
2264 
2265  // We do this on all chains, perhaps we could just update those that have changed...
2266  kinRightArm.updateMatrixFromiKin();
2267  kinLeftArm.updateMatrixFromiKin();
2268  kinRightLeg.updateMatrixFromiKin();
2269  kinLeftLeg.updateMatrixFromiKin();
2270  kinRightEye.updateMatrixFromiKin();
2271  kinLeftEye.updateMatrixFromiKin();
2272 
2273  // Also updating object groups
2274  rightHandGroup->resetObjectPositions();
2275  leftHandGroup->resetObjectPositions();
2276 
2277  // Updating all joints (see comment above, perhaps we shouldn't update all)
2278  foreach( PhyJoint* joint, leftLegJointv ) {
2279  joint->updateJointInfo();
2280  }
2281  foreach( PhyJoint* joint, rightLegJointv ) {
2282  joint->updateJointInfo();
2283  }
2284  foreach( PhyJoint* joint, torsoJointv ) {
2285  joint->updateJointInfo();
2286  }
2287  foreach( PhyJoint* joint, leftArmJointv ) {
2288  joint->updateJointInfo();
2289  }
2290  foreach( PhyJoint* joint, rightArmJointv ) {
2291  joint->updateJointInfo();
2292  }
2293  foreach( PhyJoint* joint, headNeckJointv ) {
2294  joint->updateJointInfo();
2295  }
2296 
2297  // Setting new angles for objects which don't belong to any kinematic chain. Joints and objects
2298  // matrices are updated when they are moved
2299  for (QMap<int, real>::const_iterator it = jointSetup.begin(); it != jointSetup.end(); it++) {
2300  switch (it.key()) {
2301  case left_hand_finger:
2302  leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 0);
2303  leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 0);
2304  leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 0);
2305  break;
2306  case left_thumb_oppose:
2307  leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 0);
2308  break;
2309  case left_thumb_proximal:
2310  leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 1);
2311  break;
2312  case left_thumb_distal:
2313  leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 2);
2314  leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 3);
2315  break;
2316  case left_index_proximal:
2317  leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 1);
2318  break;
2319  case left_index_distal:
2320  leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 2);
2321  leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 3);
2322  break;
2323  case left_middle_proximal:
2324  leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 1);
2325  break;
2326  case left_middle_distal:
2327  leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 2);
2328  leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 3);
2329  break;
2330  case left_pinky:
2331  leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 1);
2332  leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 2);
2333  leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 3);
2334  leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 1);
2335  leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 2);
2336  leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 3);
2337  break;
2338  case right_hand_finger:
2339  rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 0);
2340  rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 0);
2341  rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 0);
2342  break;
2343  case right_thumb_oppose:
2344  rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 0);
2345  break;
2346  case right_thumb_proximal:
2347  rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 1);
2348  break;
2349  case right_thumb_distal:
2350  rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 2);
2351  rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 3);
2352  break;
2353  case right_index_proximal:
2354  rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 1);
2355  break;
2356  case right_index_distal:
2357  rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 2);
2358  rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 3);
2359  break;
2360  case right_middle_proximal:
2361  rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 1);
2362  break;
2363  case right_middle_distal:
2364  rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 2);
2365  rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 3);
2366  break;
2367  case right_pinky:
2368  rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 1);
2369  rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 2);
2370  rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 3);
2371  rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 1);
2372  rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 2);
2373  rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 3);
2374  break;
2375  default:
2376  break;
2377  }
2378  }
2379 
2380  // Stops all Motor Controllers
2381  torsoCtrl->stop();
2382  leftArmCtrl->stop();
2383  rightArmCtrl->stop();
2384  headNeckCtrl->stop();
2385  leftLegCtrl->stop();
2386  rightLegCtrl->stop();
2387  }
2388 
2389  yarp::dev::IFrameGrabberImage* PhyiCub::leftEyeFrameGrabber()
2390  {
2391  return (leftcam == NULL) ? NULL : leftcam->getFrameGrabber();
2392  }
2393 
2394  yarp::dev::IFrameGrabberImage* PhyiCub::rightEyeFrameGrabber()
2395  {
2396  return (rightcam == NULL) ? NULL : rightcam->getFrameGrabber();
2397  }
2398 
2399  void PhyiCub::preUpdate()
2400  {
2401  // First of all updating motors
2402  if (torsoCtrl->isEnabled()) {
2403  torsoCtrl->update();
2404  }
2405  if (leftArmCtrl->isEnabled()) {
2406  leftArmCtrl->update();
2407  }
2408  if (rightArmCtrl->isEnabled()) {
2409  rightArmCtrl->update();
2410  }
2411  if (headNeckCtrl->isEnabled()) {
2412  headNeckCtrl->update();
2413  }
2414  if (leftLegCtrl->isEnabled()) {
2415  leftLegCtrl->update();
2416  }
2417  if (rightLegCtrl->isEnabled()) {
2418  rightLegCtrl->update();
2419  }
2420 
2421  // advance the cartesian servers
2422  //if ( cartThreadLeftArm ) {
2423  // cartThreadLeftArm->run();
2424  //}
2425 
2426  // Updating all enabled chains if in kinematic simulation (or updating all chains, even disabled ones
2427  // if alwaysUpdateKinematicChains is true)
2428  if (kinematicSimulation) {
2429  bool torsoUpdated = false;
2430  int lastLinkRight = -1;
2431  int lastLinkLeft = -1;
2432  if (enabledRightArm || alwaysUpdateKinematicChains) {
2433  if (enabledRightArm && collidingRightHand) {
2434  lastLinkRight = 8; // This kinematic chain also comprise torso, that's why we have to use this number
2435  }
2436  kinRightArm.updateMatrixFromiKin(0, lastLinkRight);
2437  torsoUpdated = true;
2438  }
2439  if (enabledLeftArm || alwaysUpdateKinematicChains) {
2440  if (enabledLeftArm && collidingLeftHand) {
2441  lastLinkLeft = 8; // This kinematic chain also comprise torso, that's why we have to use this number
2442  }
2443  kinLeftArm.updateMatrixFromiKin(0, lastLinkLeft);
2444  torsoUpdated = true;
2445  }
2446  if (enabledRightLeg || alwaysUpdateKinematicChains) {
2447  kinRightLeg.updateMatrixFromiKin();
2448  }
2449  if (enabledLeftLeg || alwaysUpdateKinematicChains) {
2450  kinLeftLeg.updateMatrixFromiKin();
2451  }
2452  if (enabledHead || alwaysUpdateKinematicChains) {
2453  kinRightEye.updateMatrixFromiKin();
2454  kinLeftEye.updateMatrixFromiKin();
2455  torsoUpdated = true;
2456  }
2457  // No need to check alwaysUpdateKinematicChains here, if it is true
2458  // the torso has already been updated
2459  if (!torsoUpdated && enabledTorso) {
2460  kinRightArm.updateMatrixFromiKin(0, lastLinkRight);
2461  }
2462 
2463  // Also updating object groups
2464  if (!collidingRightHand && (enabledRightArm || alwaysUpdateKinematicChains)) {
2465  rightHandGroup->resetObjectPositions();
2466  if (enabledRightKinematicHand) {
2467  rightHandGroup->updateFromDOF();
2468  }
2469  }
2470  if (!collidingLeftHand && (enabledLeftArm || alwaysUpdateKinematicChains)) {
2471  leftHandGroup->resetObjectPositions();
2472  if (enabledLeftKinematicHand) {
2473  leftHandGroup->updateFromDOF();
2474  }
2475  }
2476  }
2477  }
2478 
2479  void PhyiCub::postUpdate()
2480  {
2481  // Here we just have to update disabled kinematic chains if alwaysUpdateKinematicChains
2482  // is true and this is a dynamical simulation
2483  if (alwaysUpdateKinematicChains && !kinematicSimulation) {
2484  int upperBodyUpdateStartingChainLink = 3;
2485  if (!enabledTorso) {
2486  // If torso is disabled we update it when an arm or the head is updated (if
2487  // nothing is updated, then the torso is not updated as well)
2488  upperBodyUpdateStartingChainLink = 0;
2489  }
2490  if (!enabledRightArm) {
2491  kinRightArm.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
2492  }
2493  if (!enabledLeftArm) {
2494  kinLeftArm.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
2495  }
2496  if (!enabledRightLeg) {
2497  kinRightLeg.updateMatrixFromiKin();
2498  }
2499  if (!enabledLeftLeg) {
2500  kinLeftLeg.updateMatrixFromiKin();
2501  }
2502  if (!enabledHead) {
2503  kinRightEye.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
2504  kinLeftEye.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
2505  }
2506  }
2507  }
2508 
2509  void PhyiCub::enableLeftLeg( bool b ) {
2510  if ( enabledLeftLeg == b ) return;
2511  enabledLeftLeg = b;
2512  if ( b ) {
2513  enableObjectsAndLinks( true, leftLegv, leftLegJointv );
2514  leftLegCtrl->setEnabled( true );
2515  } else {
2516  leftLegCtrl->setEnabled( false );
2517  enableObjectsAndLinks( false, leftLegv, leftLegJointv );
2518  }
2519  }
2520 
2521  void PhyiCub::enableRightLeg( bool b ) {
2522  if ( enabledRightLeg == b ) return;
2523  enabledRightLeg = b;
2524  if ( b ) {
2525  enableObjectsAndLinks( true, rightLegv, rightLegJointv );
2526  rightLegCtrl->setEnabled( true );
2527  } else {
2528  rightLegCtrl->setEnabled( false );
2529  enableObjectsAndLinks( false, rightLegv, rightLegJointv );
2530  }
2531  }
2532 
2533  void PhyiCub::enableTorso( bool b ) {
2534  // This function is slightly different from enable functions of other robot parts
2535  // because the torso objects in dynamic simulations cannot be set to kinematic (as
2536  // done in enableObjectsAndLinks): doing so causes the first joint of arms and
2537  // head not to work correctly
2538  if ( enabledTorso == b ) return;
2539  enabledTorso = b;
2540  if ( b ) {
2541  for (int i = 0; i < torsov.size(); i++) {
2542  torsov[i]->setStatic(false);
2543  }
2544  if (!kinematicSimulation) {
2545  for (int i = 0; i < torsoJointv.size(); i++) {
2546  torsoJointv[i]->enable(true);
2547  torsoJointv[i]->updateJointInfo();
2548  }
2549  }
2550  torsoCtrl->setEnabled( true );
2551  } else {
2552  torsoCtrl->setEnabled( false );
2553  if (!kinematicSimulation) {
2554  for (int i = 0; i < torsoJointv.size(); i++) {
2555  torsoJointv[i]->enable(false);
2556  }
2557  }
2558  for (int i = 0; i < torsov.size(); i++) {
2559  torsov[i]->setStatic(true);
2560  }
2561  }
2562  }
2563 
2564  void PhyiCub::enableHead( bool b ) {
2565  if ( enabledHead == b ) return;
2566  enabledHead = b;
2567  if ( b ) {
2568  enableObjectsAndLinks( true, headNeckv, headNeckJointv );
2569  headNeckCtrl->setEnabled( true );
2570  } else {
2571  headNeckCtrl->setEnabled( false );
2572  enableObjectsAndLinks( false, headNeckv, headNeckJointv );
2573  }
2574  }
2575 
2576  void PhyiCub::enableCameras( bool b ) {
2577  if ( enabledCameras == b ) return;
2578  enabledCameras = b;
2579  if ( b && !leftcam ) {
2580  leftcam = new WCamera( world(), name()+"/cam/left", headNeckv[5], 640, 480 );
2581  rightcam = new WCamera( world(), name()+"/cam/right", headNeckv[4], 640, 480 );
2582  }
2583  if ( !b && leftcam ) {
2584  delete leftcam;
2585  delete rightcam;
2586  leftcam = 0;
2587  rightcam = 0;
2588  }
2589  }
2590 
2591  void PhyiCub::enableLeftArm( bool b ) {
2592  if ( enabledLeftArm == b ) return;
2593  enabledLeftArm = b;
2594  if ( b ) {
2595  enableObjectsAndLinks( true, leftArmv, leftArmJointv );
2596  leftArmCtrl->setEnabled( true );
2597  } else {
2598  leftArmCtrl->setEnabled( false );
2599  enableObjectsAndLinks( false, leftArmv, leftArmJointv );
2600  }
2601  }
2602 
2603  void PhyiCub::enableRightArm( bool b ) {
2604  if ( enabledRightArm == b ) return;
2605  enabledRightArm = b;
2606  if ( b ) {
2607  enableObjectsAndLinks( true, rightArmv, rightArmJointv );
2608  rightArmCtrl->setEnabled( true );
2609  } else {
2610  rightArmCtrl->setEnabled( false );
2611  enableObjectsAndLinks( false, rightArmv, rightArmJointv );
2612  }
2613  }
2614 
2615  void PhyiCub::blockTorso0( bool b ) {
2616  if ( blockedTorso0 == b ) return;
2617  blockedTorso0 = b;
2618  if ( b ) {
2619  torsov[0]->setMass( 0 );
2620  } else {
2621  torsov[0]->setMass( torsoMasses[0] );
2622  }
2623  }
2624 
2625  void PhyiCub::enableLeftKinematicHand( bool b )
2626  {
2627  enabledLeftKinematicHand = b;
2628  }
2629 
2630  void PhyiCub::enableRightKinematicHand( bool b )
2631  {
2632  enabledRightKinematicHand = b;
2633  }
2634 
2635  YARP_DECLARE_PLUGINS( icubmod )
2636  void PhyiCub::enableLeftArmCartesianController() {
2637  YARP_REGISTER_PLUGINS( icubmod );
2638  //--- Using directly the iKin library
2639  // declare the on-line arm solver called "solver"
2640  cartSolvLeftArm = new iCubArmCartesianSolver(
2641  QString( "%1/%2/cartesianSolver/left_arm" )
2642  .arg( world()->name() )
2643  .arg( name() )
2644  .toAscii().data()
2645  );
2646  Property options( QString("\
2647  (robot %1/%2) \
2648  (type left) \
2649  (pose full) \
2650  (dof (1 1 1 1 1 1 1 1 1 1) ) \
2651  (verbosity off)")
2652  .arg( world()->name() )
2653  .arg( name() )
2654  .toAscii().data() );
2655  cartSolvLeftArm->open( options );
2656  //--- create the CartesianController and export its interface
2657  // the idea is to create a cartesiancontrollerserver and
2658  // using the IMultipleWrapper attaching the PolyDriver created above
2659  // for the arms to the corresponding cartesiancontrollerserver and
2660  // finally getting the ICartesianInterface from cartesiancontrollerserver
2661  Property optServerLeftArm( QString( "\
2662  (device cartesiancontrollerserver) \
2663  (GENERAL (ControllerName %1/%2/cartesianController/left_arm) \
2664  (ControllerPeriod 10) \
2665  (SolverNameToConnect %1/%2/cartesianSolver/left_arm) \
2666  (KinematicPart arm) \
2667  (KinematicType left) \
2668  (NumberOfDrivers 2)) \
2669  (DRIVER_0 (Key torso) (JointsOrder reversed)) \
2670  (DRIVER_1 (Key left_arm) (JointsOrder direct))" )
2671  .arg( world()->name() )
2672  .arg( name() )
2673  .toAscii().data() );
2674  cartServLeftArm = new PolyDriver();
2675  if ( !cartServLeftArm->open( optServerLeftArm ) ) {
2676  //--- non posso aggiungere l'interfaccia cartesiana, peccato
2677  } else {
2678  //--- ok, wrappo
2679  PolyDriverList listL;
2680  listL.push( polydriver("torso"), "torso" );
2681  listL.push( polydriver("left_arm"), "left_arm" );
2682  IMultipleWrapper *wrapperL;
2683  cartServLeftArm->view( wrapperL );
2684  if ( !wrapperL->attachAll(listL) ) {
2685  //--- questo e' un errore !! :-S
2686  qDebug() << "Errore nell'attaccare i PolyDriver al Server Cartesiano";
2687  }
2688  //--- esporto ICartesionInterface
2689  cartServLeftArm->view( cartCtrlLeftArm );
2690  //--- suspend the rateThread and update the cartesianController in sync with the simulator
2691  //cartServLeftArm->view( cartThreadLeftArm );
2692  //cartThreadLeftArm->suspend();
2693  }
2694  }
2695 
2696  void PhyiCub::enableRightArmCartesianController() {
2697  YARP_REGISTER_PLUGINS( icubmod );
2698  //--- Using directly the iKin library
2699  // declare the on-line arm solver called "solver"
2700  cartSolvRightArm = new iCubArmCartesianSolver(
2701  QString( "%1/%2/cartesianSolver/right_arm" )
2702  .arg( world()->name() )
2703  .arg( name() )
2704  .toAscii().data()
2705  );
2706  Property options( QString("\
2707  (robot %1/%2) \
2708  (type right) \
2709  (pose full) \
2710  (verbosity off)")
2711  .arg( world()->name() )
2712  .arg( name() )
2713  .toAscii().data() );
2714  cartSolvRightArm->open( options );
2715  //--- create the CartesianController and export its interface
2716  // the idea is to create a cartesiancontrollerserver and
2717  // using the IMultipleWrapper attaching the PolyDriver created above
2718  // for the arms to the corresponding cartesiancontrollerserver and
2719  // finally getting the ICartesianInterface from cartesiancontrollerserver
2720  Property optServerRightArm( QString( "\
2721  (device cartesiancontrollerserver) \
2722  (GENERAL (ControllerName %1/%2/cartesianController/right_arm) \
2723  (ControllerPeriod 10) \
2724  (SolverNameToConnect %1/%2/cartesianSolver/right_arm) \
2725  (KinematicPart arm) \
2726  (KinematicType right) \
2727  (NumberOfDrivers 2)) \
2728  (DRIVER_0 (Key torso) (JointsOrder reversed)) \
2729  (DRIVER_1 (Key right_arm) (JointsOrder direct))" )
2730  .arg( world()->name() )
2731  .arg( name() )
2732  .toAscii().data() );
2733  cartServRightArm = new PolyDriver();
2734  if ( !cartServRightArm->open( optServerRightArm ) ) {
2735  //--- non posso aggiungere l'interfaccia cartesiana, peccato
2736  } else {
2737  //--- ok, wrappo
2738  PolyDriverList listL;
2739  listL.push( polydriver("torso"), "torso" );
2740  listL.push( polydriver("right_arm"), "right_arm" );
2741  IMultipleWrapper *wrapperL;
2742  cartServRightArm->view( wrapperL );
2743  if ( !wrapperL->attachAll(listL) ) {
2744  //--- questo e' un errore !! :-S
2745  qDebug() << "Errore nell'attaccare i PolyDriver al Server Cartesiano";
2746  }
2747  //--- esporto ICartesionInterface
2748  cartServRightArm->view( cartCtrlRightArm );
2749  }
2750  }
2751 
2752  void PhyiCub::changedMatrix() {
2753  // Resetting the matrix for torso0
2754  setTorso0Matrix();
2755 
2756  // Updating all kinematic chains
2757  kinRightArm.updateMatrixFromiKin();
2758  kinLeftArm.updateMatrixFromiKin();
2759  kinRightLeg.updateMatrixFromiKin();
2760  kinLeftLeg.updateMatrixFromiKin();
2761  kinRightEye.updateMatrixFromiKin();
2762  kinLeftEye.updateMatrixFromiKin();
2763 
2764  // Also updating object groups
2765  rightHandGroup->resetObjectPositions();
2766  leftHandGroup->resetObjectPositions();
2767 
2768  // Finally updating all joints
2769  foreach( PhyJoint* joint, leftLegJointv ) {
2770  joint->updateJointInfo();
2771  }
2772  foreach( PhyJoint* joint, rightLegJointv ) {
2773  joint->updateJointInfo();
2774  }
2775  foreach( PhyJoint* joint, torsoJointv ) {
2776  joint->updateJointInfo();
2777  }
2778  foreach( PhyJoint* joint, leftArmJointv ) {
2779  joint->updateJointInfo();
2780  }
2781  foreach( PhyJoint* joint, rightArmJointv ) {
2782  joint->updateJointInfo();
2783  }
2784  foreach( PhyJoint* joint, headNeckJointv ) {
2785  joint->updateJointInfo();
2786  }
2787  }
2788 
2789  void PhyiCub::setTorso0Matrix()
2790  {
2791  // Computing matrix for torso0. We start from H0 for the right arm
2792  // kinematic chain (its the same matrix as that of all the other chains
2793  // comprising the torso)
2794  wMatrix torso0Matrix;
2795  convertYarpMatrixToWorldMatrix(kinRightArm.getH0(), torso0Matrix);
2796  torso0Matrix = torso0Matrix * tm;
2797  const real torso0Height = (dynamic_cast<PhyBox*>(torsov[0]))->sideX();
2798  // Now we translate the torso0 object so that it touches the legs solids
2799  // (we use Z translation of the legs chain H0 - remember that YARP matrices
2800  // are transposed with respect to wMatrix)
2801  torso0Matrix.w_pos += wVector(0.0, 0.0, kinRightLeg.getH0()(2, 3) + torso0Height / 4.0);
2802  torsov[0]->setMatrix(torso0Matrix);
2803  }
2804 
2805  void PhyiCub::enableObjectsAndLinks(bool enable, QVector<PhyObject*>& objects, QVector<PhyJoint*>& joints)
2806  {
2807  // The order (objects then joints or vice-versa) is important, that's the reason for the if.
2808  // If we are doing a kinematic simulation we don't do anything on joints
2809  if (enable) {
2810  for (int i = 0; i < objects.size(); i++) {
2811  objects[i]->setKinematic(false);
2812  }
2813  if (!kinematicSimulation) {
2814  for (int i = 0; i < joints.size(); i++) {
2815  joints[i]->enable(true);
2816  joints[i]->updateJointInfo();
2817  }
2818  }
2819  } else {
2820  if (!kinematicSimulation) {
2821  for (int i = 0; i < joints.size(); i++) {
2822  joints[i]->enable(false);
2823  }
2824  }
2825  for (int i = 0; i < objects.size(); i++) {
2826  objects[i]->setKinematic(true);
2827  }
2828  }
2829  }
2830 
2831  real PhyiCub::rightEyeFromVersionAndVergence(real version, real vergence)
2832  {
2833  return -version - vergence;
2834  }
2835 
2836  real PhyiCub::leftEyeFromVersionAndVergence(real version, real vergence)
2837  {
2838  return -version + vergence;
2839  }
2840 
2841  real PhyiCub::versionFromRightAndLeftEye(real right, real left)
2842  {
2843  return (-left - right) / 2.0;
2844  }
2845 
2846  real PhyiCub::vergenceFromRightAndLeftEye(real right, real left)
2847  {
2848  return (left - right) / 2.0;
2849  }
2850 
2851  real PhyiCub::lowEyeLimitFromVersionAndVergenceLimits(real versionLow, real vergenceLow)
2852  {
2853  const real lowEyeLimit = versionLow + vergenceLow;
2854 
2855  return (lowEyeLimit < -PI_GRECO) ? -(PI_GRECO - 0.000001) : lowEyeLimit;
2856  }
2857 
2858  real PhyiCub::highEyeLimitFromVersionAndVergenceLimits(real versionHigh, real vergenceHigh)
2859  {
2860  const real highEyeLimit = versionHigh + vergenceHigh;
2861 
2862  return (highEyeLimit > PI_GRECO) ? (PI_GRECO - 0.000001) : highEyeLimit;
2863  }
2864 
2865 #ifdef __GNUC__
2866  #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
2867 #endif
2868 
2869  void PhyiCub::versionChangedDesiredPosition( real wishpos )
2870  {
2871  if (ignoreEyeSignals) {
2872  return;
2873  }
2874 
2875  const real curVergence = vergenceDOF->desiredPosition();
2876  const real rightEyeAngle = rightEyeFromVersionAndVergence(wishpos, curVergence);
2877  const real leftEyeAngle = leftEyeFromVersionAndVergence(wishpos, curVergence);
2878 
2879  ignoreEyeSignals = true;
2880  rightEyeDOF->setDesiredPosition(rightEyeAngle);
2881  leftEyeDOF->setDesiredPosition(leftEyeAngle);
2882  ignoreEyeSignals = false;
2883  }
2884 
2885  void PhyiCub::versionChangedDesiredVelocity( real wishvel )
2886  {
2887  if (ignoreEyeSignals) {
2888  return;
2889  }
2890 
2891  const real curVergenceVelocity = vergenceDOF->desiredVelocity();
2892  const real rightEyeVelocity = rightEyeFromVersionAndVergence(wishvel, curVergenceVelocity);
2893  const real leftEyeVelocity = leftEyeFromVersionAndVergence(wishvel, curVergenceVelocity);
2894 
2895  ignoreEyeSignals = true;
2896  rightEyeDOF->setDesiredVelocity(rightEyeVelocity);
2897  leftEyeDOF->setDesiredVelocity(leftEyeVelocity);
2898  ignoreEyeSignals = false;
2899  }
2900 
2901  void PhyiCub::versionChangedPosition( real newpos )
2902  {
2903  if (ignoreEyeSignals) {
2904  return;
2905  }
2906 
2907  const real curVergence = vergenceDOF->position();
2908  const real rightEyeAngle = rightEyeFromVersionAndVergence(newpos, curVergence);
2909  const real leftEyeAngle = leftEyeFromVersionAndVergence(newpos, curVergence);
2910 
2911  ignoreEyeSignals = true;
2912  rightEyeDOF->setPosition(rightEyeAngle);
2913  leftEyeDOF->setPosition(leftEyeAngle);
2914  ignoreEyeSignals = false;
2915  }
2916 
2917  void PhyiCub::versionChangedVelocity( real newvel )
2918  {
2919  if (ignoreEyeSignals) {
2920  return;
2921  }
2922 
2923  const real curVergenceVelocity = vergenceDOF->velocity();
2924  const real rightEyeVelocity = rightEyeFromVersionAndVergence(newvel, curVergenceVelocity);
2925  const real leftEyeVelocity = leftEyeFromVersionAndVergence(newvel, curVergenceVelocity);
2926 
2927  ignoreEyeSignals = true;
2928  rightEyeDOF->setVelocity(rightEyeVelocity);
2929  leftEyeDOF->setVelocity(leftEyeVelocity);
2930  ignoreEyeSignals = false;
2931  }
2932 
2933  void PhyiCub::versionChangedLimits( real lowlimit, real highlimit )
2934  {
2935  if (ignoreEyeSignals) {
2936  return;
2937  }
2938 
2939  real curVergenceLow, curVergenceHigh;
2940  vergenceDOF->limits(curVergenceLow, curVergenceHigh);
2941  const real eyeLow = lowEyeLimitFromVersionAndVergenceLimits(lowlimit, curVergenceLow);
2942  const real eyeHigh = highEyeLimitFromVersionAndVergenceLimits(highlimit, curVergenceHigh);
2943 
2944  ignoreEyeSignals = true;
2945  rightEyeDOF->setLimits(eyeLow, eyeHigh);
2946  leftEyeDOF->setLimits(eyeLow, eyeHigh);
2947  ignoreEyeSignals = false;
2948  }
2949 
2950  void PhyiCub::vergenceChangedDesiredPosition( real wishpos )
2951  {
2952  if (ignoreEyeSignals) {
2953  return;
2954  }
2955 
2956  const real curVersion = versionDOF->desiredPosition();
2957  const real rightEyeAngle = rightEyeFromVersionAndVergence(curVersion, wishpos);
2958  const real leftEyeAngle = leftEyeFromVersionAndVergence(curVersion, wishpos);
2959 
2960  ignoreEyeSignals = true;
2961  rightEyeDOF->setDesiredPosition(rightEyeAngle);
2962  leftEyeDOF->setDesiredPosition(leftEyeAngle);
2963  ignoreEyeSignals = false;
2964  }
2965 
2966  void PhyiCub::vergenceChangedDesiredVelocity( real wishvel )
2967  {
2968  if (ignoreEyeSignals) {
2969  return;
2970  }
2971 
2972  const real curVersionVelocity = versionDOF->desiredVelocity();
2973  const real rightEyeVelocity = rightEyeFromVersionAndVergence(curVersionVelocity, wishvel);
2974  const real leftEyeVelocity = leftEyeFromVersionAndVergence(curVersionVelocity, wishvel);
2975 
2976  ignoreEyeSignals = true;
2977  rightEyeDOF->setDesiredVelocity(rightEyeVelocity);
2978  leftEyeDOF->setDesiredVelocity(leftEyeVelocity);
2979  ignoreEyeSignals = false;
2980  }
2981 
2982  void PhyiCub::vergenceChangedPosition( real newpos )
2983  {
2984  if (ignoreEyeSignals) {
2985  return;
2986  }
2987 
2988  const real curVersion = versionDOF->position();
2989  const real rightEyeAngle = rightEyeFromVersionAndVergence(curVersion, newpos);
2990  const real leftEyeAngle = leftEyeFromVersionAndVergence(curVersion, newpos);
2991 
2992  ignoreEyeSignals = true;
2993  rightEyeDOF->setPosition(rightEyeAngle);
2994  leftEyeDOF->setPosition(leftEyeAngle);
2995  ignoreEyeSignals = false;
2996  }
2997 
2998  void PhyiCub::vergenceChangedVelocity( real newvel )
2999  {
3000  if (ignoreEyeSignals) {
3001  return;
3002  }
3003 
3004  const real curVersionVelocity = versionDOF->velocity();
3005  const real rightEyeVelocity = rightEyeFromVersionAndVergence(curVersionVelocity, newvel);
3006  const real leftEyeVelocity = leftEyeFromVersionAndVergence(curVersionVelocity, newvel);
3007 
3008  ignoreEyeSignals = true;
3009  rightEyeDOF->setVelocity(rightEyeVelocity);
3010  leftEyeDOF->setVelocity(leftEyeVelocity);
3011  ignoreEyeSignals = false;
3012  }
3013 
3014  void PhyiCub::vergenceChangedLimits( real lowlimit, real highlimit )
3015  {
3016  if (ignoreEyeSignals) {
3017  return;
3018  }
3019 
3020  real curVersionLow, curVersionHigh;
3021  vergenceDOF->limits(curVersionLow, curVersionHigh);
3022  const real eyeLow = lowEyeLimitFromVersionAndVergenceLimits(curVersionLow, lowlimit);
3023  const real eyeHigh = highEyeLimitFromVersionAndVergenceLimits(curVersionHigh, highlimit);
3024 
3025  ignoreEyeSignals = true;
3026  rightEyeDOF->setLimits(eyeLow, eyeHigh);
3027  leftEyeDOF->setLimits(eyeLow, eyeHigh);
3028  ignoreEyeSignals = false;
3029  }
3030 
3031  void PhyiCub::rightEyeChangedDesiredPosition( real wishpos )
3032  {
3033  if (ignoreEyeSignals) {
3034  return;
3035  }
3036 
3037  const real curLeftEyeAngle = leftEyeDOF->position();
3038  const real versionAngle = versionFromRightAndLeftEye(wishpos, curLeftEyeAngle);
3039  const real vergenceAngle = vergenceFromRightAndLeftEye(wishpos, curLeftEyeAngle);
3040 
3041  ignoreEyeSignals = true;
3042  versionDOF->setDesiredPosition(versionAngle);
3043  vergenceDOF->setDesiredPosition(vergenceAngle);
3044  ignoreEyeSignals = false;
3045  }
3046 
3047  void PhyiCub::rightEyeChangedDesiredVelocity( real wishvel )
3048  {
3049  if (ignoreEyeSignals) {
3050  return;
3051  }
3052 
3053  const real curLeftEyeVelocity = leftEyeDOF->position();
3054  const real versionVelocity = versionFromRightAndLeftEye(wishvel, curLeftEyeVelocity);
3055  const real vergenceVelocity = vergenceFromRightAndLeftEye(wishvel, curLeftEyeVelocity);
3056 
3057  ignoreEyeSignals = true;
3058  versionDOF->setDesiredVelocity(versionVelocity);
3059  vergenceDOF->setDesiredVelocity(vergenceVelocity);
3060  ignoreEyeSignals = false;
3061  }
3062 
3063  void PhyiCub::rightEyeChangedPosition( real newpos )
3064  {
3065  if (ignoreEyeSignals) {
3066  return;
3067  }
3068 
3069  const real curLeftEyeAngle = leftEyeDOF->position();
3070  const real versionAngle = versionFromRightAndLeftEye(newpos, curLeftEyeAngle);
3071  const real vergenceAngle = vergenceFromRightAndLeftEye(newpos, curLeftEyeAngle);
3072 
3073  ignoreEyeSignals = true;
3074  versionDOF->setPosition(versionAngle);
3075  vergenceDOF->setPosition(vergenceAngle);
3076  ignoreEyeSignals = false;
3077  }
3078 
3079  void PhyiCub::rightEyeChangedVelocity( real newvel )
3080  {
3081  if (ignoreEyeSignals) {
3082  return;
3083  }
3084 
3085  const real curLeftEyeVelocity = leftEyeDOF->position();
3086  const real versionVelocity = versionFromRightAndLeftEye(newvel, curLeftEyeVelocity);
3087  const real vergenceVelocity = vergenceFromRightAndLeftEye(newvel, curLeftEyeVelocity);
3088 
3089  ignoreEyeSignals = true;
3090  versionDOF->setVelocity(versionVelocity);
3091  vergenceDOF->setVelocity(vergenceVelocity);
3092  ignoreEyeSignals = false;
3093  }
3094 
3095  void PhyiCub::rightEyeChangedLimits( real , real )
3096  {
3097  // This is not implemented as we expose vergence and version, so we should never change limtis
3098  // for one eye only. Moreover implementing this is not trivial because, given the left and right
3099  // eye limits, there is not a unique possibility for vergence and version
3100  }
3101 
3102  void PhyiCub::leftEyeChangedDesiredPosition( real wishpos )
3103  {
3104  if (ignoreEyeSignals) {
3105  return;
3106  }
3107 
3108  const real curRightEyeAngle = rightEyeDOF->position();
3109  const real versionAngle = versionFromRightAndLeftEye(curRightEyeAngle, wishpos);
3110  const real vergenceAngle = vergenceFromRightAndLeftEye(curRightEyeAngle, wishpos);
3111 
3112  ignoreEyeSignals = true;
3113  versionDOF->setDesiredPosition(versionAngle);
3114  vergenceDOF->setDesiredPosition(vergenceAngle);
3115  ignoreEyeSignals = false;
3116  }
3117 
3118  void PhyiCub::leftEyeChangedDesiredVelocity( real wishvel )
3119  {
3120  if (ignoreEyeSignals) {
3121  return;
3122  }
3123 
3124  const real curRightEyeVelocity = rightEyeDOF->position();
3125  const real versionVelocity = versionFromRightAndLeftEye(curRightEyeVelocity, wishvel);
3126  const real vergenceVelocity = vergenceFromRightAndLeftEye(curRightEyeVelocity, wishvel);
3127 
3128  ignoreEyeSignals = true;
3129  versionDOF->setDesiredVelocity(versionVelocity);
3130  vergenceDOF->setDesiredVelocity(vergenceVelocity);
3131  ignoreEyeSignals = false;
3132  }
3133 
3134  void PhyiCub::leftEyeChangedPosition( real newpos )
3135  {
3136  if (ignoreEyeSignals) {
3137  return;
3138  }
3139 
3140  const real curRightEyeAngle = rightEyeDOF->position();
3141  const real versionAngle = versionFromRightAndLeftEye(curRightEyeAngle, newpos);
3142  const real vergenceAngle = vergenceFromRightAndLeftEye(curRightEyeAngle, newpos);
3143 
3144  ignoreEyeSignals = true;
3145  versionDOF->setPosition(versionAngle);
3146  vergenceDOF->setPosition(vergenceAngle);
3147  ignoreEyeSignals = false;
3148  }
3149 
3150  void PhyiCub::leftEyeChangedVelocity( real newvel )
3151  {
3152  if (ignoreEyeSignals) {
3153  return;
3154  }
3155 
3156  const real curRightEyeVelocity = rightEyeDOF->position();
3157  const real versionVelocity = versionFromRightAndLeftEye(curRightEyeVelocity, newvel);
3158  const real vergenceVelocity = vergenceFromRightAndLeftEye(curRightEyeVelocity, newvel);
3159 
3160  ignoreEyeSignals = true;
3161  versionDOF->setVelocity(versionVelocity);
3162  vergenceDOF->setVelocity(vergenceVelocity);
3163  ignoreEyeSignals = false;
3164  }
3165 
3166  void PhyiCub::leftEyeChangedLimits( real, real )
3167  {
3168  // This is not implemented as we expose vergence and version, so we should never change limtis
3169  // for one eye only. Moreover implementing this is not trivial because, given the left and right
3170  // eye limits, there is not a unique possibility for vergence and version
3171  }
3172 } // end namespace farsa
3173 
3174 #endif // FARSA_USE_YARP_AND_ICUB