/usr/lib/python3/dist-packages/morse/sensors/velocity.py is in python3-morse-simulator 1.4-2.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 | import logging; logger = logging.getLogger("morse." + __name__)
import morse.core.sensor
from morse.helpers.components import add_data, add_property
from morse.core.mathutils import *
from math import degrees
from morse.helpers.velocity import linear_velocities, angular_velocities
from copy import copy
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)')
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)
self.pp = copy(self.position_3d)
self.pt = 0.0 # previous timestamp
self.dt = 0.0 # diff
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
# 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)
def _sim_simple(self):
self.dt = self.robot_parent.gettime() - self.pt
self.pt = self.robot_parent.gettime()
if self.dt < 1e-6:
return
v = linear_velocities(self.pp, self.position_3d, self.dt)
w = angular_velocities(self.pp, self.position_3d, self.dt)
self.pp = copy(self.position_3d)
w2a = self.position_3d.rotation_matrix.transposed()
self.local_data['linear_velocity'] = w2a * v
self.local_data['angular_velocity'] = w
self.local_data['world_linear_velocity'] = v
def default_action(self):
""" Get the linear and angular velocity of the blender object. """
if self._type == 'Velocity':
# 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()
else:
self._sim_simple()
|