<commit_before>
from math import sqrt

from RULEngine.controllers.PID import PID
from RULEngine.controllers.controller_base_class import ControllerBaseClass
from RULEngine.robot import Robot
from Util import Pose, Position
from Util.geometry import wrap_to_pi, rotate


class RealVelocityController(ControllerBaseClass):

    def __init__(self, control_setting):
        self.orientation_controller = PID(**control_setting['rotation'], wrap_error=True)

    def execute(self, robot: Robot):

        target = Pose(robot.path.points[1], robot.target_orientation)

        error = Pose()
        error.position = target.position - robot.pose.position
        error.orientation = wrap_to_pi(target.orientation - robot.pose.orientation)

        speed_norm = robot.cruise_speed
        if is_time_to_break(robot.pose, robot.path.points[-1], robot.cruise_speed, robot.max_linear_acceleration):
            speed_norm = robot.min_linear_speed  # Near zero, but not quite

        # TODO: test this IRL
        # speed_norm = optimal_speed(robot.pose, path.points[-1], robot.cruise_speed, robot.max_linear_acceleration)

        vel = speed_norm * error.position / error.norm

        cmd_pos = rotate(vel, -robot.pose.orientation)
        cmd_orientation = self.orientation_controller.execute(error.orientation)

        return Pose(cmd_pos, cmd_orientation)

    def reset(self):
        pass

<commit_msg>minimum speed to 0 in real life<commit_after>
from math import sqrt

from RULEngine.controllers.PID import PID
from RULEngine.controllers.controller_base_class import ControllerBaseClass
from RULEngine.robot import Robot
from Util import Pose, Position
from Util.geometry import wrap_to_pi, rotate


class RealVelocityController(ControllerBaseClass):

    def __init__(self, control_setting):
        self.orientation_controller = PID(**control_setting['rotation'], wrap_error=True)

    def execute(self, robot: Robot):

        target = Pose(robot.path.points[1], robot.target_orientation)

        error = Pose()
        error.position = target.position - robot.pose.position
        error.orientation = wrap_to_pi(target.orientation - robot.pose.orientation)

        speed_norm = robot.cruise_speed
        if is_time_to_break(robot.pose, robot.path.points[-1], robot.cruise_speed, robot.max_linear_acceleration):
            speed_norm = 0

        # TODO: test this IRL
        # speed_norm = optimal_speed(robot.pose, path.points[-1], robot.cruise_speed, robot.max_linear_acceleration)

        vel = speed_norm * error.position / error.norm

        cmd_pos = rotate(vel, -robot.pose.orientation)
        cmd_orientation = self.orientation_controller.execute(error.orientation)

        return Pose(cmd_pos, cmd_orientation)

    def reset(self):
        pass


{'fraction_non_alphanumeric': 0.08625045110068567, 'fraction_numerical': 0.00288704438830747, 'mean_word_length': 3.64321608040201, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4033169', 'n_tokens_mistral': 866, 'n_tokens_neox': 795, 'n_words': 177}