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
self.robot_vel = self.robot_parent.bge_object.worldLinearVelocity
else:
self.pp = copy(self.position_3d)
# previous linear velocity
self.plv = mathutils.Vector((0.0, 0.0, 0.0))
# previous angular velocity
self.pav = mathutils.Vector((0.0, 0.0, 0.0))
self.gravity = -blenderapi.gravity()
# 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.debug("imu2body rotation RPY [% .3f % .3f % .3f]" %
tuple(math.degrees(a) for a in self.imu2body.euler))
logger.debug("imu2body translation [% .3f % .3f % .3f]" %
tuple(self.imu2body.translation))
if self.imu2body.translation.length > 0.01:
self.compute_offset_acceleration = True
else:
self.compute_offset_acceleration = False
# reference for rotating a vector from imu frame to world frame
self.rot_i2w = self.bge_object.worldOrientation
self.mag = MagnetoDriver()
logger.info("IMU Component initialized, runs at %.2f Hz ",
self.frequency)
