From 50b591ecf803257d4553664531a7b67aca048b78 Mon Sep 17 00:00:00 2001 From: Pokesi <81540683+Pokesi@users.noreply.github.com> Date: Mon, 6 Mar 2023 09:06:02 +1100 Subject: [PATCH 01/11] pt 1 --- components/score_tracker.py | 171 ++++++++++++++++++++++++++++++++ controllers/score_game_piece.py | 51 ++++++---- robot.py | 3 + utilities/game.py | 13 ++- 4 files changed, 216 insertions(+), 22 deletions(-) create mode 100644 components/score_tracker.py diff --git a/components/score_tracker.py b/components/score_tracker.py new file mode 100644 index 00000000..500a4689 --- /dev/null +++ b/components/score_tracker.py @@ -0,0 +1,171 @@ +from enum import Enum +import numpy as np +import numpy.typing as npt +import wpilib +import magicbot +from utilities.game import Node +from ntcore import NetworkTableInstance + +class GridNode(Enum): + CUBE = 0 + CONE = 1 + HYBRID = 2 + +class ScoreTracker: + CUBE_MASK = np.array( + [ + [0, 1, 0, 0, 1, 0, 0, 1, 0], + [0, 1, 0, 0, 1, 0, 0, 1, 0], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + ], + dtype=bool, + ) + CONE_MASK = np.array( + [ + [1, 0, 1, 1, 0, 1, 1, 0, 1], + [1, 0, 1, 1, 0, 1, 1, 0, 1], + [1, 1, 1, 1, 1, 1, 1, 1, 1], + ], + dtype=bool, + ) + + CONF_EXP_FILTER_ALPHA = 0.8 + + CONF_THRESHOLD = 0.2 + + def __init__(self) -> None: + # -1.0 - no piece for certain, 1.0 - a piece for certain, 0.0 - unsure + self.confidences_red: np.ndarray = np.zeros((3, 9), dtype=float) + self.confidences_blue: np.ndarray = np.zeros((3, 9), dtype=float) + self.state_red = np.zeros((3, 9), dtype=bool) + self.state_blue = np.zeros((3, 9), dtype=bool) + self.did_states_change = magicbot.will_reset_to(False) + self.inst = NetworkTableInstance.getDefault() + nt = self.inst.getTable("left_camera") + self.nodes = nt.getEntry("nodes") + + + def execute(self) -> None: + _data = self.nodes.getStringArray([]) + data = self.nt_data_to_node_data(_data) + for node in data: + side = wpilib.DriverStation.Alliance.kBlue if node[0] >= 27 else wpilib.DriverStation.Alliance.kRed + col = node[0] % 9 + row = (node[0] % 27) // 9 + self.add_vision_data(side=side, pos=np.array([row, col]), confidence=(1. if node[1] else -1.)) + + def nt_data_to_node_data(self, data: list[str]) -> list[tuple[int, bool]]: + nodes: list[tuple[int, bool]] = [] + for node in data: + as_array = str(node) + a = (int(f"{as_array[0]}{as_array[1]}"), as_array[2] == "1") + nodes.append(a) + return nodes + + def add_vision_data( + self, side: wpilib.DriverStation.Alliance, pos: npt.ArrayLike, confidence: float + ) -> None: + confidences = ( + self.confidences_red if side == side.kRed else self.confidences_blue + ) + confidences[pos] = confidences[ + pos + ] * ScoreTracker.CONF_EXP_FILTER_ALPHA + confidence * ( + 1.0 - ScoreTracker.CONF_EXP_FILTER_ALPHA + ) + if abs(confidences[pos]) > ScoreTracker.CONF_THRESHOLD: + self.did_states_change = True + state = self.state_red if side == side.kRed else self.state_blue + state[pos] = confidence > 0.0 + + @staticmethod + def count_links(r: npt.NDArray[bool]) -> int: + i = 0 + n = 0 + l = len(r) + while i < l - 2: + if r[i] and r[i + 1] and r[i + 2]: + n += 1 + i += 3 + continue + i += 1 + return n + + @staticmethod + def evaluate_state(a: npt.NDArray[bool]) -> int: + return ( + sum(ScoreTracker.count_links(r) for r in a) * 5 + + a[0].sum() * 5 + + a[1].sum() * 3 + + a[2].sum() * 2 + ) + + @staticmethod + def run_lengths_mod3(state: npt.NDArray[bool]) -> npt.NDArray[int]: + """ + Returns an array where corresponding in shape to the input, where + every value is replaced by the length of the longest uninterrupted + run of true values containing it, modulo 3 + """ + run_lengths = np.zeros_like(state, dtype=int) + for y in range(3): + x = 0 + while x < 9: + if not state[y, x]: + x += 1 + continue + acc = 0 + for xn in range(x, 9): + if not state[y, xn]: + break + acc += 1 + run_lengths[y, x : x + acc] = acc % 3 + x += acc + return run_lengths + + @staticmethod + def get_in_row(arr: npt.NDArray, x: int, y: int, def_val): + if x < 0 or x > 8: + return def_val + else: + return arr[y, x] + + @staticmethod + def get_best_moves( + state: npt.NDArray[bool], + type_to_test: GridNode, + link_preparation_score: float = 2.5, + ) -> npt.NDArray: + vals = np.zeros_like(state, dtype=float) + run_lengths = ScoreTracker.run_lengths_mod3(state) + for y in range(3): + for x in range(9): + if ( + state[y, x] + or ( + type_to_test == GridNode.CUBE + and not ScoreTracker.CUBE_MASK[y, x] + ) + or ( + type_to_test == GridNode.CONE + and not ScoreTracker.CONE_MASK[y, x] + ) + ): + continue + val = [5.0, 3.0, 2.0][y] + # Check link completion + if ( + ScoreTracker.get_in_row(run_lengths, x - 1, y, 0) + + ScoreTracker.get_in_row(run_lengths, x + 1, y, 0) + >= 2 + ): + val += 5.0 + # Otherwise, check link preparation (state where a link can be completed after 1 move) + else: + for o in [-2, -1, 1, 2]: + if ScoreTracker.get_in_row(run_lengths, x + o, y, 0) == 1: + val += link_preparation_score + break + vals[y, x] = val + m = vals.max() + return np.argwhere(vals == m) \ No newline at end of file diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index 3ef4ebaa..1c5f67d3 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -5,9 +5,10 @@ from controllers.movement import Movement from controllers.recover import RecoverController -from magicbot import state, timed_state, StateMachine +from magicbot import state, StateMachine, feedback from enum import Enum, auto -from utilities.game import Node, get_closest_node, get_score_location, Rows +from utilities.game import Node, get_closest_node, get_score_location, Rows, is_red, GamePiece +from components.score_tracker import ScoreTracker, GridNode class NodePickStratergy(Enum): @@ -15,6 +16,14 @@ class NodePickStratergy(Enum): OVERRIDE = auto() BEST = auto() +def piece_to_node(piece: GamePiece) -> GridNode: + if piece == GamePiece.BOTH: + return GridNode.HYBRID + if piece == GamePiece.CONE: + return GridNode.CONE + if piece == GamePiece.CUBE: + return GridNode.CUBE + class ScoreGamePieceController(StateMachine): gripper: Gripper @@ -23,7 +32,8 @@ class ScoreGamePieceController(StateMachine): movement: Movement recover: RecoverController - HARD_UP_SPEED = 0.3 + + score_tracker: ScoreTracker def __init__(self) -> None: self.node_stratergy = NodePickStratergy.CLOSEST @@ -32,53 +42,52 @@ def __init__(self) -> None: self.target_node = Node(Rows.HIGH, 0) @state(first=True, must_finish=True) - def driving_to_position(self, initial_call: bool) -> None: + def driving_to_position(self, initial_call): if initial_call: self.target_node = self.pick_node() self.movement.set_goal(*get_score_location(self.target_node)) self.movement.do_autodrive() if self.movement.is_at_goal(): - self.next_state("hard_up") - - @timed_state(next_state="deploying_arm", duration=0.3, must_finish=True) - def hard_up(self) -> None: - self.movement.inputs_lock = True - self.movement.set_input(-self.HARD_UP_SPEED, 0, 0, False, override=True) + self.next_state("deploying_arm") @state(must_finish=True) - def deploying_arm(self, initial_call: bool) -> None: + def deploying_arm(self): self.arm.go_to_setpoint(get_setpoint_from_node(self.target_node)) if self.arm.at_goal(): self.next_state("dropping") - @timed_state(duration=1, must_finish=True) - def dropping(self) -> None: + @state(must_finish=True) + def dropping(self): self.gripper.open() + if self.gripper.get_full_open(): + self.done() def done(self) -> None: super().done() - self.movement.inputs_lock = False self.recover.engage() def pick_node(self) -> Node: cur_pos = self.movement.chassis.get_pose().translation() if self.node_stratergy is NodePickStratergy.CLOSEST: return get_closest_node( - cur_pos, self.gripper.get_current_piece(), self.prefered_row + cur_pos, self.gripper.get_current_piece(), self.prefered_row, [] ) elif self.node_stratergy is NodePickStratergy.OVERRIDE: return self.override_node elif self.node_stratergy is NodePickStratergy.BEST: + state = self.score_tracker.state_blue if is_red() else self.score_tracker.state_red + self.score_tracker.get_best_moves(state, self.gripper.holding) return get_closest_node( - cur_pos, self.gripper.get_current_piece(), self.prefered_row - ) + cur_pos, self.gripper.get_current_piece(), self.prefered_row, [] + )`` + + @feedback + def pick_node_as_int(self) -> int: + node = self.pick_node() + return (node.row.value - 1) * 3 + node.col def prefer_high(self) -> None: self.prefered_row = Rows.HIGH def prefer_mid(self) -> None: self.prefered_row = Rows.MID - - def score_without_moving(self, node: Node) -> None: - self.target_node = node - self.engage("deploying_arm", force=True) diff --git a/robot.py b/robot.py index 9d329cac..cbce9548 100644 --- a/robot.py +++ b/robot.py @@ -19,6 +19,7 @@ from components.arm import Arm from components.gripper import Gripper from components.leds import StatusLights, DisplayType, LedColors +from components.score_tracker import ScoreTracker from utilities.scalers import rescale_js from utilities.game import is_red @@ -40,6 +41,7 @@ class MyRobot(magicbot.MagicRobot): intake: Intake status_lights: StatusLights gripper: Gripper + score_tracker: ScoreTracker port_localizer: VisualLocalizer starboard_localizer: VisualLocalizer @@ -233,6 +235,7 @@ def testPeriodic(self) -> None: self.port_localizer.execute() self.starboard_localizer.execute() self.status_lights.execute() + self.score_tracker.execute() def cancel_controllers(self): self.acquire_cone.done() diff --git a/utilities/game.py b/utilities/game.py index cae053fa..cbf30f1e 100644 --- a/utilities/game.py +++ b/utilities/game.py @@ -96,6 +96,9 @@ def get_valid_piece(self) -> GamePiece: return GamePiece.CUBE else: return GamePiece.CONE + + def get_id(self) -> int: + return (self.row.value - 1) * 3 + self.col def get_node_location(node: Node) -> Translation3d: @@ -123,8 +126,10 @@ def get_score_location(node: Node) -> tuple[Pose2d, Rotation2d]: return goal, approach -def get_closest_node(pos: Translation2d, piece: GamePiece, row: Rows) -> Node: +def get_closest_node(pos: Translation2d, piece: GamePiece, row: Rows, impossible: list[int]) -> Node: def get_node_dist(node: Node) -> float: + if node.get_id() in impossible: + return 999999 return get_score_location(node)[0].translation().distance(pos) if piece == GamePiece.CONE: @@ -136,6 +141,12 @@ def get_node_dist(node: Node) -> float: return min(nodes, key=get_node_dist) +def get_closest_node_in_allowed(pos: Translation2d, piece: GamePiece, allowed: list[Node]) -> Node: + def get_node_dist(node: Node) -> float: + return get_score_location(node)[0].translation().distance(pos) + + return min(allowed, key=get_node_dist) + # tag in blue loading bay, on red side of field 16=x tag_4 = apriltag_layout.getTagPose(4) From 9d89f6adf7fb33bbbe46b37c7319ce734dc948f1 Mon Sep 17 00:00:00 2001 From: Pokesi <81540683+Pokesi@users.noreply.github.com> Date: Mon, 6 Mar 2023 20:20:17 +1100 Subject: [PATCH 02/11] vision. --- components/score_tracker.py | 7 ++++-- controllers/score_game_piece.py | 39 ++++++++++++++++++++++++++------- 2 files changed, 36 insertions(+), 10 deletions(-) diff --git a/components/score_tracker.py b/components/score_tracker.py index 500a4689..3183b6c8 100644 --- a/components/score_tracker.py +++ b/components/score_tracker.py @@ -41,18 +41,21 @@ def __init__(self) -> None: self.state_blue = np.zeros((3, 9), dtype=bool) self.did_states_change = magicbot.will_reset_to(False) self.inst = NetworkTableInstance.getDefault() - nt = self.inst.getTable("left_camera") + nt = self.inst.getTable("left_cam") self.nodes = nt.getEntry("nodes") def execute(self) -> None: + if not self.nodes.exists(): + print("skipping") + return _data = self.nodes.getStringArray([]) data = self.nt_data_to_node_data(_data) for node in data: side = wpilib.DriverStation.Alliance.kBlue if node[0] >= 27 else wpilib.DriverStation.Alliance.kRed col = node[0] % 9 row = (node[0] % 27) // 9 - self.add_vision_data(side=side, pos=np.array([row, col]), confidence=(1. if node[1] else -1.)) + self.add_vision_data(side=side, pos=np.array([row, col]), confidence=(1. if node[1] else -0.5)) def nt_data_to_node_data(self, data: list[str]) -> list[tuple[int, bool]]: nodes: list[tuple[int, bool]] = [] diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index 1c5f67d3..7cf8f94d 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -7,8 +7,8 @@ from magicbot import state, StateMachine, feedback from enum import Enum, auto -from utilities.game import Node, get_closest_node, get_score_location, Rows, is_red, GamePiece -from components.score_tracker import ScoreTracker, GridNode +from utilities.game import Node, get_closest_node, get_score_location, Rows, is_red, GamePiece, get_closest_node_in_allowed +from components.score_tracker import GridNode, ScoreTracker class NodePickStratergy(Enum): @@ -36,7 +36,7 @@ class ScoreGamePieceController(StateMachine): score_tracker: ScoreTracker def __init__(self) -> None: - self.node_stratergy = NodePickStratergy.CLOSEST + self.node_stratergy = NodePickStratergy.BEST self.override_node = Node(Rows.HIGH, 0) self.prefered_row = Rows.HIGH self.target_node = Node(Rows.HIGH, 0) @@ -76,14 +76,37 @@ def pick_node(self) -> Node: return self.override_node elif self.node_stratergy is NodePickStratergy.BEST: state = self.score_tracker.state_blue if is_red() else self.score_tracker.state_red - self.score_tracker.get_best_moves(state, self.gripper.holding) - return get_closest_node( - cur_pos, self.gripper.get_current_piece(), self.prefered_row, [] - )`` + best = self.score_tracker.get_best_moves(state, piece_to_node(self.gripper.holding)) + nodes: list[Node] = [] + for i in range(len(best)): + as_tuple = tuple(best[i]) + node = Node(Rows(int(as_tuple[0])), as_tuple[1]) + nodes.append(node) + + return get_closest_node_in_allowed( + cur_pos, self.gripper.get_current_piece(), nodes + ) + + @feedback + def state_red(self) -> list[bool]: + state: list[bool] = [] + for i in self.score_tracker.state_blue.tolist(): + for j in i: + state.append(j) + return state + + @feedback + def state_blue(self) -> list[bool]: + state: list[bool] = [] + for i in self.score_tracker.state_blue.tolist(): + for j in i: + state.append(j) + return state @feedback def pick_node_as_int(self) -> int: - node = self.pick_node() + # node = self.pick_node() + node = Node(Rows.HIGH, 0) return (node.row.value - 1) * 3 + node.col def prefer_high(self) -> None: From 6a511beb8bdbfcc8d438fb2fee9c881b82b03043 Mon Sep 17 00:00:00 2001 From: Pokesi <81540683+Pokesi@users.noreply.github.com> Date: Mon, 6 Mar 2023 21:45:43 +1100 Subject: [PATCH 03/11] Update utilities/game.py Co-authored-by: David Vo --- utilities/game.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utilities/game.py b/utilities/game.py index cbf30f1e..537d42a3 100644 --- a/utilities/game.py +++ b/utilities/game.py @@ -126,7 +126,7 @@ def get_score_location(node: Node) -> tuple[Pose2d, Rotation2d]: return goal, approach -def get_closest_node(pos: Translation2d, piece: GamePiece, row: Rows, impossible: list[int]) -> Node: +def get_closest_node(pos: Translation2d, piece: GamePiece, row: Rows, impossible: set[int]) -> Node: def get_node_dist(node: Node) -> float: if node.get_id() in impossible: return 999999 From 3575799db0746acb90c72fc37ebcbdb81703bcf4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 8 Mar 2023 09:58:18 +0000 Subject: [PATCH 04/11] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- components/score_tracker.py | 18 +++++++++++++----- controllers/score_game_piece.py | 29 +++++++++++++++++++++-------- utilities/game.py | 15 ++++++++++----- 3 files changed, 44 insertions(+), 18 deletions(-) diff --git a/components/score_tracker.py b/components/score_tracker.py index 3183b6c8..a43cb4e7 100644 --- a/components/score_tracker.py +++ b/components/score_tracker.py @@ -3,14 +3,15 @@ import numpy.typing as npt import wpilib import magicbot -from utilities.game import Node from ntcore import NetworkTableInstance + class GridNode(Enum): CUBE = 0 CONE = 1 HYBRID = 2 + class ScoreTracker: CUBE_MASK = np.array( [ @@ -44,7 +45,6 @@ def __init__(self) -> None: nt = self.inst.getTable("left_cam") self.nodes = nt.getEntry("nodes") - def execute(self) -> None: if not self.nodes.exists(): print("skipping") @@ -52,10 +52,18 @@ def execute(self) -> None: _data = self.nodes.getStringArray([]) data = self.nt_data_to_node_data(_data) for node in data: - side = wpilib.DriverStation.Alliance.kBlue if node[0] >= 27 else wpilib.DriverStation.Alliance.kRed + side = ( + wpilib.DriverStation.Alliance.kBlue + if node[0] >= 27 + else wpilib.DriverStation.Alliance.kRed + ) col = node[0] % 9 row = (node[0] % 27) // 9 - self.add_vision_data(side=side, pos=np.array([row, col]), confidence=(1. if node[1] else -0.5)) + self.add_vision_data( + side=side, + pos=np.array([row, col]), + confidence=(1.0 if node[1] else -0.5), + ) def nt_data_to_node_data(self, data: list[str]) -> list[tuple[int, bool]]: nodes: list[tuple[int, bool]] = [] @@ -171,4 +179,4 @@ def get_best_moves( break vals[y, x] = val m = vals.max() - return np.argwhere(vals == m) \ No newline at end of file + return np.argwhere(vals == m) diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index 917ec33f..b7c60782 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -7,17 +7,24 @@ from magicbot import state, StateMachine, feedback from enum import Enum, auto -from utilities.game import Node, get_closest_node, get_score_location, Rows, is_red, GamePiece, get_closest_node_in_allowed +from utilities.game import ( + Node, + get_closest_node, + get_score_location, + Rows, + is_red, + GamePiece, + get_closest_node_in_allowed, +) from components.score_tracker import GridNode, ScoreTracker -from wpimath.geometry import Translation2d - class NodePickStratergy(Enum): CLOSEST = auto() OVERRIDE = auto() BEST = auto() + def piece_to_node(piece: GamePiece) -> GridNode: if piece == GamePiece.BOTH: return GridNode.HYBRID @@ -87,7 +94,7 @@ def dropping(self) -> None: def done(self) -> None: super().done() self.recover.engage() - + def pick_node(self) -> Node: cur_pos = self.movement.chassis.get_pose().translation() if self.node_stratergy is NodePickStratergy.CLOSEST: @@ -97,8 +104,14 @@ def pick_node(self) -> Node: elif self.node_stratergy is NodePickStratergy.OVERRIDE: return self.override_node elif self.node_stratergy is NodePickStratergy.BEST: - state = self.score_tracker.state_blue if is_red() else self.score_tracker.state_red - best = self.score_tracker.get_best_moves(state, piece_to_node(self.gripper.holding)) + state = ( + self.score_tracker.state_blue + if is_red() + else self.score_tracker.state_red + ) + best = self.score_tracker.get_best_moves( + state, piece_to_node(self.gripper.holding) + ) nodes: list[Node] = [] for i in range(len(best)): as_tuple = tuple(best[i]) @@ -108,7 +121,7 @@ def pick_node(self) -> Node: return get_closest_node_in_allowed( cur_pos, self.gripper.get_current_piece(), nodes ) - + @feedback def state_red(self) -> list[bool]: state: list[bool] = [] @@ -116,7 +129,7 @@ def state_red(self) -> list[bool]: for j in i: state.append(j) return state - + @feedback def state_blue(self) -> list[bool]: state: list[bool] = [] diff --git a/utilities/game.py b/utilities/game.py index 78fc5df2..b860f110 100644 --- a/utilities/game.py +++ b/utilities/game.py @@ -96,7 +96,7 @@ def get_valid_piece(self) -> GamePiece: return GamePiece.CUBE else: return GamePiece.CONE - + def get_id(self) -> int: return (self.row.value - 1) * 3 + self.col @@ -126,9 +126,11 @@ def get_score_location(node: Node) -> tuple[Pose2d, Rotation2d]: return goal, approach -def get_closest_node(pos: Translation2d, piece: GamePiece, row: Rows, impossible: set[int]) -> Node: +def get_closest_node( + pos: Translation2d, piece: GamePiece, row: Rows, impossible: set[int] +) -> Node: def get_node_dist(node: Node) -> float: - if node.get_id() in impossible: + if node.get_id() in impossible: return 999999 return get_score_location(node)[0].translation().distance(pos) @@ -141,10 +143,13 @@ def get_node_dist(node: Node) -> float: return min(nodes, key=get_node_dist) -def get_closest_node_in_allowed(pos: Translation2d, piece: GamePiece, allowed: list[Node]) -> Node: + +def get_closest_node_in_allowed( + pos: Translation2d, piece: GamePiece, allowed: list[Node] +) -> Node: def get_node_dist(node: Node) -> float: return get_score_location(node)[0].translation().distance(pos) - + return min(allowed, key=get_node_dist) From aadef3fe696f14fce13ae92890a860f6fb8c78d5 Mon Sep 17 00:00:00 2001 From: Pokesi <81540683+Pokesi@users.noreply.github.com> Date: Mon, 6 Mar 2023 09:06:02 +1100 Subject: [PATCH 05/11] pt 1 --- components/score_tracker.py | 40 +++++++++++++++++++++++++++++++++ controllers/score_game_piece.py | 1 - utilities/game.py | 8 ++++++- 3 files changed, 47 insertions(+), 2 deletions(-) diff --git a/components/score_tracker.py b/components/score_tracker.py index a43cb4e7..59650b17 100644 --- a/components/score_tracker.py +++ b/components/score_tracker.py @@ -180,3 +180,43 @@ def get_best_moves( vals[y, x] = val m = vals.max() return np.argwhere(vals == m) + + @staticmethod + def get_best_moves( + state: npt.NDArray[bool], + type_to_test: GridNode, + link_preparation_score: float = 2.5, + ) -> npt.NDArray: + vals = np.zeros_like(state, dtype=float) + run_lengths = ScoreTracker.run_lengths_mod3(state) + for y in range(3): + for x in range(9): + if ( + state[y, x] + or ( + type_to_test == GridNode.CUBE + and not ScoreTracker.CUBE_MASK[y, x] + ) + or ( + type_to_test == GridNode.CONE + and not ScoreTracker.CONE_MASK[y, x] + ) + ): + continue + val = [5.0, 3.0, 2.0][y] + # Check link completion + if ( + ScoreTracker.get_in_row(run_lengths, x - 1, y, 0) + + ScoreTracker.get_in_row(run_lengths, x + 1, y, 0) + >= 2 + ): + val += 5.0 + # Otherwise, check link preparation (state where a link can be completed after 1 move) + else: + for o in [-2, -1, 1, 2]: + if ScoreTracker.get_in_row(run_lengths, x + o, y, 0) == 1: + val += link_preparation_score + break + vals[y, x] = val + m = vals.max() + return np.argwhere(vals == m) \ No newline at end of file diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index b7c60782..e393e57a 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -24,7 +24,6 @@ class NodePickStratergy(Enum): OVERRIDE = auto() BEST = auto() - def piece_to_node(piece: GamePiece) -> GridNode: if piece == GamePiece.BOTH: return GridNode.HYBRID diff --git a/utilities/game.py b/utilities/game.py index b860f110..af338794 100644 --- a/utilities/game.py +++ b/utilities/game.py @@ -96,7 +96,7 @@ def get_valid_piece(self) -> GamePiece: return GamePiece.CUBE else: return GamePiece.CONE - + def get_id(self) -> int: return (self.row.value - 1) * 3 + self.col @@ -143,6 +143,12 @@ def get_node_dist(node: Node) -> float: return min(nodes, key=get_node_dist) +def get_closest_node_in_allowed(pos: Translation2d, piece: GamePiece, allowed: list[Node]) -> Node: + def get_node_dist(node: Node) -> float: + return get_score_location(node)[0].translation().distance(pos) + + return min(allowed, key=get_node_dist) + def get_closest_node_in_allowed( pos: Translation2d, piece: GamePiece, allowed: list[Node] From 1495c9e9debf1f5ca96f1ff1cecd3288012847ad Mon Sep 17 00:00:00 2001 From: Pokesi <81540683+Pokesi@users.noreply.github.com> Date: Mon, 6 Mar 2023 21:31:14 +1100 Subject: [PATCH 06/11] gripper feedback --- components/gripper.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/components/gripper.py b/components/gripper.py index 611c8cab..d3d7506c 100644 --- a/components/gripper.py +++ b/components/gripper.py @@ -91,6 +91,17 @@ def get_current_piece(self) -> GamePiece: if self.opened_gripper: return GamePiece.NONE return self.holding + + @feedback + def get_current_piece_as_int(self) -> int: + piece = self.get_current_piece() + if piece == GamePiece.NONE: + return 0 + if piece == GamePiece.CONE: + return 1 + if piece == GamePiece.CUBE: + return 2 + return -1 @feedback def cube_present(self) -> bool: From 07e2fbbf496fa64e3572fb8695f9f3ef03195f7a Mon Sep 17 00:00:00 2001 From: Pokesi <81540683+Pokesi@users.noreply.github.com> Date: Mon, 6 Mar 2023 21:44:40 +1100 Subject: [PATCH 07/11] x --- controllers/score_game_piece.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index e393e57a..52f352e7 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -5,7 +5,7 @@ from controllers.movement import Movement from controllers.recover import RecoverController -from magicbot import state, StateMachine, feedback +from magicbot import state, StateMachine, feedback, timed_state from enum import Enum, auto from utilities.game import ( Node, From 5cbb25b4b9d8d89c719e6c1f601953402d077962 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 8 Mar 2023 10:07:47 +0000 Subject: [PATCH 08/11] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- components/gripper.py | 2 +- components/score_tracker.py | 2 +- controllers/score_game_piece.py | 1 + utilities/game.py | 9 ++++++--- 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/components/gripper.py b/components/gripper.py index d3d7506c..f2e20428 100644 --- a/components/gripper.py +++ b/components/gripper.py @@ -91,7 +91,7 @@ def get_current_piece(self) -> GamePiece: if self.opened_gripper: return GamePiece.NONE return self.holding - + @feedback def get_current_piece_as_int(self) -> int: piece = self.get_current_piece() diff --git a/components/score_tracker.py b/components/score_tracker.py index 59650b17..c0f1f0b6 100644 --- a/components/score_tracker.py +++ b/components/score_tracker.py @@ -219,4 +219,4 @@ def get_best_moves( break vals[y, x] = val m = vals.max() - return np.argwhere(vals == m) \ No newline at end of file + return np.argwhere(vals == m) diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index 52f352e7..706ede05 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -24,6 +24,7 @@ class NodePickStratergy(Enum): OVERRIDE = auto() BEST = auto() + def piece_to_node(piece: GamePiece) -> GridNode: if piece == GamePiece.BOTH: return GridNode.HYBRID diff --git a/utilities/game.py b/utilities/game.py index af338794..5714ea9f 100644 --- a/utilities/game.py +++ b/utilities/game.py @@ -96,7 +96,7 @@ def get_valid_piece(self) -> GamePiece: return GamePiece.CUBE else: return GamePiece.CONE - + def get_id(self) -> int: return (self.row.value - 1) * 3 + self.col @@ -143,10 +143,13 @@ def get_node_dist(node: Node) -> float: return min(nodes, key=get_node_dist) -def get_closest_node_in_allowed(pos: Translation2d, piece: GamePiece, allowed: list[Node]) -> Node: + +def get_closest_node_in_allowed( + pos: Translation2d, piece: GamePiece, allowed: list[Node] +) -> Node: def get_node_dist(node: Node) -> float: return get_score_location(node)[0].translation().distance(pos) - + return min(allowed, key=get_node_dist) From d31f2828d07bfc94dca5de29fb88c37b38bbe2ee Mon Sep 17 00:00:00 2001 From: Pokesi <81540683+Pokesi@users.noreply.github.com> Date: Wed, 8 Mar 2023 21:12:58 +1100 Subject: [PATCH 09/11] use gamepiece not gridnode --- components/score_tracker.py | 54 +++------------------------------ controllers/score_game_piece.py | 12 +------- 2 files changed, 5 insertions(+), 61 deletions(-) diff --git a/components/score_tracker.py b/components/score_tracker.py index c0f1f0b6..2e4bfd71 100644 --- a/components/score_tracker.py +++ b/components/score_tracker.py @@ -4,13 +4,7 @@ import wpilib import magicbot from ntcore import NetworkTableInstance - - -class GridNode(Enum): - CUBE = 0 - CONE = 1 - HYBRID = 2 - +from utilities.game import GamePiece class ScoreTracker: CUBE_MASK = np.array( @@ -144,47 +138,7 @@ def get_in_row(arr: npt.NDArray, x: int, y: int, def_val): @staticmethod def get_best_moves( state: npt.NDArray[bool], - type_to_test: GridNode, - link_preparation_score: float = 2.5, - ) -> npt.NDArray: - vals = np.zeros_like(state, dtype=float) - run_lengths = ScoreTracker.run_lengths_mod3(state) - for y in range(3): - for x in range(9): - if ( - state[y, x] - or ( - type_to_test == GridNode.CUBE - and not ScoreTracker.CUBE_MASK[y, x] - ) - or ( - type_to_test == GridNode.CONE - and not ScoreTracker.CONE_MASK[y, x] - ) - ): - continue - val = [5.0, 3.0, 2.0][y] - # Check link completion - if ( - ScoreTracker.get_in_row(run_lengths, x - 1, y, 0) - + ScoreTracker.get_in_row(run_lengths, x + 1, y, 0) - >= 2 - ): - val += 5.0 - # Otherwise, check link preparation (state where a link can be completed after 1 move) - else: - for o in [-2, -1, 1, 2]: - if ScoreTracker.get_in_row(run_lengths, x + o, y, 0) == 1: - val += link_preparation_score - break - vals[y, x] = val - m = vals.max() - return np.argwhere(vals == m) - - @staticmethod - def get_best_moves( - state: npt.NDArray[bool], - type_to_test: GridNode, + type_to_test: GamePiece, link_preparation_score: float = 2.5, ) -> npt.NDArray: vals = np.zeros_like(state, dtype=float) @@ -194,11 +148,11 @@ def get_best_moves( if ( state[y, x] or ( - type_to_test == GridNode.CUBE + type_to_test == GamePiece.CUBE and not ScoreTracker.CUBE_MASK[y, x] ) or ( - type_to_test == GridNode.CONE + type_to_test == GamePiece.CONE and not ScoreTracker.CONE_MASK[y, x] ) ): diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index 706ede05..ab2258ba 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -24,16 +24,6 @@ class NodePickStratergy(Enum): OVERRIDE = auto() BEST = auto() - -def piece_to_node(piece: GamePiece) -> GridNode: - if piece == GamePiece.BOTH: - return GridNode.HYBRID - if piece == GamePiece.CONE: - return GridNode.CONE - if piece == GamePiece.CUBE: - return GridNode.CUBE - - class ScoreGamePieceController(StateMachine): gripper: Gripper intake: Intake @@ -110,7 +100,7 @@ def pick_node(self) -> Node: else self.score_tracker.state_red ) best = self.score_tracker.get_best_moves( - state, piece_to_node(self.gripper.holding) + state, self.gripper.holding ) nodes: list[Node] = [] for i in range(len(best)): From 4705fc20032d4ec3491b14e5e8d0c14b2c6b6472 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 8 Mar 2023 10:15:33 +0000 Subject: [PATCH 10/11] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- components/score_tracker.py | 2 +- controllers/score_game_piece.py | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/components/score_tracker.py b/components/score_tracker.py index 2e4bfd71..36aaab13 100644 --- a/components/score_tracker.py +++ b/components/score_tracker.py @@ -1,4 +1,3 @@ -from enum import Enum import numpy as np import numpy.typing as npt import wpilib @@ -6,6 +5,7 @@ from ntcore import NetworkTableInstance from utilities.game import GamePiece + class ScoreTracker: CUBE_MASK = np.array( [ diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index ab2258ba..da7b95d0 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -13,10 +13,9 @@ get_score_location, Rows, is_red, - GamePiece, get_closest_node_in_allowed, ) -from components.score_tracker import GridNode, ScoreTracker +from components.score_tracker import ScoreTracker class NodePickStratergy(Enum): @@ -24,6 +23,7 @@ class NodePickStratergy(Enum): OVERRIDE = auto() BEST = auto() + class ScoreGamePieceController(StateMachine): gripper: Gripper intake: Intake @@ -99,9 +99,7 @@ def pick_node(self) -> Node: if is_red() else self.score_tracker.state_red ) - best = self.score_tracker.get_best_moves( - state, self.gripper.holding - ) + best = self.score_tracker.get_best_moves(state, self.gripper.holding) nodes: list[Node] = [] for i in range(len(best)): as_tuple = tuple(best[i]) From b6e246963aa17d40048a49bb036708935031dc78 Mon Sep 17 00:00:00 2001 From: Pokesi <81540683+Pokesi@users.noreply.github.com> Date: Wed, 8 Mar 2023 21:29:10 +1100 Subject: [PATCH 11/11] clean up + fix some type errors --- components/score_tracker.py | 12 ++++++------ controllers/score_game_piece.py | 17 ++++++++++++++++- utilities/game.py | 9 --------- 3 files changed, 22 insertions(+), 16 deletions(-) diff --git a/components/score_tracker.py b/components/score_tracker.py index 36aaab13..c04772ba 100644 --- a/components/score_tracker.py +++ b/components/score_tracker.py @@ -84,11 +84,11 @@ def add_vision_data( state[pos] = confidence > 0.0 @staticmethod - def count_links(r: npt.NDArray[bool]) -> int: + def count_links(r: npt.NDArray[np.bool_]) -> int: i = 0 n = 0 - l = len(r) - while i < l - 2: + length = len(r) + while i < length - 2: if r[i] and r[i + 1] and r[i + 2]: n += 1 i += 3 @@ -97,7 +97,7 @@ def count_links(r: npt.NDArray[bool]) -> int: return n @staticmethod - def evaluate_state(a: npt.NDArray[bool]) -> int: + def evaluate_state(a: npt.NDArray[np.bool_]) -> int: return ( sum(ScoreTracker.count_links(r) for r in a) * 5 + a[0].sum() * 5 @@ -106,7 +106,7 @@ def evaluate_state(a: npt.NDArray[bool]) -> int: ) @staticmethod - def run_lengths_mod3(state: npt.NDArray[bool]) -> npt.NDArray[int]: + def run_lengths_mod3(state: npt.NDArray[np.bool_]) -> npt.NDArray[np.int_]: """ Returns an array where corresponding in shape to the input, where every value is replaced by the length of the longest uninterrupted @@ -137,7 +137,7 @@ def get_in_row(arr: npt.NDArray, x: int, y: int, def_val): @staticmethod def get_best_moves( - state: npt.NDArray[bool], + state: npt.NDArray[np.bool_], type_to_test: GamePiece, link_preparation_score: float = 2.5, ) -> npt.NDArray: diff --git a/controllers/score_game_piece.py b/controllers/score_game_piece.py index da7b95d0..215c97cd 100644 --- a/controllers/score_game_piece.py +++ b/controllers/score_game_piece.py @@ -85,11 +85,22 @@ def done(self) -> None: super().done() self.recover.engage() + def score_best(self) -> None: + self.node_stratergy = NodePickStratergy.BEST + + def score_closest_high(self) -> None: + self.node_stratergy = NodePickStratergy.CLOSEST + self.prefer_high() + + def score_closest_mid(self) -> None: + self.node_stratergy = NodePickStratergy.CLOSEST + self.prefer_high() + def pick_node(self) -> Node: cur_pos = self.movement.chassis.get_pose().translation() if self.node_stratergy is NodePickStratergy.CLOSEST: return get_closest_node( - cur_pos, self.gripper.get_current_piece(), self.prefered_row, [] + cur_pos, self.gripper.get_current_piece(), self.prefered_row, set() ) elif self.node_stratergy is NodePickStratergy.OVERRIDE: return self.override_node @@ -137,3 +148,7 @@ def prefer_high(self) -> None: def prefer_mid(self) -> None: self.prefered_row = Rows.MID + + def score_without_moving(self, node: Node) -> None: + self.target_node = node + self.engage("deploying_arm", force=True) diff --git a/utilities/game.py b/utilities/game.py index 5714ea9f..b860f110 100644 --- a/utilities/game.py +++ b/utilities/game.py @@ -153,15 +153,6 @@ def get_node_dist(node: Node) -> float: return min(allowed, key=get_node_dist) -def get_closest_node_in_allowed( - pos: Translation2d, piece: GamePiece, allowed: list[Node] -) -> Node: - def get_node_dist(node: Node) -> float: - return get_score_location(node)[0].translation().distance(pos) - - return min(allowed, key=get_node_dist) - - # tag in blue loading bay, on red side of field 16=x tag_4 = apriltag_layout.getTagPose(4) assert tag_4 is not None