Пример #1
0
    def default_action(self):
        """ Apply rotation to the platine unit """
        # Reset movement variables
        rx, ry, rz = 0.0, 0.0, 0.0

        if self._is_manual_mode:
            return

        # Update the postition of the base platforms
        try:
            self._pan_position_3d.update(self._pan_base)
            self._tilt_position_3d.update(self._tilt_base)
        except AttributeError as detail:
            logger.error("The PTU is missing the pan and/or tilt bases. Discarding action.")
            return

        try:
            normal_speed = self._speed / self.frequency
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        current_pan = self._pan_position_3d.yaw
        current_tilt = self._tilt_position_3d.pitch

        logger.debug("PTU: pan=%.4f, tilt=%.4f" % (current_pan, current_tilt))

        # Get the angles in a range of -PI, PI
        target_pan = morse_math.normalise_angle(self.local_data['pan'])
        target_tilt = morse_math.normalise_angle(self.local_data['tilt'])
        logger.debug("Targets: pan=%.4f, tilt=%.4f" % (target_pan, target_tilt))

        # Get the current rotation of the parent robot
        parent_pan = self.robot_parent.position_3d.euler.z
        parent_tilt = self.robot_parent.position_3d.euler.y
        logger.debug("Parent: pan=%.4f, tilt=%.4f" % (parent_pan, parent_tilt))

        # Compute the rotation relative to the parent robot
        relative_pan = current_pan - parent_pan
        correct_pan = morse_math.normalise_angle(relative_pan)
        relative_tilt = current_tilt - parent_tilt
        correct_tilt = morse_math.normalise_angle(relative_tilt)

        # Store the variables to acces as a service:
        self._current_pan = correct_pan
        self._current_tilt = correct_tilt

        if (abs(target_pan - correct_pan) < self._tolerance and \
            abs(target_tilt - correct_tilt) < self._tolerance):
            self.completed((status.SUCCESS))

        # Determine the direction of the rotation, if any
        ry = morse_math.rotation_direction(correct_tilt, target_tilt, self._tolerance, normal_speed)
        rz = morse_math.rotation_direction(correct_pan, target_pan, self._tolerance, normal_speed)

        # Give the rotation instructions directly to the parent
        # The second parameter specifies a "local" movement
        self._pan_base.applyRotation([0.0, 0.0, rz], True)
        self._tilt_base.applyRotation([0.0, ry, 0.0], True)
Пример #2
0
    def default_action(self):
        """ Apply rotation angles to the segments of the arm """

        armature = self.blender_obj
        logger.debug("The armature is: '%s' (%s)" % (armature, type(armature)))

        for channel in armature.channels:
            segment_angle = channel.joint_rotation
            logger.debug("Channel '%s' st: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))

            # Get the normalised angle for this segment
            target_angle = morse_math.normalise_angle(self.local_data[channel.name])

            # Use the corresponding direction for each rotation
            if self._dofs[channel.name][1] == 1:
                #ry = morse_math.rotation_direction(segment_angle[1], target_angle, self._tolerance, rotation)
                segment_angle[1] = target_angle
            elif self._dofs[channel.name][2] == 1:
                #rz = morse_math.rotation_direction(segment_angle[2], target_angle, self._tolerance, rotation)
                segment_angle[2] = target_angle

            logger.debug("Channel '%s' fn: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
            channel.joint_rotation = segment_angle

            armature.update()
Пример #3
0
    def default_action(self):
        """ Apply rotation angles to the segments of the arm """

        armature = self.blender_obj
        logger.debug("The armature is: '%s' (%s)" % (armature, type(armature)))

        for channel in armature.channels:
            segment_angle = channel.joint_rotation
            logger.debug("Channel '%s' st: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))

            # Get the normalised angle for this segment
            target_angle = morse_math.normalise_angle(self.local_data[channel.name])

            # Use the corresponding direction for each rotation
            if self._dofs[channel.name][1] == 1:
                #ry = morse_math.rotation_direction(segment_angle[1], target_angle, self._tolerance, rotation)
                segment_angle[1] = target_angle
            elif self._dofs[channel.name][2] == 1:
                #rz = morse_math.rotation_direction(segment_angle[2], target_angle, self._tolerance, rotation)
                segment_angle[2] = target_angle

            logger.debug("Channel '%s' fn: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
            channel.joint_rotation = segment_angle

            armature.update()
Пример #4
0
    def default_action(self):
        """ Compute the relative position and rotation of the robot

        The measurements are taken with respect to the previous position
        and orientation of the robot
        """
        # Compute the difference in positions with the previous loop
        self.local_data['dx'] = self.robot_parent.position_3d.x - self.previous_position[0]
        self.local_data['dy'] = self.robot_parent.position_3d.y - self.previous_position[1]
        self.local_data['dz'] = self.robot_parent.position_3d.z - self.previous_position[2]

        # Compute the difference in orientation with the previous loop
        dyaw = self.robot_parent.position_3d.yaw - self.previous_orientation[0]
        dpitch = self.robot_parent.position_3d.pitch - self.previous_orientation[1]
        droll = self.robot_parent.position_3d.roll - self.previous_orientation[2]
        self.local_data['dyaw'] = morse_math.normalise_angle(dyaw)
        self.local_data['dpitch'] = morse_math.normalise_angle(dpitch)
        self.local_data['droll'] = morse_math.normalise_angle(droll)

        # Store the 'new' previous data
        self.previous_position = [self.robot_parent.position_3d.x, self.robot_parent.position_3d.y, self.robot_parent.position_3d.z]
        self.previous_orientation = [self.robot_parent.position_3d.yaw, self.robot_parent.position_3d.pitch, self.robot_parent.position_3d.roll]
Пример #5
0
    def default_action(self):
        """ Compute the relative position and rotation of the robot

        The measurements are taken with respect to the previous position
        and orientation of the robot
        """
        # Compute the difference in positions with the previous loop
        self.local_data['dx'] = self.robot_parent.position_3d.x - self.previous_position[0]
        self.local_data['dy'] = self.robot_parent.position_3d.y - self.previous_position[1]
        self.local_data['dz'] = self.robot_parent.position_3d.z - self.previous_position[2]

        # Compute the difference in orientation with the previous loop
        dyaw = self.robot_parent.position_3d.yaw - self.previous_orientation[0]
        dpitch = self.robot_parent.position_3d.pitch - self.previous_orientation[1]
        droll = self.robot_parent.position_3d.roll - self.previous_orientation[2]
        self.local_data['dyaw'] = morse_math.normalise_angle(dyaw)
        self.local_data['dpitch'] = morse_math.normalise_angle(dpitch)
        self.local_data['droll'] = morse_math.normalise_angle(droll)

        # Store the 'new' previous data
        self.previous_position = [self.robot_parent.position_3d.x, self.robot_parent.position_3d.y, self.robot_parent.position_3d.z]
        self.previous_orientation = [self.robot_parent.position_3d.yaw, self.robot_parent.position_3d.pitch, self.robot_parent.position_3d.roll]
Пример #6
0
    def default_action(self):
        """ Apply rotation to the arm segments """

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

        # Tick rate is the real measure of time in Blender.
        # By default it is set to 60, regardles of the FPS
        # If logic tick rate is 60, then: 1 second = 60 ticks
        ticks = GameLogic.getLogicTicRate()
        # Scale the speeds to the time used by Blender
        try:
            rotation = self._speed / ticks
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        # Use the length of _dofs, since it won't change.
        # The length of _segments will change if more objects are attached
        #  at the end of the arm, as in the case of a hand
        for i in range(len(self._dofs)):
            key = ('seg%d' % i)
            target_angle = morse_math.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 = morse_math.rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
            elif self._dofs[i] == '-y':
                ry = morse_math.rotation_direction(segment_euler[1], -target_angle, self._tolerance, rotation)
            elif self._dofs[i] == 'z':
                rz = morse_math.rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
                logger.debug("PARAMETERS: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, self._tolerance, rotation, rz))

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

            # Reset the rotations for the next segment
            ry = rz = 0
Пример #7
0
    def default_action(self):
        """ Apply rotation angles to the segments of the arm """

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

        # Tick rate is the real measure of time in Blender.
        # By default it is set to 60, regardles of the FPS
        # If logic tick rate is 60, then: 1 second = 60 ticks
        ticks = GameLogic.getLogicTicRate()
        # Scale the speeds to the time used by Blender
        try:
            rotation = self._speed / ticks
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        armature = self.blender_obj
        logger.info("The armature is: '%s' (%s)" % (armature, type(armature)))

        i = 0
        for channel in armature.channels:
            segment_angle = channel.joint_rotation
            logger.debug("\tChannel '%s': (%.4f, %.4f, %.4f)" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))

            key = ('seg%d' % i)
            # Get the normalised angle for this segment
            target_angle = morse_math.normalise_angle(self.local_data[key])
            logger.info("%.4f " % target_angle, end='')

            # Use the corresponding direction for each rotation
            if self._dofs[i] == 'y':
                ry = morse_math.rotation_direction(segment_angle[1], target_angle, self._tolerance, rotation)
            elif self._dofs[i] == 'z':
                rz = morse_math.rotation_direction(segment_angle[2], target_angle, self._tolerance, rotation)

            logger.info("[%.4f, %.4f, %.4f] " % (rx, ry, rz))

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

            # Reset the rotations for the next segment
            ry = rz = 0
            i = i + 1
Пример #8
0
    def default_action(self):
        """ Apply rotation to the arm segments """
        # Get the reference to the Sound actuator
        if self._sound == None:
            logger.debug ("ACTIVATING THE SOUND ACTUATOR")
            contr = bge.logic.getCurrentController()
            self._sound = contr.actuators['Sound']
            contr.activate(self._sound)
            self._sound.stopSound()

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

        # Tick rate is the real measure of time in Blender.
        # By default it is set to 60, regardles of the FPS
        # If logic tick rate is 60, then: 1 second = 60 ticks
        ticks = bge.logic.getLogicTicRate()
        # Scale the speeds to the time used by Blender
        try:
            rotation = self._speed / ticks
        # 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 = morse_math.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 = morse_math.rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
                #logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, self._tolerance, rotation, ry))
            elif self._dofs[i] == 'z':
                rz = morse_math.rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
                #logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, self._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")
Пример #9
0
    def default_action(self):
        """ Apply rotation to the platine unit """
        # Reset movement variables
        rx, ry, rz = 0.0, 0.0, 0.0

        if self._is_manual_mode:
            return

        # Update the postition of the base platforms
        try:
            self._pan_position_3d.update(self._pan_base)
            self._tilt_position_3d.update(self._tilt_base)
        except AttributeError as detail:
            logger.error(
                "Platine is missing the pan and tilt bases. Platine does not work!"
            )
            return

        # Tick rate is the real measure of time in Blender.
        # By default it is set to 60, regardles of the FPS
        # If logic tick rate is 60, then: 1 second = 60 ticks
        ticks = bge.logic.getLogicTicRate()

        try:
            normal_speed = self._speed / ticks
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        current_pan = self._pan_position_3d.yaw
        current_tilt = self._tilt_position_3d.pitch

        logger.debug("Platine: pan=%.4f, tilt=%.4f" %
                     (current_pan, current_tilt))

        # Get the angles in a range of -PI, PI
        target_pan = morse_math.normalise_angle(self.local_data['pan'])
        target_tilt = morse_math.normalise_angle(self.local_data['tilt'])
        logger.debug("Targets: pan=%.4f, tilt=%.4f" %
                     (target_pan, target_tilt))

        # Get the current rotation of the parent robot
        parent_pan = self.robot_parent.position_3d.euler.z
        parent_tilt = self.robot_parent.position_3d.euler.y
        logger.debug("Parent: pan=%.4f, tilt=%.4f" % (parent_pan, parent_tilt))

        # Compute the rotation relative to the parent robot
        relative_pan = current_pan - parent_pan
        correct_pan = morse_math.normalise_angle(relative_pan)
        relative_tilt = current_tilt - parent_tilt
        correct_tilt = morse_math.normalise_angle(relative_tilt)

        # Store the variables to acces as a service:
        self._current_pan = correct_pan
        self._current_tilt = correct_tilt

        if (abs(target_pan - correct_pan) < self._tolerance and \
            abs(target_tilt - correct_tilt) < self._tolerance ):
            self.completed((status.SUCCESS))

        # Determine the direction of the rotation, if any
        ry = morse_math.rotation_direction(correct_tilt, target_tilt,
                                           self._tolerance, normal_speed)
        rz = morse_math.rotation_direction(correct_pan, target_pan,
                                           self._tolerance, normal_speed)

        # Give the rotation instructions directly to the parent
        # The second parameter specifies a "local" movement
        self._pan_base.applyRotation([0.0, 0.0, rz], True)
        self._tilt_base.applyRotation([0.0, ry, 0.0], True)
Пример #10
0
    def default_action(self):
        """ Apply rotation to the arm segments """
        # Get the reference to the Sound actuator
        if self._sound == None:
            logger.debug("ACTIVATING THE SOUND ACTUATOR")
            contr = bge.logic.getCurrentController()
            self._sound = contr.actuators['Sound']
            contr.activate(self._sound)
            self._sound.stopSound()

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

        # Tick rate is the real measure of time in Blender.
        # By default it is set to 60, regardles of the FPS
        # If logic tick rate is 60, then: 1 second = 60 ticks
        ticks = bge.logic.getLogicTicRate()
        # Scale the speeds to the time used by Blender
        try:
            rotation = self._speed / ticks
        # 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 = morse_math.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 = morse_math.rotation_direction(segment_euler[1],
                                                   target_angle,
                                                   self._tolerance, rotation)
                #logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, self._tolerance, rotation, ry))
            elif self._dofs[i] == 'z':
                rz = morse_math.rotation_direction(segment_euler[2],
                                                   target_angle,
                                                   self._tolerance, rotation)
                #logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, self._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")
Пример #11
0
    def default_action(self):
        """ Apply rotation to the platine unit """
        # Reset movement variables
        rx, ry, rz = 0.0, 0.0, 0.0

        if self._is_manual_mode:
            return

        # Update the postition of the base platforms
        try:
            self._pan_position_3d.update(self._pan_base)
            self._tilt_position_3d.update(self._tilt_base)
        except AttributeError as detail:
            logger.error("Platine is missing the pan and tilt bases. Platine does not work!")
            return

        # Tick rate is the real measure of time in Blender.
        # By default it is set to 60, regardles of the FPS
        # If logic tick rate is 60, then: 1 second = 60 ticks
        ticks = GameLogic.getLogicTicRate()

        try:
            normal_speed = self._speed / ticks
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        current_pan = self._pan_position_3d.yaw
        current_tilt = self._tilt_position_3d.pitch

        if (
            abs(current_pan - self.local_data["pan"]) < self._tolerance
            and abs(current_tilt - self.local_data["tilt"]) < self._tolerance
        ):
            self.completed((status.SUCCESS))

        logger.debug("Platine: pan=%.4f, tilt=%.4f" % (current_pan, current_tilt))

        # Get the angles in a range of -PI, PI
        target_pan = morse_math.normalise_angle(self.local_data["pan"])
        target_tilt = morse_math.normalise_angle(self.local_data["tilt"])
        logger.debug("Targets: pan=%.4f, tilt=%.4f" % (target_pan, target_tilt))

        # Get the current rotation of the parent robot
        parent_pan = self.robot_parent.position_3d.euler.z
        parent_tilt = self.robot_parent.position_3d.euler.y
        logger.debug("Parent: pan=%.4f, tilt=%.4f" % (parent_pan, parent_tilt))

        # Compute the rotation relative to the parent robot
        relative_pan = current_pan - parent_pan
        correct_pan = morse_math.normalise_angle(relative_pan)
        relative_tilt = current_tilt - parent_tilt
        correct_tilt = morse_math.normalise_angle(relative_tilt)

        # Determine the direction of the rotation, if any
        ry = morse_math.rotation_direction(correct_tilt, target_tilt, self._tolerance, normal_speed)
        rz = morse_math.rotation_direction(correct_pan, target_pan, self._tolerance, normal_speed)

        # Give the rotation instructions directly to the parent
        # The second parameter specifies a "local" movement
        self._pan_base.applyRotation([0.0, 0.0, rz], True)
        self._tilt_base.applyRotation([0.0, ry, 0.0], True)