1import logging; logger = logging.getLogger("morse." + __name__)
2from morse.core import mathutils
3import morse.core.actuator
4from morse.helpers.components import add_data, add_property
5
6class ForceTorque(morse.core.actuator.Actuator):
7    """
8    This class will read force and torque as input
9    from an external middleware.
10    The forces and torques are transformed from the actuator frame to the
11    parent robot frame and then applied to the robot blender object.
12    If the property RobotFrame is set to True it will be applied
13    directly in the robot frame without changes.
14    """
15
16    _name = "Force/Torque Motion Controller"
17    _short_desc="Motion controller using force and torque"
18
19
20    add_data('force', [0.0, 0.0, 0.0], "vec3<float>", "force along x, y, z")
21    add_data('torque', [0.0, 0.0, 0.0], "vec3<float>", "torque around x, y, z")
22
23    add_property('_robot_frame', False, 'RobotFrame', 'bool', 'If set to true '
24            'the inputs are applied in the Robot coordinate frame instead of the '
25            'actuator frame.')
26
27
28    def __init__(self, obj, parent=None):
29        logger.info('%s initialization' % obj.name)
30        # Call the constructor of the parent class
31        morse.core.actuator.Actuator.__init__(self, obj, parent)
32
33        logger.info('Component initialized')
34
35
36    def default_action(self):
37        """ Apply (force, torque) to the parent robot. """
38        # Get the the parent robot
39        robot = self.robot_parent
40
41        if self._robot_frame:
42            # directly apply local forces and torques to the blender object of the parent robot
43            robot.bge_object.applyForce(self.local_data['force'], True)
44            robot.bge_object.applyTorque(self.local_data['torque'], True)
45        else:
46            (loc, rot, scale) = robot.position_3d.transformation3d_with(self.position_3d).matrix.decompose()
47            # rotate into robot frame, but still at actuator origin
48            force = rot * mathutils.Vector(self.local_data['force'])
49            torque = rot * mathutils.Vector(self.local_data['torque'])
50            # add torque due to lever arm
51            torque += loc.cross(force)
52            robot.bge_object.applyForce(force, True)
53            robot.bge_object.applyTorque(torque, True)
54