Source code for morse.actuators.teleport

import logging; logger = logging.getLogger("morse." + __name__)
import morse.core.actuator
from morse.core.services import service
from morse.core import mathutils
from morse.helpers.components import add_data
from morse.helpers.transformation import Transformation3d

[docs]class Teleport(morse.core.actuator.Actuator): """ This actuator teleports the robot to the absolute position and orientation with respect to the origin of the Blender coordinate reference. Angles are expected in radians. .. note:: Coordinates are given with respect to the origin of Blender's coordinate axis. """ _name = "Teleport" _short_desc = "Motion controller which changes instantly robot pose \ (position and orientation)" add_data('x', 'initial robot X position', "float", "X coordinate of the destination, in meter") add_data('y', 'initial robot Y position', "float", "Y coordinate of the destination, in meter") add_data('z', 'initial robot Z position', "float", "Z coordinate of the destination, in meter") add_data('yaw', 'Initial robot yaw', "float", 'Rotation of the robot around Z axis, in radian') add_data('pitch', 'Initial robot pitch', "float", 'Rotation of the robot around Y axis, in radian') add_data('roll', 'Initial robot roll', "float", 'Rotation of the robot around X axis, in radian') def __init__(self, obj, parent=None): logger.info('%s initialization' % obj.name) # Call the constructor of the parent class morse.core.actuator.Actuator.__init__(self, obj, parent) pose3d = self.position_3d self.local_data['x'] = pose3d.x self.local_data['y'] = pose3d.y self.local_data['z'] = pose3d.z self.local_data['roll'] = pose3d.roll self.local_data['pitch'] = pose3d.pitch self.local_data['yaw'] = pose3d.yaw self.actuator2robot = self.position_3d.transformation3d_with(self.robot_parent.position_3d) logger.info('Component initialized') @service
[docs] def translate(self, x, y = 0.0, z = 0.0): """ Translate the actuator owner by the given (x,y,z) vector. This is a **relative** displacement. :param x: X translation, in meter :param y: (default: 0.0) Y translation, in meter :param z: (default: 0.0) Z translation, in meter """ self.local_data['x'] += x self.local_data['y'] += y self.local_data['z'] += z
@service
[docs] def rotate(self, roll, pitch = 0.0, yaw=0.0): """ Rotates the actuator owner by the given (roll,pitch,yaw). This is a **relative** rotation. :param roll: roll rotation, in radians :param pitch: (default: 0.0) pitch rotation, in radians :param yaw: (default: 0.0) yaw rotation, in radians """ self.local_data['roll'] += roll self.local_data['pitch'] += pitch self.local_data['yaw'] += yaw
[docs] def default_action(self): """ Change the parent robot pose. """ # New parent position position = mathutils.Vector((self.local_data['x'], self.local_data['y'], self.local_data['z'])) # New parent orientation orientation = mathutils.Euler([self.local_data['roll'], self.local_data['pitch'], self.local_data['yaw']]) world2actuator = Transformation3d(None) world2actuator.translation = position world2actuator.rotation = orientation (loc, rot, _) = (world2actuator.matrix * self.actuator2robot.matrix).decompose() self.robot_parent.force_pose(loc, rot)