def __init__(self, wphAddr=WAYPOINT_HOST, *arg, **kw):
JoyApp.__init__(
self,
confPath="$/cfg/JoyApp.yml",
)
self.srvAddr = (wphAddr, APRIL_DATA_PORT)
self.auto = False
def __init__(self,wphAddr=WAYPOINT_HOST,*arg,**kw):
"""
Initialize the simulator
"""
JoyApp.__init__( self,
confPath="$/cfg/JoyApp.yml", *arg, **kw
)
self.srvAddr = (wphAddr, APRIL_DATA_PORT)
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 __init__(self):
JoyApp.__init__( self, confPath="$/cfg/JoyApp.yml",)
#self.c = ckbot.logical.Cluster() #program recognises how many and which modules are attached
self.nservos = 4
self.watchdogTick = 2
self.c = ckbot.logical.Cluster()
self.c.populate(3,{ 0x03 : 'D0', 0x04 : 'D1', 0x0B : 'D3'} )
#for i in range(0,self.nservos):
#self.c.populate() # node IDs of modules are stored
self.state = (0.,0., 0.,0.) # State (x,y,theta,phi)
#TODO: Check for motor enumeration
#self.dMot = [self.c.at.Nx03, self.c.at.Nx04, self.c.at.Nx0B] # Drive motors
#self.lMot = self.c.at.Nx08 # Laser Turret
self.watchdogTime = self.onceEvery(4)
self.controls = {'up':False, 'down':False, 'left':False, 'right':False, 'cw':False, 'ccw':False}
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 __init__(self, wlc, Tws2w, Tp2ws, *arg, **kw):
if 'cfg' not in kw:
kw['cfg'] = {}
kw['cfg'].update(windowSize=[1200, 800])
JoyApp.__init__(self, *arg, **kw)
self.arm = ArmSim(wlc)
self.Tp2w = dot(Tws2w, Tp2ws)
# World to paper
self.Tw2p = inv(self.Tp2w)
# Paper with origin at origin
self.paper_p = asarray([[8, 11, -1 / 2.56, 1]]) * xyzCube
self.paper_w = dot(self.paper_p, self.Tp2w.T)
# Workspace box
L = 12 # Workspace in arm units
self.ws_w = dot(asarray([[L, L, L, 1]]) * xyzCube, Tws2w.T)
self.ws_p = dot(self.ws_w, self.Tw2p.T)
# Projection onto paper in world
self.Tprj = dot(self.Tp2w, asarray([[1, 1, 0, 1]]).T * self.Tw2p)
# World to relative paper (i.e. paper is unit cube)
self.Tw2rp = self.Tw2p / self.paper_p[-1][:, newaxis]
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 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 onEvent(self, evt):
"""
The keyboard row: asdfghjkl moves your motors one way
The keyboard row: zxcvbnm,. moves your motors the other way
'q' will quit and store the results in a results.png image and results.csv
file.
"""
# Ignore everything except keydown events
if evt.type != KEYDOWN:
return
# Punt exit keys
if evt.key in {K_q, K_ESCAPE}:
return JoyApp.onEvent(self, evt)
# row of 'a' on QWERTY keyboard increments motors
p = "asdfghjkl".find(evt.unicode)
if p >= 0:
self.arm[p].set_pos(self.arm[p].get_goal() + 500)
return
# row of 'z' in QWERTY keyboard decrements motors
p = "zxcvbnm,.".find(evt.unicode)
if p >= 0:
self.arm[p].set_pos(self.arm[p].get_goal() - 500)
return
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 __init__(self, camera):
self.ncam = camera
self.controls = {'up':False, 'down':False, 'left':False, 'right':False, 'cw':False, 'ccw':False}
JoyApp.__init__( self, confPath="$/cfg/JoyApp.yml")
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 onEvent(self, evt):
if self.oe():
progress("OE:" + str(self.dt.blobs[-1:]))
# Punt keydown events to superclass
if evt.type == KEYDOWN:
return JoyApp.onEvent(evt)
def onEvent(self, evt):
# periodically, show the sensor reading we got from the waypointServer
if self.timeForStatus():
self.showSensors()
return JoyApp.onEvent(self, evt)
def __init__(self,*arg,**kw):
if 'cfg' not in kw:
kw['cfg'] = dict(remote=False)
else:
kw['cfg'].update(remote=False)
return JoyApp.__init__(self,*arg,**kw)
def onEvent( self , evt ): #print 'Event Occurred', str(evt) if self.timeForControl(): # Print self.currState = self.vplan.getState() # Get latest state feedback from vision system self.printState() waypointsRaw = np.float32(self.sensor.lastWaypoints[1]) waypoints = self.swapWaypointXY(waypointsRaw) print 'Waypoints:',str(waypoints) self.vplan.setWaypoints(waypoints)# Pass waypoints to vision system self.tagEst = self.robotTagLocation() print 'Tag Estimate', str(self.tagEst) #self.vplan.setTagLocation(self.tagEst) #print 'Current state:', str(currState) if(self.opState == DoritoState.RUNNING): # Only update commands while running print 'Handling Control Update' print 'Found', len(waypoints), ' waypoints' if(len(waypoints) > 0): # If waypoint exists currWp = waypoints[1] print 'Next Waypoint:', str(currWp) # tError, rError = self.controlHandler(currWp) # Run control handler print 'X-Y Error', tError #self.vplan.setControlVectorRender(self.tagEst, self.tagEst + tError) print 'Rotation Error', rError else: print 'No more waypoints available, holding' if(self.servoErrorFlag == False): print self.servoErrorFlag #self.drive.setSpeed(np.asfarray([0.,0.]), 0) # Send command requests to the motion drive# Stop holdWP = vu.toWaypointCoordinates( np.array( [self.currState['tagX'],self.currState['tagY']] )) # Current tag location in waypoint space self.controlHandler(holdWP)# Run controller to hold orientation # Manual control handling if evt.type == KEYUP: if evt.key == K_UP: self.controls['up'] = False elif evt.key == K_DOWN: self.controls['down'] = False elif evt.key == K_LEFT: self.controls['left'] = False elif evt.key == K_RIGHT: self.controls['right'] = False elif evt.key == K_PAGEUP: self.controls['ccw'] = False elif evt.key == K_PAGEDOWN: self.controls['cw'] = False self._parseControls() if evt.type == KEYDOWN: if evt.key == K_UP: self.controls['up'] = True elif evt.key == K_DOWN: self.controls['down'] = True elif evt.key == K_LEFT: self.controls['left'] = True elif evt.key == K_RIGHT: self.controls['right'] = True elif evt.key == K_PAGEUP: self.controls['ccw'] = True elif evt.key == K_PAGEDOWN: self.controls['cw'] = True elif evt.key == K_r: # Run mode print '=== Set mode to Run ===\n\n' self.opState = DoritoState.RUNNING elif evt.key == K_s: # E-stop self.opState = DoritoState.STOPPED if(self.servoErrorFlag == False): self.drive.stopAll() elif evt.key == K_m: # Manual drive self.opState = DoritoState.MANUAL self._parseControls() return JoyApp.onEvent(self,evt)
def __init__(self, *arg, **kw):
JoyApp.__init__(self, *arg, **kw)
def __init__(self,wphAddr=WAYPOINT_HOST,*arg,**kw):
JoyApp.__init__( self,
confPath="$/cfg/JoyApp.yml",
)
self.srvAddr = (wphAddr, APRIL_DATA_PORT)
def onEvent( self, evt ):
if self.watchdogTime():
self.watchdogTick = self.watchdogTick - 1
if(self.watchdogTick == 0):
self._setMotorCommand(self.c.at.D0,0)
self._setMotorCommand(self.c.at.D1,0)
self._setMotorCommand(self.c.at.D2,0)
# TODO: watchdog
if evt.type == KEYUP:
self.watchdogTick = 2
if evt.key == K_UP:
self.controls['up'] = False
#self.setSpeed((0,1),0)
elif evt.key == K_DOWN:
self.controls['down'] = False
#self.setSpeed((0,-1),0)
elif evt.key == K_LEFT:
self.controls['left'] = False
#self.setSpeed((-1,0),0)
elif evt.key == K_RIGHT:
self.controls['right'] = False
#self.setSpeed((1,0),0)
elif evt.key == K_PAGEUP:
self.controls['ccw'] = False
#self.setSpeed((0,0),1)
elif evt.key == K_PAGEDOWN:
self.controls['cw'] = False
#self.setSpeed((0,0),-1)
self._parseControls()
'''
if evt.key == K_UP or \
evt.key == K_DOWN or \
evt.key == K_LEFT or \
evt.key == K_RIGHT or \
evt.key == K_PAGEUP or \
evt.key == K_PAGEDOWN:
self.setDriveMotorCommands([0,0,0])
'''
if evt.type == KEYDOWN:
self.watchdogTick = 2
if evt.key == K_UP:
self.controls['up'] = True
#self.setSpeed((0,1),0)
elif evt.key == K_DOWN:
self.controls['down'] = True
#self.setSpeed((0,-1),0)
elif evt.key == K_LEFT:
self.controls['left'] = True
#self.setSpeed((-1,0),0)
elif evt.key == K_RIGHT:
self.controls['right'] = True
#self.setSpeed((1,0),0)
elif evt.key == K_PAGEUP:
self.controls['ccw'] = True
#self.setSpeed((0,0),1)
elif evt.key == K_PAGEDOWN:
self.controls['cw'] = True
#self.setSpeed((0,0),-1)
self._parseControls()
# Use superclass to show any other events
return JoyApp.onEvent(self,evt)
def onEvent(self, evt):
if evt.type == KEYDOWN:
return JoyApp.onEvent(self, evt)
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
