From a83b628253c0cb3c383568bc0f599e1f8a6c5af2 Mon Sep 17 00:00:00 2001 From: Kevin Fu <30275369+kfu02@users.noreply.github.com> Date: Fri, 18 Jun 2021 15:36:26 -0400 Subject: [PATCH] Revert "New gameplay passing" --- rj_gameplay/rj_gameplay/action/capture.py | 6 +- rj_gameplay/rj_gameplay/action/kick.py | 42 +++---- rj_gameplay/rj_gameplay/action/pivot.py | 39 ++----- rj_gameplay/rj_gameplay/action/receive.py | 34 ------ rj_gameplay/rj_gameplay/gameplay_node.py | 4 +- rj_gameplay/rj_gameplay/play/test_pass.py | 53 --------- rj_gameplay/rj_gameplay/skill/capture.py | 5 +- rj_gameplay/rj_gameplay/skill/pivot_kick.py | 36 ++---- rj_gameplay/rj_gameplay/skill/receive.py | 52 --------- rj_gameplay/rj_gameplay/tactic/pass_tactic.py | 110 ------------------ rj_gameplay/stp/skill/action_behavior.py | 22 +--- rj_gameplay/stp/skill/rj_sequence.py | 33 ------ 12 files changed, 48 insertions(+), 388 deletions(-) delete mode 100644 rj_gameplay/rj_gameplay/action/receive.py delete mode 100644 rj_gameplay/rj_gameplay/play/test_pass.py delete mode 100644 rj_gameplay/rj_gameplay/skill/receive.py delete mode 100644 rj_gameplay/rj_gameplay/tactic/pass_tactic.py delete mode 100644 rj_gameplay/stp/skill/rj_sequence.py diff --git a/rj_gameplay/rj_gameplay/action/capture.py b/rj_gameplay/rj_gameplay/action/capture.py index f1ed95b491d..2e6fc5390c7 100644 --- a/rj_gameplay/rj_gameplay/action/capture.py +++ b/rj_gameplay/rj_gameplay/action/capture.py @@ -11,6 +11,7 @@ class Capture(action.IAction): """ Capture action + TODO: update with actions implementation """ def __init__(self, robot_id: int = None): @@ -21,10 +22,11 @@ def tick(self, intent) -> None: collect_command = CollectMotionCommand() intent.motion_command.collect_command = [collect_command] intent.dribbler_speed = 1.0 - intent.is_active = True + intent.active = True + return intent def is_done(self, world_state) -> bool: - if not self.robot_id is None and world_state.our_robots[self.robot_id].has_ball_sense: + if world_state.our_robots[self.robot_id].has_ball_sense: return True return False diff --git a/rj_gameplay/rj_gameplay/action/kick.py b/rj_gameplay/rj_gameplay/action/kick.py index 6903cf3c94b..4c4c2cab405 100644 --- a/rj_gameplay/rj_gameplay/action/kick.py +++ b/rj_gameplay/rj_gameplay/action/kick.py @@ -6,9 +6,6 @@ import stp.action as action import numpy as np import stp.rc as rc -from typing import Optional -from rj_msgs.msg import RobotIntent, EmptyMotionCommand -from rj_geometry_msgs.msg import Point class IKick(action.IAction, ABC): @@ -19,30 +16,19 @@ def done(self) -> bool: class Kick(IKick): """ Kick action + TODO: update with actions implementation """ - def __init__(self, robot_id:int, chip:bool, kick_speed:float) -> None: - self.robot_id = robot_id - self.chip = chip - self.kick_speed = kick_speed - - def tick(self, intent:RobotIntent) -> RobotIntent: - new_intent = intent - empty_command = EmptyMotionCommand() - new_intent.motion_command.empty_command = [empty_command] - intent.kick_speed = self.kick_speed - new_intent.trigger_mode = 2 - new_intent.shoot_mode = self.chip - new_intent.is_active = True - return new_intent - - def is_done(self, world_state:rc.WorldState) -> bool: - if self.robot_id is None: - return False - ball_vel_unit = world_state.ball.vel / np.linalg.norm(world_state.ball.vel) - heading_angle = world_state.our_robots[self.robot_id].pose[2] - heading_vect = np.array([np.cos(heading_angle), np.sin(heading_angle)]) - dot_product = np.dot(heading_vect, ball_vel_unit) - #TODO: Make this threshold a local param - if dot_product > 0.1: - return True + def __init__(self, point: np.ndarray): + self.point = point + self.count = -1 + #for stub + + def tick(self, robot: rc.Robot, ctx: action.Ctx) -> None: + print('robot:', robot.id, 'kicking') + self.count += 1 + + def done(self) -> bool: + return self.count == 1 + + def fail(self): return False diff --git a/rj_gameplay/rj_gameplay/action/pivot.py b/rj_gameplay/rj_gameplay/action/pivot.py index cc0419d5dc3..93abbfc6122 100644 --- a/rj_gameplay/rj_gameplay/action/pivot.py +++ b/rj_gameplay/rj_gameplay/action/pivot.py @@ -13,37 +13,20 @@ class Pivot(action.IFiniteAction): - def __init__(self, robot_id:int, pivot_point:np.ndarray, target_point:np.ndarray, dribble_speed:float, priority:int=1): - self.robot_id = robot_id + """ + Pivot Skill + """ + def __init__(self, robot_id : int, pivot_point: np.ndarray, target_point: np.ndarray, priority : int = 1): self.pivot_point = pivot_point self.target_point = target_point - self.dribble_speed = dribble_speed - def tick(self, intent: RobotIntent) -> None: - new_intent = intent + def tick(self, intent) -> None: pivot_command = PivotMotionCommand() pivot_command.pivot_point = Point(x=self.pivot_point[0], y=self.pivot_point[1]) - pivot_command.pivot_target = Point(x=self.target_point[0], y=self.target_point[1]) - new_intent.motion_command.pivot_command = [pivot_command] - new_intent.dribbler_speed = self.dribble_speed - new_intent.is_active = True - return new_intent - - def is_done(self, world_state:rc.WorldState) -> bool: - #TODO: Change this when we get action state feedback - angle_threshold = 0.1 - #TODO: Make these local params - stopped_threshold = 1*10**(-5) - if self.robot_id is None: - return False - robot = world_state.our_robots[self.robot_id] - robot_pos_to_target = self.target_point - robot.pose[0:2] - robot_to_target_unit = robot_pos_to_target / np.linalg.norm(robot_pos_to_target) - heading_vect = np.array([np.cos(robot.pose[2]), np.sin(robot.pose[2])]) - dot_product = np.dot(heading_vect, robot_to_target_unit) - angle = np.arccos(dot_product) - if (angle < angle_threshold) and (abs(world_state.our_robots[self.robot_id].twist[2]) < stopped_threshold): - return True - else: - return False + intent.motion_command.pivot_motion_command = [pivot_command] + intent.active = True + def is_done(self, world_state) -> bool: + #vec = self.target_point + #world_state.our_robots[self.robot_id].pose[2] + return False \ No newline at end of file diff --git a/rj_gameplay/rj_gameplay/action/receive.py b/rj_gameplay/rj_gameplay/action/receive.py deleted file mode 100644 index 942b3c6d6f1..00000000000 --- a/rj_gameplay/rj_gameplay/action/receive.py +++ /dev/null @@ -1,34 +0,0 @@ -"""This module contains the interface and action for receive.""" - -from abc import ABC, abstractmethod - -import stp.role as role -import stp.action as action -import stp.rc as rc -import numpy as np -from rj_msgs.msg import RobotIntent, SettleMotionCommand - -class Receive(action.IAction): - """ - Receive action - """ - - def __init__(self, robot_id: int = None): - self.robot_id = robot_id - - - def tick(self, intent) -> None: - settle_command = SettleMotionCommand() - intent.motion_command.settle_command = [settle_command] - intent.dribbler_speed = 1.0 - intent.is_active = True - - return intent - - def is_done(self, world_state) -> bool: - if self.robot_id is None: - return False - #TODO: Use local params for this threshold - if world_state.our_robots[self.robot_id].has_ball_sense or np.linalg.norm(world_state.ball.vel) < 10**(-6): - return True - return False diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py index e06a9ff65e0..6526507d441 100644 --- a/rj_gameplay/rj_gameplay/gameplay_node.py +++ b/rj_gameplay/rj_gameplay/gameplay_node.py @@ -12,7 +12,7 @@ from stp.global_parameters import GlobalParameterClient import numpy as np from rj_gameplay.action.move import Move -from rj_gameplay.play import line_up, test_pass +from rj_gameplay.play import defensive_clear from typing import List, Optional, Tuple NUM_ROBOTS = 16 @@ -25,7 +25,7 @@ def select(self, world_state: rc.WorldState) -> None: class TestPlaySelector(situation.IPlaySelector): def select(self, world_state: rc.WorldState) -> Tuple[situation.ISituation, stp.play.IPlay]: - return (None, test_pass.PassPlay()) + return (None, line_up.LineUp()) class GameplayNode(Node): """ diff --git a/rj_gameplay/rj_gameplay/play/test_pass.py b/rj_gameplay/rj_gameplay/play/test_pass.py deleted file mode 100644 index b5974041410..00000000000 --- a/rj_gameplay/rj_gameplay/play/test_pass.py +++ /dev/null @@ -1,53 +0,0 @@ -import stp.play as play -import stp.tactic as tactic - -from rj_gameplay.tactic import pass_tactic -import stp.skill as skill -import stp.role as role -from stp.role.assignment.naive import NaiveRoleAssignment -import stp.rc as rc -from typing import Dict, Generic, Iterator, List, Optional, Tuple, Type, TypeVar -import numpy as np - -class PassPlay(play.IPlay): - """A play which makes one robot pass to another - """ - - def __init__(self): - self.target_point = np.array([1.0,1.0]) - self.pass_tactic = pass_tactic.Pass(self.target_point) - self.role_assigner = NaiveRoleAssignment() - - - def compute_props(self, prev_props): - pass - - def tick( - self, - world_state: rc.WorldState, - prev_results: role.assignment.FlatRoleResults, - props, - ) -> Tuple[Dict[Type[tactic.SkillEntry], List[role.RoleRequest]], List[tactic.SkillEntry]]: - # Get role requests from all tactics and put them into a dictionary - role_requests: play.RoleRequests = {} - if not self.pass_tactic.is_done(world_state): - role_requests[self.pass_tactic] = self.pass_tactic.get_requests(world_state, None) - else: - pass - # Flatten requests and use role assigner on them - flat_requests = play.flatten_requests(role_requests) - flat_results = self.role_assigner.assign_roles(flat_requests, world_state, prev_results) - role_results = play.unflatten_results(flat_results) - - # Get list of all skills with assigned roles from tactics - skill_dict = {} - if not self.pass_tactic.is_done(world_state): - skills = self.pass_tactic.tick(role_results[self.pass_tactic], world_state) - skill_dict.update(role_results[self.pass_tactic]) - else: - skills = [] - - return (skill_dict, skills) - - def is_done(self, world_state: rc.WorldState): - return self.pass_tactic.is_done(world_state) \ No newline at end of file diff --git a/rj_gameplay/rj_gameplay/skill/capture.py b/rj_gameplay/rj_gameplay/skill/capture.py index d8b8c50655c..5be01ab2526 100644 --- a/rj_gameplay/rj_gameplay/skill/capture.py +++ b/rj_gameplay/rj_gameplay/skill/capture.py @@ -12,7 +12,6 @@ from rj_gameplay.action import capture from stp.skill.action_behavior import ActionBehavior import stp.rc as rc -from typing import Optional class ICapture(skill.ISkill, ABC): ... @@ -23,8 +22,8 @@ class ICapture(skill.ISkill, ABC): """ class Capture(ICapture): - def __init__(self, robot: Optional[rc.Robot]=None): - self.robot = robot + def __init__(self): + self.robot: rc.Robot = None self.__name__ = 'Capture' self.capture = capture.Capture() self.capture_behavior = ActionBehavior('Capture', self.capture, self.robot) diff --git a/rj_gameplay/rj_gameplay/skill/pivot_kick.py b/rj_gameplay/rj_gameplay/skill/pivot_kick.py index 5c525a0010b..acb42641bab 100644 --- a/rj_gameplay/rj_gameplay/skill/pivot_kick.py +++ b/rj_gameplay/rj_gameplay/skill/pivot_kick.py @@ -9,13 +9,10 @@ import stp.skill as skill import stp.role as role import rj_gameplay.action as action -from stp.skill.action_behavior import ActionBehavior, RobotActions -from stp.skill.rj_sequence import RjSequence as Sequence +from stp.skill.action_behavior import ActionBehavior import stp.rc as rc import numpy as np -MAX_DRIBBLER_SPEED = 1.0 - class IPivotKick(skill.ISkill, ABC): ... @@ -24,33 +21,18 @@ class PivotKick(IPivotKick): A pivot kick skill """ - def __init__(self, target_point: np.array, chip: bool, kick_speed: float, robot: rc.Robot=None) -> None: - #TODO: Have something which automatically determines kick speed based on target point distance - self.__name__ = 'pivot kick' - self.robot = robot - self.chip = chip - self.kick_speed = kick_speed - self.root = Sequence("Sequence") - self.target_point = target_point - if robot is not None: - self.pivot = action.pivot.Pivot(robot.id ,robot.pose[0:2], target_point, MAX_DRIBBLER_SPEED) - self.kick = action.kick.Kick(self.robot.id, self.chip, self.kick_speed) - else: - self.pivot = action.pivot.Pivot(None, np.array([0.0,0.0]), target_point, MAX_DRIBBLER_SPEED) - self.kick = action.kick.Kick(self.robot, self.chip, self.kick_speed) + def __init__(self, role: role.Role, target_point: np.array) -> None: + self.robot = role.robot + self.root = py_trees.composites.Sequence("Sequence") self.capture = action.capture.Capture() + self.pivot = action.pivot.Pivot(self.robot.pos, self.robot.pose, target_point) + self.kick = action.kick.Kick(target_point) self.capture_behavior = ActionBehavior('Capture', self.capture) self.pivot_behavior = ActionBehavior('Pivot', self.pivot) self.kick_behavior = ActionBehavior('Kick', self.kick) self.root.add_children([self.capture_behavior, self.pivot_behavior, self.kick_behavior]) self.root.setup_with_descendants() - def tick(self, robot: rc.Robot, world_state: rc.WorldState) -> RobotActions: - self.robot = robot - self.pivot.pivot_point = world_state.ball.pos - self.pivot.target_point = self.target_point - actions = self.root.tick_once(robot, world_state) - return actions - - def is_done(self, world_state: rc.WorldState) -> bool: - return self.pivot.is_done(world_state) and self.kick.is_done(world_state) \ No newline at end of file + def tick(self, world_state: rc.WorldState, robot: rc.Robot) -> None: + self.root.tick_once(robot) + # TODO: change so this properly returns the actions intent messages diff --git a/rj_gameplay/rj_gameplay/skill/receive.py b/rj_gameplay/rj_gameplay/skill/receive.py deleted file mode 100644 index 5498a9d9aca..00000000000 --- a/rj_gameplay/rj_gameplay/skill/receive.py +++ /dev/null @@ -1,52 +0,0 @@ -from abc import ABC, abstractmethod - -import rj_gameplay.eval as eval -import argparse -import py_trees -import sys -import time -import numpy as np -from typing import Optional - -import stp.skill as skill -import stp.role as role -import stp.action as action -from rj_gameplay.action import receive, capture -from stp.skill.action_behavior import ActionBehavior -from stp.skill.rj_sequence import RjSequence as Sequence -import stp.rc as rc -from rj_msgs import msg - -class IReceive(skill.ISkill, ABC): - ... - - -""" -A skill version of receive so that actions don't have to be called in tactics -""" -class Receive(IReceive): - - def __init__(self, - robot:rc.Robot = None): - - self.robot = robot - if self.robot is not None: - self.receive = receive.Receive(self.robot.id) - self.capture = capture.Capture(self.robot.id) - else: - self.receive = receive.Receive(self.robot) - self.capture = capture.Capture() - self.receive_behavior = ActionBehavior('Receive', self.receive) - self.capture_behavior = ActionBehavior('Capture', self.capture) - self.root = Sequence('Sequence') - self.root.add_children([self.receive_behavior, self.capture_behavior]) - self.root.setup_with_descendants() - self.__name__ = 'receive skill' - - def tick(self, robot:rc.Robot, world_state:rc.WorldState): #returns dict of robot and actions - self.robot = robot - actions = self.root.tick_once(self.robot, world_state) - return actions - - def is_done(self, world_state:rc.WorldState): - return self.capture.is_done(world_state) diff --git a/rj_gameplay/rj_gameplay/tactic/pass_tactic.py b/rj_gameplay/rj_gameplay/tactic/pass_tactic.py deleted file mode 100644 index a15560e4886..00000000000 --- a/rj_gameplay/rj_gameplay/tactic/pass_tactic.py +++ /dev/null @@ -1,110 +0,0 @@ -from dataclasses import dataclass -from typing import List, Optional -from typing import Dict, Generic, List, Optional, Tuple, Type, TypeVar - -import stp.action as action -import stp.rc as rc -import stp.tactic as tactic -import stp.role as role - -import rj_gameplay.eval -import rj_gameplay.skill as skills -from rj_gameplay.skill import pivot_kick, receive -import stp.skill as skill -import numpy as np - - -class Receiver_cost(role.CostFn): - """ - A cost function for how to choose a robot to pass to - TODO: Implement a better cost function - """ - def __init__(self, target_point:Optional[np.ndarray] = None): - self.target_point = target_point - - def __call__( - self, - robot: rc.Robot, - prev_result: Optional["RoleResult"], - world_state: rc.WorldState, - ) -> float: - - if robot.id == 7: - return 0.0 - return 1.0 - -class Passer_cost(role.CostFn): - """ - A cost function for how to choose a robot that will pass - TODO: Implement a better cost function - """ - def __call__(self, - robot:rc.Robot, - prev_result:Optional["RoleResult"], - world_state:rc.WorldState) -> float: - if robot.has_ball_sense: - return 0 - else: - if robot.id == 1: - return 0 - else: - return 1 - - - -class Pass(tactic.ITactic): - """ - A passing tactic which captures then passes the ball - """ - - def __init__(self, target_point:np.ndarray): - self.target_point = target_point - self.pivot_kick = tactic.SkillEntry(pivot_kick.PivotKick(target_point = target_point, chip=False, kick_speed=4.0)) - self.receive = tactic.SkillEntry(receive.Receive()) - self.receiver_cost = Receiver_cost(target_point) - self.Passer_cost = Passer_cost() - - def compute_props(self): - pass - - def create_request(self, **kwargs) -> role.RoleRequest: - """Creates a sane default RoleRequest. - :return: A list of size 1 of a sane default RoleRequest. - """ - pass - - def get_requests( - self, world_state:rc.WorldState, props) -> List[tactic.RoleRequests]: - """ Checks if we have the ball and returns the proper request - :return: A list of size 2 of role requests - """ - - role_requests: tactic.RoleRequests = {} - - passer_request = role.RoleRequest(role.Priority.HIGH, True, self.Passer_cost) - role_requests[self.pivot_kick] = [passer_request] - receive_request = role.RoleRequest(role.Priority.HIGH, True, self.receiver_cost) - role_requests[self.receive] = [receive_request] - - return role_requests - - def tick(self, role_results:tactic.RoleResults, world_state:rc.WorldState) -> List[tactic.SkillEntry]: - """ - :return: A list of size 1 or 2 skills depending on which roles are filled and state of aiming - TODO: Come up with better timings for starting receive - """ - pivot_result = role_results[self.pivot_kick] - receive_result = role_results[self.receive] - - if pivot_result and receive_result and pivot_result[0].is_filled() and receive_result[0].is_filled(): - self.pivot_kick.skill.target_point = np.array(receive_result[0].role.robot.pose[0:2]) - if self.pivot_kick.skill.pivot.is_done(world_state): - return [self.pivot_kick, self.receive] - else: - return [self.pivot_kick] - elif pivot_result and pivot_result[0].is_filled(): - return [self.pivot_kick] - return [] - - def is_done(self, world_state:rc.WorldState): - return self.receive.skill.is_done(world_state) diff --git a/rj_gameplay/stp/skill/action_behavior.py b/rj_gameplay/stp/skill/action_behavior.py index 54bddb8342e..144e4232a9a 100644 --- a/rj_gameplay/stp/skill/action_behavior.py +++ b/rj_gameplay/stp/skill/action_behavior.py @@ -3,38 +3,28 @@ import stp.action as action from typing import Optional, Any import stp.rc as rc -from typing import TypedDict, List -class RobotActions(TypedDict): - robot_id: int - actions: List[action.IAction] class ActionBehavior(py_trees.behaviour.Behaviour): """ A behaviour for behviour trees which ticks its action when ticked """ - def __init__(self, name:str, action:action.IAction, robot:rc.Robot=None, ctx=None) -> None: + def __init__(self, name: str, action: action.IAction, robot: rc.Robot=None, ctx=None) -> None: self.action = action self.ctx = ctx self.robot = robot self.world_state = None super(ActionBehavior, self).__init__(name) - def tick_once(self, robot:rc.Robot, world_state:rc.WorldState, ctx=None) -> RobotActions: + def tick_once(self, robot: rc.Robot, world_state, ctx=None) -> None: """ Ticks its action using the robot given (if root) or the robot from its parent. This will probably become tick() or spin() once action server is implemented TODO: Should return a list of robot intents """ - if robot is not None: - self.robot = robot - else: - self.robot = self.parent.robot + self.robot = robot self.action.robot_id = robot.id - if world_state is not None: - self.world_state = world_state - else: - self.world_state = self.parent.world_state + self.world_state = world_state # if robot is None: # self.robot = self.parent.robot self.ctx = ctx @@ -44,7 +34,6 @@ def tick_once(self, robot:rc.Robot, world_state:rc.WorldState, ctx=None) -> Robo def initialise(self) -> None: """ Begin spinning the action - TODO: Implement with action server """ pass @@ -53,7 +42,8 @@ def update(self) -> py_trees.common.Status: Check action and return the current state of the aciton TODO: Needs to somehow use robot intents to check on status of action """ - if not self.world_state is None and self.action.is_done(self.world_state): + + if self.action.is_done(self.world_state): return py_trees.common.Status.SUCCESS else: return py_trees.common.Status.RUNNING diff --git a/rj_gameplay/stp/skill/rj_sequence.py b/rj_gameplay/stp/skill/rj_sequence.py deleted file mode 100644 index 44e260b98b4..00000000000 --- a/rj_gameplay/stp/skill/rj_sequence.py +++ /dev/null @@ -1,33 +0,0 @@ -import py_trees -import random -import stp.action as action -from typing import Optional, Any -import stp.rc as rc -from typing import List -from stp.skill.action_behavior import ActionBehavior, RobotActions - -class RjSequence(py_trees.composites.Sequence): - """ - A sequence block for our use - This is currently needed since we need a way to send wolrdstate down the tree for actions's is_done() functions - This will not be needed once the action server is implemented - """ - - def __init__(self, name:str) -> None: - self.robot = None - self.world_state = None - self.curr_index = -1 - self.current_child = None - super().__init__(name) - - def tick_once(self, robot:rc.Robot, world_state:rc.WorldState) -> RobotActions: - self.robot = robot - self.world_state = world_state - if self.current_child is None: - self.current_child = self.children[0] - self.curr_index += 1 - elif self.current_child.action.is_done(world_state) and self.curr_index < len(self.children)-1: - self.curr_index += 1 - self.current_child = self.children[self.curr_index] - actions = self.current_child.tick_once(robot, world_state) - return actions