diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index a4e4c1a..3078560 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -7,11 +7,11 @@ from components.vision import Vision from pyswervedrive.chassis import SwerveChassis from utilities.navx import NavX -from utilities.pure_pursuit import PurePursuit, insert_trapezoidal_waypoints +from utilities.pure_pursuit import PurePursuit, Waypoint, insert_trapezoidal_waypoints -def reflect_2d_y(v: tuple) -> tuple: - return (v[0], -v[1], v[2], v[3]) +def reflect_y(v: Waypoint) -> Waypoint: + return Waypoint(v.x, -v.y, v.theta, v.v) class AutoBase(AutonomousStateMachine): @@ -30,17 +30,16 @@ class AutoBase(AutonomousStateMachine): def __init__(self): super().__init__() - self.front_cargo_bay = (5.6 - SwerveChassis.LENGTH / 2, 0.2, 0, 0.75) - self.setup_loading_bay = (3.3, 3.3, math.pi, 2) - self.loading_bay = (0.2 + SwerveChassis.LENGTH / 2, 3.4, math.pi, 1) - self.side_cargo_bay = (7, 0.8 + SwerveChassis.WIDTH / 2, -math.pi / 2, 1) - self.side_cargo_bay_alignment_point = ( - 7, - 1.8 + SwerveChassis.WIDTH / 2, - -math.pi / 2, - 0.75, + self.front_cargo_bay = Waypoint(5.6 - SwerveChassis.LENGTH / 2, 0.2, 0, 0.75) + self.setup_loading_bay = Waypoint(3.3, 3.3, math.pi, 2) + self.loading_bay = Waypoint(0.2 + SwerveChassis.LENGTH / 2, 3.4, math.pi, 1) + self.side_cargo_bay = Waypoint( + 7, 0.8 + SwerveChassis.WIDTH / 2, -math.pi / 2, 1 ) - self.start_pos = ( + self.side_cargo_bay_alignment_point = Waypoint( + 7, 1.8 + SwerveChassis.WIDTH / 2, -math.pi / 2, 0.75 + ) + self.start_pos = Waypoint( 1.2 + SwerveChassis.LENGTH / 2, 0, 0, @@ -111,7 +110,7 @@ def drive_to_loading_bay(self, initial_call): waypoints = insert_trapezoidal_waypoints( ( self.current_pos, - ( + Waypoint( self.current_pos[0] - 0.5, self.current_pos[1], self.imu.getAngle(), @@ -153,21 +152,17 @@ def stop(self): @property def current_pos(self): - return ( - self.chassis.odometry_x, - self.chassis.odometry_y, - self.imu.getAngle(), - 2, + return Waypoint( + self.chassis.odometry_x, self.chassis.odometry_y, self.imu.getAngle(), 2 ) def follow_path(self): - vx, vy, heading = self.pursuit.find_velocity(self.current_pos) + vx, vy, heading = self.pursuit.find_velocity(self.chassis.position) if self.pursuit.completed_path: self.chassis.set_inputs(0, 0, 0, field_oriented=False) return # TODO implement a system to allow for rotation in waypoints - self.chassis.set_heading_sp(heading) - self.chassis.set_inputs(vx, vy, 0) + self.chassis.set_velocity_heading(vx, vy, heading) def ready_for_vision(self): if self.pursuit.waypoints[-1][4] - self.pursuit.distance_traveled < 1: @@ -181,10 +176,10 @@ class RightStartAuto(AutoBase): def __init__(self): super().__init__() - self.front_cargo_bay = reflect_2d_y(self.front_cargo_bay) - self.setup_loading_bay = reflect_2d_y(self.setup_loading_bay) - self.loading_bay = reflect_2d_y(self.loading_bay) - self.side_cargo_bay = reflect_2d_y(self.side_cargo_bay) + self.front_cargo_bay = reflect_y(self.front_cargo_bay) + self.setup_loading_bay = reflect_y(self.setup_loading_bay) + self.loading_bay = reflect_y(self.loading_bay) + self.side_cargo_bay = reflect_y(self.side_cargo_bay) class LeftStartAuto(AutoBase): diff --git a/autonomous/test.py b/autonomous/test.py index e10a101..6957018 100644 --- a/autonomous/test.py +++ b/autonomous/test.py @@ -1,7 +1,8 @@ from magicbot.state_machine import AutonomousStateMachine, state -from utilities.pure_pursuit import PurePursuit + from pyswervedrive.chassis import SwerveChassis from utilities.navx import NavX +from utilities.pure_pursuit import PurePursuit class TestPursuitAuto(AutonomousStateMachine): @@ -22,10 +23,7 @@ def __init__(self): def move_forwards(self, initial_call): if initial_call: self.pursuit.build_path(self.points) - heading = self.imu.getAngle() - x, y = self.chassis.position - position = (x, y, heading) - vx, vy, vz = self.pursuit.find_velocity(position) + vx, vy, vz = self.pursuit.find_velocity(self.chassis.position) self.chassis.set_inputs(vx, vy, 0) if self.pursuit.completed_path: self.chassis.set_inputs(0, 0, 0) diff --git a/tests/test_pure_pursuit.py b/tests/test_pure_pursuit.py index 5dcd7ae..57d9e40 100644 --- a/tests/test_pure_pursuit.py +++ b/tests/test_pure_pursuit.py @@ -2,14 +2,14 @@ def test_trapezoidal(): - waypoints = [(0, 0, 0, 0), (10, 10, 0, 2)] + waypoints = [pp.Waypoint(0, 0, 0, 0), pp.Waypoint(10, 10, 0, 2)] trap = pp.insert_trapezoidal_waypoints(waypoints, 2, -2) assert len(trap) == 3 assert trap[1][2] == waypoints[1][2], ( "Intermediate waypoint should have end speed when accelerating: %s" % trap ) - waypoints = [(10, 10, 0, 2), (0, 0, 0, 0)] + waypoints = [pp.Waypoint(10, 10, 0, 2), pp.Waypoint(0, 0, 0, 0)] trap = pp.insert_trapezoidal_waypoints(waypoints, 2, -2) assert len(trap) == 3 assert trap[1][2] == waypoints[0][2], ( diff --git a/utilities/pure_pursuit.py b/utilities/pure_pursuit.py index f8ec629..00d627b 100644 --- a/utilities/pure_pursuit.py +++ b/utilities/pure_pursuit.py @@ -1,80 +1,109 @@ import math +from typing import List, NamedTuple, Optional, Sequence, Tuple + import numpy as np -# from magicbot import tunable +#: A point in 2D cartesian space. +Cartesian2D = Tuple[float, float] + + +class Waypoint(NamedTuple): + """A waypoint to feed into PurePursuit.build_path.""" + + x: float + y: float + #: Desired robot heading + theta: float + #: Desired velocity + v: float + + +class Segment(NamedTuple): + """A Waypoint with an additional cumulative displacement.""" + + x: float + y: float + theta: float + v: float + #: Cumulative displacement + s: float class PurePursuit: """ - Pure Pursuit controller for navigation with absolute waypoints + Pure Pursuit controller for navigation with absolute waypoints. + Uses the method outlined here with some changes to be suitible for a swervedrive https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf """ - # look_ahead = tunable(0) # m - # speed_modifier = tunable(0) - # ending_tolerance = tunable(0) # m + waypoints: List[Segment] - def __init__(self, look_ahead, look_ahead_speed_modifier): + def __init__(self, look_ahead: float, look_ahead_speed_modifier: float): self.waypoints = [] self.current_waypoint_number = 0 self.look_ahead = look_ahead self.look_ahead_speed_modifier = look_ahead_speed_modifier self.speed_look_ahead = look_ahead self.completed_path = False - self.distance_traveled = 0 + self.distance_traveled = 0.0 - def find_intersections(self, waypoint_start, waypoint_end, robot_position): + def find_intersections( + self, + waypoint_start: Segment, + waypoint_end: Segment, + robot_position: Cartesian2D, + ) -> Optional[np.ndarray]: """ Find the intersection/s between our lookahead distance and path. + http://mathworld.wolfram.com/Circle-LineIntersection.html NOTE: this will return the intersections in global co-ordinates """ - x_1, y_1 = waypoint_start[0], waypoint_start[1] - x_2, y_2 = waypoint_end[0], waypoint_end[1] - robot_x, robot_y, _, __ = robot_position - x_2 -= robot_x - x_1 -= robot_x - y_2 -= robot_y - y_1 -= robot_y - segment_end = np.array((x_2, y_2)) - - d_x = x_2 - x_1 - d_y = y_2 - y_1 - d_r = math.sqrt(d_x ** 2 + d_y ** 2) - D = x_1 * y_2 - x_2 * y_1 + x1, y1 = waypoint_start.x, waypoint_start.y + x2, y2 = waypoint_end.x, waypoint_end.y + robot_x, robot_y = robot_position + x2 -= robot_x + x1 -= robot_x + y2 -= robot_y + y1 -= robot_y + segment_end = np.array((x2, y2)) + + dx = x2 - x1 + dy = y2 - y1 + dr = math.hypot(dx, dy) + D = x1 * y2 - x2 * y1 r = self.speed_look_ahead - discriminent = r ** 2 * d_r ** 2 - D ** 2 - - if discriminent >= 0: # if an intersection exists - intersection_1 = np.zeros((2)) - intersection_2 = np.zeros((2)) - sqrt_discriminent = math.sqrt(discriminent) - - right_x = self.sgn(d_y) * d_x * sqrt_discriminent - left_x = D * d_y - right_y = abs(d_y) * sqrt_discriminent - left_y = -1 * D * d_x - denominator = d_r ** 2 + delta = r ** 2 * dr ** 2 - D ** 2 + + if delta >= 0: # if an intersection exists + sqrt_delta = math.sqrt(delta) + + right_x = self.sgn(dy) * dx * sqrt_delta + left_x = D * dy + right_y = abs(dy) * sqrt_delta + left_y = -D * dx + denominator = dr ** 2 if denominator == 0: print("Pursuit: caught division by zero") - return - intersection_1[0] = (left_x + right_x) / denominator - intersection_1[1] = (left_y + right_y) / denominator - if discriminent == 0: # if we are tangent to our path + return None + intersection_1 = np.array((left_x + right_x, left_y + right_y)) + intersection_1 /= denominator + if delta == 0: # if we are tangent to our path return intersection_1 - intersection_2[0] = (left_x - right_x) / denominator - intersection_2[1] = (left_y - right_y) / denominator - if np.linalg.norm((intersection_1) - (segment_end)) < np.linalg.norm( - (intersection_2) - (segment_end) + intersection_2 = np.array((left_x - right_x, left_y - right_y)) + intersection_2 /= denominator + if np.linalg.norm(intersection_1 - segment_end) < np.linalg.norm( + intersection_2 - segment_end ): return intersection_1 else: return intersection_2 else: print("No intersection found") + return None - def build_path(self, waypoints): + def build_path(self, waypoints: Sequence[Waypoint]) -> None: """ Take in a list of waypoints used to build a path. @@ -82,27 +111,31 @@ def build_path(self, waypoints): create waypoints with these co-ordinates and distance along the path from the start of the trajectory. """ - self.last_robot_x = waypoints[0][0] - self.last_robot_y = waypoints[0][1] + self.last_robot_x = waypoints[0].x + self.last_robot_y = waypoints[0].y self.completed_path = False self.distance_traveled = 0 self.waypoints = [] - waypoint_distance = 0 + waypoint_distance = 0.0 previous_waypoint = waypoints[0] for waypoint in waypoints: x, y, theta, speed = waypoint # print(waypoint) waypoint_distance += math.hypot( - x - previous_waypoint[0], y - previous_waypoint[1] + x - previous_waypoint.x, y - previous_waypoint.y ) previous_waypoint = waypoint - self.waypoints.append((x, y, theta, speed, waypoint_distance)) + self.waypoints.append(Segment(x, y, theta, speed, waypoint_distance)) self.current_waypoint_number = 0 print(f"waypoints = {self.waypoints}") def compute_direction( - self, robot_position, segment_start, segment_end, distance_along_path - ): + self, + robot_position: Cartesian2D, + segment_start: Segment, + segment_end: Segment, + distance_along_path: float, + ) -> np.ndarray: """Find the goal_point and convert it to relative co-ordinates""" goal_point = self.find_intersections(segment_start, segment_end, robot_position) if goal_point is None: @@ -110,41 +143,41 @@ def compute_direction( # use the next waypoint as our goal point goal_point = segment_end[:2] # print(goal_point) - goal_point = goal_point / np.linalg.norm(goal_point) + goal_point /= np.linalg.norm(goal_point) return goal_point - def sgn(self, number): + @staticmethod + def sgn(number: float) -> int: """Returns the sign of a number, 0 is positive""" if number < 0: return -1 else: return 1 - def distance_along_path(self, robot_position): + def distance_along_path(self, robot_position: Cartesian2D) -> float: """ Find the robots position on the path using odometry. Every timestep, add the distance the robot has travelled to a running total used to check for waypoints. """ - robot_x, robot_y, _, __ = robot_position + robot_x, robot_y = robot_position self.distance_traveled += math.hypot( robot_x - self.last_robot_x, robot_y - self.last_robot_y ) self.last_robot_x = robot_x self.last_robot_y = robot_y - # print(self.distance_traveled) return self.distance_traveled def find_speed( self, - start_path_distance, - end_path_distance, - start_speed, - end_speed, - distance_along_path, - ): + start_path_distance: float, + end_path_distance: float, + start_speed: float, + end_speed: float, + distance_along_path: float, + ) -> float: """ - Find the how fast the robot should be moving at it's current point. + Find the how fast the robot should be moving at its current point. """ local_robot_distance = distance_along_path - start_path_distance local_end_distance = end_path_distance - start_path_distance @@ -153,11 +186,11 @@ def find_speed( target_speed = speed_difference * portion_path_completed + start_speed return target_speed - def find_velocity(self, robot_position): + def find_velocity(self, robot_position: Cartesian2D) -> Tuple[float, float, float]: if self.current_waypoint_number >= len(self.waypoints) - 1: self.completed_path = True - print("path completed") - return None, None, None + print("WARNING: path completed") + return 0, 0, 0 distance_along_path = self.distance_along_path(robot_position) segment_start = self.waypoints[self.current_waypoint_number] segment_end = self.waypoints[self.current_waypoint_number + 1] @@ -170,36 +203,35 @@ def find_velocity(self, robot_position): start_distance, end_distance, start_speed, end_speed, distance_along_path ) vx, vy = direction * speed - heading = segment_end[2] + heading = segment_end.theta self.speed_look_ahead = self.look_ahead + self.look_ahead_speed_modifier * speed - if self.distance_traveled + self.speed_look_ahead >= segment_end[4]: + if self.distance_traveled + self.speed_look_ahead >= end_distance: # if we have reached the end of our current segment self.current_waypoint_number += 1 print("changed segment") return vx, vy, heading -def insert_trapezoidal_waypoints(waypoints, acceleration, deceleration): +def insert_trapezoidal_waypoints( + waypoints: Sequence[Waypoint], acceleration: float, deceleration: float +) -> List[Waypoint]: """Generate how far you have to travel to accelerate and decelerate for speed control. Assumes that the robot should accelerate then cruise when v_init < v_final, otherwise we cruise then decelerate. Args: - acceleration = acceleration when increasing speed - deceleration = acceleration when decreasing speed + acceleration: acceleration when increasing speed + deceleration: acceleration when decreasing speed """ trap_waypoints = [] - for idx in range(len(waypoints) - 1): - segment_start = waypoints[idx] - segment_end = waypoints[idx + 1] - - dx = segment_end[0] - segment_start[0] - dy = segment_end[1] - segment_start[1] + for segment_start, segment_end in zip(waypoints, waypoints[1:]): + dx = segment_end.x - segment_start.x + dy = segment_end.y - segment_start.y segment_distance = math.hypot(dx, dy) - u = segment_start[3] - v = segment_end[3] + u = segment_start.v + v = segment_end.v trap_waypoints.append(segment_start) if v > u: @@ -211,10 +243,11 @@ def insert_trapezoidal_waypoints(waypoints, acceleration, deceleration): # Cannot actually get to speed in time # Leave the segments as they are continue - intermediate = ( - dx * s / segment_distance + segment_start[0], - dy * s / segment_distance + segment_start[1], - ) + segment_end[2:] + intermediate = Waypoint( + dx * s / segment_distance + segment_start.x, + dy * s / segment_distance + segment_start.y, + *segment_end[2:], + ) trap_waypoints.append(intermediate) elif u > v: @@ -225,10 +258,11 @@ def insert_trapezoidal_waypoints(waypoints, acceleration, deceleration): # Not enough time to decelerate # Leave the segments as they are continue - intermediate = ( + intermediate = Waypoint( dx * s / segment_distance + segment_start[0], dy * s / segment_distance + segment_start[1], - ) + segment_start[2:] + *segment_start[2:], + ) trap_waypoints.append(intermediate) trap_waypoints.append(waypoints[-1])