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