-
Notifications
You must be signed in to change notification settings - Fork 0
/
Triangle_Pass.py
executable file
·235 lines (194 loc) · 10.6 KB
/
Triangle_Pass.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
import composite_behavior
import behavior
import skills.pivot_kick
import skills.pass_receive
import time
import enum
import logging
## This handles passing from one bot to another
# Simply run it and set it's receive point, the rest is handled for you
# It starts out by assigning a kicker and a receiver and instructing them to lineup for the pass
# Once they're aligned, the kicker kicks and the receiver adjusts itself based on the ball's movement
# Note: due to mechanical limitations, a kicker often gets stuck trying to adjust its angle while it's just outside of it's
# aim error threshold. If this happens, the CoordinatedPass will adjust the receive point slightly
# because it's easier to move the receiver over a bit than have the kicker adjust its angle. This solves
# the problem of having a pass get stuck indefinitely while the kicker sits there not moving.
# TODO: # If an opponent blocks the pass channel, it will wait until it moves - you can cancel it at this point if you wish
# As soon as the kicker kicks, it is no longer and is released by this behavior so other behaviors can be assigned to it
# If the receiver gets the ball, CoordinatedPass transitions to the completed state, otherwise it goes to the failed state
class CoordinatedPass(composite_behavior.CompositeBehavior):
KickPower = 0.6
class State(enum.Enum):
preparing = 1 # the kicker is aiming and the receiver is getting ready
kicking = 2 # waiting for the kicker to kick
receiving = 3 # the kicker has kicked and the receiver is trying to get the ball
timeout = 4
## Init method for CoordinatedPass
# @param skillreceiver an instance of a class that will handle the receiving robot. See pass_receive and angle_receive for examples.
# Using this, you can change what the receiving robot does (rather than just receiving the ball, it can pass or shoot it).
# Subclasses of pass_receive are preferred, but check the usage of this variable to be sure.
# @param receive_point The point that will be kicked too. (Target point)
# @param skillkicker A tuple of this form (kicking_class instance, ready_lambda). If none, it will use (pivot_kick lambda x: x == pivot_kick.State.aimed).
# @param receiver_required Whether the receiver subbehavior should be required or not
# @param kicker_required Whether the kicker subbehavior should be required or not
# The lambda equation is called (passed with the state of your class instance) to see if your class is ready. Simple implementations will just compare it to your ready state.
def __init__(self,
receive_point=None,
skillreceiver=None,
skillkicker=None,
prekick_timeout=None,
receiver_required=True,
kicker_required=True):
super(CoordinatedPass,self).__init__(continuous=False)
# This creates a new instance of skillreceiver every time the constructor is
# called (instead of pulling from a single static instance).
# if skillreceiver == None:
# skillreceiver = skills.pass_receive.PassReceive()
# if skillkicker == None:
# skillkicker = (
# skills.pivot_kick.PivotKick(),
# lambda x: x == skills.pivot_kick.PivotKick.State.aimed)
# self.receive_point = receive_point
# self.skillreceiver = skillreceiver
# self.skillkicker = skillkicker
# self.prekick_timeout = prekick_timeout
# self.receiver_required = receiver_required
# self.kicker_required = kicker_required
self.add_state(CoordinatedPass.State.preparing,
behavior.Behavior.State.running)
self.add_state(CoordinatedPass.State.kicking,
behavior.Behavior.State.running)
self.add_state(CoordinatedPass.State.receiving,
behavior.Behavior.State.running)
self.add_state(CoordinatedPass.State.timeout,
behavior.Behavior.State.failed)
self.add_transition(behavior.Behavior.State.start,
CoordinatedPass.State.preparing, lambda: True,
'immediately')
self.add_transition(
CoordinatedPass.State.preparing, CoordinatedPass.State.kicking,
lambda: (skillkicker[1](self.subbehavior_with_name('kicker').state) and self.subbehavior_with_name('receiver').state == self.skillreceiver.State.aligned),
'kicker and receiver ready')
self.add_transition(
CoordinatedPass.State.preparing, CoordinatedPass.State.timeout,
self.prekick_timeout_exceeded, 'Timed out on prepare')
self.add_transition(
CoordinatedPass.State.kicking, CoordinatedPass.State.timeout,
self.prekick_timeout_exceeded, 'Timed out on prepare')
self.add_transition(CoordinatedPass.State.kicking,
CoordinatedPass.State.timeout,
self.prekick_timeout_exceeded, 'Timed out on kick')
self.add_transition(
CoordinatedPass.State.kicking, CoordinatedPass.State.receiving,
lambda: self.subbehavior_with_name('kicker').state == behavior.Behavior.State.completed,
'kicker kicked')
self.add_transition(
CoordinatedPass.State.receiving, behavior.Behavior.State.completed,
lambda: self.subbehavior_with_name('receiver').state == behavior.Behavior.State.completed,
'pass received!')
self.add_transition(
CoordinatedPass.State.receiving, behavior.Behavior.State.failed,
lambda: self.subbehavior_with_name('receiver').state == behavior.Behavior.State.failed,
'pass failed :(')
## Handles restarting this behaivor.
# Since we save a few sub-behaviors, we need to restart those when we restart.
# def restart(self):
# super().restart()
# self.skillreceiver.restart()
# self.skillkicker[0].restart()
# set the location where the receiving bot should camp out and wait for the ball
# Default: None
@property
def receive_point(self):
return self._receive_point
@receive_point.setter
def receive_point(self, value):
self._receive_point = value
# set receive_point for kicker and receiver (if present)
if self.has_subbehavior_with_name('kicker'):
self.subbehavior_with_name('kicker').target = self.receive_point
if self.has_subbehavior_with_name('receiver'):
self.subbehavior_with_name('receiver').receive_point = self.receive_point
def on_enter_running(self):
receiver = self.skillreceiver
receiver.receive_point = self.receive_point
self.add_subbehavior(receiver,
'receiver',
required=self.receiver_required)
def on_exit_running(self):
self.remove_subbehavior('receiver')
def on_enter_kicking(self):
self.subbehavior_with_name('kicker').enable_kick = True
def on_enter_preparing(self):
kicker = self.skillkicker[0]
kicker.target = self.receive_point
kicker.kick_power = 7
kicker.enable_kick = False # we'll re-enable kick once both bots are ready
# we use tighter error thresholds because passing is hard
kicker.aim_params['error_threshold'] = 0.2
kicker.aim_params['max_steady_ang_vel'] = 3.0
kicker.aim_params['min_steady_duration'] = 0.15
kicker.aim_params['desperate_timeout'] = 3.0
self.add_subbehavior(kicker, 'kicker', required=self.kicker_required)
# receive point renegotiation
self._last_unsteady_time = None
self._has_renegotiated_receive_point = False
self._preparing_start = time.time()
def execute_running(self):
# The shot obstacle doesn't apply to the receiver
if self.has_subbehavior_with_name('kicker'):
kicker = self.subbehavior_with_name('kicker')
receiver = self.subbehavior_with_name('receiver')
kicker.shot_obstacle_ignoring_robots = [receiver.robot]
# gets robots involved with the pass
def get_robots(self):
kicker = None
receiver = None
if self.has_subbehavior_with_name('kicker'):
kicker = self.subbehavior_with_name('kicker')
if self.has_subbehavior_with_name('receiver'):
receiver = self.subbehavior_with_name('receiver')
toReturn = []
if receiver != None and receiver.robot != None:
toReturn.extend([receiver.robot])
if kicker != None and kicker.robot != None:
toReturn.extend([kicker.robot])
return toReturn
def execute_preparing(self):
kicker = self.subbehavior_with_name('kicker')
# receive point renegotiation
# if the kicker sits there aiming close to target and gets stuck,
# we set the receive point to the point the kicker is currently aiming at
if kicker.current_shot_point(
) != None and not self._has_renegotiated_receive_point:
if (not kicker.is_steady() and self.skillkicker[1](kicker.state)):
self._last_unsteady_time = time.time()
if (self._last_unsteady_time != None and
time.time() - self._last_unsteady_time > 0.75 and
kicker.current_shot_point().dist_to(self.receive_point) <
0.1):
# renegotiate receive_point
logging.info("Pass renegotiated RCV PT")
self.receive_point = kicker.current_shot_point()
self._has_renegotiated_receive_point = True
def prekick_timeout_exceeded(self):
if self._preparing_start == None or self.prekick_timeout == None or self.prekick_timeout <= 0:
return False
if time.time() - self._preparing_start > self.prekick_timeout:
return True
return False
def time_remaining(self):
if self._preparing_start == None or self.prekick_timeout == None or self.prekick_timeout <= 0:
return 0
return self.prekick_timeout - (time.time() - self._preparing_start)
def on_enter_receiving(self):
# once the ball's been kicked, the kicker can go relax or do another job
self.subbehavior_with_name('receiver').ball_kicked = True
self.remove_subbehavior('kicker')
def __str__(self):
desc = super().__str__()
desc += "\n rcv_pt=" + str(self.receive_point)
if not (self._preparing_start == None or self.prekick_timeout == None
or self.prekick_timeout <= 0):
desc += "\n timeout=" + str(round(self.time_remaining(), 2))
return desc