Source code for morse.actuators.force_torque
import logging; logger = logging.getLogger("morse." + __name__)
from morse.core import mathutils
import morse.core.actuator
from morse.helpers.components import add_data, add_property
[docs]class ForceTorque(morse.core.actuator.Actuator):
"""
This class will read force and torque as input
from an external middleware.
The forces and torques are transformed from the actuator frame to the
parent robot frame and then applied to the robot blender object.
If the property RobotFrame is set to True it will be applied
directly in the robot frame without changes.
"""
_name = "Force/Torque Motion Controller"
_short_desc="Motion controller using force and torque"
add_data('force', [0.0, 0.0, 0.0], "vec3<float>", "force along x, y, z")
add_data('torque', [0.0, 0.0, 0.0], "vec3<float>", "torque around x, y, z")
add_property('_robot_frame', False, 'RobotFrame', 'bool', 'If set to true '
'the inputs are applied in the Robot coordinate frame instead of the '
'actuator frame.')
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)
logger.info('Component initialized')
[docs] def default_action(self):
""" Apply (force, torque) to the parent robot. """
# Get the the parent robot
robot = self.robot_parent
if self._robot_frame:
# directly apply local forces and torques to the blender object of the parent robot
robot.bge_object.applyForce(self.local_data['force'], True)
robot.bge_object.applyTorque(self.local_data['torque'], True)
else:
(loc, rot, scale) = robot.position_3d.transformation3d_with(self.position_3d).matrix.decompose()
# rotate into robot frame, but still at actuator origin
force = rot * mathutils.Vector(self.local_data['force'])
torque = rot * mathutils.Vector(self.local_data['torque'])
# add torque due to lever arm
torque += loc.cross(force)
robot.bge_object.applyForce(force, True)
robot.bge_object.applyTorque(torque, True)