/
controller.py
301 lines (272 loc) · 12.6 KB
/
controller.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
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
from klampt import *
from klampt import gldraw
from stateestimation import *
from stateprediction import *
from OpenGL.GL import *
import sys
import time
import matplotlib.pyplot as plot
LOG_BALL_PATH = False
TRIAL_LENGTH = 1500
PREP_RECOVER = np.array([0.0, 2.0, -.98, .8, math.pi/2, 0.0, 0.0])
POST_RECOVER = np.array([0.0, 0.7, -.98, 1.32, math.pi/2, 0.0, 0.0])
STROKES = np.array([[0.0, 0.7, -1.08, 1.187, math.pi/2, 0.0, 0.0],#PREP_L
[0.0, 2.0, -1.08, 1.187, math.pi/2, 0.0, 0.0],#POST_L
[0.0, 0.7, -0.98 , 1.32, math.pi/2, 0.0, 0.0],#PREP_C
[0.0, 2.0, -0.98 , 1.32, math.pi/2, 0.0, 0.0],#POST_C
[0.0, 0.7, -0.92 , 1.45, math.pi/2, 0.0, 0.0],#PREP_R
[0.0, 2.0, -0.92 , 1.45, math.pi/2, 0.0, 0.0]])#POST_R
TRAVELTIMES = [[2.093, 2.373, 2.640],
[1.853, 2.133, 2.400],
[2.173, 2.467, 2.740]]
INTERSECTIONS = [[+.20, +.30, +.40],
[-.15, -.08, +.03],
[-.73, -.77, -.80]]
BALL = (1, 0, 0, 1)
GOALIES = [(1, 0.5, 0, 1), (1, 1, 0, 1), (0.5, 1, 0, 1)]
SETUP_TIMES = [0, 0, 0]
STILL_LIMIT = .01
MIN_CLEARANCE = 0.55
INIT_TIME = 2.0
class MyController:
"""Attributes:
- world: the WorldModel instance used for planning.
- objectStateEstimator: a StateEstimator instance, which you may set up.
- state: a string indicating the state of the state machine. TODO:
decide what states you want in your state machine and how you want
them to be named. By default, this will go into the 'waiting' state
on startup, and will go into the 'user' state when 'u' is pressed.
By default, if 'u' is pressed, then this switches to 'user' mode,
and uses the keys.
- 1,2,3,4,5,6 to increase the robot's joint angles and
- q,w,e,r,t,y to decrease the robot's joint angles.
If 'u' is pressed again, it switches back to 'waiting' mode
"""
def __init__(self,world,robotController):
self.world = world
self.objectStateEstimator = None
self.goaliePredictor = None
self.state = None
self.robotController = robotController
self.reset(robotController)
def reset(self,robotController):
"""Called on initialization, and when the simulator is reset."""
self.objectStateEstimator = MyObjectStateEstimator()
self.goaliePredictor = YSinePredictor()
self.objectEstimates = None
self.state = 'precycle0'
self.qdes = robotController.getCommandedConfig()
self.t = 0
self.substate = 0
if LOG_BALL_PATH:
self.xplot = []
self.yplot = []
'''Returns whether q1 is close to q2'''
def close(self, q1, q2):
if len(q1) != len(q2):
print "self.close requires arguments of same length"
sys.stdout.flush()
sys.exit()
return False
sum = 0
for i in range(len(q1)):
sum += abs(q1[i] - q2[i])
return sum < 1e-1
'''Returns whether the ball is waiting to be struck'''
def ballWaiting(self, ballState):
if ballState is None:
return False
p = ballState.meanPosition()
v = ballState.meanVelocity()
return (-1.5 < p[0] and p[0] < -0.5 and
-1.0 < p[1] and p[1] < +0.0 and
.072 < p[2] and p[2] < .082 and
-0.5 < v[0] and v[0] < +0.5 and
-0.5 < v[1] and v[1] < +0.5 and
-.01 < v[2] and v[2] < +.01)
'''Returns index of best stroke (0, 1, 2) or -1 to indicate no valid path'''
def best_stroke(self):
answer = -1
best_clearance = MIN_CLEARANCE
for i in range(len(TRAVELTIMES)):
clearance = 50
for j in range(len(GOALIES)):
prediction = self.goaliePredictor.predict(self.t + TRAVELTIMES[i][j] + SETUP_TIMES[i], GOALIES[j])
if prediction is None:
return -1
yhat = prediction[1]
clearance = min(clearance, abs(yhat - INTERSECTIONS[i][j]))
if clearance > best_clearance:
answer = i
best_clearance = clearance
return answer
def myPlayerLogic(self,
dt,
sensorReadings,
objectStateEstimate,
robotController):
"""
TODO: fill this out to updates the robot's low level controller
in response to a new time step. This is allowed to set any
attributes of MyController that you wish, such as self.state.
Arguments:
- dt: the simulation time elapsed since the last call
- sensorReadings: the sensor readings given on the current time step.
this will be a dictionary mapping sensor names to sensor data.
The name "blobdetector" indicates a sensor reading coming from the
blob detector. The name "omniscient" indicates a sensor reading
coming from the omniscient object sensor. You will not need to
use raw sensor data directly, if you have a working state estimator.
- objectStateEstimate: a MultiObjectStateEstimate class (see
stateestimation.py) produced by the state estimator.
- robotController: a SimRobotController instance giving access
to the robot's low-level controller. You can call any of the
methods. At the end of this call, you can either compute some
PID command via robotController.setPIDCommand(), or compute a
trajectory to execute via robotController.set/addMilestone().
(if you are into masochism you can use robotController.setTorque())
"""
self.t += dt
qcmd = robotController.getCommandedConfig()
vcmd = robotController.getCommandedVelocity()
qsns = robotController.getSensedConfig()
vsns = robotController.getSensedVelocity()
if self.t > dt:
for goalie in GOALIES:
self.goaliePredictor.addPoint(self.t, objectStateEstimate.get(goalie))
''' Confusing function that plans path on first call, maintains state between calls
and keeps on going on the pre-planned path'''
def moveAndGoToState(qdes, next_state, frames=1):
if self.substate == 0:
self.qdes = qdes
self.start = qsns
if self.substate < frames:
self.substate += 1
next_goal = self.start + (1.0 * self.substate / frames) * (self.qdes - self.start)
robotController.setPIDCommand(next_goal, [0.0]*7)
if self.close(qsns, self.qdes) and np.linalg.norm(vsns) < STILL_LIMIT:
self.state = next_state
self.substate = 0
if self.state == 'precycle0':
moveAndGoToState(POST_RECOVER, 'waiting', 22)
if self.state == 'post_recover':
moveAndGoToState(POST_RECOVER, 'waiting', 10)
if self.state == 'waiting':
if self.ballWaiting(objectStateEstimate.get(BALL)) and self.t > INIT_TIME:
best_stroke = self.best_stroke()
if best_stroke != -1:
self.state = 'pre_stroke' + str(best_stroke)
else:
robotController.setPIDCommand(POST_RECOVER, [0.0]*7)
if self.state[:10] == 'pre_stroke':
stroke_index = int(self.state[10])
moveAndGoToState(STROKES[2 * stroke_index], 'stroke' + str(stroke_index), 1)
if self.state[:6] == 'stroke':
stroke_index = int(self.state[6])
moveAndGoToState(STROKES[2 * stroke_index + 1], 'pre_recover', 25)
if self.state == 'pre_recover':
moveAndGoToState(PREP_RECOVER, 'recover', 10)
if self.state == 'recover':
moveAndGoToState(POST_RECOVER, 'waiting', 25)
if self.state == 'user':
robotController.setPIDCommand(self.qdes,[0.0]*7)
if LOG_BALL_PATH and self.t:
if len(self.xplot) < TRIAL_LENGTH:
ballstate = objectStateEstimate.get(BALL)
self.xplot.append(ballstate.meanPosition()[0])
self.yplot.append(ballstate.meanPosition()[1])
else:
print np.array(self.yplot).shape
np.save('x.npy', np.array(self.xplot))
np.save('y.npy', np.array(self.yplot))
plot.scatter(self.xplot, self.yplot)
plot.show()
sys.exit()
#print self.t
#print self.state
sys.stdout.flush()
return
def loop(self,dt,robotController,sensorReadings):
"""Called every control loop (every dt seconds).
Input:
- dt: the simulation time elapsed since the last call
- robotController: a SimRobotController instance. Use this to get
sensor data, like the commanded and sensed configurations.
- sensorReadings: a dictionary mapping sensor names to sensor data.
The name "blobdetector" indicates a sensor reading coming from the
blob detector. The name "omniscient" indicates a sensor reading coming
from the omniscient object sensor.
Output: None. However, you should produce a command sent to
robotController, e.g., robotController.setPIDCommand(qdesired).
"""
multiObjectStateEstimate = None
if self.objectStateEstimator and 'blobdetector' in sensorReadings:
multiObjectStateEstimate = self.objectStateEstimator.update(sensorReadings['blobdetector'])
self.objectEstimates = multiObjectStateEstimate
if 'omniscient' in sensorReadings:
omniscientObjectState = OmniscientStateEstimator().update(sensorReadings['omniscient'])
#TODO: Comment out the following line when you are ready to test your state estimator
#multiObjectStateEstimate = omniscientObjectState
self.myPlayerLogic(dt,
sensorReadings,multiObjectStateEstimate,
robotController)
return
def keypress(self,key):
"""If you want to implement some interactivity while debugging,
you can do it here. By default, it uses 1,2,3,4,5,6 to increase
the robot's joint angles and q,w,e,r,t,y to to decrease them.
"""
if key == 'u':
if self.state == 'user':
print "Switching out of user mode..."
self.state = 'waiting'
else:
print "Switching into user mode..."
self.state = 'user'
self.qdes = self.robotController.getCommandedConfig()
if self.state == 'user':
#note: joint 0 is a dummy joint
upkeys = {'1':1,'2':2,'3':3,'4':4,'5':5,'6':6}
downkeys = {'\'':1,',':2,'.':3,'p':4,'y':5,'f':6}
if key in upkeys:
self.qdes[upkeys[key]] += 0.1
elif key in downkeys:
self.qdes[downkeys[key]] -= 0.1
print self.qdes
sys.stdout.flush()
def drawGL(self):
"""This gets called every time an OpenGL rendering loop is called.
TODO: You may consider visually debugging some of your code here.
For example, to draw the robot at a given configuration q, you can call:
self.world.robot(0).setConfig(q)
self.world.robot(0).drawGL()
To draw a point with size s, color (r,g,b), and world position (x,y,z)
you can call:
glDisable(GL_LIGHTING)
glColor3f(r,g,b)
glPointSize(s)
gldraw.point([x,y,z])
The current code draws gravity-inflenced arcs leading from all the
object position / velocity estimates from your state estimator. Event C
folks should set gravity=0 in the following code.
"""
if self.objectEstimates:
for o in self.objectEstimates.objects:
glDisable(GL_LIGHTING)
glColor3f(o.name[0],o.name[1],o.name[2])
#draw a point
glPointSize(5.0)
gldraw.point([o.x[0],o.x[1],o.x[2]])
#draw an arc
glBegin(GL_LINE_STRIP)
x = [o.x[0],o.x[1],o.x[2]]
v = [o.x[3],o.x[4],o.x[5]]
#TODO: are you doing event C? If so, you should
#set gravity=0 to get more useful visual feedback
#about your state estimates.
gravity = 0
#gravity = 9.8
for i in range(20):
t = i*0.05
glVertex3f(*vectorops.sub(vectorops.madd(x,v,t),[0,0,0.5*gravity*t*t]))
glEnd()