def behavior(self):
"""execute an interaction of the controller update loop"""
self.is_slack = False
self.desAng = self.get_ang()
while True:
a = exp(1j * self.get_ang() * 2 * pi)
a0 = exp(1j * self.desAng * 2 * pi)
lead = angle(a / a0)
if abs(lead) > 0.7 * pi:
if "F" in DEBUG:
progress("FB desRPM %g out of range" % (self.desRPM))
# outside of capture range; ignore
return
pFB = clip(self.Kp * lead, -45, 45)
if isnan(self._v):
vFB = 0
else:
vFB = self.Kv * (self._v - self.desRPM)
rpm = self.desRPM - pFB - vFB
if abs(rpm) < 0.1:
rpm = 0
if "F" in DEBUG:
progress("FB desRPM %g p %g v %g" % (self.desRPM, pFB, vFB))
# Push into the motor
if (self.is_slack):
self.servo.go_slack()
else:
self._set_rpm(rpm)
yield self.forDuration(self.rate)
def onStart( self ):
# Set up socket for emitting fake tag messages
s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)
s.bind(("",0))
self.sock = s
# Set up the sensor receiver plan
self.sensor = SensorPlanTCP(self,server=self.srvAddr[0])
self.sensor.start()
self.timeForStatus = self.onceEvery(1)
self.timeForLaser = self.onceEvery(1/15.0)
self.timeForFrame = self.onceEvery(1/20.0)
self.timeForFilter = self.onceEvery(1.0/10.0)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.T0 = self.now
# Setup autonomous mode timer
self.timeForAuto = self.onceEvery(1)
self.auto = False
self.core = Core(Mode.SIMULATION, self)
self.robSim = RedRobotSim(self.core, fn=None)
self.core.setSim(self.robSim)
self.core.start()
self.startedFilter = False
def updateTrajectory(self):
self.writeAngle()
self.stateInfo["trajectory"] = self.stateInfo["trajectoryList"][0]
progress("In update trajectory, new angle is " +
str(self.stateInfo["trajectory"]))
self.stateInfo["switch"] = True
self.stateInfo["orientationChecked"] = False
def _set_rpm(self, rpm):
"""Push an RPM setting to the motor"""
self._ensure_motor()
tq = clip(self.rpmScl * self.ori * rpm, -0.999, 0.999)
if "r" in DEBUG:
progress("%s.set_torque(%g) <-- rpm %g" % (self.servo.name, tq, rpm))
self.servo.set_torque(tq)
def onStart(self):
# Set up the sensor receiver plan
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.timeForStatus = self.onceEvery(0.33)
self.T0 = self.now
def behavior(self):
"""execute an interaction of the controller update loop"""
self.is_slack = False
self.desAng = self.get_ang()
while True:
a = exp(1j * self.get_ang()*2*pi)
a0 = exp(1j * self.desAng*2*pi)
lead = angle(a / a0)
if abs(lead)>0.7*pi:
if "F" in DEBUG:
progress("FB desRPM %g out of range" % (self.desRPM))
# outside of capture range; ignore
return
pFB = clip(self.Kp * lead, -45, 45)
if isnan(self._v):
vFB = 0
else:
vFB = self.Kv * (self._v - self.desRPM)
rpm = self.desRPM - pFB - vFB
if abs(rpm)<0.1:
rpm = 0
if "F" in DEBUG:
progress("FB desRPM %g p %g v %g" % (self.desRPM, pFB, vFB))
# Push into the motor
if (self.is_slack):
self.servo.go_slack()
else:
self._set_rpm(rpm)
yield self.forDuration(self.rate)
def _set_rpm(self, rpm):
"""Push an RPM setting to the motor"""
self._ensure_motor()
tq = clip(self.rpmScl * self.ori * rpm, -0.999, 0.999)
if "r" in DEBUG:
progress("%s.set_torque(%g) <-- rpm %g" % (self.servo.name, tq, rpm))
self.servo.set_torque(tq)
def get_ang(self):
pos = self.servo.get_pos()
ang = (pos - self.posOfs) / self.aScl / self.ori
if "g" in DEBUG:
progress("%s.get_angle --> %g" % (self.servo.name, ang))
self._updateV(ang)
return ang
def _nextMessage(self):
"""
Obtain the next message; kill socket on error.
returns '' if nothing was received
"""
# if buffer contains no complete messages --> read socket
if self.buf.find(b'}') < 0:
# receive an update / skip
try:
msg = self.sock.recv(1024)
except SocketError as se:
# If there was no data on the socket
# --> not a real error, else kill socket and start a new one
if se.errno != EAGAIN:
progress("Connection failed: " + str(se))
self.sock.close()
self.sock = None
return b''
self.buf = self.buf + msg
# Use previously buffered data
buf = self.buf
# End of dictionary should be end of message
f = buf.find(b"}")
if f < 0:
return b''
# Pull out the first dictionary
self.buf = buf[f + 1:]
return buf[:f + 1]
def onStart(self):
# Set up socket for emitting fake tag messages
s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)
s.bind(("", 0))
self.sock = s
# Set up the sensor receiver plan
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
self.timeForStatus = self.onceEvery(1)
self.timeForLaser = self.onceEvery(1 / 15.0)
self.timeForFrame = self.onceEvery(1 / 20.0)
self.timeForFilter = self.onceEvery(1.0 / 10.0)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.T0 = self.now
# Setup autonomous mode timer
self.timeForAuto = self.onceEvery(1)
self.auto = False
self.core = Core(Mode.SIMULATION, self)
self.robSim = RedRobotSim(self.core, fn=None)
self.core.setSim(self.robSim)
self.core.start()
self.startedFilter = False
def get_ang(self):
pos = self.servo.get_pos()
ang = (pos - self.posOfs) / self.aScl / self.ori
if "g" in DEBUG:
progress("%s.get_angle --> %g" % (self.servo.name, ang))
self._updateV(ang)
return ang
def _connect( self ):
"""Set up the socket"""
s = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)
s.connect(self.svrAddr)
s.setblocking(0)
self.sock = s
self.buf = ''
progress("Sensor connected to %s:%d" % self.svrAddr)
def _set_motor(self):
"""(private) set module in motor mode
also configures the _ensure_* callbacks
"""
self.servo.set_mode(1)
if "h" in DEBUG:
progress("%s.set_mode('Motor')" % (self.servo.name))
if self.servo.get_mode() == 1:
self._ensure_motor = lambda: None
self._ensure_servo = self._set_servo
def onEvent(self,evt):
if evt.type != KEYDOWN:
return
else:
f = "1234567890".find(evt.unicode)
if f>=0:
progress("MXs to " + str(f))
for s in self.smx:
s.set_ang(f*0.1)
return
return JoyApp.onEvent(self,evt)
def _set_motor(self):
"""(private) set module in motor mode
also configures the _ensure_* callbacks
"""
self.servo.set_mode(1)
if "h" in DEBUG:
progress("%s.set_mode('Motor')" % (self.servo.name))
if self.servo.get_mode() == 1:
self._ensure_motor = lambda: None
self._ensure_servo = self._set_servo
def onStart( self ):
# Set up the sensor receiver plan
self.sensor = SensorPlan(self)
self.sensor.start()
self.robSim = HoloRobotSim(fn=None)
self.timeForStatus = self.onceEvery(1)
self.timeForLaser = self.onceEvery(1/15.0)
self.timeForFrame = self.onceEvery(1/20.0)
self.timeForFeedback = self.onceEvery(1/10.0)
self.timeForDynamics = self.onceEvery(1/50.0)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
def behavior(self):
s = self.simIX
progress("Pos: %s" % (s.tagPos))
s.ang = self.ang
s.yVis = True
s.xVis = False
# Compute step along the forward direction
step = self.dist / float(self.N)
dt = self.dur / float(self.N)
for k in range(self.N):
s.move(step)
yield self.forDuration(dt)
def onStart(self):
# Set up socket for emitting fake tag messages
s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)
s.bind(("", 0))
self.sock = s
# Set up the sensor receiver plan
self.sensor = SensorPlanTCP(self, server=self.srvAddr[0])
self.sensor.start()
self.robSim = DummyRobotSim(fn=None)
self.timeForStatus = self.onceEvery(1)
self.timeForLaser = self.onceEvery(1 / 15.0)
self.timeForFrame = self.onceEvery(1 / 20.0)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.T0 = self.now
def _nextMessage( self ):
"""
Obtain the next message; kill socket on error.
returns '' if nothing was received
"""
# receive an update / skip
try:
return self.sock.recv(1024)
except SocketError, se:
# If there was no data on the socket
# --> not a real error, else kill socket and start a new one
if se.errno != 11:
progress("Connection failed: "+str(se))
self.sock.close()
self.sock = None
def _nextMessage( self ):
"""
Obtain the next message; kill socket on error.
returns '' if nothing was received
"""
# receive an update / skip
try:
msg = self.sock.recv(1024)
except SocketError, se:
# If there was no data on the socket
# --> not a real error, else kill socket and start a new one
if se.errno != 11:
progress("Connection failed: "+str(se))
self.sock.close()
self.sock = None
return ''
def showSensors(self):
ts, f, b = self.sensor.lastSensor
if ts:
progress("Sensor: %4d f %d b %d" % (ts - self.T0, f, b))
else:
progress("Sensor: << no reading >>")
ts, w = self.sensor.lastWaypoints
if ts:
progress("Waypoints: %4d " % (ts - self.T0) + str(w))
else:
progress("Waypoints: << no reading >>")
def showSensors( self ):
ts,f,b = self.sensor.lastSensor
if ts:
progress( "Sensor: %d f %d b %d" % (ts,f,b) )
else:
progress( "Sensor: << no reading >>" )
ts,w = self.sensor.lastWaypoints
if ts:
progress( "Waypoints: %d " % ts + str(w))
else:
progress( "Waypoints: << no reading >>" )
def _animation(fig):
s = socket(AF_INET, SOCK_DGRAM)
s.bind(("", 0xB00))
s.setblocking(0)
fig.clf()
ax = fig.add_subplot(111)
msg = None
src = None
last = now()
while True:
try:
# read data as fast as possible
m, msrc = s.recvfrom(1 << 16)
if not (msrc == src):
src = msrc
progress("New tag data source: %s:%d" % src)
if m and len(m) > 2:
msg = m
continue
except SocketError as se:
# until we've run out; last message remains in m
pass
# make sure we got something
if not msg:
yield
continue
# display it
ax.cla()
ax.set_title("%.1f fps" % (1 / (now() - last)))
last = now()
for d in json_loads(msg):
if type(d) is not dict:
continue
a = array(d['p'])
ax.plot(a[[0, 1, 2, 3, 0], 0], a[[0, 1, 2, 3, 0], 1], '.-b')
ax.plot(a[[0], 0], a[[0], 1], 'og')
ax.text(mean(a[:, 0]),
mean(a[:, 1]),
d['i'],
ha='center',
va='center')
ax.axis([0, 1600, 0, 1200])
yield
def writeAngleInit(self):
ts_w, w = self.sp.lastWaypoints
i = 0
while i < (len(w) - 2):
v1 = math.hypot(w[i][0] - w[i + 1][0], w[i][1] - w[i + 1][1])
v2 = math.hypot(w[i + 1][0] - w[i + 2][0],
w[i + 1][1] - w[i + 2][1])
v3 = math.hypot(w[i][0] - w[i + 2][0], w[i][1] - w[i + 2][1])
num = ((v3**2) - (v2**2) - (v1**2)) / (-2 * v1 * v2)
angle = math.acos(num)
angle = angle * (180 / math.pi)
self.stateInfo["trajectoryList"].append(
int(-1 * (angle + self.stateInfo["trajectory"])))
i = i + 1
#Then, calculate vector between remaining waypoints, i.e. (0,1), (1,2), (2,3), or just (1,2) and (2,3) using loop
#Then, calculate angle between each pair of vectors using law of cosines
progress(str(self.stateInfo["trajectoryList"]))
def onEvent(self, evt):
# periodically, show the sensor reading we got from the waypointServer
if self.timeForStatus():
self.showSensors()
progress(self.robSim.logLaserValue(self.now))
# generate simulated laser readings
elif self.timeForLaser():
self.robSim.logLaserValue(self.now)
# update the robot and simulate the tagStreamer
if self.timeForFrame():
self.emitTagMessage()
if evt.type == KEYDOWN:
if evt.key == K_UP:
self.robSim.move(0.5)
return progress("(say) Move forward")
elif evt.key == K_DOWN:
self.robSim.move(-0.5)
return progress("(say) Move back")
elif evt.key == K_LEFT:
self.robSim.turn(0.5)
return progress("(say) Turn left")
elif evt.key == K_RIGHT:
self.robSim.turn(-0.5)
return progress("(say) Turn right")
# Use superclass to show any other events
else:
return JoyApp.onEvent(self, evt)
return # ignoring non-KEYDOWN events
def onEvent(self, evt):
#### DO NOT MODIFY --------------------------------------------
# periodically, show the sensor reading we got from the waypointServer
if self.timeForStatus():
self.showSensors()
progress(self.robSim.logLaserValue(self.now))
# generate simulated laser readings
elif self.timeForLaser():
self.robSim.logLaserValue(self.now)
# update the robot and simulate the tagStreamer
if self.timeForFrame():
self.emitTagMessage()
#### MODIFY FROM HERE ON ----------------------------------------
if evt.type == KEYDOWN:
if evt.key == K_UP and not self.moveP.isRunning():
self.moveP.dist = 100.0
self.moveP.start()
return progress("(say) Move forward")
elif evt.key == K_DOWN and not self.moveP.isRunning():
self.moveP.dist = -100.0
self.moveP.start()
return progress("(say) Move back")
if evt.key == K_LEFT and not self.turnP.isRunning():
self.turnP.ang = 0.5
self.turnP.start()
return progress("(say) Turn left")
if evt.key == K_RIGHT and not self.turnP.isRunning():
self.turnP.ang = -0.5
self.turnP.start()
return progress("(say) Turn right")
### DO NOT MODIFY -----------------------------------------------
else: # Use superclass to show any other events
return JoyApp.onEvent(self, evt)
return # ignoring non-KEYDOWN events
def onStart( self ):
"""
Sets up the JoyApp and configures the simulation
"""
### DO NOT MODIFY ------------------------------------------
# Set up socket for emitting fake tag messages
s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)
s.bind(("",0))
self.sock = s
# Set up the sensor receiver plan
self.sensor = SensorPlanTCP(self,server=self.srvAddr[0])
self.sensor.start()
self.timeForStatus = self.onceEvery(1)
self.timeForLaser = self.onceEvery(1/15.0)
self.timeForFrame = self.onceEvery(1/20.0)
progress("Using %s:%d as the waypoint host" % self.srvAddr)
self.T0 = self.now
### MODIFY FROM HERE ------------------------------------------
self.robSim = SimpleRobotSim(fn=None)
self.yMove = yMovePlan(self,self.robSim)
self.xMove = xMovePlan(self,self.robSim)
self.auto = Auto(self, self. robSim, self.yMove, self.xMove)
def showSensors(self):
"""
Display sensor readings
"""
# This code should help you understand how you access sensor information
ts, f, b = self.sensor.lastSensor
if ts:
progress("Sensor: %4d f %d b %d" % (ts - self.T0, f, b))
else:
progress("Sensor: << no reading >>")
ts, w = self.sensor.lastWaypoints
if ts:
progress("Waypoints: %4d " % (ts - self.T0) + str(w))
else:
progress("Waypoints: << no reading >>")
def onEvent( self, evt ):
# periodically, show the sensor reading we got from the waypointServer
if self.timeForStatus():
self.showSensors()
# generate simulated laser readings
if self.timeForLaser():
progress( self.robSim.logLaserValue(self.now) )
# update the robot and simulate the tagStreamer
if self.timeForFrame():
self.emitTagMessage()
if self.timeForFeedback():
self.robSim.computeStep()
#"
#if evt.type == KEYDOWN:
# #slip event
# if evt.key == K_s:
# self.robSim.slipmove()
# elif evt.key == K_m:
#if evt.key == K_UP:
# self.robSim.move(0.5)
# return progress('(say) Move forward')
# elif evt.key == K_DOWN:
# self.robSim.move(-0.5)
# return progress('(say) Move back')
# elif evt.key == K_LEFT:
# self.robSim.turn(-0.5)
# return progress('(say) Turn left')
# elif evt.key == K_RIGHT:
# self.robSim.turn(0.5)
# return progress('(say) Turn right')
# else:
# self.robSim.move()
# Use superclass to show any other events
return JoyApp.onEvent(self,evt)
def onEvent(self, evt):
if self.timeForUpdate():
#get the latest information from sensors and update the controller
#with those values
ts,f,b = self.sensor.lastSensor
ts2, w = self.sensor.lastWaypoints
if ts:
self.controller.update_waypoint(w)
if ts2:
self.controller.update_sensor_values(f,b)
if evt.type == KEYDOWN:
if evt.key == K_UP:
self.bot.robotMove(10,1)
return progress("(say) Move forward")
elif evt.key == K_DOWN:
self.bot.robotMove(10,-1)
return progress("(say) Move back")
elif evt.key == K_LEFT:
self.bot.robotTurn(-pi/2)
#self.bot.unitRobotRotate(1)
#self.bot.unitLaserRotate(1)
return progress("(say) Turn left")
elif evt.key == K_RIGHT:
self.bot.robotTurn(pi/2)
#self.bot.unitRobotRotate(-1)
#self.bot.unitLaserRotate(-1)
return progress("(say) Turn right")
elif evt.key == K_q:
self.bot.unitLaserRotate(1)
#self.bot.tagRotate(pi/2.0)
return progress("(say) Turning tag")
elif evt.key == K_w:
self.bot.unitLaserRotate(-1) #laserRotate(-pi/8.0)
return progress("(say) Turning laser")
elif evt.key == K_e:
self.bot.unitTagRotate(-1)
elif evt.key == K_r:
self.bot.unitTagRotate(1)
elif evt.key == K_a:
self.controller.start()
self.controller.go_autonomous()
return progress("(say) Going Autonomous!")
elif evt.key == K_m:
self.controller.autonomous = False
# Use superclass to show any other events
return JoyApp.onEvent(self,evt)
def updateSoftwareState(self, f, b):
#ts,f,b = self.sp.lastSensor
if (f < MIN_THRESH or b < MIN_THRESH):
self.stateInfo["state"] = 1
elif (f > OFF_LINE or b > OFF_LINE):
self.stateInfo["state"] = 2
elif (((f > MIN_THRESH and f < OFF_LINE) and (b > 90))
or ((b > MIN_THRESH and b < OFF_LINE) and (f > 90))
or ((b > MIN_THRESH and b < OFF_LINE) and
(f > MIN_THRESH and f < OFF_LINE))):
self.stateInfo["state"] = 3
progress("In software update: " + str(self.stateInfo["state"]))
progress("f: " + str(f))
progress("b: " + str(b))
def onEvent(self, evt):
# periodically, show the sensor reading we got from the waypointServer
if self.timeForStatus():
self.showSensors()
progress(self.robSim.logLaserValue(self.now))
# generate simulated laser readings
elif self.timeForLaser():
self.robSim.logLaserValue(self.now)
# update the robot and simulate the tagStreamer
if self.timeForFrame():
self.emitTagMessage()
if evt.type == KEYDOWN:
if evt.key == K_UP and not self.moveP.isRunning():
self.moveP.dist = 0.2
self.moveP.start()
return progress("(say) Move forward")
elif evt.key == K_DOWN and not self.moveP.isRunning():
self.moveP.dist = -0.2
self.moveP.start()
return progress("(say) Move back")
if evt.key == K_LEFT and not self.turnP.isRunning():
self.turnP.ang = 0.3
self.turnP.start()
return progress("(say) Turn left")
if evt.key == K_RIGHT and not self.turnP.isRunning():
self.turnP.ang = -0.3
self.turnP.start()
return progress("(say) Turn right")
elif evt.key == K_a:
self.autoP.start()
return progress("(say) Moving autonomously")
elif evt.key == K_s:
self.autoP.stop()
return progress("(say) Stop autonomous control")
# Use superclass to show any other events
else:
return JoyApp.onEvent(self, evt)
return # ignoring non-KEYDOWN events
def behavior(self):
angOffset = zeros(len(self.app.arm))
if self.calibrated == True:
#Interpolate between angle grid positions and angle error
progress('Offset applied')
angOffset = griddata(self.app.calib_grid[:,:-2],self.app.calib_ang,(self.pos[:-2]),method='linear')[0]
print(angOffset)
self.syncArm()
if self.CalDone == False or self.square == False:
#forward kinematics - get current end effector position given joint angles
self.currentPos = self.app.idealArm.getTool(self.moveArm.angles)
#Create a number of evenly spaced steps between current position and goal position
self.steps = linspace(self.currentPos,self.pos,5)[:,:-1] #Can adjust number of steps.
#execute movement along path of steps
for stepCount,step in enumerate(self.steps):
progress('Step #%d, %s' % (stepCount,str(step)))
self.app.currStep = stepCount
self.moveArm.ee = step
for i,motor in enumerate(self.app.arm):
#Calculate angles to move each step and move the motors
motor.set_pos(rad2deg(self.moveArm.angles[i]+angOffset[i])*100) #feed in angle to set_pos as centidegrees
yield self.forDuration(4)
progress('Move complete')
# Collect the corner tags and estimate homography
nprj = None
try:
roi = array([mean(pts[nm], 0) for nm in corners])
# Homography mapping roi to ref
nprj = fitHomography(roi, ref)
except KeyError, ck:
progress("-- missing corner %s" % str(c))
#
# If no previous homography --> try again
if prj is None:
# If no homography found
if nprj is None:
yield
continue
progress("(say) Homography initialized")
prj = nprj
#
# Apply homography to all the points
uvs = dot(pts, prj)
z = uvs[..., 0] + 1j * uvs[..., 1]
nz = ~isnan(z[:, 0])
nz &= asarray(uvs[:, 0, -1], dtype=bool)
z[nz, ...] /= uvs[nz, ..., [-1]]
# Centroids of tags
zc = mean(z, 1)
#
### At this point, z has all tag corner points; zc centroids
### the nz index indicated tags for which we have locations
#
f1.clf()
def _animation(f1):
global s, app
# Open socket
try:
s = socket(AF_INET, SOCK_DGRAM)
s.bind(("", APRIL_DATA_PORT))
s.setblocking(0)
# Server socket
srv = socket(AF_INET, SOCK_STREAM)
srv.bind(("0.0.0.0", 8080))
srv.listen(2)
except:
app.stop()
raise
client = None
logfile = None
# Axes for arena display
ax = array(
[min(ref[:, 0]),
max(ref[:, 0]),
min(ref[:, 1]),
max(ref[:, 1])]) * 1.2
# Allowed tag IDS
allow = set(corners + waypoints + ROBOT_TAGID)
# Array size; must store all allowed tags
N = max(allow) + 1
# Array holding all point locations
pts = zeros((N, 4, 3), dtype=float)
# Indicator array for point that are assumed static
# and whose locations will be lowpass filtered
statics = zeros(N, dtype=bool)
statics[corners + waypoints] = True
# Legend for point update indicator strings
lbl = array(list(".+ld:*LD"))
# (CONST) Reference locations for tag corners
ang0 = array([-1 + 1j, 1 + 1j, 1 - 1j, -1 - 1j]) * -1j
# (CONST) Point on a circle
circ = exp(1j * linspace(0, 2 * pi, 16))
### Initial values for variables
# Configure sensor line-types
sensorF = Sensor(':om', lw=2)
sensorB = Sensor(':oc', lw=2)
# Last message received from TagStreamer
msg = None
# Last waypoint visited
M = 0
# Dynamic zoom scale for robot view
zoom = None
# Last homography
prj = None
# Start time
T0 = now()
# Time last waypoint message was sent
lastWay = T0 - WAY_RATE - 1
# Number of messages received
nmsg = 0
#
### MAIN LOOP ###
#
while len(waypoints) > M + 1: # continue until goal is reached
#
### Read data from April
#
try:
while True:
# read data as fast as possible
msg = s.recv(1 << 16)
nmsg += 1
except SocketError, se:
# until we've run out; last message remains in m
pass
# make sure we got something
if not msg:
continue
# Parse tag information from UDP packet
dat = json_loads(msg)
msg = ''
# Collect allowed tags
h = empty_like(pts)
h[:] = nan
for d in dat:
nm = d['i']
if not nm in allow:
continue
#if ~isnan(h[nm,0,0]):
# print '(dup)',
p = asarray(d['p']) / 100
h[nm, :, :2] = p
h[nm, :, 2] = 1
#
# at this point, all observed tag locations are in the dictionary h
#
### Update pts array
#
# Tags seen in this frame
fidx = ~isnan(h[:, 0, 0])
# Tags previously unseen
uidx = isnan(pts[:, 0, 0])
# Tags to update directly: non static, or static and first time seen
didx = (uidx & fidx) | ~statics
if any(didx):
pts[didx, ...] = h[didx, ...]
# Tags to update with lowpass: static, seen and previously seen
lidx = fidx & statics & ~uidx
if any(lidx):
pts[lidx, ...] *= (1 - alpha)
pts[lidx, ...] += alpha * h[lidx, ...]
# Print indicator telling operator what we did
progress("%7.2f %5d " % (now() - T0, nmsg) +
lbl[didx + 2 * lidx + 4 * fidx].tostring(),
sameLine=True)
#
# Collect the corner tags and estimate homography
nprj = None
try:
roi = array([mean(pts[nm], 0) for nm in corners])
# Homography mapping roi to ref
nprj = fitHomography(roi, ref)
except KeyError, ck:
progress("-- missing corner %s" % str(c))
def set_ang(self, ang):
self.desAng = ang
if "s" in DEBUG:
progress("%s.set_angle(%g)" % (self.servo.name, ang))
def onEvent( self, evt ):
# periodically, show the sensor reading we got from the waypointServer
if self.timeForStatus():
self.showSensors()
print("CurrPos(Camera): " + str(self.core.coordinateFrames.convertRealToCamera(self.robSim.pos)))
print("CurrPos(Real): " + str(self.robSim.pos))
try:
# print("Estimated Pos(Real): " + str(self.core.particleFilter.getState().pos) + "\t" + str(self.core.particleFilter.getState().yaw))
print("CurrPos(waypts): " + str(self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos)) + "\t" + str(self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw()))
print("EstimatedPos(waypts): " + str(self.core.particleFilter.getWaypointState().pos) + "\t" + str(self.core.particleFilter.getWaypointState().yaw))
except:
pass
progress( self.robSim.logLaserValue(self.now) )
# logging
# if (self.core.coordinateFrames.waypoint != None):
# rotatedLength = CoordinateFrames.rotateCCW([Constants.tagLength, 0], self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw())
# posWaypoint = self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos)
# frontSensor = posWaypoint[1] + rotatedLength[1]
# backSensor = posWaypoint[1] - rotatedLength[1]
# if (self.sensor.lastSensor[1] > 5 and self.sensor.lastSensor[2] > 5):
# self.logFile.write(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]))
# self.logFile.write(str([frontSensor, backSensor]) + "\n")
# print(rotatedLength)
# print(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]) + str([frontSensor, backSensor]))
# generate simulated laser readings
elif self.timeForLaser():
self.robSim.logLaserValue(self.now)
# update the robot and simulate the tagStreamer
if self.timeForFrame():
self.emitTagMessage()
if self.timeForFilter():
self.core.setSensorAndWaypoints(array([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]),
self.sensor.lastWaypoints[1])
if self.timeForAuto() and self.auto:
self.core.autonomousPlanner.plan(self.sensor.lastWaypoints[1])
if evt.type == KEYDOWN:
if evt.key == K_UP:
self.core.movePosY()
return progress("(say) Up")
elif evt.key == K_DOWN:
self.core.moveNegY()
return progress("(say) Down")
elif evt.key == K_LEFT:
self.core.moveNegX()
return progress("(say) Left")
elif evt.key == K_RIGHT:
self.core.movePosX()
return progress("(say) Right")
elif evt.key == K_RETURN:
self.startedFilter = not self.startedFilter
if (self.startedFilter):
self.core.startFilter()
return progress("(say) Started Filter")
else:
self.core.stopFilter()
return progress("(say) Stopped Filter")
elif evt.key == K_TAB:
self.auto = ~self.auto
if self.auto:
return progress("(say) Autonomous On")
else:
return progress("(say) Autonomous Off")
else:
return JoyApp.onEvent(self,evt)
return # ignoring non-KEYDOWN events
def behavior(self):
progress('Move started!')
self.syncArm()
corner = self.app.square_w[0]
currentPos = self.app.idealArm.getTool(self.ang)
self.steps = linspace(currentPos,corner,15)[:,:-1]
for stepCount,step in enumerate(self.steps[:-1]):
progress('Step #%d, %s' % (stepCount,str(step)))
self.app.currStep = stepCount + 1
Jt = self.app.idealArm.getToolJac(self.ang) #takes in angle as radian
d = self.steps[stepCount+1] - step #distance betwen current position and next position
angDiff = dot(pinv(Jt)[:,:len(d)],d)
progress('Angle Diff 1: %s'% str(angDiff*180/3.14159)) #show angle diff in degrees
for i,angle in enumerate(angDiff):
progress('its happening')
print(angle)
if angle > 3.1415 :
print('angDiff1: ', angDiff[i])
angDiff[i] = angDiff[i] - 2*3.1415
print('angDiff2: ', angDiff[i])
elif angle < -3.1415:
print('angDiff1: ', angDiff[i])
angDiff[i] = angDiff[i] + 2*3.1415
print('angDiff2: ', angDiff[i])
progress('Angle Diff 2: %s'% str(angDiff*180/3.14159)) #show angle diff in degrees
self.ang += angDiff
for i,motor in enumerate(self.app.arm):
motor.set_pos(self.ang[i]*18000./3.14159) #feed in angle to set_pos as centidegrees
#
# progress('Step #%d, %s' % (stepCount,str(step)))
# progress('Distance %s' % (str(d)))
# progress('Angle Diff: %s'% str(angDiff*180/3.14159)) #show angle diff in degrees
# progress('Angles: %s'% str(self.ang*180/3.14159)) #show angles in degrees
yield self.forDuration(7)
progress('Move complete')
def onEvent(self, evt):
# periodically, show the sensor reading we got from the waypointServer
if self.timeForStatus():
self.showSensors()
print(
"CurrPos(Camera): " + str(
self.core.coordinateFrames.convertRealToCamera(
self.robSim.pos)))
print("CurrPos(Real): " + str(self.robSim.pos))
try:
# print("Estimated Pos(Real): " + str(self.core.particleFilter.getState().pos) + "\t" + str(self.core.particleFilter.getState().yaw))
print(
"CurrPos(waypts): " + str(
self.core.coordinateFrames.convertRealToWaypoint(
self.robSim.pos)) + "\t" +
str(self.robSim.yaw -
self.core.coordinateFrames.getRealToWaypointYaw()))
print("EstimatedPos(waypts): " +
str(self.core.particleFilter.getWaypointState().pos) +
"\t" +
str(self.core.particleFilter.getWaypointState().yaw))
except:
pass
progress(self.robSim.logLaserValue(self.now))
# logging
# if (self.core.coordinateFrames.waypoint != None):
# rotatedLength = CoordinateFrames.rotateCCW([Constants.tagLength, 0], self.robSim.yaw - self.core.coordinateFrames.getRealToWaypointYaw())
# posWaypoint = self.core.coordinateFrames.convertRealToWaypoint(self.robSim.pos)
# frontSensor = posWaypoint[1] + rotatedLength[1]
# backSensor = posWaypoint[1] - rotatedLength[1]
# if (self.sensor.lastSensor[1] > 5 and self.sensor.lastSensor[2] > 5):
# self.logFile.write(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]))
# self.logFile.write(str([frontSensor, backSensor]) + "\n")
# print(rotatedLength)
# print(str([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]) + str([frontSensor, backSensor]))
# generate simulated laser readings
elif self.timeForLaser():
self.robSim.logLaserValue(self.now)
# update the robot and simulate the tagStreamer
if self.timeForFrame():
self.emitTagMessage()
if self.timeForFilter():
self.core.setSensorAndWaypoints(
array([self.sensor.lastSensor[1], self.sensor.lastSensor[2]]),
self.sensor.lastWaypoints[1])
if self.timeForAuto() and self.auto:
self.core.autonomousPlanner.plan(self.sensor.lastWaypoints[1])
if evt.type == KEYDOWN:
if evt.key == K_UP:
self.core.movePosY()
return progress("(say) Up")
elif evt.key == K_DOWN:
self.core.moveNegY()
return progress("(say) Down")
elif evt.key == K_LEFT:
self.core.moveNegX()
return progress("(say) Left")
elif evt.key == K_RIGHT:
self.core.movePosX()
return progress("(say) Right")
elif evt.key == K_RETURN:
self.startedFilter = not self.startedFilter
if (self.startedFilter):
self.core.startFilter()
return progress("(say) Started Filter")
else:
self.core.stopFilter()
return progress("(say) Stopped Filter")
elif evt.key == K_TAB:
self.auto = ~self.auto
if self.auto:
return progress("(say) Autonomous On")
else:
return progress("(say) Autonomous Off")
else:
return JoyApp.onEvent(self, evt)
return # ignoring non-KEYDOWN events
for d in dat:
nm = d['i']
if not nm in allow:
continue
p = asarray(d['p']) / 100
c[nm] = mean(p, 0)
h[nm] = p
acc.append(str(nm))
# Collect the corner tags
try:
roi = array([[c[nm][0], c[nm][1], 1] for nm in corners])
except KeyError, ck:
progress("-- missing corner %d" % ck)
yield
continue
progress(",".join(acc))
# Homography mapping roi to ref
prj = fitHomography(roi, ref)
mrk = dot(roi, prj)
mrk = mrk[:, :2] / mrk[:, [-1]]
print
# display it
fig.clf()
fa = fig.gca()
# Mark the corner tags
fa.plot(mrk[:, 0], mrk[:, 1], 'sc', ms=15)
ang0 = [-1 + 1j, 1 + 1j, 1 - 1j, -1 - 1j]
# Loop on all tags
for nm, p in h.iteritems():
# Project back
a = dot(c_[p, [1] * len(p)], prj)
def _animation(fig):
global s
s = socket(AF_INET, SOCK_DGRAM)
s.bind(("", 0xB00))
s.setblocking(0)
lst = []
msg = None
rh = {}
i = 0
# Corners of the arena, in order
corners = [26, 23, 27, 22, 29, 24, 28, 25]
ref = array([[-1, 0, 1, 1, 1, 0, -1, -1], [1, 1, 1, 0, -1, -1, -1, 0],
[1.0 / 100] * 8]).T * 100
ax = array(
[min(ref[:, 0]),
max(ref[:, 0]),
min(ref[:, 1]),
max(ref[:, 1])]) * 1.2
allow = set(corners + [12, 14, 13, 15])
fr = 0
while True: #number of samples
try:
while True:
# read data as fast as possible
msg = s.recv(1 << 16)
except SocketError, se:
# until we've run out; last message remains in m
pass
# make sure we got something
if not msg:
yield
continue
# Parse tag information from UDP packet
dat = json_loads(msg)
# Make sure there are enough tags to make sense
if len(dat) < 5:
yield
continue
# Collect allowed tags
c = {}
h = {}
acc = []
for d in dat:
nm = d['i']
if not nm in allow:
continue
p = asarray(d['p']) / 100
c[nm] = mean(p, 0)
h[nm] = p
acc.append(str(nm))
# Collect the corner tags
try:
roi = array([[c[nm][0], c[nm][1], 1] for nm in corners])
except KeyError, ck:
progress("-- missing corner %d" % ck)
yield
continue
def behavior(self):
"""
Plan main loop
"""
while not self.stop:
ts, f, b = self.sp.lastSensor
ts_w, w = self.sp.lastWaypoints
# comment below out when filter finished
#self.filterState( ts, f, b)
progress("(say)f: " + str(f))
progress("(say)b: " + str(b))
progress("(say)w: " + str(w))
if ts > self.stateInfo["ts"]:
if (f < SENSE_THRESH or b < SENSE_THRESH):
if (len(w) == 4 and f == None or b == None):
# Sensors not orthogonal and no delta info available
# Should be for initial movement from 1st waypoint
self.moveP.torque = MOVE_TORQUE
yield self.moveP
progress("(say) Hunting")
logging.info('Hunting Mode. State info (f: ' + str(f) +
' b: ' + str(b) + 'w: ' + str(w) + ')')
#if (f < SENSE_THRESH and b >= SENSE_THRESH):
if (f < b):
self.turnP.torque = RIGHT_TORQUE
self.moveP.torque = MOVE_TORQUE
yield self.turnP
yield self.turnP
yield self.moveP
progress("(say) Turning right")
self.stateInfo["angle"] += TURN_RES
logging.info(
'Off-Line, Turning Right. State info (f: ' +
str(f) + ' b: ' + str(b) + 'w: ' + str(w) + ')')
#elif (b < SENSE_THRESH and f >= SENSE_THRESH):
elif (b < f):
self.turnP.torque = LEFT_TORQUE
self.moveP.torque = MOVE_TORQUE
yield self.turnP
yield self.turnP
yield self.moveP
progress("(say) Turning left")
self.stateInfo["angle"] -= TURN_RES
logging.info(
'Off-line, turning left. State info (f: ' +
str(f) + ' b: ' + str(b) + 'w: ' + str(w) + ')')
else:
self.turnP.torque = 2 * TURN_TORQUE
self.moveP.torque = 2 * MOVE_TORQUE
yield self.turnP
yield self.moveP
progress("(say) Turning left")
self.stateInfo["angle"] += 2 * TURN_RES
logging.info('Off-line, else. State info (f: ' +
str(f) + ' b: ' + str(b) + 'w: ' +
str(w) + ')')
elif (f > SENSE_THRESH and b > SENSE_THRESH):
dist_dif = f - b
sensor_sum = f + b
if (f > ON_LINE and b > ON_LINE):
self.moveP.torque = MOVE_TORQUE
yield self.moveP
progress("(say) On the line")
logging.info(
'On-line, moving straight. State info (f: ' +
str(f) + ' b: ' + str(b) + 'w: ' + str(w) + ')')
elif (f > ON_LINE and b < ON_LINE):
self.turnP.torque = LEFT_TORQUE
self.moveP.torque = MOVE_TORQUE
yield self.turnP
yield self.moveP
progress("(say) unsure 1")
self.stateInfo["angle"] += TURN_RES
logging.info('On-line, turning left. State info (f: ' +
str(f) + ' b: ' + str(b) + 'w: ' +
str(w) + ')')
elif (f < ON_LINE and b > ON_LINE):
self.turnP.torque = RIGHT_TORQUE
self.moveP.torque = MOVE_TORQUE
yield self.turnP
yield self.moveP
progress("(say) unsure 2")
self.stateInfo["angle"] += TURN_RES
logging.info(
'On-line, turning right. State info (f: ' +
str(f) + ' b: ' + str(b) + 'w: ' + str(w) + ')')
# pause after every action because there is sensor lag
yield self.forDuration(self.wait)
self.updateState(ts, f, b, w)
yield
