Source code for morse.sensors.attitude

import logging; logger = logging.getLogger("morse." + __name__)
import morse.core.sensor
from morse.core import mathutils, blenderapi
from morse.helpers.components import add_data, add_property
from morse.helpers.coordinates import CoordinateConverter
from morse.helpers.velocity import angular_velocities
from morse.helpers.morse_math import normalise_angle
from copy import copy

[docs]class Attitude(morse.core.sensor.Sensor): """ This sensor is an high-level sensor, returning the attitude of the sensor (i.e. angles and angular velocities). It can be seen as the integration of an IMU, or a couple gyroscope/gyrometer. If the robot has a physics controller, the velocities are directly read from its property ``localAngularVelocity``. Otherwise the velocities are calculated by simple differentiation. The measurements are given in the sensor coordinate system. """ _name = "Attitude sensor" add_data('rotation', [0.0, 0.0, 0.0], "Euler angles (XYZ)", 'rotation of the sensor, in radian') add_data('angular_velocity', [0.0, 0.0, 0.0], "vec3<float>", 'rates in the sensors axis x, y, z axes (in radian . sec ^ -1)') add_property('_use_angle_against_north', False, 'UseAngleAgainstNorth', 'bool', "If set to true, return the absolute yaw against North. The whole " "geodetic coordinates (longitude, latitude, altitude, angle_against_north)" " must be configured. Otherwise, return the yaw against the Blender " "coordinates") add_property('_type', 'Automatic', 'ComputationMode', 'string', "Kind of computation, can be one of ['Velocity', 'Position']. " "Only robot with dynamic and Velocity control can choose Velocity " "computation. Default choice is Velocity for robot with physics, " "and Position for others") 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 morse.core.sensor.Sensor.__init__(self, obj, parent) has_physics = bool(self.robot_parent.bge_object.getPhysicsId()) if self._type == 'Automatic': if has_physics: self._type = 'Velocity' else: self._type = 'Position' if self._type == 'Velocity' and not has_physics: logger.error("Invalid configuration : Velocity computation without " "physics") return if self._type == 'Velocity': # make new references to the robot velocities and use those. self.robot_w = self.robot_parent.bge_object.localAngularVelocity else: # previous attitude euler angles as vector self.pp = copy(self.position_3d) if self._use_angle_against_north: self._coord_converter = CoordinateConverter.instance() # imu2body will transform a vector from imu frame to body frame self.imu2body = self.sensor_to_robot_position_3d() # rotate vector from body to imu frame self.rot_b2i = self.imu2body.rotation.conjugated() logger.info("Attitude Component initialized, runs at %.2f Hz ", self.frequency)
[docs] def sim_attitude_simple(self): """ Simulate angular velocity measurements via simple differences. """ # linear and angular velocities rates = angular_velocities(self.pp, self.position_3d, 1 / self.frequency) self.pp = copy(self.position_3d) return rates
[docs] def sim_attitude_with_physics(self): """ Simulate angular velocity using the physics of the robot. """ return self.rot_b2i * self.robot_w
[docs] def default_action(self): if self._type == 'Velocity': rates = self.sim_attitude_with_physics() else: rates = self.sim_attitude_simple() # Store the important data self.local_data['rotation'] = self.position_3d.euler if self._use_angle_against_north: self.local_data['rotation'][2] = \ normalise_angle( - self._coord_converter.angle_against_geographic_north(self.position_3d.euler)) self.local_data['angular_velocity'] = rates