python3-morse-simulator 1.4-2 / usr / lib / python3 / dist-packages / morse / actuators / pa_10.py

import logging; logger = logging.getLogger("morse." + __name__)
import math
import morse.core.actuator
from morse.core import blenderapi
from morse.core import mathutils
from morse.helpers.morse_math import normalise_angle, rotation_direction
from morse.core.services import service
from morse.helpers.components import add_data, add_property

class PA10(morse.core.actuator.Actuator):
    """
    This actuator reads a list of angles for the segments of the
    Mitsubishi PA-10 arm and applies them as local rotations.

    Angles are expected in radians.
    """

    _name = "Mitsubishi PA-10"
    _short_desc = "PA-10 6-DOF robotic arm"

    add_property('_speed', 1.0, "Speed", "float", 'speed of each joint, in rad/s')
    add_property('_tolerance', math.radians(5), "Tolerance", "float", 'tolerance on the position, in radians')

    add_data('seg0', 0.0, "float", "first joint (base), in radians")
    add_data('seg1', 0.0, "float", "second joint, in radians")
    add_data('seg2', 0.0, "float", "third joint, in radians")
    add_data('seg3', 0.0, "float", "fourth joint, in radians")
    add_data('seg4', 0.0, "float", "fifth joint, in radians")
    add_data('seg5', 0.0, "float", "sixth joint (wrist), in radians")

    def __init__(self, obj, parent=None):
        # Call the constructor of the parent class
        morse.core.actuator.Actuator.__init__(self, obj, parent)

        # The axis along which the different segments rotate
        # Considering the rotation of the arm as installed in Jido
        self._dofs = ['z', 'y', 'y', 'z', 'y', 'z']

        self._segments = []
        segment = self.bge_object.children[0]
        for i in range(6):
            self._segments.append(segment)
            try:
                segment = segment.children[0]
            except IndexError:
                break
        logger.info ("Arm segment list: ", self._segments)

        # Get the references to the segment at the tip of the arm
        for child in self.bge_object.childrenRecursive:
            if 'PA10-6' in child.name:
                self._arm_tip = child
                break

        # Any other objects children of the Kuka arm are assumed
        #  to be mounted on the tip of the arm
        for child in self.bge_object.children:
            if not 'PA10' in child.name:
                child.setParent(self._arm_tip)

        # Variable to store the reference to the Sound actuator
        self._sound = None

        self._moving = False

        logger.info('Component initialized')
        #logger.setLevel(logging.DEBUG)


    def default_action(self):
        """ Apply rotation to the arm segments """
        # Get the reference to the Sound actuator
        if self._sound is None:
            logger.debug ("ACTIVATING THE SOUND ACTUATOR")
            contr = blenderapi.controller()
            self._sound = contr.actuators['Sound']
            contr.activate(self._sound)
            self._sound.stopSound()

        # Reset movement variables
        rx, ry, rz = 0.0, 0.0, 0.0

        # Scale the speeds to the time used by Blender
        try:
            rotation = self._speed / self.frequency
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        self._moving = False

        for i in range(6):
            key = ('seg%d' % i)
            target_angle = normalise_angle(self.local_data[key])

            # Get the next segment
            segment = self._segments[i]

            # Extract the angles
            rot_matrix = segment.localOrientation
            segment_matrix = mathutils.Matrix((rot_matrix[0], rot_matrix[1], rot_matrix[2]))
            segment_euler = segment_matrix.to_euler()

            # Use the corresponding direction for each rotation
            if self._dofs[i] == 'y':
                ry = rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
                #logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, _tolerance, rotation, ry))

            elif self._dofs[i] == 'z':
                rz = rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
                #logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, _tolerance, rotation, rz))

            logger.debug("ry = %.4f, rz = %.4f" % (ry, rz))

            # Give the movement instructions directly to the parent
            # The second parameter specifies a "local" movement
            segment.applyRotation([rx, ry, rz], True)

            if ry != 0.0 or rz != 0.0:
                self._moving = True

            # Reset the rotations for the next segment
            ry = rz = 0

        if self._moving:
            self._sound.startSound()
            logger.debug("STARTING SOUND")
        else:
            self._sound.stopSound()
            logger.debug("STOPPING SOUND")


    @service
    def set_rotation_array(self, seg0=0, seg1=0, seg2=0, seg3=0, seg4=0, seg5=0):
        """
        MORSE service to set the rotation for each of the arm joints.
        It receives an array containing the angle to give to each of
        the robot articulations. Angles are expected in radians. The length
        of the array should be equal to 6 or less, where any values not 
        specified will be considered as 0.0.

        :param seg0: 1st joint angle (base)
        :param seg1: 2nd joint angle
        :param seg2: 3rd joint angle
        :param seg3: 4th joint angle
        :param seg4: 5th joint angle
        :param seg5: 6th joint angle (wrist)
        """
        self.local_data['seg0'] = seg0
        self.local_data['seg1'] = seg1
        self.local_data['seg2'] = seg2
        self.local_data['seg3'] = seg3
        self.local_data['seg4'] = seg4
        self.local_data['seg5'] = seg5
        return None