WorldSim - 1.2.0

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  Property prop;
1529  if ( QFile::exists(rf.findFile("icub_head_torso.ini").c_str()) ) {
1530  prop.fromConfigFile( rf.findFile("icub_head_torso.ini"), true );
1531  } else {
1532  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_head_torso.ini").toAscii().data() );
1533  }
1534  Bottle lstM = prop.findGroup("LIMITS").findGroup("Max");
1535  Bottle lstm = prop.findGroup("LIMITS").findGroup("Min");
1536  QVector<double> headMax(6), headMin(6), torsoMax(3), torsoMin(3);
1537  for( int i=0; i<6; i++ ) {
1538  headMax[i] = lstM.get(i+1).asDouble();
1539  headMin[i] = lstm.get(i+1).asDouble();
1540  }
1541  for( int i=6; i<9; i++ ) {
1542  torsoMax[i-6] = lstM.get(i+1).asDouble();
1543  torsoMin[i-6] = lstm.get(i+1).asDouble();
1544  }
1545  if ( QFile::exists(rf.findFile("icub_left_leg.ini").c_str()) ) {
1546  prop.fromConfigFile( rf.findFile("icub_left_leg.ini"), true );
1547  } else {
1548  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_leg.ini").toAscii().data() );
1549  }
1550  lstM = prop.findGroup("LIMITS").findGroup("Max");
1551  lstm = prop.findGroup("LIMITS").findGroup("Min");
1552  QVector<double> leftLegMax(6), leftLegMin(6);
1553  for( int i=0; i<6; i++ ) {
1554  leftLegMax[i] = lstM.get(i+1).asDouble();
1555  leftLegMin[i] = lstm.get(i+1).asDouble();
1556  }
1557  if ( QFile::exists(rf.findFile("icub_right_leg.ini").c_str()) ) {
1558  prop.fromConfigFile( rf.findFile("icub_right_leg.ini"), true );
1559  } else {
1560  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_leg.ini").toAscii().data() );
1561  }
1562  lstM = prop.findGroup("LIMITS").findGroup("Max");
1563  lstm = prop.findGroup("LIMITS").findGroup("Min");
1564  QVector<double> rightLegMax(6), rightLegMin(6);
1565  for( int i=0; i<6; i++ ) {
1566  rightLegMax[i] = lstM.get(i+1).asDouble();
1567  rightLegMin[i] = lstm.get(i+1).asDouble();
1568  }
1569  if ( QFile::exists(rf.findFile("icub_left_arm.ini").c_str()) ) {
1570  prop.fromConfigFile( rf.findFile("icub_left_arm.ini"), true );
1571  } else {
1572  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_arm.ini").toAscii().data() );
1573  }
1574  lstM = prop.findGroup("LIMITS").findGroup("Max");
1575  lstm = prop.findGroup("LIMITS").findGroup("Min");
1576  QVector<double> leftArmMax(16), leftArmMin(16);
1577  //--- first 8 are in icub_left_arm.ini,
1578  // second 8 in icub_left_hand.ini
1579  for( int i=0; i<8; i++ ) {
1580  leftArmMax[i] = lstM.get(i+1).asDouble();
1581  leftArmMin[i] = lstm.get(i+1).asDouble();
1582  }
1583  if ( QFile::exists(rf.findFile("icub_left_hand.ini").c_str()) ) {
1584  prop.fromConfigFile( rf.findFile("icub_left_hand.ini"), true );
1585  } else {
1586  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_left_hand.ini").toAscii().data() );
1587  }
1588  lstM = prop.findGroup("LIMITS").findGroup("Max");
1589  lstm = prop.findGroup("LIMITS").findGroup("Min");
1590  for( int i=0; i<8; i++ ) {
1591  leftArmMax[8+i] = lstM.get(i+1).asDouble();
1592  leftArmMin[8+i] = lstm.get(i+1).asDouble();
1593  }
1594  if ( QFile::exists(rf.findFile("icub_right_arm.ini").c_str()) ) {
1595  prop.fromConfigFile( rf.findFile("icub_right_arm.ini"), true );
1596  } else {
1597  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_arm.ini").toAscii().data() );
1598  }
1599  lstM = prop.findGroup("LIMITS").findGroup("Max");
1600  lstm = prop.findGroup("LIMITS").findGroup("Min");
1601  QVector<double> rightArmMax(16), rightArmMin(16);
1602  //--- first 8 are in icub_right_arm.ini,
1603  // second 8 in icub_right_hand.ini
1604  for( int i=0; i<8; i++ ) {
1605  rightArmMax[i] = lstM.get(i+1).asDouble();
1606  rightArmMin[i] = lstm.get(i+1).asDouble();
1607  }
1608  if ( QFile::exists(rf.findFile("icub_right_hand.ini").c_str()) ) {
1609  prop.fromConfigFile( rf.findFile("icub_right_hand.ini"), true );
1610  } else {
1611  prop.fromConfigFile( phyiCubConfTmpl.arg("icub_right_hand.ini").toAscii().data() );
1612  }
1613  lstM = prop.findGroup("LIMITS").findGroup("Max");
1614  lstm = prop.findGroup("LIMITS").findGroup("Min");
1615  for( int i=0; i<8; i++ ) {
1616  rightArmMax[8+i] = lstM.get(i+1).asDouble();
1617  rightArmMin[8+i] = lstm.get(i+1).asDouble();
1618  }
1619 
1620  // Here we set limits both for the motor controllers and the kinematic chain
1621  QVector<PhyDOF*> motors;
1622  //--- number of controlled DOF of joints
1623  int counter = 0;
1624  //--- create MotorController for TORSO
1625  motors.clear();
1626  for( int i=0; i<torsoJointv.size(); i++ ) {
1627  motors << torsoJointv[i]->dofs()[0];
1628  motors[i]->setLimits( toRad(torsoMin[i]), toRad(torsoMax[i]) );
1629  motors[i]->enableLimits();
1630 
1631  // Setting limits for iKin chain. We don't use "i" because joint in the
1632  // torso are in inverse order (I really can't figure out why...)
1633  kinRightArm.getLinkInfo(torsoJointv.size() - i - 1).setiKinLinkLimits(toRad(torsoMin[i]), toRad(torsoMax[i]));
1634  }
1635  torsoCtrl = new MultiMotorController( motors, world );
1636  torsoCtrl->setOwner(this, false);
1637  if ( enabledServerControlBoards ) {
1638  registerServerControlBoard( torsoCtrl, "torso" );
1639  }
1640  counter += motors.size();
1641 
1642  //--- create MotorController for LEFT ARM
1643  motors.clear();
1644  motors << leftArmJointv[0]->dofs()[0];
1645  motors << leftArmJointv[1]->dofs()[0];
1646  motors << leftArmJointv[2]->dofs()[0];
1647  motors << leftArmJointv[3]->dofs()[0];
1648  motors << leftArmJointv[4]->dofs()[0];
1649  motors << leftArmJointv[5]->dofs()[0];
1650  motors << leftArmJointv[6]->dofs()[0];
1651  motors << leftArmJointv[7]->dofs()[0];
1652  motors << leftArmJointv[23]->dofs()[0];
1653  motors << leftArmJointv[24]->dofs()[0];
1654  motors << leftArmJointv[25]->dofs()[0];
1655  motors << leftArmJointv[11]->dofs()[0];
1656  motors << leftArmJointv[12]->dofs()[0];
1657  motors << leftArmJointv[14]->dofs()[0];
1658  motors << leftArmJointv[15]->dofs()[0];
1659  motors << leftArmJointv[17]->dofs()[0];
1660  leftArmCtrl = new MultiMotorController( motors, world );
1661  //--- setting limits
1662  for( unsigned int i=0; i<16; i++ ) {
1663  if ( i<7 ) {
1664  //--- only for the first seven joints the raw limits correspond to public limits
1665  leftArmCtrl->setLimitsRaw( i, leftArmMin[i], leftArmMax[i] );
1666  }
1667  leftArmCtrl->setLimits( i, leftArmMin[i], leftArmMax[i] );
1668  leftArmCtrl->setEnableLimitsRaw( i, true );
1669  // Setting limits for iKin chain
1670  if ((i + 3) < kinLeftArm.getN()) {
1671  kinLeftArm.getLinkInfo(i + 3).setiKinLinkLimits(toRad(leftArmMin[i]), toRad(leftArmMax[i]));
1672  }
1673  }
1674  //--- the joints of the hand has different angular movement respect from what reported by robotMotorGUI
1675  // This mismatch is implemented using setLimitsRaw of MultiMotorController that will change the
1676  // real movement of the joint, but will not change what reported on the robotMotorGUI
1677  leftArmCtrl->setLimitsRaw( 7, -15, 0 );
1678  leftArmCtrl->setLimitsRaw( 8, 10, 90 );
1679  leftArmCtrl->setLimitsRaw( 9, 0, 90 );
1680  leftArmCtrl->setLimitsRaw( 10, 0, 90 );
1681  leftArmCtrl->setLimitsRaw( 11, 0, 90 );
1682  leftArmCtrl->setLimitsRaw( 12, 0, 90 );
1683  leftArmCtrl->setLimitsRaw( 13, 0, 90 );
1684  leftArmCtrl->setLimitsRaw( 14, 0, 90 );
1685  leftArmCtrl->setLimitsRaw( 15, 0, 90 );
1686  leftArmCtrl->setOwner(this, false);
1687  if ( enabledServerControlBoards ) {
1688  registerServerControlBoard( leftArmCtrl, "left_arm" );
1689  }
1690  counter += motors.size();
1691 
1692  //--- create MotorController for RIGHT ARM
1693  motors.clear();
1694  motors << rightArmJointv[0]->dofs()[0];
1695  motors << rightArmJointv[1]->dofs()[0];
1696  motors << rightArmJointv[2]->dofs()[0];
1697  motors << rightArmJointv[3]->dofs()[0];
1698  motors << rightArmJointv[4]->dofs()[0];
1699  motors << rightArmJointv[5]->dofs()[0];
1700  motors << rightArmJointv[6]->dofs()[0];
1701  motors << rightArmJointv[7]->dofs()[0];
1702  motors << rightArmJointv[23]->dofs()[0];
1703  motors << rightArmJointv[24]->dofs()[0];
1704  motors << rightArmJointv[25]->dofs()[0];
1705  motors << rightArmJointv[11]->dofs()[0];
1706  motors << rightArmJointv[12]->dofs()[0];
1707  motors << rightArmJointv[14]->dofs()[0];
1708  motors << rightArmJointv[15]->dofs()[0];
1709  motors << rightArmJointv[17]->dofs()[0];
1710  rightArmCtrl = new MultiMotorController( motors, world );
1711  //--- setting limits
1712  for( unsigned int i=0; i<16; i++ ) {
1713  if ( i<7 ) {
1714  //--- only for the first seven joints the raw limits correspond to public limits
1715  rightArmCtrl->setLimitsRaw( i, rightArmMin[i], rightArmMax[i] );
1716  }
1717  rightArmCtrl->setLimits( i, rightArmMin[i], rightArmMax[i] );
1718  rightArmCtrl->setEnableLimitsRaw( i, true );
1719  // Setting limits for iKin chain
1720  if ((i + 3) < kinRightArm.getN()) {
1721  kinRightArm.getLinkInfo(i + 3).setiKinLinkLimits(toRad(rightArmMin[i]), toRad(rightArmMax[i]));
1722  }
1723  }
1724  //--- the joints of the hand has different angular movement respect from what reported by robotMotorGUI
1725  // This mismatch is implemented using setLimitsRaw of MultiMotorController that will change the
1726  // real movement of the joint, but will not change what reported on the robotMotorGUI
1727  rightArmCtrl->setLimitsRaw( 7, -15, 0 );
1728  rightArmCtrl->setLimitsRaw( 8, 10, 90 );
1729  rightArmCtrl->setLimitsRaw( 9, 0, 90 );
1730  rightArmCtrl->setLimitsRaw( 10, 0, 90 );
1731  rightArmCtrl->setLimitsRaw( 11, 0, 90 );
1732  rightArmCtrl->setLimitsRaw( 12, 0, 90 );
1733  rightArmCtrl->setLimitsRaw( 13, 0, 90 );
1734  rightArmCtrl->setLimitsRaw( 14, 0, 90 );
1735  rightArmCtrl->setLimitsRaw( 15, 0, 90 );
1736  rightArmCtrl->setOwner(this, false);
1737  if ( enabledServerControlBoards ) {
1738  registerServerControlBoard( rightArmCtrl, "right_arm" );
1739  }
1740  counter += motors.size();
1741 
1742  //--- create MotorController for HEAD-NECK
1743  motors.clear();
1744  motors << headNeckJointv[0]->dofs()[0];
1745  motors << headNeckJointv[1]->dofs()[0];
1746  motors << headNeckJointv[2]->dofs()[0];
1747  motors << headNeckJointv[3]->dofs()[0];
1748  motors << versionDOF;
1749  motors << vergenceDOF;
1750  for( int i=0; i<6; i++ ) {
1751  motors[i]->setLimits( toRad(headMin[i]), toRad(headMax[i]) );
1752  motors[i]->enableLimits();
1753 
1754  if ((i + 3) < 7) {
1755  kinRightEye.getLinkInfo(i + 3).setiKinLinkLimits(toRad(headMin[i]), toRad(headMax[i]));
1756  }
1757  }
1758  // Setting limits on the last joint of right and left eye kinematic chain. They are computed from the limits
1759  // for version and vergence (4 is the limit for version, 5 for vergence)
1760  const real minEye = max(headMin[4] + headMin[5], -179.99999);
1761  const real maxEye = min(headMax[4] + headMax[5], +179.99999);
1762  headNeckJointv[4]->dofs()[0]->setLimits( toRad(minEye), toRad(maxEye) );
1763  headNeckJointv[4]->dofs()[0]->enableLimits();
1764  headNeckJointv[5]->dofs()[0]->setLimits( toRad(minEye), toRad(maxEye) );
1765  headNeckJointv[5]->dofs()[0]->enableLimits();
1766  kinRightEye.getLinkInfo(7).setiKinLinkLimits(toRad(minEye), toRad(maxEye));
1767  kinLeftEye.getLinkInfo(7).setiKinLinkLimits(toRad(minEye), toRad(maxEye));
1768  // Creating controller for head
1769  headNeckCtrl = new MultiMotorController( motors, world );
1770  headNeckCtrl->setOwner(this, false);
1771  if ( enabledServerControlBoards ) {
1772  registerServerControlBoard( headNeckCtrl, "head" );
1773  }
1774  counter += motors.size();
1775 
1776  //--- create MotorController for LEFT LEG
1777  motors.clear();
1778  for( int i=0; i<leftLegJointv.size(); i++ ) {
1779  motors << leftLegJointv[i]->dofs()[0];
1780  motors[i]->setLimits( toRad(leftLegMin[i]), toRad(leftLegMax[i]) );
1781  motors[i]->enableLimits();
1782 
1783  // Setting limits for iKin chain
1784  kinLeftLeg.getLinkInfo(i).setiKinLinkLimits(toRad(leftLegMin[i]), toRad(leftLegMax[i]));
1785  }
1786  leftLegCtrl = new MultiMotorController( motors, world );
1787  leftLegCtrl->setOwner(this, false);
1788  if ( enabledServerControlBoards ) {
1789  registerServerControlBoard( leftLegCtrl, "left_leg" );
1790  }
1791  counter += motors.size();
1792 
1793  //--- create MotorController for RIGHT LEG
1794  motors.clear();
1795  for( int i=0; i<rightLegJointv.size(); i++ ) {
1796  motors << rightLegJointv[i]->dofs()[0];
1797  motors[i]->setLimits( toRad(rightLegMin[i]), toRad(rightLegMax[i]) );
1798  motors[i]->enableLimits();
1799 
1800  // Setting limits for iKin chain
1801  kinRightLeg.getLinkInfo(i).setiKinLinkLimits(toRad(rightLegMin[i]), toRad(rightLegMax[i]));
1802  }
1803  rightLegCtrl = new MultiMotorController( motors, world );
1804  rightLegCtrl->setOwner(this, false);
1805  if ( enabledServerControlBoards ) {
1806  registerServerControlBoard( rightLegCtrl, "right_leg" );
1807  }
1808  counter += motors.size();
1809 
1810  //--- Creating the COVERS
1811  wMatrix matrix = wMatrix::yaw( PI_GRECO * 0.5f ) * wMatrix::pitch( PI_GRECO*0.5f );
1812  // Previous vector values: wVector(0.0, 0.034 /*0.037*/, 0.0)
1813  matrix.w_pos = matrix.rotateVector( wVector(0.0, 0.05f, -0.013f) );
1814  WMesh* mesh = new WMesh( world, name+":faceCover", matrix );
1815  mesh->loadMS3DModel( ":/covers/head.ms3d" );
1816  mesh->setTexture( "icubFace" );
1817  mesh->attachTo( headNeckv[2] );
1818  mesh->setOwner(this, false);
1819  coversv << mesh;
1820 
1821  //--- counting the number of effective DOF of joints created
1822  int counter2= 0;
1823  foreach( PhyJoint* joint, leftLegJointv ) {
1824  counter2 += joint->numDofs();
1825  joint->setOwner(this, false);
1826  }
1827  foreach( PhyJoint* joint, rightLegJointv ) {
1828  counter2 += joint->numDofs();
1829  joint->setOwner(this, false);
1830  }
1831  foreach( PhyJoint* joint, torsoJointv ) {
1832  counter2 += joint->numDofs();
1833  joint->setOwner(this, false);
1834  }
1835  foreach( PhyJoint* joint, leftArmJointv ) {
1836  counter2 += joint->numDofs();
1837  joint->setOwner(this, false);
1838  }
1839  foreach( PhyJoint* joint, rightArmJointv ) {
1840  counter2 += joint->numDofs();
1841  joint->setOwner(this, false);
1842  }
1843  foreach( PhyJoint* joint, headNeckJointv ) {
1844  counter2 += joint->numDofs();
1845  joint->setOwner(this, false);
1846  }
1847  //qDebug( "Total Joints: %d - %d", counter, counter2 );
1848 
1849  //--- calculate the total mass of the robot
1850  //--- and setting up masses vectors
1851  //--- This is required for enable/disable operations
1852  //--- Also Setting the Texture
1853  real mass = 0.0;
1854  leftLegMasses.resize(0);
1855  foreach( PhyObject* obj, leftLegv ) {
1856  mass += obj->mass();
1857  leftLegMasses << obj->mass();
1858  obj->setTexture( "icub" );
1859  obj->setOwner(this, false);
1860  }
1861  rightLegMasses.resize(0);
1862  foreach( PhyObject* obj, rightLegv ) {
1863  mass += obj->mass();
1864  rightLegMasses << obj->mass();
1865  obj->setTexture( "icub" );
1866  obj->setOwner(this, false);
1867  }
1868  torsoMasses.resize(0);
1869  foreach( PhyObject* obj, torsov ) {
1870  mass += obj->mass();
1871  torsoMasses << obj->mass();
1872  obj->setTexture( "icub" );
1873  obj->setOwner(this, false);
1874  }
1875  leftArmMasses.resize(0);
1876  foreach( PhyObject* obj, leftArmv ) {
1877  mass += obj->mass();
1878  leftArmMasses << obj->mass();
1879  obj->setTexture( "icub" );
1880  obj->setOwner(this, false);
1881  }
1882  rightArmMasses.resize(0);
1883  foreach( PhyObject* obj, rightArmv ) {
1884  mass += obj->mass();
1885  rightArmMasses << obj->mass();
1886  obj->setTexture( "icub" );
1887  obj->setOwner(this, false);
1888  }
1889  headNeckMasses.resize(0);
1890  foreach( PhyObject* obj, headNeckv ) {
1891  mass += obj->mass();
1892  headNeckMasses << obj->mass();
1893  obj->setTexture( "icub" );
1894  obj->setOwner(this, false);
1895  }
1896  headNeckv[4]->setTexture( "blueye" );
1897  headNeckv[5]->setTexture( "blueye" );
1898  //qDebug( "Total Mass: %g", mass );
1899 
1900  setTexture( "icub" );
1901  world->pushObject( this );
1902  }
1903 
1904  PhyiCub::~PhyiCub() {
1905  if ( cartCtrlLeftArm ) {
1906  //delete cartCtrlLeftArm;
1907  delete cartServLeftArm;
1908  delete cartSolvLeftArm;
1909  }
1910  if ( cartCtrlRightArm ) {
1911  //delete cartCtrlRightArm;
1912  delete cartServRightArm;
1913  delete cartSolvRightArm;
1914  }
1915  if ( enabledServerControlBoards ) {
1916  //--- stops the serverControlBoards
1917  QStringList servers;
1918  servers << "torso" << "left_arm" << "right_arm" << "head" << "left_leg" << "right_leg";
1919  foreach( QString srv, servers ) {
1920  //--- this call also delete all MultiMotorControllers attached to ServerControlBoards
1921  removeServerControlBoard( srv );
1922  }
1923  } else {
1924  //--- when there aren't ServerControlBoards enabled, it is necessary to destroy MultiMotorControllers
1925  delete torsoCtrl;
1926  delete leftArmCtrl;
1927  delete rightArmCtrl;
1928  delete headNeckCtrl;
1929  delete leftLegCtrl;
1930  delete rightLegCtrl;
1931  }
1932  if ( !leftcam ) {
1933  delete leftcam;
1934  delete rightcam;
1935  }
1936  for( int i=0; i<coversv.size(); i++ ) {
1937  delete (coversv[i]);
1938  }
1939  foreach( PhyJoint* joint, leftLegJointv ) {
1940  delete joint;
1941  }
1942  foreach( PhyJoint* joint, rightLegJointv ) {
1943  delete joint;
1944  }
1945  foreach( PhyJoint* joint, torsoJointv ) {
1946  delete joint;
1947  }
1948  foreach( PhyJoint* joint, leftArmJointv ) {
1949  delete joint;
1950  }
1951  foreach( PhyJoint* joint, rightArmJointv ) {
1952  delete joint;
1953  }
1954  foreach( PhyJoint* joint, headNeckJointv ) {
1955  delete joint;
1956  }
1957  foreach( PhyObject* obj, leftLegv ) {
1958  delete obj;
1959  }
1960  foreach( PhyObject* obj, rightLegv ) {
1961  delete obj;
1962  }
1963  foreach( PhyObject* obj, torsov ) {
1964  delete obj;
1965  }
1966  foreach( PhyObject* obj, leftArmv ) {
1967  delete obj;
1968  }
1969  foreach( PhyObject* obj, rightArmv ) {
1970  delete obj;
1971  }
1972  foreach( PhyObject* obj, headNeckv ) {
1973  delete obj;
1974  }
1975  delete leftHandGroup;
1976  delete rightHandGroup;
1977  delete vergenceDOF;
1978  delete versionDOF;
1979  }
1980 
1981  void PhyiCub::doKinematicSimulation(bool k, bool clh, bool crh)
1982  {
1983  kinematicSimulation = k;
1984  collidingLeftHand = clh;
1985  collidingRightHand = crh;
1986 
1987  if (kinematicSimulation) {
1988  // First disabling all joints...
1989  for (int i = 0; i < leftLegJointv.size(); i++) {
1990  leftLegJointv[i]->enable(false);
1991  }
1992  for (int i = 0; i < rightLegJointv.size(); i++) {
1993  rightLegJointv[i]->enable(false);
1994  }
1995  for (int i = 0; i < torsoJointv.size(); i++) {
1996  torsoJointv[i]->enable(false);
1997  }
1998  for (int i = 0; i < leftArmJointv.size(); i++) {
1999  if (collidingLeftHand && (i >= 6)) {
2000  leftArmJointv[i]->enable(true);
2001  } else {
2002  leftArmJointv[i]->enable(false);
2003  }
2004  }
2005  for (int i = 0; i < rightArmJointv.size(); i++) {
2006  if (collidingRightHand && (i >= 6)) {
2007  rightArmJointv[i]->enable(true);
2008  } else {
2009  rightArmJointv[i]->enable(false);
2010  }
2011  }
2012  for (int i = 0; i < headNeckJointv.size(); i++) {
2013  headNeckJointv[i]->enable(false);
2014  }
2015 
2016  // ... then setting all objects to kinematic behaviour
2017  for (int i = 0; i < leftLegv.size(); i++) {
2018  leftLegv[i]->setKinematic(true);
2019  }
2020  for (int i = 0; i < rightLegv.size(); i++) {
2021  rightLegv[i]->setKinematic(true);
2022  }
2023  for (int i = 0; i < torsov.size(); i++) {
2024  torsov[i]->setKinematic(true);
2025  }
2026  for (int i = 0; i < leftArmv.size(); i++) {
2027  if (collidingLeftHand) {
2028  if (i < 5) {
2029  leftArmv[i]->setKinematic(true, false);
2030  } else if (i == 5) {
2031  leftArmv[i]->setKinematic(true, true);
2032  } else {
2033  leftArmv[i]->setKinematic(false);
2034  }
2035  } else {
2036  leftArmv[i]->setKinematic(true, false);
2037  }
2038  }
2039  for (int i = 0; i < rightArmv.size(); i++) {
2040  if (collidingRightHand) {
2041  if (i < 5) {
2042  rightArmv[i]->setKinematic(true, false);
2043  } else if (i == 5) {
2044  rightArmv[i]->setKinematic(true, true);
2045  } else {
2046  rightArmv[i]->setKinematic(false);
2047  }
2048  } else {
2049  rightArmv[i]->setKinematic(true, false);
2050  }
2051  }
2052  for (int i = 0; i < headNeckv.size(); i++) {
2053  headNeckv[i]->setKinematic(true);
2054  }
2055  } else {
2056  // First setting all objects to dynamic behaviour...
2057  for (int i = 0; i < leftLegv.size(); i++) {
2058  leftLegv[i]->setKinematic( !enabledLeftLeg );
2059  }
2060  for (int i = 0; i < rightLegv.size(); i++) {
2061  rightLegv[i]->setKinematic( !enabledRightLeg );
2062  }
2063  for (int i = 0; i < torsov.size(); i++) {
2064  torsov[i]->setKinematic( !enabledTorso );
2065  }
2066  for (int i = 0; i < leftArmv.size(); i++) {
2067  leftArmv[i]->setKinematic( !enabledLeftArm );
2068  }
2069  for (int i = 0; i < rightArmv.size(); i++) {
2070  rightArmv[i]->setKinematic( !enabledRightArm );
2071  }
2072  for (int i = 0; i < headNeckv.size(); i++) {
2073  headNeckv[i]->setKinematic( !enabledHead );
2074  }
2075 
2076  // ... then enabling all joints (if the corresponding part is enabled)
2077  for (int i = 0; i < leftLegJointv.size(); i++) {
2078  leftLegJointv[i]->enable(enabledLeftLeg);
2079  if (enabledLeftLeg) {
2080  leftLegJointv[i]->updateJointInfo();
2081  }
2082  }
2083  for (int i = 0; i < rightLegJointv.size(); i++) {
2084  rightLegJointv[i]->enable(enabledRightLeg);
2085  if (enabledRightLeg) {
2086  rightLegJointv[i]->updateJointInfo();
2087  }
2088  }
2089  for (int i = 0; i < torsoJointv.size(); i++) {
2090  torsoJointv[i]->enable(enabledTorso);
2091  if (enabledTorso) {
2092  torsoJointv[i]->updateJointInfo();
2093  }
2094  }
2095  for (int i = 0; i < leftArmJointv.size(); i++) {
2096  leftArmJointv[i]->enable(enabledLeftArm);
2097  if (enabledLeftArm) {
2098  leftArmJointv[i]->updateJointInfo();
2099  }
2100  }
2101  for (int i = 0; i < rightArmJointv.size(); i++) {
2102  rightArmJointv[i]->enable(enabledRightArm);
2103  if (enabledRightArm) {
2104  rightArmJointv[i]->updateJointInfo();
2105  }
2106  }
2107  for (int i = 0; i < headNeckJointv.size(); i++) {
2108  headNeckJointv[i]->enable(enabledHead);
2109  if (enabledHead) {
2110  headNeckJointv[i]->updateJointInfo();
2111  }
2112  }
2113  }
2114  }
2115 
2116  void PhyiCub::configurePosture( QMap<int, real> jointSetup )
2117  {
2118 #ifdef __GNUC__
2119  #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)
2120 #endif
2121 #ifdef __GNUC__
2122  #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?
2123 #endif
2124  // First of all storing the new relative positions of objects in objects groups
2125  leftHandGroup->updateRelativePositions();
2126  rightHandGroup->updateRelativePositions();
2127 
2128  // Setting new angles in the kinematic chains
2129  for (QMap<int, real>::const_iterator it = jointSetup.begin(); it != jointSetup.end(); it++) {
2130  // Big switch to set new postures in the kinematic chains. Joints which are not in
2131  // a kinematic chain are checked below, after these have been set in the new position
2132  switch (it.key()) {
2133  case torso_yaw:
2134  kinRightArm.getLinkInfo(2).setLinkAngle(toRad(it.value()));
2135  break;
2136  case torso_roll:
2137  kinRightArm.getLinkInfo(1).setLinkAngle(toRad(it.value()));
2138  break;
2139  case torso_pitch:
2140  kinRightArm.getLinkInfo(0).setLinkAngle(toRad(it.value()));
2141  break;
2142  case left_shoulder_pitch:
2143  kinLeftArm.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2144  break;
2145  case left_shoulder_roll:
2146  kinLeftArm.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2147  break;
2148  case left_shoulder_yaw:
2149  kinLeftArm.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2150  break;
2151  case left_elbow:
2152  kinLeftArm.getLinkInfo(6).setLinkAngle(toRad(it.value()));
2153  break;
2154  case left_wrist_prosup:
2155  kinLeftArm.getLinkInfo(7).setLinkAngle(toRad(it.value()));
2156  break;
2157  case left_wrist_pitch:
2158  kinLeftArm.getLinkInfo(8).setLinkAngle(toRad(it.value()));
2159  break;
2160  case left_wrist_yaw:
2161  kinLeftArm.getLinkInfo(9).setLinkAngle(toRad(it.value()));
2162  break;
2163  case right_shoulder_pitch:
2164  kinRightArm.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2165  break;
2166  case right_shoulder_roll:
2167  kinRightArm.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2168  break;
2169  case right_shoulder_yaw:
2170  kinRightArm.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2171  break;
2172  case right_elbow:
2173  kinRightArm.getLinkInfo(6).setLinkAngle(toRad(it.value()));
2174  break;
2175  case right_wrist_prosup:
2176  kinRightArm.getLinkInfo(7).setLinkAngle(toRad(it.value()));
2177  break;
2178  case right_wrist_pitch:
2179  kinRightArm.getLinkInfo(8).setLinkAngle(toRad(it.value()));
2180  break;
2181  case right_wrist_yaw:
2182  kinRightArm.getLinkInfo(9).setLinkAngle(toRad(it.value()));
2183  break;
2184  case neck_pitch:
2185  kinRightEye.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2186  break;
2187  case neck_roll:
2188  kinRightEye.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2189  break;
2190  case neck_yaw:
2191  kinRightEye.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2192  break;
2193  case eyes_tilt:
2194  kinRightEye.getLinkInfo(6).setLinkAngle(toRad(it.value()));
2195  break;
2196  case eyes_version: case eyes_vergence:
2197  {
2198  // Here we need to use both vergence and version. We use the current angles
2199  // unless they are in the posture map
2200  real versionAngle;
2201  if (jointSetup. contains(eyes_version)) {
2202  versionAngle = toRad(jointSetup[eyes_version]);
2203  } else {
2204  versionAngle = versionDOF->position();
2205  }
2206  real vergenceAngle;
2207  if (jointSetup. contains(eyes_vergence)) {
2208  vergenceAngle = toRad(jointSetup[eyes_vergence]);
2209  } else {
2210  vergenceAngle = vergenceDOF->position();
2211  }
2212 
2213  // Now computing the new positions for the right and left eye
2214  const real rightEyeAngle = rightEyeFromVersionAndVergence(versionAngle, vergenceAngle);
2215  const real leftEyeAngle = leftEyeFromVersionAndVergence(versionAngle, vergenceAngle);
2216 
2217  // Finally setting angles in both the right and left kinemati link
2218  kinRightEye.getLinkInfo(7).setLinkAngle(rightEyeAngle);
2219  kinLeftEye.getLinkInfo(7).setLinkAngle(leftEyeAngle);
2220  }
2221  break;
2222  case left_hip_pitch:
2223  kinLeftLeg.getLinkInfo(0).setLinkAngle(toRad(it.value()));
2224  break;
2225  case left_hip_roll:
2226  kinLeftLeg.getLinkInfo(1).setLinkAngle(toRad(it.value()));
2227  break;
2228  case left_hip_yaw:
2229  kinLeftLeg.getLinkInfo(2).setLinkAngle(toRad(it.value()));
2230  break;
2231  case left_knee:
2232  kinLeftLeg.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2233  break;
2234  case left_ankle_pitch:
2235  kinLeftLeg.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2236  break;
2237  case left_ankle_roll:
2238  kinLeftLeg.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2239  break;
2240  case right_hip_pitch:
2241  kinRightLeg.getLinkInfo(0).setLinkAngle(toRad(it.value()));
2242  break;
2243  case right_hip_roll:
2244  kinRightLeg.getLinkInfo(1).setLinkAngle(toRad(it.value()));
2245  break;
2246  case right_hip_yaw:
2247  kinRightLeg.getLinkInfo(2).setLinkAngle(toRad(it.value()));
2248  break;
2249  case right_knee:
2250  kinRightLeg.getLinkInfo(3).setLinkAngle(toRad(it.value()));
2251  break;
2252  case right_ankle_pitch:
2253  kinRightLeg.getLinkInfo(4).setLinkAngle(toRad(it.value()));
2254  break;
2255  case right_ankle_roll:
2256  kinRightLeg.getLinkInfo(5).setLinkAngle(toRad(it.value()));
2257  break;
2258  default:
2259  break;
2260  }
2261  }
2262 
2263  // We do this on all chains, perhaps we could just update those that have changed...
2264  kinRightArm.updateMatrixFromiKin();
2265  kinLeftArm.updateMatrixFromiKin();
2266  kinRightLeg.updateMatrixFromiKin();
2267  kinLeftLeg.updateMatrixFromiKin();
2268  kinRightEye.updateMatrixFromiKin();
2269  kinLeftEye.updateMatrixFromiKin();
2270 
2271  // Also updating object groups
2272  rightHandGroup->resetObjectPositions();
2273  leftHandGroup->resetObjectPositions();
2274 
2275  // Updating all joints (see comment above, perhaps we shouldn't update all)
2276  foreach( PhyJoint* joint, leftLegJointv ) {
2277  joint->updateJointInfo();
2278  }
2279  foreach( PhyJoint* joint, rightLegJointv ) {
2280  joint->updateJointInfo();
2281  }
2282  foreach( PhyJoint* joint, torsoJointv ) {
2283  joint->updateJointInfo();
2284  }
2285  foreach( PhyJoint* joint, leftArmJointv ) {
2286  joint->updateJointInfo();
2287  }
2288  foreach( PhyJoint* joint, rightArmJointv ) {
2289  joint->updateJointInfo();
2290  }
2291  foreach( PhyJoint* joint, headNeckJointv ) {
2292  joint->updateJointInfo();
2293  }
2294 
2295  // Setting new angles for objects which don't belong to any kinematic chain. Joints and objects
2296  // matrices are updated when they are moved
2297  for (QMap<int, real>::const_iterator it = jointSetup.begin(); it != jointSetup.end(); it++) {
2298  switch (it.key()) {
2299  case left_hand_finger:
2300  leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 0);
2301  leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 0);
2302  leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 0);
2303  break;
2304  case left_thumb_oppose:
2305  leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 0);
2306  break;
2307  case left_thumb_proximal:
2308  leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 1);
2309  break;
2310  case left_thumb_distal:
2311  leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 2);
2312  leftHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 3);
2313  break;
2314  case left_index_proximal:
2315  leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 1);
2316  break;
2317  case left_index_distal:
2318  leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 2);
2319  leftHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 3);
2320  break;
2321  case left_middle_proximal:
2322  leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 1);
2323  break;
2324  case left_middle_distal:
2325  leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 2);
2326  leftHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 3);
2327  break;
2328  case left_pinky:
2329  leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 1);
2330  leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 2);
2331  leftHandGroup->setDOFPosition(toRad(it.value()), RingChain, 3);
2332  leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 1);
2333  leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 2);
2334  leftHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 3);
2335  break;
2336  case right_hand_finger:
2337  rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 0);
2338  rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 0);
2339  rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 0);
2340  break;
2341  case right_thumb_oppose:
2342  rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 0);
2343  break;
2344  case right_thumb_proximal:
2345  rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 1);
2346  break;
2347  case right_thumb_distal:
2348  rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 2);
2349  rightHandGroup->setDOFPosition(toRad(it.value()), ThumbChain, 3);
2350  break;
2351  case right_index_proximal:
2352  rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 1);
2353  break;
2354  case right_index_distal:
2355  rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 2);
2356  rightHandGroup->setDOFPosition(toRad(it.value()), IndexChain, 3);
2357  break;
2358  case right_middle_proximal:
2359  rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 1);
2360  break;
2361  case right_middle_distal:
2362  rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 2);
2363  rightHandGroup->setDOFPosition(toRad(it.value()), MiddleChain, 3);
2364  break;
2365  case right_pinky:
2366  rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 1);
2367  rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 2);
2368  rightHandGroup->setDOFPosition(toRad(it.value()), RingChain, 3);
2369  rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 1);
2370  rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 2);
2371  rightHandGroup->setDOFPosition(toRad(it.value()), PinkyChain, 3);
2372  break;
2373  default:
2374  break;
2375  }
2376  }
2377 
2378  // Stops all Motor Controllers
2379  torsoCtrl->stop();
2380  leftArmCtrl->stop();
2381  rightArmCtrl->stop();
2382  headNeckCtrl->stop();
2383  leftLegCtrl->stop();
2384  rightLegCtrl->stop();
2385  }
2386 
2387  yarp::dev::IFrameGrabberImage* PhyiCub::leftEyeFrameGrabber()
2388  {
2389  return (leftcam == NULL) ? NULL : leftcam->getFrameGrabber();
2390  }
2391 
2392  yarp::dev::IFrameGrabberImage* PhyiCub::rightEyeFrameGrabber()
2393  {
2394  return (rightcam == NULL) ? NULL : rightcam->getFrameGrabber();
2395  }
2396 
2397  void PhyiCub::preUpdate()
2398  {
2399  // First of all updating motors
2400  if (torsoCtrl->isEnabled()) {
2401  torsoCtrl->update();
2402  }
2403  if (leftArmCtrl->isEnabled()) {
2404  leftArmCtrl->update();
2405  }
2406  if (rightArmCtrl->isEnabled()) {
2407  rightArmCtrl->update();
2408  }
2409  if (headNeckCtrl->isEnabled()) {
2410  headNeckCtrl->update();
2411  }
2412  if (leftLegCtrl->isEnabled()) {
2413  leftLegCtrl->update();
2414  }
2415  if (rightLegCtrl->isEnabled()) {
2416  rightLegCtrl->update();
2417  }
2418 
2419  // advance the cartesian servers
2420  //if ( cartThreadLeftArm ) {
2421  // cartThreadLeftArm->run();
2422  //}
2423 
2424  // Updating all enabled chains if in kinematic simulation (or updating all chains, even disabled ones
2425  // if alwaysUpdateKinematicChains is true)
2426  if (kinematicSimulation) {
2427  bool torsoUpdated = false;
2428  int lastLinkRight = -1;
2429  int lastLinkLeft = -1;
2430  if (enabledRightArm || alwaysUpdateKinematicChains) {
2431  if (enabledRightArm && collidingRightHand) {
2432  lastLinkRight = 8; // This kinematic chain also comprise torso, that's why we have to use this number
2433  }
2434  kinRightArm.updateMatrixFromiKin(0, lastLinkRight);
2435  torsoUpdated = true;
2436  }
2437  if (enabledLeftArm || alwaysUpdateKinematicChains) {
2438  if (enabledLeftArm && collidingLeftHand) {
2439  lastLinkLeft = 8; // This kinematic chain also comprise torso, that's why we have to use this number
2440  }
2441  kinLeftArm.updateMatrixFromiKin(0, lastLinkLeft);
2442  torsoUpdated = true;
2443  }
2444  if (enabledRightLeg || alwaysUpdateKinematicChains) {
2445  kinRightLeg.updateMatrixFromiKin();
2446  }
2447  if (enabledLeftLeg || alwaysUpdateKinematicChains) {
2448  kinLeftLeg.updateMatrixFromiKin();
2449  }
2450  if (enabledHead || alwaysUpdateKinematicChains) {
2451  kinRightEye.updateMatrixFromiKin();
2452  kinLeftEye.updateMatrixFromiKin();
2453  torsoUpdated = true;
2454  }
2455  // No need to check alwaysUpdateKinematicChains here, if it is true
2456  // the torso has already been updated
2457  if (!torsoUpdated && enabledTorso) {
2458  kinRightArm.updateMatrixFromiKin(0, lastLinkRight);
2459  }
2460 
2461  // Also updating object groups
2462  if (!collidingRightHand && (enabledRightArm || alwaysUpdateKinematicChains)) {
2463  rightHandGroup->resetObjectPositions();
2464  if (enabledRightKinematicHand) {
2465  rightHandGroup->updateFromDOF();
2466  }
2467  }
2468  if (!collidingLeftHand && (enabledLeftArm || alwaysUpdateKinematicChains)) {
2469  leftHandGroup->resetObjectPositions();
2470  if (enabledLeftKinematicHand) {
2471  leftHandGroup->updateFromDOF();
2472  }
2473  }
2474  }
2475  }
2476 
2477  void PhyiCub::postUpdate()
2478  {
2479  // Here we just have to update disabled kinematic chains if alwaysUpdateKinematicChains
2480  // is true and this is a dynamical simulation
2481  if (alwaysUpdateKinematicChains && !kinematicSimulation) {
2482  int upperBodyUpdateStartingChainLink = 3;
2483  if (!enabledTorso) {
2484  // If torso is disabled we update it when an arm or the head is updated (if
2485  // nothing is updated, then the torso is not updated as well)
2486  upperBodyUpdateStartingChainLink = 0;
2487  }
2488  if (!enabledRightArm) {
2489  kinRightArm.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
2490  }
2491  if (!enabledLeftArm) {
2492  kinLeftArm.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
2493  }
2494  if (!enabledRightLeg) {
2495  kinRightLeg.updateMatrixFromiKin();
2496  }
2497  if (!enabledLeftLeg) {
2498  kinLeftLeg.updateMatrixFromiKin();
2499  }
2500  if (!enabledHead) {
2501  kinRightEye.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
2502  kinLeftEye.updateMatrixFromiKin(upperBodyUpdateStartingChainLink);
2503  }
2504  }
2505  }
2506 
2507  void PhyiCub::enableLeftLeg( bool b ) {
2508  if ( enabledLeftLeg == b ) return;
2509  enabledLeftLeg = b;
2510  if ( b ) {
2511  enableObjectsAndLinks( true, leftLegv, leftLegJointv );
2512  leftLegCtrl->setEnabled( true );
2513  } else {
2514  leftLegCtrl->setEnabled( false );
2515  enableObjectsAndLinks( false, leftLegv, leftLegJointv );
2516  }
2517  }
2518 
2519  void PhyiCub::enableRightLeg( bool b ) {
2520  if ( enabledRightLeg == b ) return;
2521  enabledRightLeg = b;
2522  if ( b ) {
2523  enableObjectsAndLinks( true, rightLegv, rightLegJointv );
2524  rightLegCtrl->setEnabled( true );
2525  } else {
2526  rightLegCtrl->setEnabled( false );
2527  enableObjectsAndLinks( false, rightLegv, rightLegJointv );
2528  }
2529  }
2530 
2531  void PhyiCub::enableTorso( bool b ) {
2532  // This function is slightly different from enable functions of other robot parts
2533  // because the torso objects in dynamic simulations cannot be set to kinematic (as
2534  // done in enableObjectsAndLinks): doing so causes the first joint of arms and
2535  // head not to work correctly
2536  if ( enabledTorso == b ) return;
2537  enabledTorso = b;
2538  if ( b ) {
2539  for (int i = 0; i < torsov.size(); i++) {
2540  torsov[i]->setStatic(false);
2541  }
2542  if (!kinematicSimulation) {
2543  for (int i = 0; i < torsoJointv.size(); i++) {
2544  torsoJointv[i]->enable(true);
2545  torsoJointv[i]->updateJointInfo();
2546  }
2547  }
2548  torsoCtrl->setEnabled( true );
2549  } else {
2550  torsoCtrl->setEnabled( false );
2551  if (!kinematicSimulation) {
2552  for (int i = 0; i < torsoJointv.size(); i++) {
2553  torsoJointv[i]->enable(false);
2554  }
2555  }
2556  for (int i = 0; i < torsov.size(); i++) {
2557  torsov[i]->setStatic(true);
2558  }
2559  }
2560  }
2561 
2562  void PhyiCub::enableHead( bool b ) {
2563  if ( enabledHead == b ) return;
2564  enabledHead = b;
2565  if ( b ) {
2566  enableObjectsAndLinks( true, headNeckv, headNeckJointv );
2567  headNeckCtrl->setEnabled( true );
2568  } else {
2569  headNeckCtrl->setEnabled( false );
2570  enableObjectsAndLinks( false, headNeckv, headNeckJointv );
2571  }
2572  }
2573 
2574  void PhyiCub::enableCameras( bool b ) {
2575  if ( enabledCameras == b ) return;
2576  enabledCameras = b;
2577  if ( b && !leftcam ) {
2578  leftcam = new WCamera( world(), name()+"/cam/left", headNeckv[5], 640, 480 );
2579  rightcam = new WCamera( world(), name()+"/cam/right", headNeckv[4], 640, 480 );
2580  }
2581  if ( !b && leftcam ) {
2582  delete leftcam;
2583  delete rightcam;
2584  leftcam = 0;
2585  rightcam = 0;
2586  }
2587  }
2588 
2589  void PhyiCub::enableLeftArm( bool b ) {
2590  if ( enabledLeftArm == b ) return;
2591  enabledLeftArm = b;
2592  if ( b ) {
2593  enableObjectsAndLinks( true, leftArmv, leftArmJointv );
2594  leftArmCtrl->setEnabled( true );
2595  } else {
2596  leftArmCtrl->setEnabled( false );
2597  enableObjectsAndLinks( false, leftArmv, leftArmJointv );
2598  }
2599  }
2600 
2601  void PhyiCub::enableRightArm( bool b ) {
2602  if ( enabledRightArm == b ) return;
2603  enabledRightArm = b;
2604  if ( b ) {
2605  enableObjectsAndLinks( true, rightArmv, rightArmJointv );
2606  rightArmCtrl->setEnabled( true );
2607  } else {
2608  rightArmCtrl->setEnabled( false );
2609  enableObjectsAndLinks( false, rightArmv, rightArmJointv );
2610  }
2611  }
2612 
2613  void PhyiCub::blockTorso0( bool b ) {
2614  if ( blockedTorso0 == b ) return;
2615  blockedTorso0 = b;
2616  if ( b ) {
2617  torsov[0]->setMass( 0 );
2618  } else {
2619  torsov[0]->setMass( torsoMasses[0] );
2620  }
2621  }
2622 
2623  void PhyiCub::enableLeftKinematicHand( bool b )
2624  {
2625  enabledLeftKinematicHand = b;
2626  }
2627 
2628  void PhyiCub::enableRightKinematicHand( bool b )
2629  {
2630  enabledRightKinematicHand = b;
2631  }
2632 
2633  YARP_DECLARE_PLUGINS( icubmod )
2634  void PhyiCub::enableLeftArmCartesianController() {
2635  YARP_REGISTER_PLUGINS( icubmod );
2636  //--- Using directly the iKin library
2637  // declare the on-line arm solver called "solver"
2638  cartSolvLeftArm = new iCubArmCartesianSolver(
2639  QString( "%1/%2/cartesianSolver/left_arm" )
2640  .arg( world()->name() )
2641  .arg( name() )
2642  .toAscii().data()
2643  );
2644  Property options( QString("\
2645  (robot %1/%2) \
2646  (type left) \
2647  (pose full) \
2648  (dof (1 1 1 1 1 1 1 1 1 1) ) \
2649  (verbosity off)")
2650  .arg( world()->name() )
2651  .arg( name() )
2652  .toAscii().data() );
2653  cartSolvLeftArm->open( options );
2654  //--- create the CartesianController and export its interface
2655  // the idea is to create a cartesiancontrollerserver and
2656  // using the IMultipleWrapper attaching the PolyDriver created above
2657  // for the arms to the corresponding cartesiancontrollerserver and
2658  // finally getting the ICartesianInterface from cartesiancontrollerserver
2659  Property optServerLeftArm( QString( "\
2660  (device cartesiancontrollerserver) \
2661  (GENERAL (ControllerName %1/%2/cartesianController/left_arm) \
2662  (ControllerPeriod 10) \
2663  (SolverNameToConnect %1/%2/cartesianSolver/left_arm) \
2664  (KinematicPart arm) \
2665  (KinematicType left) \
2666  (NumberOfDrivers 2)) \
2667  (DRIVER_0 (Key torso) (JointsOrder reversed)) \
2668  (DRIVER_1 (Key left_arm) (JointsOrder direct))" )
2669  .arg( world()->name() )
2670  .arg( name() )
2671  .toAscii().data() );
2672  cartServLeftArm = new PolyDriver();
2673  if ( !cartServLeftArm->open( optServerLeftArm ) ) {
2674  //--- non posso aggiungere l'interfaccia cartesiana, peccato
2675  } else {
2676  //--- ok, wrappo
2677  PolyDriverList listL;
2678  listL.push( polydriver("torso"), "torso" );
2679  listL.push( polydriver("left_arm"), "left_arm" );
2680  IMultipleWrapper *wrapperL;
2681  cartServLeftArm->view( wrapperL );
2682  if ( !wrapperL->attachAll(listL) ) {
2683  //--- questo e' un errore !! :-S
2684  qDebug() << "Errore nell'attaccare i PolyDriver al Server Cartesiano";
2685  }
2686  //--- esporto ICartesionInterface
2687  cartServLeftArm->view( cartCtrlLeftArm );
2688  //--- suspend the rateThread and update the cartesianController in sync with the simulator
2689  //cartServLeftArm->view( cartThreadLeftArm );
2690  //cartThreadLeftArm->suspend();
2691  }
2692  }
2693 
2694  void PhyiCub::enableRightArmCartesianController() {
2695  YARP_REGISTER_PLUGINS( icubmod );
2696  //--- Using directly the iKin library
2697  // declare the on-line arm solver called "solver"
2698  cartSolvRightArm = new iCubArmCartesianSolver(
2699  QString( "%1/%2/cartesianSolver/right_arm" )
2700  .arg( world()->name() )
2701  .arg( name() )
2702  .toAscii().data()
2703  );
2704  Property options( QString("\
2705  (robot %1/%2) \
2706  (type right) \
2707  (pose full) \
2708  (verbosity off)")
2709  .arg( world()->name() )
2710  .arg( name() )
2711  .toAscii().data() );
2712  cartSolvRightArm->open( options );
2713  //--- create the CartesianController and export its interface
2714  // the idea is to create a cartesiancontrollerserver and
2715  // using the IMultipleWrapper attaching the PolyDriver created above
2716  // for the arms to the corresponding cartesiancontrollerserver and
2717  // finally getting the ICartesianInterface from cartesiancontrollerserver
2718  Property optServerRightArm( QString( "\
2719  (device cartesiancontrollerserver) \
2720  (GENERAL (ControllerName %1/%2/cartesianController/right_arm) \
2721  (ControllerPeriod 10) \
2722  (SolverNameToConnect %1/%2/cartesianSolver/right_arm) \
2723  (KinematicPart arm) \
2724  (KinematicType right) \
2725  (NumberOfDrivers 2)) \
2726  (DRIVER_0 (Key torso) (JointsOrder reversed)) \
2727  (DRIVER_1 (Key right_arm) (JointsOrder direct))" )
2728  .arg( world()->name() )
2729  .arg( name() )
2730  .toAscii().data() );
2731  cartServRightArm = new PolyDriver();
2732  if ( !cartServRightArm->open( optServerRightArm ) ) {
2733  //--- non posso aggiungere l'interfaccia cartesiana, peccato
2734  } else {
2735  //--- ok, wrappo
2736  PolyDriverList listL;
2737  listL.push( polydriver("torso"), "torso" );
2738  listL.push( polydriver("right_arm"), "right_arm" );
2739  IMultipleWrapper *wrapperL;
2740  cartServRightArm->view( wrapperL );
2741  if ( !wrapperL->attachAll(listL) ) {
2742  //--- questo e' un errore !! :-S
2743  qDebug() << "Errore nell'attaccare i PolyDriver al Server Cartesiano";
2744  }
2745  //--- esporto ICartesionInterface
2746  cartServRightArm->view( cartCtrlRightArm );
2747  }
2748  }
2749 
2750  void PhyiCub::changedMatrix() {
2751  // Resetting the matrix for torso0
2752  setTorso0Matrix();
2753 
2754  // Updating all kinematic chains
2755  kinRightArm.updateMatrixFromiKin();
2756  kinLeftArm.updateMatrixFromiKin();
2757  kinRightLeg.updateMatrixFromiKin();
2758  kinLeftLeg.updateMatrixFromiKin();
2759  kinRightEye.updateMatrixFromiKin();
2760  kinLeftEye.updateMatrixFromiKin();
2761 
2762  // Also updating object groups
2763  rightHandGroup->resetObjectPositions();
2764  leftHandGroup->resetObjectPositions();
2765 
2766  // Finally updating all joints
2767  foreach( PhyJoint* joint, leftLegJointv ) {
2768  joint->updateJointInfo();
2769  }
2770  foreach( PhyJoint* joint, rightLegJointv ) {
2771  joint->updateJointInfo();
2772  }
2773  foreach( PhyJoint* joint, torsoJointv ) {
2774  joint->updateJointInfo();
2775  }
2776  foreach( PhyJoint* joint, leftArmJointv ) {
2777  joint->updateJointInfo();
2778  }
2779  foreach( PhyJoint* joint, rightArmJointv ) {
2780  joint->updateJointInfo();
2781  }
2782  foreach( PhyJoint* joint, headNeckJointv ) {
2783  joint->updateJointInfo();
2784  }
2785  }
2786 
2787  void PhyiCub::setTorso0Matrix()
2788  {
2789  // Computing matrix for torso0. We start from H0 for the right arm
2790  // kinematic chain (its the same matrix as that of all the other chains
2791  // comprising the torso)
2792  wMatrix torso0Matrix;
2793  convertYarpMatrixToWorldMatrix(kinRightArm.getH0(), torso0Matrix);
2794  torso0Matrix = torso0Matrix * tm;
2795  const real torso0Height = (dynamic_cast<PhyBox*>(torsov[0]))->sideX();
2796  // Now we translate the torso0 object so that it touches the legs solids
2797  // (we use Z translation of the legs chain H0 - remember that YARP matrices
2798  // are transposed with respect to wMatrix)
2799  torso0Matrix.w_pos += wVector(0.0, 0.0, kinRightLeg.getH0()(2, 3) + torso0Height / 4.0);
2800  torsov[0]->setMatrix(torso0Matrix);
2801  }
2802 
2803  void PhyiCub::enableObjectsAndLinks(bool enable, QVector<PhyObject*>& objects, QVector<PhyJoint*>& joints)
2804  {
2805  // The order (objects then joints or vice-versa) is important, that's the reason for the if.
2806  // If we are doing a kinematic simulation we don't do anything on joints
2807  if (enable) {
2808  for (int i = 0; i < objects.size(); i++) {
2809  objects[i]->setKinematic(false);
2810  }
2811  if (!kinematicSimulation) {
2812  for (int i = 0; i < joints.size(); i++) {
2813  joints[i]->enable(true);
2814  joints[i]->updateJointInfo();
2815  }
2816  }
2817  } else {
2818  if (!kinematicSimulation) {
2819  for (int i = 0; i < joints.size(); i++) {
2820  joints[i]->enable(false);
2821  }
2822  }
2823  for (int i = 0; i < objects.size(); i++) {
2824  objects[i]->setKinematic(true);
2825  }
2826  }
2827  }
2828 
2829  real PhyiCub::rightEyeFromVersionAndVergence(real version, real vergence)
2830  {
2831  return -version - vergence;
2832  }
2833 
2834  real PhyiCub::leftEyeFromVersionAndVergence(real version, real vergence)
2835  {
2836  return -version + vergence;
2837  }
2838 
2839  real PhyiCub::versionFromRightAndLeftEye(real right, real left)
2840  {
2841  return (-left - right) / 2.0;
2842  }
2843 
2844  real PhyiCub::vergenceFromRightAndLeftEye(real right, real left)
2845  {
2846  return (left - right) / 2.0;
2847  }
2848 
2849  real PhyiCub::lowEyeLimitFromVersionAndVergenceLimits(real versionLow, real vergenceLow)
2850  {
2851  const real lowEyeLimit = versionLow + vergenceLow;
2852 
2853  return (lowEyeLimit < -PI_GRECO) ? -(PI_GRECO - 0.000001) : lowEyeLimit;
2854  }
2855 
2856  real PhyiCub::highEyeLimitFromVersionAndVergenceLimits(real versionHigh, real vergenceHigh)
2857  {
2858  const real highEyeLimit = versionHigh + vergenceHigh;
2859 
2860  return (highEyeLimit > PI_GRECO) ? (PI_GRECO - 0.000001) : highEyeLimit;
2861  }
2862 
2863 #ifdef __GNUC__
2864  #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
2865 #endif
2866 
2867  void PhyiCub::versionChangedDesiredPosition( real wishpos )
2868  {
2869  if (ignoreEyeSignals) {
2870  return;
2871  }
2872 
2873  const real curVergence = vergenceDOF->desiredPosition();
2874  const real rightEyeAngle = rightEyeFromVersionAndVergence(wishpos, curVergence);
2875  const real leftEyeAngle = leftEyeFromVersionAndVergence(wishpos, curVergence);
2876 
2877  ignoreEyeSignals = true;
2878  rightEyeDOF->setDesiredPosition(rightEyeAngle);
2879  leftEyeDOF->setDesiredPosition(leftEyeAngle);
2880  ignoreEyeSignals = false;
2881  }
2882 
2883  void PhyiCub::versionChangedDesiredVelocity( real wishvel )
2884  {
2885  if (ignoreEyeSignals) {
2886  return;
2887  }
2888 
2889  const real curVergenceVelocity = vergenceDOF->desiredVelocity();
2890  const real rightEyeVelocity = rightEyeFromVersionAndVergence(wishvel, curVergenceVelocity);
2891  const real leftEyeVelocity = leftEyeFromVersionAndVergence(wishvel, curVergenceVelocity);
2892 
2893  ignoreEyeSignals = true;
2894  rightEyeDOF->setDesiredVelocity(rightEyeVelocity);
2895  leftEyeDOF->setDesiredVelocity(leftEyeVelocity);
2896  ignoreEyeSignals = false;
2897  }
2898 
2899  void PhyiCub::versionChangedPosition( real newpos )
2900  {
2901  if (ignoreEyeSignals) {
2902  return;
2903  }
2904 
2905  const real curVergence = vergenceDOF->position();
2906  const real rightEyeAngle = rightEyeFromVersionAndVergence(newpos, curVergence);
2907  const real leftEyeAngle = leftEyeFromVersionAndVergence(newpos, curVergence);
2908 
2909  ignoreEyeSignals = true;
2910  rightEyeDOF->setPosition(rightEyeAngle);
2911  leftEyeDOF->setPosition(leftEyeAngle);
2912  ignoreEyeSignals = false;
2913  }
2914 
2915  void PhyiCub::versionChangedVelocity( real newvel )
2916  {
2917  if (ignoreEyeSignals) {
2918  return;
2919  }
2920 
2921  const real curVergenceVelocity = vergenceDOF->velocity();
2922  const real rightEyeVelocity = rightEyeFromVersionAndVergence(newvel, curVergenceVelocity);
2923  const real leftEyeVelocity = leftEyeFromVersionAndVergence(newvel, curVergenceVelocity);
2924 
2925  ignoreEyeSignals = true;
2926  rightEyeDOF->setVelocity(rightEyeVelocity);
2927  leftEyeDOF->setVelocity(leftEyeVelocity);
2928  ignoreEyeSignals = false;
2929  }
2930 
2931  void PhyiCub::versionChangedLimits( real lowlimit, real highlimit )
2932  {
2933  if (ignoreEyeSignals) {
2934  return;
2935  }
2936 
2937  real curVergenceLow, curVergenceHigh;
2938  vergenceDOF->limits(curVergenceLow, curVergenceHigh);
2939  const real eyeLow = lowEyeLimitFromVersionAndVergenceLimits(lowlimit, curVergenceLow);
2940  const real eyeHigh = highEyeLimitFromVersionAndVergenceLimits(highlimit, curVergenceHigh);
2941 
2942  ignoreEyeSignals = true;
2943  rightEyeDOF->setLimits(eyeLow, eyeHigh);
2944  leftEyeDOF->setLimits(eyeLow, eyeHigh);
2945  ignoreEyeSignals = false;
2946  }
2947 
2948  void PhyiCub::vergenceChangedDesiredPosition( real wishpos )
2949  {
2950  if (ignoreEyeSignals) {
2951  return;
2952  }
2953 
2954  const real curVersion = versionDOF->desiredPosition();
2955  const real rightEyeAngle = rightEyeFromVersionAndVergence(curVersion, wishpos);
2956  const real leftEyeAngle = leftEyeFromVersionAndVergence(curVersion, wishpos);
2957 
2958  ignoreEyeSignals = true;
2959  rightEyeDOF->setDesiredPosition(rightEyeAngle);
2960  leftEyeDOF->setDesiredPosition(leftEyeAngle);
2961  ignoreEyeSignals = false;
2962  }
2963 
2964  void PhyiCub::vergenceChangedDesiredVelocity( real wishvel )
2965  {
2966  if (ignoreEyeSignals) {
2967  return;
2968  }
2969 
2970  const real curVersionVelocity = versionDOF->desiredVelocity();
2971  const real rightEyeVelocity = rightEyeFromVersionAndVergence(curVersionVelocity, wishvel);
2972  const real leftEyeVelocity = leftEyeFromVersionAndVergence(curVersionVelocity, wishvel);
2973 
2974  ignoreEyeSignals = true;
2975  rightEyeDOF->setDesiredVelocity(rightEyeVelocity);
2976  leftEyeDOF->setDesiredVelocity(leftEyeVelocity);
2977  ignoreEyeSignals = false;
2978  }
2979 
2980  void PhyiCub::vergenceChangedPosition( real newpos )
2981  {
2982  if (ignoreEyeSignals) {
2983  return;
2984  }
2985 
2986  const real curVersion = versionDOF->position();
2987  const real rightEyeAngle = rightEyeFromVersionAndVergence(curVersion, newpos);
2988  const real leftEyeAngle = leftEyeFromVersionAndVergence(curVersion, newpos);
2989 
2990  ignoreEyeSignals = true;
2991  rightEyeDOF->setPosition(rightEyeAngle);
2992  leftEyeDOF->setPosition(leftEyeAngle);
2993  ignoreEyeSignals = false;
2994  }
2995 
2996  void PhyiCub::vergenceChangedVelocity( real newvel )
2997  {
2998  if (ignoreEyeSignals) {
2999  return;
3000  }
3001 
3002  const real curVersionVelocity = versionDOF->velocity();
3003  const real rightEyeVelocity = rightEyeFromVersionAndVergence(curVersionVelocity, newvel);
3004  const real leftEyeVelocity = leftEyeFromVersionAndVergence(curVersionVelocity, newvel);
3005 
3006  ignoreEyeSignals = true;
3007  rightEyeDOF->setVelocity(rightEyeVelocity);
3008  leftEyeDOF->setVelocity(leftEyeVelocity);
3009  ignoreEyeSignals = false;
3010  }
3011 
3012  void PhyiCub::vergenceChangedLimits( real lowlimit, real highlimit )
3013  {
3014  if (ignoreEyeSignals) {
3015  return;
3016  }
3017 
3018  real curVersionLow, curVersionHigh;
3019  vergenceDOF->limits(curVersionLow, curVersionHigh);
3020  const real eyeLow = lowEyeLimitFromVersionAndVergenceLimits(curVersionLow, lowlimit);
3021  const real eyeHigh = highEyeLimitFromVersionAndVergenceLimits(curVersionHigh, highlimit);
3022 
3023  ignoreEyeSignals = true;
3024  rightEyeDOF->setLimits(eyeLow, eyeHigh);
3025  leftEyeDOF->setLimits(eyeLow, eyeHigh);
3026  ignoreEyeSignals = false;
3027  }
3028 
3029  void PhyiCub::rightEyeChangedDesiredPosition( real wishpos )
3030  {
3031  if (ignoreEyeSignals) {
3032  return;
3033  }
3034 
3035  const real curLeftEyeAngle = leftEyeDOF->position();
3036  const real versionAngle = versionFromRightAndLeftEye(wishpos, curLeftEyeAngle);
3037  const real vergenceAngle = vergenceFromRightAndLeftEye(wishpos, curLeftEyeAngle);
3038 
3039  ignoreEyeSignals = true;
3040  versionDOF->setDesiredPosition(versionAngle);
3041  vergenceDOF->setDesiredPosition(vergenceAngle);
3042  ignoreEyeSignals = false;
3043  }
3044 
3045  void PhyiCub::rightEyeChangedDesiredVelocity( real wishvel )
3046  {
3047  if (ignoreEyeSignals) {
3048  return;
3049  }
3050 
3051  const real curLeftEyeVelocity = leftEyeDOF->position();
3052  const real versionVelocity = versionFromRightAndLeftEye(wishvel, curLeftEyeVelocity);
3053  const real vergenceVelocity = vergenceFromRightAndLeftEye(wishvel, curLeftEyeVelocity);
3054 
3055  ignoreEyeSignals = true;
3056  versionDOF->setDesiredVelocity(versionVelocity);
3057  vergenceDOF->setDesiredVelocity(vergenceVelocity);
3058  ignoreEyeSignals = false;
3059  }
3060 
3061  void PhyiCub::rightEyeChangedPosition( real newpos )
3062  {
3063  if (ignoreEyeSignals) {
3064  return;
3065  }
3066 
3067  const real curLeftEyeAngle = leftEyeDOF->position();
3068  const real versionAngle = versionFromRightAndLeftEye(newpos, curLeftEyeAngle);
3069  const real vergenceAngle = vergenceFromRightAndLeftEye(newpos, curLeftEyeAngle);
3070 
3071  ignoreEyeSignals = true;
3072  versionDOF->setPosition(versionAngle);
3073  vergenceDOF->setPosition(vergenceAngle);
3074  ignoreEyeSignals = false;
3075  }
3076 
3077  void PhyiCub::rightEyeChangedVelocity( real newvel )
3078  {
3079  if (ignoreEyeSignals) {
3080  return;
3081  }
3082 
3083  const real curLeftEyeVelocity = leftEyeDOF->position();
3084  const real versionVelocity = versionFromRightAndLeftEye(newvel, curLeftEyeVelocity);
3085  const real vergenceVelocity = vergenceFromRightAndLeftEye(newvel, curLeftEyeVelocity);
3086 
3087  ignoreEyeSignals = true;
3088  versionDOF->setVelocity(versionVelocity);
3089  vergenceDOF->setVelocity(vergenceVelocity);
3090  ignoreEyeSignals = false;
3091  }
3092 
3093  void PhyiCub::rightEyeChangedLimits( real , real )
3094  {
3095  // This is not implemented as we expose vergence and version, so we should never change limtis
3096  // for one eye only. Moreover implementing this is not trivial because, given the left and right
3097  // eye limits, there is not a unique possibility for vergence and version
3098  }
3099 
3100  void PhyiCub::leftEyeChangedDesiredPosition( real wishpos )
3101  {
3102  if (ignoreEyeSignals) {
3103  return;
3104  }
3105 
3106  const real curRightEyeAngle = rightEyeDOF->position();
3107  const real versionAngle = versionFromRightAndLeftEye(curRightEyeAngle, wishpos);
3108  const real vergenceAngle = vergenceFromRightAndLeftEye(curRightEyeAngle, wishpos);
3109 
3110  ignoreEyeSignals = true;
3111  versionDOF->setDesiredPosition(versionAngle);
3112  vergenceDOF->setDesiredPosition(vergenceAngle);
3113  ignoreEyeSignals = false;
3114  }
3115 
3116  void PhyiCub::leftEyeChangedDesiredVelocity( real wishvel )
3117  {
3118  if (ignoreEyeSignals) {
3119  return;
3120  }
3121 
3122  const real curRightEyeVelocity = rightEyeDOF->position();
3123  const real versionVelocity = versionFromRightAndLeftEye(curRightEyeVelocity, wishvel);
3124  const real vergenceVelocity = vergenceFromRightAndLeftEye(curRightEyeVelocity, wishvel);
3125 
3126  ignoreEyeSignals = true;
3127  versionDOF->setDesiredVelocity(versionVelocity);
3128  vergenceDOF->setDesiredVelocity(vergenceVelocity);
3129  ignoreEyeSignals = false;
3130  }
3131 
3132  void PhyiCub::leftEyeChangedPosition( real newpos )
3133  {
3134  if (ignoreEyeSignals) {
3135  return;
3136  }
3137 
3138  const real curRightEyeAngle = rightEyeDOF->position();
3139  const real versionAngle = versionFromRightAndLeftEye(curRightEyeAngle, newpos);
3140  const real vergenceAngle = vergenceFromRightAndLeftEye(curRightEyeAngle, newpos);
3141 
3142  ignoreEyeSignals = true;
3143  versionDOF->setPosition(versionAngle);
3144  vergenceDOF->setPosition(vergenceAngle);
3145  ignoreEyeSignals = false;
3146  }
3147 
3148  void PhyiCub::leftEyeChangedVelocity( real newvel )
3149  {
3150  if (ignoreEyeSignals) {
3151  return;
3152  }
3153 
3154  const real curRightEyeVelocity = rightEyeDOF->position();
3155  const real versionVelocity = versionFromRightAndLeftEye(curRightEyeVelocity, newvel);
3156  const real vergenceVelocity = vergenceFromRightAndLeftEye(curRightEyeVelocity, newvel);
3157 
3158  ignoreEyeSignals = true;
3159  versionDOF->setVelocity(versionVelocity);
3160  vergenceDOF->setVelocity(vergenceVelocity);
3161  ignoreEyeSignals = false;
3162  }
3163 
3164  void PhyiCub::leftEyeChangedLimits( real, real )
3165  {
3166  // This is not implemented as we expose vergence and version, so we should never change limtis
3167  // for one eye only. Moreover implementing this is not trivial because, given the left and right
3168  // eye limits, there is not a unique possibility for vergence and version
3169  }
3170 } // end namespace farsa
3171 
3172 #endif // FARSA_USE_YARP_AND_ICUB