def update(self, message):
self.data["x"] = message.position.x
self.data["y"] = message.position.y
self.data["z"] = message.position.z
self.data['position'] = mathutils.Vector(
(message.position.x, message.position.y, message.position.z))
quaternion = mathutils.Quaternion(
(message.orientation.w, message.orientation.x,
message.orientation.y, message.orientation.z))
self.data['orientation'] = quaternion
euler = quaternion.to_euler()
self.data['roll'] = euler.x
self.data['pitch'] = euler.y
self.data['yaw'] = euler.z
def default_action(self):
""" Main loop of the actuator.
Implements the component behaviour
"""
""" Move the robot. """
quaternion = mathutils.Quaternion(
(self.local_data['qw'], self.local_data['qx'],
self.local_data['qy'], self.local_data['qz']))
euler = quaternion.to_euler()
# New parent position
position = mathutils.Vector(
(self.local_data['x'], -self.local_data['y'],
-self.local_data['z']))
# New parent orientation
orientation = mathutils.Euler([euler.x, -euler.y, -euler.z])
self.robot_parent.force_pose(position, orientation.to_matrix())