Source code for morse.sensors.accelerometer

import logging; logger = logging.getLogger("morse." + __name__)
import math
import morse.core.sensor
from morse.helpers.components import add_data

[docs]class Accelerometer(morse.core.sensor.Sensor): """ This sensor emulates an Accelerometer/Podometer, measuring the distance that a robot has moved, the current speed and current acceleration. Measurements are done for the 3 axes (X, Y, Z) for velocity and acceleration. The values for velocity and acceleration are measured at each tic of the Game Engine, measuring the difference in distance from the previous tic, and the estimated time between tics (60 tics per second is the default in Blender). """ _name = "Accelerometer" add_data('distance', 0.0, "float", 'distance travelled since the last tick, in meter') add_data('velocity', [0.0, 0.0, 0.0], "vec3<float>", 'Instantaneous speed in X, Y, Z, in meter sec^-1') add_data('acceleration', [0.0, 0.0, 0.0], "vec3<float>", 'Instantaneous acceleration in X, Y, Z, in meter sec^-2') def __init__(self, obj, parent=None): """ Constructor method. Receives the reference to the Blender object. The second parameter should be the name of the object's parent. """ logger.info('%s initialization' % obj.name) # Call the constructor of the parent class super(self.__class__, self).__init__(obj, parent) # Variables to store the previous position self.ppx = 0.0 self.ppy = 0.0 self.ppz = 0.0 # Variables to store the previous velocity self.pvx = 0.0 self.pvy = 0.0 self.pvz = 0.0 # Make a new reference to the sensor position self.p = self.bge_object.position self.v = [0.0, 0.0, 0.0] # Velocity self.pv = [0.0, 0.0, 0.0] # Previous Velocity self.a = [0.0, 0.0, 0.0] # Acceleration logger.info('Component initialized')
[docs] def default_action(self): """ Compute the speed and accleration of the robot The speed and acceleration are computed using the blender tics to measure time. When computing velocity as v = d / t, and t = 1 / frequency, then v = d * frequency where frequency is computed from the blender tics and number of skipped logic steps for this sensor. """ # Compute the difference in positions with the previous loop dx = self.p[0] - self.ppx dy = self.p[1] - self.ppy dz = self.p[2] - self.ppz self.local_data['distance'] = math.sqrt(dx**2 + dy**2 + dz**2) logger.debug("DISTANCE: %.4f" % self.local_data['distance']) # Store the position in this instant self.ppx = self.p[0] self.ppy = self.p[1] self.ppz = self.p[2] # Scale the speeds to the time used by Blender self.v[0] = dx * self.frequency self.v[1] = dy * self.frequency self.v[2] = dz * self.frequency logger.debug("SPEED: (%.4f, %.4f, %.4f)" % (self.v[0], self.v[1], self.v[2])) self.a[0] = (self.v[0] - self.pvx) * self.frequency self.a[1] = (self.v[1] - self.pvy) * self.frequency self.a[2] = (self.v[2] - self.pvz) * self.frequency logger.debug("ACCELERATION: (%.4f, %.4f, %.4f)" % (self.a[0], self.a[1], self.a[2])) # Update the data for the velocity self.pvx = self.v[0] self.pvy = self.v[1] self.pvz = self.v[2] # Store the important data self.local_data['velocity'] = self.v self.local_data['acceleration'] = self.a