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