marxbotsensors.cpp
1 /********************************************************************************
2  * FARSA Experiments Library *
3  * Copyright (C) 2007-2012 *
4  * Gianluca Massera <emmegian@yahoo.it> *
5  * Stefano Nolfi <stefano.nolfi@istc.cnr.it> *
6  * Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it> *
7  * Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.it> *
8  * *
9  * This program is free software; you can redistribute it and/or modify *
10  * it under the terms of the GNU General Public License as published by *
11  * the Free Software Foundation; either version 2 of the License, or *
12  * (at your option) any later version. *
13  * *
14  * This program is distributed in the hope that it will be useful, *
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
17  * GNU General Public License for more details. *
18  * *
19  * You should have received a copy of the GNU General Public License *
20  * along with this program; if not, write to the Free Software *
21  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
22  ********************************************************************************/
23 
24 #include "marxbotsensors.h"
25 #include "configurationhelper.h"
26 #include "logger.h"
27 #include "graphicalwobject.h"
28 #include <QMutex>
29 
30 namespace farsa {
31 
33  Sensor(params, prefix),
34  m_marxbotResource("robot"),
35  m_neuronsIteratorResource("neuronsIterator")
36 {
37  // Reading parameters
40 
41  // Declaring the resources that are needed here
43 }
44 
46 {
47  // Nothing to do here
48 }
49 
50 void MarXbotSensor::save(ConfigurationParameters& params, QString prefix)
51 {
52  // Calling parent function
53  Sensor::save(params, prefix);
54 
55  // Saving parameters
56  params.startObjectParameters(prefix, "MarXbotSensor", this);
57  params.createParameter(prefix, "marxbot", m_marxbotResource);
58  params.createParameter(prefix, "neuronsIterator", m_neuronsIteratorResource);
59 }
60 
61 void MarXbotSensor::describe(QString type)
62 {
63  // Calling parent function
64  Sensor::describe(type);
65 
66  // Describing our parameters
67  Descriptor d = addTypeDescription(type, "The base class for MarXbot sensors");
68  d.describeString("marxbot").def("robot").help("the name of the resource associated with the MarXbot robot to use (default is \"robot\")");
69  d.describeString("neuronsIterator").def("neuronsIterator").help("the name of the resource associated with the neural network iterator (default is \"neuronsIterator\")");
70 }
71 
72 void MarXbotSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
73 {
74  // Calling parent function
75  Sensor::resourceChanged(resourceName, changeType);
76 
77  // Here we only check whether the resource has been deleted and reset the check flag, the
78  // actual work is done in subclasses
79  if (changeType == Deleted) {
81  return;
82  }
83 }
84 
86  MarXbotSensor(params, prefix),
87  m_robot(NULL),
88  m_neuronsIterator(NULL)
89 {
90 }
91 
93 {
94  // Nothing to do here
95 }
96 
98 {
99  // Calling parent function
100  MarXbotSensor::save(params, prefix);
101 
102  // Saving parameters
103  params.startObjectParameters(prefix, "MarXbotProximityIRSensor", this);
104 }
105 
107 {
108  // Calling parent function
110 
111  // Describing our parameters
112  Descriptor d = addTypeDescription(type, "The infrared proximity sensors of the MarXbot robot", "The infrared proximity sensors of the MarXbot robot. These are the very short range IR sensors all around the base");
113 }
114 
116 {
117  // Checking all resources we need exist
119 
120  // Acquiring the lock to get resources
121  ResourcesLocker locker( this );
122 
123  m_neuronsIterator->setCurrentBlock(name());
124  for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
125  m_neuronsIterator->setInput(m_robot->proximityIRSensorController()->activation(i));
126  }
127 }
128 
130 {
131  return 24;
132 }
133 
134 void MarXbotProximityIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
135 {
136  // Calling parent function
137  MarXbotSensor::resourceChanged(resourceName, changeType);
138 
139  if (changeType == Deleted) {
140  return;
141  }
142 
143  if (resourceName == m_marxbotResource) {
144  m_robot = getResource<PhyMarXbot>();
145 
146  // Eanbling sensors
147  m_robot->proximityIRSensorController()->setEnabled(true);
148  } else if (resourceName == m_neuronsIteratorResource) {
149  m_neuronsIterator = getResource<NeuronsIterator>();
150  m_neuronsIterator->setCurrentBlock(name());
151  for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
152  m_neuronsIterator->setGraphicProperties("ir" + QString::number(i), 0.0, 1.0, Qt::red);
153  }
154  } else {
155  Logger::info("Unknown resource " + resourceName + " for " + name());
156  }
157 }
158 
160  MarXbotSensor(params, prefix),
161  m_robot(NULL),
162  m_neuronsIterator(NULL)
163 {
164 }
165 
167 {
168  // Nothing to do here
169 }
170 
172 {
173  // Calling parent function
174  MarXbotSensor::save(params, prefix);
175 
176  // Saving parameters
177  params.startObjectParameters(prefix, "MarXbotGroundBottomIRSensor", this);
178 }
179 
181 {
182  // Calling parent function
184 
185  // Describing our parameters
186  Descriptor d = addTypeDescription(type, "The infrared ground bottom sensors of the MarXbot robot", "The infrared ground bottom sensors of the MarXbot robot. These are the four ground sensors below the robot battery pack.");
187 }
188 
190 {
191  // Checking all resources we need exist
193 
194  // Acquiring the lock to get resources
195  ResourcesLocker locker( this );
196 
197  m_neuronsIterator->setCurrentBlock(name());
198  for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
199  m_neuronsIterator->setInput(m_robot->groundBottomIRSensorController()->activation(i));
200  }
201 }
202 
204 {
205  return 4;
206 }
207 
208 void MarXbotGroundBottomIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
209 {
210  // Calling parent function
211  MarXbotSensor::resourceChanged(resourceName, changeType);
212 
213  if (changeType == Deleted) {
214  return;
215  }
216 
217  if (resourceName == m_marxbotResource) {
218  m_robot = getResource<PhyMarXbot>();
219 
220  // Eanbling sensors
221  m_robot->groundBottomIRSensorController()->setEnabled(true);
222  } else if (resourceName == m_neuronsIteratorResource) {
223  m_neuronsIterator = getResource<NeuronsIterator>();
224  m_neuronsIterator->setCurrentBlock(name());
225  for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
226  m_neuronsIterator->setGraphicProperties("gb" + QString::number(i), 0.0, 1.0, Qt::red);
227  }
228  } else {
229  Logger::info("Unknown resource " + resourceName + " for " + name());
230  }
231 }
232 
234  MarXbotSensor(params, prefix),
235  m_robot(NULL),
236  m_neuronsIterator(NULL)
237 {
238 }
239 
241 {
242  // Nothing to do here
243 }
244 
246 {
247  // Calling parent function
248  MarXbotSensor::save(params, prefix);
249 
250  // Saving parameters
251  params.startObjectParameters(prefix, "MarXbotGroundAroundIRSensor", this);
252 }
253 
255 {
256  // Calling parent function
258 
259  // Describing our parameters
260  Descriptor d = addTypeDescription(type, "The infrared ground around sensors of the MarXbot robot", "The infrared ground around sensors of the MarXbot robot. These are the eight ground sensors below the base of the robot (just above the wheels).");
261 }
262 
264 {
265  // Checking all resources we need exist
267 
268  // Acquiring the lock to get resources
269  ResourcesLocker locker( this );
270 
271  m_neuronsIterator->setCurrentBlock(name());
272  for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
273  m_neuronsIterator->setInput(m_robot->groundAroundIRSensorController()->activation(i));
274  }
275 }
276 
278 {
279  return 8;
280 }
281 
282 void MarXbotGroundAroundIRSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
283 {
284  // Calling parent function
285  MarXbotSensor::resourceChanged(resourceName, changeType);
286 
287  if (changeType == Deleted) {
288  return;
289  }
290 
291  if (resourceName == m_marxbotResource) {
292  m_robot = getResource<PhyMarXbot>();
293 
294  // Eanbling sensors
295  m_robot->groundAroundIRSensorController()->setEnabled(true);
296  } else if (resourceName == m_neuronsIteratorResource) {
297  m_neuronsIterator = getResource<NeuronsIterator>();
298  m_neuronsIterator->setCurrentBlock(name());
299  for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
300  m_neuronsIterator->setGraphicProperties("ga" + QString::number(i), 0.0, 1.0, Qt::red);
301  }
302  } else {
303  Logger::info("Unknown resource " + resourceName + " for " + name());
304  }
305 }
306 
308  MarXbotSensor(params, prefix),
309  m_robot(NULL),
310  m_neuronsIterator(NULL),
311  m_numReceptors(ConfigurationHelper::getInt(params, prefix + "numReceptors", 8)),
312  m_aperture(ConfigurationHelper::getDouble(params, prefix + "aperture", 360.0f)),
313  m_camera(NULL),
314  m_drawCamera(ConfigurationHelper::getBool(params, prefix + "drawCamera", true))
315 {
316 }
317 
319 {
320  // Deleting the camera
321  delete m_camera;
322 }
323 
325 {
326  // Calling parent function
327  MarXbotSensor::save(params, prefix);
328 
329  // Saving parameters
330  params.startObjectParameters(prefix, "MarXbotLinearCameraSensor", this);
331  params.createParameter(prefix, "numReceptors", QString::number(m_numReceptors));
332  params.createParameter(prefix, "aperture", QString::number(m_aperture));
333  params.createParameter(prefix, "drawCamera", (m_drawCamera ? "true" : "false"));
334 }
335 
337 {
338  // Calling parent function
340 
341  // Describing our parameters
342  Descriptor d = addTypeDescription(type, "The linear camera sensor of the MarXbot robot", "This is a 360° linear camera");
343  d.describeInt("numReceptors").def(8).limits(1, MaxInteger).help("The number of receptors of the sensor", "Each receptor returns three values, one for each of the three colors (red, green, blue). This means that the size returned by this sensor is 3 * numReceptors (default is 8)");
344  d.describeReal("aperture").def(360.0f).limits(0.0f, 360.0f).help("The aperture of the camera in degrees", "The real MarXbot has an omnidirectional camera, so you can use here any value up to 360 degrees (default is 360)");
345  d.describeBool("drawCamera").def(true).help("Whether to draw the camera or not");
346 }
347 
349 {
350  // Checking all resources we need exist
352 
353  // Acquiring the lock to get resources
354  ResourcesLocker locker( this );
355 
356  // Updating the camera
357  m_camera->update();
358 
359  // Reading activations: first the red one, then the green one and finally the blue one
360  m_neuronsIterator->setCurrentBlock(name());
361  for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
362  m_neuronsIterator->setInput(m_camera->colorForReceptor(i).redF());
363  }
364  for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
365  m_neuronsIterator->setInput(m_camera->colorForReceptor(i).greenF());
366  }
367  for (int i = 0; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
368  m_neuronsIterator->setInput(m_camera->colorForReceptor(i).blueF());
369  }
370 }
371 
373 {
374  return m_numReceptors * 3;
375 }
376 
377 void MarXbotLinearCameraSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
378 {
379  // Calling parent function
380  MarXbotSensor::resourceChanged(resourceName, changeType);
381 
382  if (changeType == Deleted) {
383  // Deleting the camera if the robot was deleted
384  if (resourceName == m_marxbotResource) {
385  delete m_camera;
386  m_camera = NULL;
387  }
388 
389  return;
390  }
391 
392  if (resourceName == m_marxbotResource) {
393  m_robot = getResource<PhyMarXbot>();
394 
395  // Now we can also create the camera
396  wMatrix mtr = wMatrix::roll(-PI_GRECO / 2.0);
397 #ifdef __GNUC__
398  #warning QUI INVECE DI UNA COSTANTE, CALCOLARSI UNA POSIZIONE DALLE DIMENSIONI DEL ROBOT
399 #endif
400  mtr.w_pos.z = 0.13f;
401  m_camera = new LinearCamera(m_robot, mtr, toRad(m_aperture), m_numReceptors, Qt::black);
402 
403  // Sharing resources with the camera
404  m_camera->shareResourcesWith(this);
405  m_camera->drawCamera(m_drawCamera);
406  } else if (resourceName == m_neuronsIteratorResource) {
407  m_neuronsIterator = getResource<NeuronsIterator>();
408  m_neuronsIterator->setCurrentBlock(name());
409  int i = 0;
410  for (; i < m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
411  m_neuronsIterator->setGraphicProperties("lr" + QString::number(i % m_numReceptors), 0.0, 1.0, Qt::red);
412  }
413  for (; i < 2 * m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
414  m_neuronsIterator->setGraphicProperties("lg" + QString::number(i % m_numReceptors), 0.0, 1.0, Qt::red);
415  }
416  for (; i < 3 * m_numReceptors; i++, m_neuronsIterator->nextNeuron()) {
417  m_neuronsIterator->setGraphicProperties("lb" + QString::number(i % m_numReceptors), 0.0, 1.0, Qt::red);
418  }
419  } else {
420  Logger::info("Unknown resource " + resourceName + " for " + name());
421  }
422 }
423 
424 namespace __MarXbotTractionSensor_internal {
425  #ifndef GLMultMatrix
426  #define GLMultMatrix glMultMatrixf
427  // for double glMultMatrixd
428  #endif
429 
437  {
438  public:
459  TractionSensorGraphic(WObject *object, const wVector& offset, real radius, real scale = 1.0, real maxLength = 1.0, QString name = "unamed") :
460  GraphicalWObject(object->world(), name),
461  m_object(object),
462  m_offset(offset),
463  m_radius(radius),
464  m_scale(scale),
465  m_maxLength(maxLength),
466  m_vector(0.0, 0.0, 0.0),
467  m_vectorMutex()
468  {
469  // Attaching to object (which also becomes our owner)
470  attachToObject(m_object, true);
471 
472  // We also use our own color and texture
474  setTexture("");
475  }
476 
481  {
482  }
483 
492  void setVector(const wVector vector)
493  {
494  m_vectorMutex.lock();
495  m_vector = vector;
496  m_vectorMutex.unlock();
497  }
498 
499  protected:
508  virtual void render(RenderWObject* renderer, QGLContext* gw)
509  {
510  // Copying the m_activations vector to a local vector to avoid concurrent accesses.
511  m_vectorMutex.lock();
512  const wVector vector = m_vector;
513  m_vectorMutex.unlock();
514 
515  // Setting the matrix to draw the vector (we have to push both the transformation
516  // matrix and the offset)
517  wMatrix offsetMatrix = wMatrix::identity();
518  offsetMatrix.w_pos = m_offset;
519  glPushMatrix();
520  GLMultMatrix(&tm[0][0]);
521  GLMultMatrix(&offsetMatrix[0][0]);
522 
523  // Before drawing computing the length od the body and of the tip of the arrow. The
524  // tip is always m_radius long, unless the arrow is so small that no body is present.
525  // The bodyLength will be negative if the vector is very short: in this cas we won't
526  // draw the arrow body. The radius of the tip is 3/2 of the cylinder radius unless the
527  // arrow is very small (and has no body): in this case the radius is scaled accordingly.
528  // We also have to take into account the maximum allowed length od the vector and
529  // change color if it is too long
530  const real originalVectorLength = vector.norm();
531  const wVector axis = vector.scale(1.0 / originalVectorLength);
532  const QColor vectorColor = (originalVectorLength > m_maxLength) ? Qt::red : Qt::cyan;
533  const real vectorLength = min(originalVectorLength, m_maxLength) * m_scale;
534  const real tipLength = min(vectorLength, m_radius);
535  const real tipRadius = ((m_radius * 3.0) / 2.0) * (tipLength / m_radius);
536  const real bodyLength = vectorLength - m_radius;
537 
538  // Setting the color of the vector
539  setColor(vectorColor);
540  renderer->container()->setupColorTexture(gw, renderer);
541 
542  // First drawing the body if its length is positive
543  if (bodyLength > 0.0) {
544  RenderWObjectContainer::drawCylinder(axis, axis.scale(0.5 * bodyLength), bodyLength, m_radius, color());
545  }
546 
547  // Now drawing the tip as a cone:
548  {
549  GLUquadricObj *pObj;
550 
551  // opengl cylinder are aligned alogn the z axis, we want it along our axis,
552  // we create a rotation matrix to do the alignment. Moreover the cone must
553  // start at the end of the cylinder we draw before
554  const wVector tipDisplacement = (bodyLength < 0.0) ? wVector(0.0, 0.0, 0.0) : axis.scale(bodyLength);
555  glPushMatrix();
557  matrix.w_pos = tipDisplacement;
558  GLMultMatrix(&matrix[0][0]);
559 
560  // Get a new Quadric off the stack
561  pObj = gluNewQuadric();
562  gluQuadricTexture(pObj, true);
563  gluCylinder(pObj, tipRadius, 0, tipLength, 20, 2);
564 
565  // render the caps
566  gluQuadricOrientation(pObj, GLU_INSIDE);
567  gluDisk(pObj, 0.0f, tipRadius, 20, 1);
568 
569  gluDeleteQuadric(pObj);
570  glPopMatrix();
571  }
572 
573  // Popping twice because we pushed both our matrix and the offset matrix
574  glPopMatrix();
575  glPopMatrix();
576  }
577 
582 
589  const wVector m_offset;
590 
595  const real m_radius;
596 
600  const real m_scale;
601 
609  const real m_maxLength;
610 
616  wVector m_vector;
617 
625  };
626 }
627 
628 using namespace __MarXbotTractionSensor_internal;
629 
631  MarXbotSensor(params, prefix),
632  m_robot(NULL),
633  m_neuronsIterator(NULL),
634  m_maxForce(ConfigurationHelper::getDouble(params, prefix + "maxForce", 1.0f)),
635  m_drawSensor(ConfigurationHelper::getBool(params, prefix + "drawSensor", true)),
636  m_graphics(NULL)
637 {
638  if (m_maxForce < 0.0) {
639  ConfigurationHelper::throwUserConfigError(prefix + "maxForce", params.getValue(prefix + "maxForce"), "The parameter must be a positive real number");
640  }
641 }
642 
644 {
645  // Nothing to do here
646 }
647 
649 {
650  // Calling parent function
651  MarXbotSensor::save(params, prefix);
652 
653  // Saving parameters
654  params.startObjectParameters(prefix, "MarXbotTractionSensor", this);
655  params.createParameter(prefix, "maxForce", QString::number(m_maxForce));
656  params.createParameter(prefix, "drawSensor", m_drawSensor ? QString("true") : QString("false"));
657 }
658 
660 {
661  // Calling parent function
663 
664  // Describing our parameters
665  Descriptor d = addTypeDescription(type, "The traction sensors of the MarXbot robot", "The traction sensors of the MarXbot robot. It is placed between the base and the turret. Note that this sensor only works when the robot is in dynamic mode");
666  d.describeReal("maxForce").def(1.0f).limits(0.0f, +Infinity).help("The maximum possible value of the force", "This is the value of the force on one axis over which the corresponding neuron is activated with 1");
667  d.describeBool("drawSensor").def(true).help("Whether to draw the sensor", "If true the sensor is graphically represented by an arrow in the direction of the current traction");
668 }
669 
671 {
672  // Checking all resources we need exist
674 
675  // Acquiring the lock to get resources
676  ResourcesLocker locker( this );
677 
678  m_neuronsIterator->setCurrentBlock(name());
679  const wVector& t = m_robot->tractionSensorController()->traction();
680 
681  // The first two neurons correspond to the x axis, the second two to the y axis
682  if (t.x > 0.0) {
683  const real f = min(t.x, m_maxForce);
684  m_neuronsIterator->setInput(f / m_maxForce);
685  m_neuronsIterator->nextNeuron();
686  m_neuronsIterator->setInput(0.0);
687  m_neuronsIterator->nextNeuron();
688  } else {
689  const real f = max(t.x, -m_maxForce);
690  m_neuronsIterator->setInput(0.0);
691  m_neuronsIterator->nextNeuron();
692  m_neuronsIterator->setInput(-f / m_maxForce);
693  m_neuronsIterator->nextNeuron();
694  }
695 
696  if (t.y > 0.0) {
697  const real f = min(t.y, m_maxForce);
698  m_neuronsIterator->setInput(f / m_maxForce);
699  m_neuronsIterator->nextNeuron();
700  m_neuronsIterator->setInput(0.0);
701  m_neuronsIterator->nextNeuron();
702  } else {
703  const real f = max(t.y, -m_maxForce);
704  m_neuronsIterator->setInput(0.0);
705  m_neuronsIterator->nextNeuron();
706  m_neuronsIterator->setInput(-f / m_maxForce);
707  m_neuronsIterator->nextNeuron();
708  }
709 
710  // Updating graphical representation if we have to
711  if (m_drawSensor) {
712  // We only draw the x and y components of the vector
713  m_graphics->setVector(wVector(t.x, t.y, 0.0));
714  }
715 }
716 
718 {
719  return 4;
720 }
721 
722 void MarXbotTractionSensor::resourceChanged(QString resourceName, ResourceChangeType changeType)
723 {
724  // Calling parent function
725  MarXbotSensor::resourceChanged(resourceName, changeType);
726 
727  if (changeType == Deleted) {
728  return;
729  }
730 
731  if (resourceName == m_marxbotResource) {
732  m_robot = getResource<PhyMarXbot>();
733 
734  // Eanbling sensors
735  m_robot->proximityIRSensorController()->setEnabled(true);
736 
737  // If graphics is enabled, creating the graphical object
738  if (m_drawSensor) {
739  const wVector offset(0.0, 0.0, PhyMarXbot::basez + PhyMarXbot::bodyh + 0.1);
740 
741  m_graphics = new TractionSensorGraphic(m_robot, offset, 0.005, 0.1 / m_maxForce, m_maxForce, "Traction sensor");
742  }
743  } else if (resourceName == m_neuronsIteratorResource) {
744  m_neuronsIterator = getResource<NeuronsIterator>();
745  m_neuronsIterator->setCurrentBlock(name());
746  m_neuronsIterator->setGraphicProperties("t+x", 0.0, 1.0, Qt::red);
747  m_neuronsIterator->nextNeuron();
748  m_neuronsIterator->setGraphicProperties("t-x", 0.0, 1.0, Qt::red);
749  m_neuronsIterator->nextNeuron();
750  m_neuronsIterator->setGraphicProperties("t+y", 0.0, 1.0, Qt::red);
751  m_neuronsIterator->nextNeuron();
752  m_neuronsIterator->setGraphicProperties("t-y", 0.0, 1.0, Qt::red);
753  } else {
754  Logger::info("Unknown resource " + resourceName + " for " + name());
755  }
756 }
757 
758 }