Merge branch 'labyrinth'

python_scripts/paint_scalar_field.py

@@ -26,8 +26,8 @@
 
 #width = 800
 #height = 600
-width = 600
-height = 400
+width = 1024
+height = 540
 
 cell_radius = 0#1#10
 cell_width = cell_radius * 2 + 1
@@ -103,7 +103,7 @@ def application_function_probability(value_0_to_1):
 #paint(image, value_function_top, application_function_direct)
 #paint(image, value_function_top, application_function_probability)
 #paint(image, value_function_centre, application_function_probability)
-paint(image, value_function_centre, application_function_direct)
+#paint(image, value_function_centre, application_function_direct)
 #paint(image, value_function_segment, application_function_direct)
 
 print("Writing {}".format(filename))

src/common/puck/index.js

@@ -21,13 +21,15 @@ export default class Puck {
     // Create Matter.js body and attach it to world
     this.body = Bodies.circle(position.x, position.y, this.radius);
     // this.body.friction = 0;
-    this.body.frictionAir = 1;
+    this.body.frictionAir = 0.5;
     // this.body.frictionStatic = 0;
     // this.body.restitution = 0;
+    //Body.setDensity(this.body, 0.001);
+
     // this.body.collisionFilter = {
-    //   group: -1,
-    //   category: 1,
-    //   mask: 1,
+    //    group: 0,
+    //    category: 0x0002,
+    //    mask: 0x0001 | 0x0002 | 0x0004,
     // };
     World.add(this.world, this.body);
 

src/common/robot/robot.js

@@ -1,6 +1,6 @@
-import { Body, World, Bodies } from 'matter-js';
+import { Body, World, Bodies, Composite, Constraint } from 'matter-js';
 
-import { getDistance } from '../utils/geometry';
+import { getDistance, getSmallestSignedAngularDifference } from '../utils/geometry';
 
 import SensorManager from './sensors/sensorManager';
 import ActuatorManager from './actuators/actuatorsManager';
@@ -79,33 +79,41 @@ export default class Robot {
 
     // Add scene specific misc values to robots
     if (misc && typeof misc === 'object' && Object.keys(misc).length > 0) {
-      Object.entries(misc).forEach(([miscKey, miscVal]) => { this[miscKey] = miscVal; });
+      Object.entries(misc).forEach(([miscKey, miscVal]) => {
+        this[miscKey] = miscVal;
+      });
     }
 
     // Create Matter.js body and attach it to world
+    let parts = [ Bodies.circle(position.x, position.y, this.radius) ]
+
+    if (this.tail) {
+      this.tailLength = 1.0*this.radius;
+
+      this.tailBody = Bodies.trapezoid(
+         position.x,
+         position.y - this.radius - this.tailLength/2.0,
+         this.radius / 5,
+         this.tailLength,
+         -this.tailSlope
+      );
+      Body.rotate(this.tailBody, -Math.PI/2.0, {x: position.x, y: position.y} );
+      Body.setDensity(this.tailBody, 0.00001);
+      parts.push(this.tailBody)
+    }
+
     const compoundBody = Body.create({
-      parts: [
-        Bodies.circle(position.x, position.y, this.radius)
-        // If you want to add more parts to the robot body, add them here.
-        // Make sure to also change renderabes to render all parts of the robot.
-        // Example of compound body:
-        // ,
-        // Bodies.polygon(
-        //   position.x + this.radius / 10 + this.radius / 2,
-        //   position.y - (2 * this.radius) / 5,
-        //   3,
-        //   this.radius * 1.2,
-        //   { angle: (1.6 * Math.PI) / 2 }
-        // )
-      ],
-      isStatic: externalEngine
+      parts: parts
     });
     this.body = compoundBody;
+
     this.body.friction = 0;
     this.body.frictionAir = 0;
     this.body.frictionStatic = 0;
     this.body.restitution = 0;
-    this.body.angle = Math.random() * 2 * Math.PI; // Randomize orientations
+
+    Body.setAngle(this.body, Math.random() * 2 * Math.PI); // Randomize orientations
+
     World.add(this.world, this.body);
     Body.setAngularVelocity(this.body, 1);
     this.engine.velocityIterations = 10;

src/common/scene/renderables.js

@@ -14,7 +14,7 @@ export default [
       height: { prop: 'height' }
     },
     styles: {
-      fill: '#000000'
+      fill: '#444444'
     }
   },
   {
@@ -31,7 +31,7 @@ export default [
       r: { prop: 'radius' }
     },
     styles: {
-      fill: '#000000'
+      fill: '#444444'
     }
   }
 ];

src/common/staticObjects/staticCircle.js

@@ -29,9 +29,9 @@ export default class StaticCircle {
 
     this.body = Bodies.circle(this.center.x, this.center.y, this.radius, { isStatic: true });
     // this.body.collisionFilter = {
-    //   group: 1,
-    //   category: 1,
-    //   mask: 1,
+    //    group: 0,
+    //    category: 0x0001,
+    //    mask: 0x0001 | 0x0002,
     // };
 
     if (shouldAddToWorld) {

src/common/staticObjects/staticRectangle.js

@@ -42,9 +42,9 @@ export default class StaticRectangle {
     );
 
     // this.body.collisionFilter = {
-    //   group: 1,
-    //   category: 1,
-    //   mask: 1,
+    //    group: 0,
+    //    category: 0x0001,
+    //    mask: 0x0001 | 0x0002,
     // };
 
     if (shouldAddToWorld) {

src/common/utils/geometry.js

@@ -12,6 +12,35 @@ export function normalizeAngle(angle) {
   return angle % (2 * Math.PI);
 }
 
+export function normalizeAnglePlusMinusPi(a) {
+  // BAD: These loops should be replaced.
+  while (a > Math.PI) {
+    a -= 2 * Math.PI;
+  }
+  while (a <= -Math.PI) {
+    a += 2 * Math.PI;
+  }
+  return a;
+}
+
+export function getAngularDifference(angleA, angleB) {
+    angleA = normalizeAnglePlusMinusPi(angleA);
+    angleB = normalizeAnglePlusMinusPi(angleB);
+    let error = Math.abs(angleA - angleB);
+    if (error > Math.PI) {
+        error -= Math.PI * 2;
+        error = Math.abs(error);
+    }
+    return error;
+}
+
+export function getSmallestSignedAngularDifference(a, b) {
+    /* Return angle between the two given angles with the smallest absolute
+       value.  Meanwhile, the value returned will have a sign. */
+    // From: https://stackoverflow.com/questions/1878907/the-smallest-difference-between-2-angles
+    return Math.atan2(Math.sin(a-b), Math.cos(a-b))
+}
+
 /*
 * Calculates the angle ABC (in radians)
 *

src/common/utils/positionsGenerators/randomPositionsGenerator.js

@@ -20,9 +20,11 @@ export default function getRandCollFreePosGenerator(
   const yCount = envHeight / resolution;
   const positionsCount = parseInt(numOfPos, 10);
 
+  /*
   if (xCount * yCount < positionsCount * 4) {
     throw new Error('Invalid inputs, number and size of robots and pucks are too high for this environment size!');
   }
+  */
 
   let i = 0;
   while (positions.length < positionsCount * 3 && i < positionsCount * 100) {
@@ -47,9 +49,11 @@ export default function getRandCollFreePosGenerator(
     i += 1;
   }
 
+  /*
   if (positions.length < positionsCount * 2) {
     throw new Error('Invalid inputs, number and size of robots are too high for this environment!');
   }
+  */
 
   return getPos;
 }

src/scenes/Labyrinth/benchmarking/puckFieldValueTracker.js

@@ -0,0 +1,29 @@
+import Tracker from '@common/benchmarking/performanceTrackers/tracker';
+import { sampleFieldAtPoint } from '@common/robot/sensors/sensorUtils';
+
+class PuckFieldValueTracker extends Tracker {
+  constructor(ideal_tau) {
+    super(
+      'PuckFieldValue',
+      'Sum of Squared Differences',
+      'average',
+      'average',
+      { xTitle: 'Time (ms)', yTitle: 'SSD' }
+    );
+
+    this.getValue = (scene) => {
+      let ssd = 0;
+
+      scene.pucks.forEach((p) => {
+        let point = {x: Math.round(p.position.x), y: Math.round(p.position.y)};
+        const fieldValue = sampleFieldAtPoint(scene.fields.travelTime.src, point)[0] / 255.0;
+        ssd += (fieldValue - ideal_tau) ** 2;
+      });
+
+      return 100 * ssd / scene.pucks.length;
+    };
+  }
+}
+
+//export default new PuckFieldValueTracker();
+export default PuckFieldValueTracker;