WorldSim - 1.2.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phymarxbot.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2012-2013 *
4  * Gianluca Massera <emmegian@yahoo.it> *
5  * Fabrizio Papi <erkito87@gmail.com> *
6  * *
7  * This program is free software; you can redistribute it and/or modify *
8  * it under the terms of the GNU General Public License as published by *
9  * the Free Software Foundation; either version 2 of the License, or *
10  * (at your option) any later version. *
11  * *
12  * This program is distributed in the hope that it will be useful, *
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
15  * GNU General Public License for more details. *
16  * *
17  * You should have received a copy of the GNU General Public License *
18  * along with this program; if not, write to the Free Software *
19  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
20  ********************************************************************************/
21 
22 #include "phymarxbot.h"
23 #include "phybox.h"
24 #include "phycylinder.h"
25 #include "phyfixed.h"
26 #include "phyballandsocket.h"
27 #include "physphere.h"
28 #include "physuspension.h"
29 #include "phycompoundobject.h"
30 #include "phycone.h"
31 #include "mathutils.h"
32 #include <cmath>
33 
34 namespace farsa {
35 
36 const real PhyMarXbot::basex = 0.034f;
37 const real PhyMarXbot::basey = 0.143f;
38 const real PhyMarXbot::basez = 0.048f;
39 const real PhyMarXbot::basem = 0.4f;
40 const real PhyMarXbot::bodyr = 0.085f;
41 const real PhyMarXbot::bodyh = 0.0055f;
42 const real PhyMarXbot::bodym = 0.02f;
43 const real PhyMarXbot::axledistance = 0.104f;
44 const real PhyMarXbot::trackradius = 0.022f;
45 const real PhyMarXbot::trackheight = 0.0295f;
46 const real PhyMarXbot::trackm = 0.05f;
47 const real PhyMarXbot::treaddepth = 0.004f;
48 const real PhyMarXbot::wheelr = 0.027f;
49 const real PhyMarXbot::wheelh = 0.0215f;
50 const real PhyMarXbot::wheelm = 0.02f;
51 const real PhyMarXbot::attachringh = 0.0285f;
52 const real PhyMarXbot::attachringm = 0.08f;
53 const real PhyMarXbot::ledsh = 0.010f;
54 const real PhyMarXbot::ledsradius = 0.080f;
55 const real PhyMarXbot::ledsm = 0.03f;
56 
57 PhyMarXbot::PhyMarXbot( World* w, QString name, const wMatrix& basetm ) : WObject( w, name, basetm, false ) {
58  kinematicSimulation = false;
59 
60  // --- reference frame
61  // X
62  // ^
63  // | ------
64  // | ______|____|_________
65  // | |_|_| track |_|_|_|_|
66  // | -------------------
67  // | | battery |
68  // | -------------------
69  // | |_|_| track |_|_|_|_|
70  // | | |
71  // | ------
72  // |
73  // ---------------------> Y
74  // The front of the robot is towards -Y (positive speeds of the wheel make the robot move towards -Y)
75 
76  // --- create material for the marXbot
77  // introducing a semplification for the collision: two robot collide only with the attachring
78  w->materials().createMaterial( "marXbotLowerBody" );
79  w->materials().setElasticity( "marXbotLowerBody", "default", 0.01f );
80  w->materials().setSoftness( "marXbotLowerBody", "default", 0.01f );
81  w->materials().createMaterial( "marXbotUpperBody" );
82  w->materials().enableCollision( "marXbotLowerBody", "marXbotLowerBody", false );
83  w->materials().enableCollision( "marXbotLowerBody", "marXbotUpperBody", false );
84 
85  QVector<PhyObject*> objs;
86  PhyObject* obj;
87  // --- create the body of the base of the marxbot
88 #ifdef MARXBOT_DETAILED
89  wMatrix tm = wMatrix::identity();
90  // battery container
91  obj = new PhyBox( basex, basey, basez, w );
92  tm.w_pos[2] = basez/2.0f + 0.015f;
93  obj->setMass( basem );
94  obj->setMatrix( tm );
95  objs << obj;
96  // cylinder body
97  obj = new PhyCylinder( bodyr, bodyh, w );
98  obj->setMass( bodym );
99  tm = wMatrix::yaw( toRad(90.0f) );
100  tm.w_pos[2] = 0.015f + basez + bodyh/2.0f;
101  obj->setMatrix( tm );
102  objs << obj;
103  // right track
104  obj = new PhyBox( trackheight, axledistance, trackradius*2.0f, w );
105  tm = wMatrix::identity();
106  tm.w_pos[0] = (basex + trackheight)/2.0f;
107  tm.w_pos[2] = trackradius + treaddepth;
108  obj->setMass( trackm );
109  obj->setMatrix( tm );
110  objs << obj;
111  obj = new PhyCylinder( trackradius, trackheight, w );
112  obj->setMass( wheelm );
113  tm.w_pos[1] = axledistance/2.0;
114  obj->setMatrix( tm );
115  objs << obj;
116  obj = new PhyCylinder( trackradius, trackheight, w );
117  obj->setMass( wheelm );
118  tm.w_pos[1] = -axledistance/2.0;
119  obj->setMatrix( tm );
120  objs << obj;
121  // left track
122  obj = new PhyBox( trackheight, axledistance, trackradius*2.0f, w );
123  tm = wMatrix::identity();
124  tm.w_pos[0] = -(basex + trackheight)/2.0f;
125  tm.w_pos[2] = trackradius + treaddepth;
126  obj->setMass( trackm );
127  obj->setMatrix( tm );
128  objs << obj;
129  obj = new PhyCylinder( trackradius, trackheight, w );
130  obj->setMass( wheelm );
131  tm.w_pos[1] = axledistance/2.0;
132  obj->setMatrix( tm );
133  objs << obj;
134  obj = new PhyCylinder( trackradius, trackheight, w );
135  obj->setMass( wheelm );
136  tm.w_pos[1] = -axledistance/2.0;
137  obj->setMatrix( tm );
138  objs << obj;
139 #else
140  // simplified version of the marxbot physic
141  obj = new PhyBox( basex+2.0f*trackheight, axledistance, basez+trackradius*2.0f, w );
142  obj->setMass( basem+bodym );
143  tm = wMatrix::identity();
144  tm.w_pos[2] = (basez+trackradius*2.0f)/2.0f + treaddepth;
145  obj->setMatrix( tm );
146  objs << obj;
147 #endif
148  // merge all togheter
149  base = new PhyCompoundObject( objs, w, "base" );
150  base->setMatrix( basetm );
151  base->setMaterial("marXbotLowerBody");
152  base->setOwner( this, false );
153 
154  // --- create the two external wheels
155  // they are motorized and they move the robot
156  // and create four spheres for supporting the tracks
157  // NOTE: the tracks are not simulated, they are just rigid bodies and
158  // the four sphere move freely in order to approximate the real movement
159  // --- position of wheels and their IDs in the vector
160  // +-----+
161  // |3|| ||2|
162  // |1| | | |0|
163  // |5|| ||4|
164  // +-----+
165  // --------------------> X
166  // arrays of X and Y offsets for calculate the positions
167  double xoffs[6] = { 1, -1, 1, -1, 1, -1 };
168  double yoffs[6] = { 0, 0, 1, 1, -1, -1 };
169  PhyObject* awheel;
170  PhyJoint* joint;
171  for( int i=0; i<6; i++ ) {
172  // --- relative to base
173  wMatrix wtm = wMatrix::identity();
174  wtm.w_pos = wVector(
175  xoffs[i] * (basex + trackheight)/2.0f + (yoffs[i]==0 ? xoffs[i]*(trackheight/2.0+wheelh/2.0f+0.006f) : 0.0f),
176  yoffs[i] * (axledistance/2.0f),
177  0.0f );
178  if ( i<2 ) {
179  // the two motorized wheels
180  wtm.w_pos[2] = wheelr;
181  awheel = new PhyCylinder( wheelr, wheelh, w, "motorWheel" );
182  } else {
183  // the four sphere supporting the track
184  wtm.w_pos[2] = treaddepth-0.003f;
185  awheel = new PhySphere( treaddepth, w, "passiveWheel" );
186  }
187  wheelstm.append( wtm );
188  awheel->setMass( wheelm );
189  awheel->setColor( Qt::blue );
190  awheel->setMaterial( "marXbotLowerBody" );
191  awheel->setOwner( this, false );
192  awheel->setMatrix( wtm * basetm );
193  wheels.append( awheel );
194 
195  //--- create the joints
196  if ( i<2 ) {
197  // the two motorized wheels
198  joint = new PhySuspension(
199  base->matrix().x_ax, // rotation axis
200  wheels[i]->matrix().w_pos,
201  base->matrix().z_ax, // suspension 'vertical' axis
202  base, wheels[i] );
203  joint->dofs()[0]->disableLimits();
204  } else {
205  // the four sphere supporting the track
206  joint = new PhyBallAndSocket(
207  wheels[i]->matrix().w_pos,
208  base, wheels[i] );
209  }
210  joint->setOwner( this, false );
211  joint->updateJointInfo();
212  wheelJoints.append( joint );
213  }
214 
215  // --- create the attachment module with the 12 RGB LEDs part
216 #ifdef MARXBOT_DETAILED
217  objs.clear();
218  obj = new PhyCone( bodyr, attachringh+0.010f, w );
219  obj->setMass( attachringm );
220  tm = wMatrix::identity();
221  tm.w_pos[0] = 0.005f;
222  obj->setMatrix( tm );
223  objs << obj;
224  obj = new PhyCone( bodyr, attachringh+0.010f, w );
225  obj->setMass( attachringm );
226  tm = wMatrix::roll( toRad(180.0f) );
227  tm.w_pos[0] = -0.005f;
228  obj->setMatrix( tm );
229  objs << obj;
230  obj = new PhyCylinder( ledsradius, ledsh, w );
231  obj->setMass( ledsm );
232  obj->setTexture( "marXbot_12leds" );
233  obj->setUseColorTextureOfOwner( false );
234  tm.w_pos[0] = attachringh/2.0f + ledsh/2.0f + 0.0015f;
235  obj->setMatrix( tm );
236  objs << obj;
237 #else
238  objs.clear();
239  obj = new PhyCylinder( bodyr, attachringh+ledsh, w );
240  obj->setMass( attachringm );
241  tm = wMatrix::identity();
242  tm.w_pos[0] = attachringh/2.0f-0.010f;
243  obj->setMatrix( tm );
244  objs << obj;
245 #endif
246  // merge all toghether
247  attachring = new PhyCompoundObject( objs, w );
248  attachring->setMaterial("marXbotUpperBody");
249  attachring->setOwner( this, false );
250  attachring->setUseColorTextureOfOwner( false );
251  attachringtm = wMatrix::yaw( toRad(-90.0f) );
252  attachringtm.w_pos[2] = 0.015f + basez + bodyh + attachringh/2.0f + 0.0015f;
253  attachring->setMatrix( attachringtm * basetm );
254  // --- create the joint for sensing the force
255  forcesensor = new PhyFixed( base, attachring, w );
256  forcesensor->setOwner( this, false );
257 
258  // create the motor controller
259  QVector<PhyDOF*> motors;
260  motors << wheelJoints[0]->dofs()[0] << wheelJoints[1]->dofs()[0];
261  wheelsCtrl = new WheelMotorController( motors, w );
262  wheelsCtrl->setOwner( this, false );
263 #ifdef __GNUC__
264  #warning QUI DECIDERE PER LE VELOCITÀ MASSIME E MINIME DELLE RUOTE
265 #endif
266  wheelsCtrl->setSpeedLimits( -10.0, -10.0, 10.0, 10.0 );
267 
268  // Connecting wheels speed signals to be able to move the robot when in kinematic
269  connect(wheelJoints[0]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setRightWheelDesideredVelocity(real)));
270  connect(wheelJoints[1]->dofs()[0], SIGNAL(changedDesiredVelocity(real)), this, SLOT(setLeftWheelDesideredVelocity(real)));
271 
272  // Creating the proximity IR sensors
273  QVector<SingleIR> sensors;
274 
275 #ifdef __GNUC__
276  #warning QUI DECIDERE PER I RANGE/APERTURE E LE POSIZIONI (SPECIE PER I GROUND)
277 #endif
278  // Adding the sensors. The marxbot has 24 proximity infrared sensors
279  for (unsigned int i = 0; i < 24; i++) {
280  const double curAngle = double(i) * (360.0 / 24.0) + ((360.0 / 24.0) / 2.0);
281  const double radius = bodyr;
282 
283  wMatrix mtr = wMatrix::yaw(deg2rad(curAngle + 90.0)) * wMatrix::pitch(deg2rad(90.0f));
284  mtr.w_pos.x = radius * cos(deg2rad(curAngle));
285  mtr.w_pos.y = radius * sin(deg2rad(curAngle));
286  mtr.w_pos.z = 0.015f + basez + bodyh/2.0f;
287 
288  sensors.append(SingleIR(base, mtr, 0.02, 0.1, 10.0, 5));
289  }
290  proximityIR = new SimulatedIRProximitySensorController(world(), sensors);
291 
292  // Now creating the ground IR sensors below the battery pack
293  sensors.clear();
294  wMatrix mtr = wMatrix::yaw(deg2rad(180.0));
295  const double distFromBorder = 0.003;
296 
297  // Adding the sensors. The marxbot has 4 ground infrared sensors below the battery pack
298  mtr.w_pos = wVector(basex / 2.0f - distFromBorder, basey / 2.0f - distFromBorder, 0.015f);
299  sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
300  mtr.w_pos = wVector(basex / 2.0f - distFromBorder, -basey / 2.0f + distFromBorder, 0.015f);
301  sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
302  mtr.w_pos = wVector(-basex / 2.0f + distFromBorder, basey / 2.0f - distFromBorder, 0.015f);
303  sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
304  mtr.w_pos = wVector(-basex / 2.0f + distFromBorder, -basey / 2.0f + distFromBorder, 0.015f);
305  sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
306  groundBottomIR = new SimulatedIRGroundSensorController(world(), sensors);
307 
308  // Creating the ground IR sensors below the base
309  sensors.clear();
310 
311  // Adding the sensors. The marxbot has 8 ground infrared sensors below the base
312  for (unsigned int i = 0; i < 8; i++) {
313  const double curAngle = double(i) * (360.0 / 8.0);
314  const double radius = bodyr - 0.003;
315 
316  wMatrix mtr = wMatrix::yaw(deg2rad(180.0));
317  mtr.w_pos.x = radius * cos(deg2rad(curAngle));
318  mtr.w_pos.y = radius * sin(deg2rad(curAngle));
319  mtr.w_pos.z = 0.015f + basez;
320 
321  sensors.append(SingleIR(base, mtr, 0.0, mtr.w_pos.z + 0.005, 0.0, 1));
322  }
323  groundAroundIR = new SimulatedIRGroundSensorController(world(), sensors);
324 
325  // Creating the traction sensor
326  tractionSensor = new TractionSensorController(world(), *forcesensor);
327 
328  w->pushObject( this );
329 }
330 
331 PhyMarXbot::~PhyMarXbot() {
332  foreach( PhyJoint* susp, wheelJoints ) {
333  delete susp;
334  }
335  foreach( PhyObject* w, wheels ) {
336  delete w;
337  }
338  delete wheelsCtrl;
339  delete forcesensor;
340  delete attachring;
341  delete base;
342  delete proximityIR;
343  delete groundBottomIR;
344  delete groundAroundIR;
345 }
346 
347 void PhyMarXbot::preUpdate()
348 {
349  // Updating motors
350  if (wheelsCtrl->isEnabled()) {
351  wheelsCtrl->update();
352  }
353 
354  if (kinematicSimulation) {
355  // In kinematic mode, we have to manually move the robot depending on the wheel velocities
356  wMatrix mtr = matrix();
357  wheeledRobotsComputeKinematicMovement(mtr, leftWheelVelocity, rightWheelVelocity, wheelr, wheelstm[0].w_pos.x - wheelstm[1].w_pos.x, world()->timeStep());
358 
359  // This will also update the position of all pieces
360  setMatrix(mtr);
361  }
362 }
363 
364 void PhyMarXbot::postUpdate()
365 {
366  // Updating sensors
367  if (proximityIR->isEnabled()) {
368  proximityIR->update();
369  }
370  if (groundBottomIR->isEnabled()) {
371  groundBottomIR->update();
372  }
373  if (groundAroundIR->isEnabled()) {
374  groundAroundIR->update();
375  }
376 
377  // Updating the transformation matrix of the robot. It is coincident with the matrix of the base
378  tm = base->matrix();
379 }
380 
381 void PhyMarXbot::setProximityIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
382 {
383  proximityIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
384 }
385 
386 void PhyMarXbot::setGroundBottomIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
387 {
388  groundBottomIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
389 }
390 
391 void PhyMarXbot::setGroundAroundIRSensorsGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
392 {
393  groundAroundIR->setGraphicalProperties(drawSensor, drawRay, drawRealRay);
394 }
395 
396 void PhyMarXbot::doKinematicSimulation(bool k)
397 {
398  if (kinematicSimulation == k) {
399  return;
400  }
401 
402  kinematicSimulation = k;
403  if (kinematicSimulation) {
404  // First disabling all joints...
405  for (int i = 0; i < wheelJoints.size(); i++) {
406  wheelJoints[i]->enable(false);
407  }
408  forcesensor->enable(false);
409 
410  // ... then setting all objects to kinematic behaviour
411  base->setKinematic(true, true);
412  for (int i = 0; i < wheels.size(); i++) {
413  wheels[i]->setKinematic(true, true);
414  }
415  attachring->setKinematic(true, true);
416  } else {
417  // First setting all objects to dynamic behaviour...
418  base->setKinematic(false);
419  for (int i = 0; i < wheels.size(); i++) {
420  wheels[i]->setKinematic(false);
421  }
422  attachring->setKinematic(false);
423 
424  // ... then enabling all joints (if the corresponding part is enabled)
425  for (int i = 0; i < wheelJoints.size(); i++) {
426  wheelJoints[i]->enable(true);
427  wheelJoints[i]->updateJointInfo();
428  }
429  forcesensor->enable(true);
430  forcesensor->updateJointInfo();
431  }
432 }
433 
434 void PhyMarXbot::setLeftWheelDesideredVelocity(real velocity)
435 {
436  leftWheelVelocity = velocity;
437 }
438 
439 void PhyMarXbot::setRightWheelDesideredVelocity(real velocity)
440 {
441  rightWheelVelocity = velocity;
442 }
443 
444 void PhyMarXbot::changedMatrix() {
445  wMatrix tm = matrix();
446  base->setMatrix( tm );
447  for( int i=0; i<wheels.size(); i++ ) {
448  wheels[i]->setMatrix( wheelstm[i] * tm );
449  }
450  foreach( PhyJoint* ajoint, wheelJoints ) {
451  ajoint->updateJointInfo();
452  }
453  attachring->setMatrix( attachringtm * tm );
454  forcesensor->updateJointInfo();
455 }
456 
457 } // end namespace farsa