def update(self, message):
quaternion = mathutils.Quaternion((message.w, message.x, message.y, message.z))
euler = quaternion.to_euler()
self.data["roll"] = euler.x
self.data["pitch"] = euler.y
self.data["yaw"] = euler.z
logger.debug("Set orientation to RPY (%.3f %.3f %.3f)" % \
tuple(math.degrees(a) for a in euler))
def update(self, message):
self.data["x"] = message.position.x
self.data["y"] = message.position.y
self.data["z"] = message.position.z
quaternion = mathutils.Quaternion(
(message.orientation.w, message.orientation.x,
message.orientation.y, message.orientation.z))
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())