Source code for morse.sensors.velocity

import logging; logger = logging.getLogger("morse." + __name__)

import morse.core.sensor
from morse.helpers.components import add_data
from math import degrees

[docs]class Velocity(morse.core.sensor.Sensor): """ This sensor returns the linear and angular velocity of the sensor, both in robot frame and in world frame. Linear velocities are expressed in meter . sec ^ -1 while angular velocities are expressed in radian . sec ^ -1. The sensor expects that the associated robot has a physics controller. """ _name = "Velocity" _short_descr = "A Velocity Sensor" add_data('linear_velocity', [0.0, 0.0, 0.0], "vec3<float>", 'velocity in sensor x, y, z axes (in meter . sec ^ -1)') add_data('angular_velocity', [0.0, 0.0, 0.0], "vec3<float>", 'rates in sensor x, y, z axes (in radian . sec ^ -1)') add_data('world_linear_velocity', [0.0, 0.0, 0.0], "vec3<float>", 'velocity in world x, y, z axes (in meter . sec ^ -1)') 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) # The robot needs a physics controller! # Since the sensor does not have physics if not bool(self.robot_parent.bge_object.getPhysicsId()): logger.error("The robot doesn't have a physics controller!") # make new references to the robot velocities and use those. self.robot_w = self.robot_parent.bge_object.localAngularVelocity self.robot_v = self.robot_parent.bge_object.localLinearVelocity self.robot_world_v = self.robot_parent.bge_object.worldLinearVelocity # get the quaternion which will rotate a vector from body to sensor frame self.rot_b2s = self.sensor_to_robot_position_3d().rotation.conjugated() logger.debug("body2sensor rotation RPY [% .3f % .3f % .3f]" % tuple(degrees(a) for a in self.rot_b2s.to_euler())) logger.info("Component initialized, runs at %.2f Hz", self.frequency)
[docs] def default_action(self): """ Get the linear and angular velocity of the blender object. """ # Store the important data self.local_data['linear_velocity'] = self.rot_b2s * self.robot_v self.local_data['angular_velocity'] = self.rot_b2s * self.robot_w self.local_data['world_linear_velocity'] = self.robot_world_v.copy()