def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotSystem = robotsystem.create(self.view) self.config = drcargs.getDirectorConfig() jointGroups = self.config['teleopJointGroups'] self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups) self.jointCommandPanel = JointCommandPanel(self.robotSystem) self.jointCommandPanel.ui.speedSpinBox.setEnabled(False) self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms) self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs) self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders) self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged) self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged) self.widget = QtGui.QWidget() gl = QtGui.QGridLayout(self.widget) gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan gl.addWidget(self.view, 0, 1, 1, 1) gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1) gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1) gl.setRowStretch(0,1) gl.setColumnStretch(1,1) #self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan) lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal)
def main(): app = ConsoleApp() camVis = CameraVisualizer() camVis.createUI() camVis.showUI() app.start()
def main(): app = ConsoleApp() view = app.createView() vis_item = {"current": None} def handle_data(msg): # msg = lcmdrake.lcmt_viewer_geometry_data.decode(msg_data) side_length = int(round(np.power(len(msg.float_data), 1. / 3))) data = np.reshape(msg.float_data, (side_length, side_length, side_length)) # convert to a vtkImageData image = numpyToVtkImage(data) # compute iso contour as value 0.5 polyData = applyContourFilter(image, 0.0) # show data if vis_item["current"] is not None: vis_item["current"].removeFromAllViews() vis_item["current"] = vis.showPolyData(polyData, 'contour') view.resetCamera() lcmUtils.addSubscriber("FIELD_DATA", lcmdrake.lcmt_viewer_geometry_data, handle_data) # start app view.show() view.resetCamera() app.start()
def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setZeroPose() self.view.show() self.sub = lcmUtils.addSubscriber('ATLAS_COMMAND', lcmbotcore.atlas_command_t, self.onAtlasCommand) self.sub.setSpeedLimit(60)
def main(): app = ConsoleApp() view = app.createView(useGrid=False) imageManager = cameraview.ImageManager() cameraView = cameraview.CameraView(imageManager, view) view.show() app.start()
def __init__(self): self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.robot_state_t, self.onJointPositionGoal) self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause) self.debug = False if self.debug: self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose) self.view.show() self.timer = TimerCallback(targetFps=30) self.timer.callback = self.onDebug
def __init__( self, percentObsDensity=20, endTime=40, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True, ): self.verbose = verbose self.startSimTime = time.time() self.collisionThreshold = 0.4 self.randomSeed = 5 self.Sensor_rayLength = 8 self.percentObsDensity = percentObsDensity self.defaultControllerTime = 1000 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() if autoInitialize: self.initialize() self.XVelocity_drawing = 0.0 self.YVelocity_drawing = 0.0
def __init__(self): self.timer = TimerCallback(targetFps=10) #self.timer.disableScheduledTimer() self.app = ConsoleApp() self.robotModel, self.jointController = roboturdf.loadRobotModel( 'robot model') self.fpsCounter = simpletimer.FPSCounter() self.drakePoseJointNames = robotstate.getDrakePoseJointNames() self.fpsCounter.printToConsole = True self.timer.callback = self._tick self._initialized = False self.publishChannel = 'JOINT_POSITION_GOAL' # self.lastCommandMessage = newAtlasCommandMessageAtZero() self._numPositions = len(robotstate.getDrakePoseJointNames()) self._previousElapsedTime = 100 self._baseFlag = 0 self.jointLimitsMin = np.array([ self.robotModel.model.getJointLimits(jointName)[0] for jointName in robotstate.getDrakePoseJointNames() ]) self.jointLimitsMax = np.array([ self.robotModel.model.getJointLimits(jointName)[1] for jointName in robotstate.getDrakePoseJointNames() ]) self.useControllerFlag = False self.drivingGainsFlag = False self.applyDefaults()
def __init__(self, world): """Constructs the simulator. Args: world: World. """ self._robots = [] self._obstacles = [] self._world = world self._app = ConsoleApp() self._view = self._app.createView(useGrid=False) # performance tracker self._num_targets = 0 self._num_crashes = 0 self._run_ticks = 0 self._initialize()
def robotMain(useDrivingGains=False, useController=False): print 'waiting for robot state...' commandStream.waitForRobotState() print 'starting.' commandStream.timer.targetFps = 1000 if useController==True: commandStream.useController() else: commandStream.publishChannel = 'ROBOT_COMMAND' if useDrivingGains: commandStream.applyDrivingDefaults() commandStream.startStreaming() positionListener = PositionGoalListener() planListener = CommittedRobotPlanListener() ConsoleApp.start()
def robotMain(useDrivingGains=False, useController=False): print 'waiting for robot state...' commandStream.waitForRobotState() print 'starting.' commandStream.timer.targetFps = 1000 if useController == True: commandStream.useController() else: commandStream.publishChannel = 'ROBOT_COMMAND' if useDrivingGains: commandStream.applyDrivingDefaults() commandStream.startStreaming() positionListener = PositionGoalListener() planListener = CommittedRobotPlanListener() ConsoleApp.start()
class PositionGoalListener(object): def __init__(self): self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.robot_state_t, self.onJointPositionGoal) self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause) self.debug = False if self.debug: self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel( 'robot model', self.view) self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose) self.view.show() self.timer = TimerCallback(targetFps=30) self.timer.callback = self.onDebug def onPause(self, msg): commandStream.stopStreaming() def onJointPositionGoal(self, msg): #lcmUtils.publish('ATLAS_COMMAND', msg) commandStream.startStreaming() pose = robotstate.convertStateMessageToDrakePose(msg) self.setGoalPose(pose) def setGoalPose(self, pose): commandStream.setGoalPose(pose) def onDebug(self): self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose) def onSingleJointPositionGoal(self, msg): jointPositionGoal = msg.joint_position jointName = msg.joint_name allowedJointNames = ['l_leg_aky', 'l_arm_lwy'] if not (jointName in allowedJointNames): print 'Position goals are not allowed for this joint' print 'ignoring this position goal' print 'use the sliders instead' return commandStream.setIndividualJointGoalPose(jointPositionGoal, jointName)
def __init__(self, percentObsDensity=20, endTime=40, randomizeControl=False, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, supervisedTrainingTime=500, autoInitialize=True, verbose=True, sarsaType="discrete"): self.verbose = verbose self.randomizeControl = randomizeControl self.startSimTime = time.time() self.collisionThreshold = 1.3 self.randomSeed = 5 self.Sarsa_numInnerBins = 4 self.Sarsa_numOuterBins = 4 self.Sensor_rayLength = 8 self.sarsaType = sarsaType self.percentObsDensity = percentObsDensity self.supervisedTrainingTime = 10 self.learningRandomTime = 10 self.learningEvalTime = 10 self.defaultControllerTime = 10 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() # view = app.createView(useGrid=False) self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() if autoInitialize: self.initialize()
def main(): app = ConsoleApp() view = app.createView(useGrid=False) view.orientationMarkerWidget().Off() view.backgroundRenderer().SetBackground([0,0,0]) view.backgroundRenderer().SetBackground2([0,0,0]) cameraChannel = parseChannelArgument() imageManager = cameraview.ImageManager() imageManager.queue.addCameraStream(cameraChannel) imageManager.addImage(cameraChannel) cameraView = cameraview.CameraImageView(imageManager, cameraChannel, view=view) cameraView.eventFilterEnabled = False view.renderWindow().GetInteractor().SetInteractorStyle(vtk.vtkInteractorStyleImage()) view.show() app.start()
def main(): app = ConsoleApp() view = app.createView(useGrid=False) view.orientationMarkerWidget().Off() view.backgroundRenderer().SetBackground([0, 0, 0]) view.backgroundRenderer().SetBackground2([0, 0, 0]) cameraChannel = parseChannelArgument() imageManager = cameraview.ImageManager() imageManager.queue.addCameraStream(cameraChannel) imageManager.addImage(cameraChannel) cameraView = cameraview.CameraImageView(imageManager, cameraChannel, view=view) cameraView.eventFilterEnabled = False view.renderWindow().GetInteractor().SetInteractorStyle( vtk.vtkInteractorStyleImage()) view.show() app.start()
class DebugAtlasCommandListener(object): def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setZeroPose() self.view.show() self.sub = lcmUtils.addSubscriber('ATLAS_COMMAND', lcmbotcore.atlas_command_t, self.onAtlasCommand) self.sub.setSpeedLimit(60) def onAtlasCommand(self, msg): pose = atlasCommandToDrakePose(msg) self.jointController.setPose('ATLAS_COMMAND', pose)
class PositionGoalListener(object): def __init__(self): self.sub = lcmUtils.addSubscriber('JOINT_POSITION_GOAL', lcmbotcore.robot_state_t, self.onJointPositionGoal) self.sub = lcmUtils.addSubscriber('SINGLE_JOINT_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('COMMITTED_PLAN_PAUSE', lcmdrc.plan_control_t, self.onPause) self.debug = False if self.debug: self.app = ConsoleApp() self.view = self.app.createView() self.robotModel, self.jointController = roboturdf.loadRobotModel('robot model', self.view) self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose) self.view.show() self.timer = TimerCallback(targetFps=30) self.timer.callback = self.onDebug def onPause(self, msg): commandStream.stopStreaming() def onJointPositionGoal(self, msg): #lcmUtils.publish('ATLAS_COMMAND', msg) commandStream.startStreaming() pose = robotstate.convertStateMessageToDrakePose(msg) self.setGoalPose(pose) def setGoalPose(self, pose): commandStream.setGoalPose(pose) def onDebug(self): self.jointController.setPose('ATLAS_COMMAND', commandStream._currentCommandedPose) def onSingleJointPositionGoal(self, msg): jointPositionGoal = msg.joint_position jointName = msg.joint_name allowedJointNames = ['l_leg_aky','l_arm_lwy'] if not (jointName in allowedJointNames): print 'Position goals are not allowed for this joint' print 'ignoring this position goal' print 'use the sliders instead' return commandStream.setIndividualJointGoalPose(jointPositionGoal, jointName)
class Simulator(object): def __init__( self, percentObsDensity=20, endTime=40, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True, ): self.verbose = verbose self.startSimTime = time.time() self.collisionThreshold = 0.4 self.randomSeed = 5 self.Sensor_rayLength = 8 self.percentObsDensity = percentObsDensity self.defaultControllerTime = 1000 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() if autoInitialize: self.initialize() self.XVelocity_drawing = 0.0 self.YVelocity_drawing = 0.0 def initializeOptions(self): self.options = dict() self.options["World"] = dict() self.options["World"]["obstaclesInnerFraction"] = 0.98 self.options["World"]["randomSeed"] = 40 self.options["World"]["percentObsDensity"] = 0.0 self.options["World"]["nonRandomWorld"] = True self.options["World"]["circleRadius"] = 1.0 self.options["World"]["scale"] = 1 self.options["Sensor"] = dict() self.options["Sensor"]["rayLength"] = 10 self.options["Sensor"]["numRays"] = 50 self.options["Car"] = dict() self.options["Car"]["velocity"] = 4.0 self.options["dt"] = 0.05 self.options["runTime"] = dict() self.options["runTime"]["defaultControllerTime"] = 100 def setDefaultOptions(self): defaultOptions = dict() defaultOptions["World"] = dict() defaultOptions["World"]["obstaclesInnerFraction"] = 0.98 defaultOptions["World"]["randomSeed"] = 40 defaultOptions["World"]["percentObsDensity"] = 30 defaultOptions["World"]["nonRandomWorld"] = True defaultOptions["World"]["circleRadius"] = 1.75 defaultOptions["World"]["scale"] = 2.5 defaultOptions["Sensor"] = dict() defaultOptions["Sensor"]["rayLength"] = 10 defaultOptions["Sensor"]["numRays"] = 51 defaultOptions["Car"] = dict() defaultOptions["Car"]["velocity"] = 20 defaultOptions["dt"] = 0.05 defaultOptions["runTime"] = dict() defaultOptions["runTime"]["defaultControllerTime"] = 100 for k in defaultOptions: self.options.setdefault(k, defaultOptions[k]) for k in defaultOptions: if not isinstance(defaultOptions[k], dict): continue for j in defaultOptions[k]: self.options[k].setdefault(j, defaultOptions[k][j]) def initializeColorMap(self): self.colorMap = dict() self.colorMap["default"] = [0, 1, 0] def initialize(self): self.dt = self.options["dt"] self.controllerTypeOrder = ["default"] self.setDefaultOptions() self.Sensor = SensorObj( rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"] ) self.SensorApproximator = SensorApproximatorObj( numRays=self.options["Sensor"]["numRays"], circleRadius=self.options["World"]["circleRadius"] ) self.Controller = ControllerObj(self.Sensor, self.SensorApproximator) self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"]) self.Controller.initializeVelocity(self.Car.v) self.ActionSet = ActionSetObj() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName("world")) self.world = World.buildLineSegmentTestWorld( percentObsDensity=self.options["World"]["percentObsDensity"], circleRadius=self.options["World"]["circleRadius"], nonRandom=self.options["World"]["nonRandomWorld"], scale=self.options["World"]["scale"], randomSeed=self.options["World"]["randomSeed"], obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"], ) om.removeFromObjectModel(om.findObjectByName("robot")) self.robot, self.frame = World.buildRobot() self.frame = self.robot.getChildFrame() self.frame.setProperty("Scale", 3) # self.frame.setProperty('Visible', False) # self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"] self.Car.setFrame(self.frame) print "Finished initialization" def runSingleSimulation(self, controllerType="default", simulationCutoff=None): # self.setRandomCollisionFreeInitialState() self.setInitialStateAtZero() currentCarState = np.copy(self.Car.state) nextCarState = np.copy(self.Car.state) self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2]) firstRaycast = self.Sensor.raycastAll(self.frame) firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame) # self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) # self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) # self.Sensor.setLocator(self.LineSegmentLocator) nextRaycast = np.zeros(self.Sensor.numRays) # record the reward data runData = dict() startIdx = self.counter thisRunIndex = 0 NMaxSteps = 100 while self.counter < self.numTimesteps - 1: thisRunIndex += 1 idx = self.counter currentTime = self.t[idx] self.stateOverTime[idx, :] = currentCarState x = self.stateOverTime[idx, 0] y = self.stateOverTime[idx, 1] self.setRobotFrameState(x, y, 0.0) # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2]) currentRaycast = self.Sensor.raycastAll(self.frame) self.raycastData[idx, :] = currentRaycast S_current = (currentCarState, currentRaycast) if controllerType not in self.colorMap.keys(): print raise ValueError("controller of type " + controllerType + " not supported") if controllerType in ["default", "defaultRandom"]: controlInput, controlInputIdx = self.Controller.computeControlInput( currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=False ) self.controlInputData[idx] = controlInput nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt) print "NEXTCARSTATE is ", nextCarState x = nextCarState[0] y = nextCarState[1] self.setRobotFrameState(x, y, 0.0) nextRaycast = self.Sensor.raycastAll(self.frame) # Compute the next control input S_next = (nextCarState, nextRaycast) if controllerType in ["default", "defaultRandom"]: nextControlInput, nextControlInputIdx = self.Controller.computeControlInput( nextCarState, currentTime, self.frame, raycastDistance=firstRaycast, randomize=False ) # bookkeeping currentCarState = nextCarState currentRaycast = nextRaycast self.counter += 1 # break if we are in collision if self.checkInCollision(nextRaycast): if self.verbose: print "Had a collision, terminating simulation" break if thisRunIndex > NMaxSteps: print "was safe during N steps" break if self.counter >= simulationCutoff: break # fill in the last state by hand self.stateOverTime[self.counter, :] = currentCarState self.raycastData[self.counter, :] = currentRaycast # this just makes sure we don't get stuck in an infinite loop. if startIdx == self.counter: self.counter += 1 return runData def setNumpyRandomSeed(self, seed=1): np.random.seed(seed) def runBatchSimulation(self, endTime=None, dt=0.05): self.controllerTypeOrder = ["default"] self.counter = 0 self.simulationData = [] self.initializeStatusBar() self.idxDict = dict() numRunsCounter = 0 self.idxDict["default"] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.defaultControllerTime / dt, self.numTimesteps) while (self.counter - loopStartIdx < self.defaultControllerTime / dt) and self.counter < self.numTimesteps - 1: self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(controllerType="default", simulationCutoff=simCutoff) runData["startIdx"] = startIdx runData["controllerType"] = "default" runData["duration"] = self.counter - runData["startIdx"] runData["endIdx"] = self.counter runData["runNumber"] = numRunsCounter numRunsCounter += 1 self.simulationData.append(runData) # BOOKKEEPING # truncate stateOverTime, raycastData, controlInputs to be the correct size self.numTimesteps = self.counter + 1 self.stateOverTime = self.stateOverTime[0 : self.counter + 1, :] self.raycastData = self.raycastData[0 : self.counter + 1, :] self.controlInputData = self.controlInputData[0 : self.counter + 1] self.endTime = 1.0 * self.counter / self.numTimesteps * self.endTime def initializeStatusBar(self): self.numTicks = 10 self.nextTickComplete = 1.0 / float(self.numTicks) self.nextTickIdx = 1 print "Simulation percentage complete: (", self.numTicks, " # is complete)" def printStatusBar(self): fractionDone = float(self.counter) / float(self.numTimesteps) if fractionDone > self.nextTickComplete: self.nextTickIdx += 1 self.nextTickComplete += 1.0 / self.numTicks timeSoFar = time.time() - self.startSimTime estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0 print "#" * self.nextTickIdx, "-" * ( self.numTicks - self.nextTickIdx ), "estimated", estimatedTimeLeft_minutes, "minutes left" def setCollisionFreeInitialState(self): tol = 5 while True: x = 0.0 y = -5.0 theta = 0 # + np.random.uniform(0,2*np.pi,1)[0] * 0.01 self.Car.setCarState(x, y, 0.0, 0.0) self.setRobotFrameState(x, y, theta) print "In loop" if not self.checkInCollision(): break return x, y, theta def setInitialStateAtZero(self): x = 0.0 y = 0.0 theta = 0.0 self.Car.setCarState(x, y, 0.0, 0.0) self.setRobotFrameState(x, y, theta) return x, y, theta def setRandomCollisionFreeInitialState(self): tol = 5 while True: x = np.random.uniform(self.world.Xmin + tol, self.world.Xmax - tol, 1)[0] y = np.random.uniform(self.world.Ymin + tol, self.world.Ymax - tol, 1)[0] theta = np.random.uniform(0, 2 * np.pi, 1)[0] self.Car.setCarState(x, y, theta) self.setRobotFrameState(x, y, theta) if not self.checkInCollision(): break return x, y, theta def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0 / self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) self.max_velocity = 20.0 sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderXVelocity.connect("valueChanged(int)", self.onXVelocityChanged) sliderXVelocity.setMaximum(self.max_velocity) sliderXVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderXVelocity) sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderYVelocity.connect("valueChanged(int)", self.onYVelocityChanged) sliderYVelocity.setMaximum(self.max_velocity) sliderYVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderYVelocity) firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0 self.drawFirstIntersections(self.frame, firstRaycast) randomGlobalGoalButton = QtGui.QPushButton("Initialize Random Global Goal") randomGlobalGoalButton.connect("clicked()", self.onRandomGlobalGoalButton) l.addWidget(randomGlobalGoalButton) randomObstaclesButton = QtGui.QPushButton("Initialize Random Obstacles") randomObstaclesButton.connect("clicked()", self.onRandomObstaclesButton) l.addWidget(randomObstaclesButton) buildWorldFromRandomObstaclesButton = QtGui.QPushButton("Generate Polygon World") buildWorldFromRandomObstaclesButton.connect("clicked()", self.onBuildWorldFromRandomObstacles) l.addWidget(buildWorldFromRandomObstaclesButton) findLocalGoalButton = QtGui.QPushButton("Find Local Goal (Heuristic)") findLocalGoalButton.connect("clicked()", self.onFindLocalGoalButton) l.addWidget(findLocalGoalButton) drawActionSetButton = QtGui.QPushButton("Draw Action Set") drawActionSetButton.connect("clicked()", self.onDrawActionSetButton) l.addWidget(drawActionSetButton) runSimButton = QtGui.QPushButton("Simulate") runSimButton.connect("clicked()", self.onRunSimButton) l.addWidget(runSimButton) playButton = QtGui.QPushButton("Play/Pause") playButton.connect("clicked()", self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect("valueChanged(int)", self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider # slider4 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider4.setMaximum(self.sliderMax) # l.addWidget(slider4) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider6 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider6.setMaximum(self.sliderMax) # l.addWidget(slider6) # slider7 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider7.setMaximum(self.sliderMax) # l.addWidget(slider7) l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2, 0, -1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter / elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.onRandomObstaclesButton() self.app.start() def drawFirstIntersections(self, frame, firstRaycast): origin = np.array(frame.transform.GetPosition()) d = DebugData() firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast) for i in xrange(self.Sensor.numRays): endpoint = firstRaycastLocations[i, :] if firstRaycast[i] >= self.Sensor.rayLength - 0.1: d.addLine(origin, endpoint, color=[0, 1, 0]) else: d.addLine(origin, endpoint, color=[1, 0, 0]) vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255") self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) self.locator = self.LineSegmentLocator self.Sensor.setLocator(self.LineSegmentLocator) def updateDrawIntersection(self, frame, locator="None"): if locator == "None": locator = self.locator origin = np.array(frame.transform.GetPosition()) # print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) # print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), "rays", colorByName="RGB255") # camera = self.view.camera() # camera.SetFocalPoint(frame.transform.GetPosition()) # camera.SetPosition(frame.transform.TransformPoint((-30,0,10))) def getControllerTypeFromCounter(self, counter): name = self.controllerTypeOrder[0] for controllerType in self.controllerTypeOrder[1:]: if counter >= self.idxDict[controllerType]: name = controllerType return name def setRobotFrameState(self, x, y, theta): t = vtk.vtkTransform() t.Translate(x, y, 0.0) t.RotateZ(np.degrees(theta)) self.robot.getChildFrame().copyFrame(t) # returns true if we are in collision def checkInCollision(self, raycastDistance=None): if raycastDistance is None: self.setRobotFrameState(self.Car.state[0], self.Car.state[1], self.Car.state[2]) raycastDistance = self.Sensor.raycastAll(self.frame) if np.min(raycastDistance) < self.collisionThreshold: return True else: return False # if raycastDistance[(len(raycastDistance)+1)/2] < self.collisionThreshold: # return True # else: # return False def tick(self): # print timer.elapsed # simulate(t.elapsed) x = np.sin(time.time()) y = np.cos(time.time()) self.setRobotFrameState(x, y, 0.0) if (time.time() - self.playTime) > self.endTime: self.playTimer.stop() def tick2(self): newtime = time.time() - self.playTime print time.time() - self.playTime x = np.sin(newtime) y = np.cos(newtime) self.setRobotFrameState(x, y, 0.0) # just increment the slider, stop the timer if we get to the end def playTimerCallback(self): self.sliderMovedByPlayTimer = True currentIdx = self.slider.value nextIdx = currentIdx + 1 self.slider.setSliderPosition(nextIdx) if currentIdx >= self.sliderMax: print "reached end of tape, stopping playTime" self.playTimer.stop() def onSliderChanged(self, value): if not self.sliderMovedByPlayTimer: self.playTimer.stop() numSteps = len(self.stateOverTime) idx = int(np.floor(numSteps * (1.0 * value / self.sliderMax))) idx = min(idx, numSteps - 1) x, y, xdot, ydot = self.stateOverTime[idx] self.setRobotFrameState(x, y, 0) self.sliderMovedByPlayTimer = False def onXVelocityChanged(self, value): self.XVelocity_drawing = value self.onDrawActionSetButton() print "x velocity changed to ", value def onYVelocityChanged(self, value): print value self.YVelocity_drawing = -value self.onDrawActionSetButton() print "y velocity changed to ", -value def onShowSensorsButton(self): print "I pressed the show sensors button" self.setInitialStateAtZero() firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0 self.drawFirstIntersections(self.frame, firstRaycast) def onRandomObstaclesButton(self): print "random obstacles button pressed" self.setInitialStateAtZero() self.world = World.buildLineSegmentTestWorld( percentObsDensity=8.0, circleRadius=self.options["World"]["circleRadius"], nonRandom=False, scale=self.options["World"]["scale"], randomSeed=self.options["World"]["randomSeed"], obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"], ) self.locator = World.buildCellLocator(self.world.visObj.polyData) self.Sensor.setLocator(self.locator) self.updateDrawIntersection(self.frame, locator=self.locator) def onRandomGlobalGoalButton(self): print "random global goal button pressed" self.globalGoal = World.buildGlobalGoal() def onBuildWorldFromRandomObstacles(self): distances = self.Sensor.raycastAll(self.frame) firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, distances) self.polygon_initial_distances = distances self.polygon_initial_raycastLocations = firstRaycastLocations self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) self.locator = self.LineSegmentLocator self.Sensor.setLocator(self.LineSegmentLocator) self.updateDrawIntersection(self.frame) def onFindLocalGoalButton(self): print "find local goal button pressed" local_goal = self.findLocalGoal() print local_goal, "is my local goal" self.localGoal = World.placeLocalGoal(local_goal) def findLocalGoal(self): biggest_gap_width = 0 biggest_gap_start_index = 0 current_gap_width = 0 current_gap_start_index = 0 for index, value in enumerate(self.polygon_initial_distances): # if still in a gap if value == self.Sensor.rayLength: current_gap_width += 1 # else terminate counting for this gap else: if current_gap_width > biggest_gap_width: biggest_gap_width = current_gap_width biggest_gap_start_index = current_gap_start_index current_gap_width = 0 current_gap_start_index = index + 1 if current_gap_width > biggest_gap_width: biggest_gap_width = current_gap_width biggest_gap_start_index = current_gap_start_index middle_index_of_gap = biggest_gap_start_index + biggest_gap_width / 2 return self.polygon_initial_raycastLocations[middle_index_of_gap, :] def onDrawActionSetButton(self): print "drawing action set" # self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing) self.ActionSet.computeAllPositions(self.XVelocity_drawing, self.YVelocity_drawing) # self.ActionSet.drawActionSetFinal() self.ActionSet.drawActionSetFull() def onRunSimButton(self): self.runBatchSimulation() self.saveToFile("latest") def onPlayButton(self): if self.playTimer.isActive(): self.onPauseButton() return print "play" self.playTimer.start() self.playTime = time.time() def onPauseButton(self): print "pause" self.playTimer.stop() def saveToFile(self, filename): # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime filename = "data/" + filename + ".out" my_shelf = shelve.open(filename, "n") my_shelf["options"] = self.options my_shelf["simulationData"] = self.simulationData my_shelf["stateOverTime"] = self.stateOverTime my_shelf["raycastData"] = self.raycastData my_shelf["controlInputData"] = self.controlInputData my_shelf["numTimesteps"] = self.numTimesteps my_shelf["idxDict"] = self.idxDict my_shelf["counter"] = self.counter my_shelf.close() def run(self, launchApp=True): self.counter = 1 # for use in playback self.dt = self.options["dt"] self.endTime = self.defaultControllerTime # used to be the sum of the other times as well self.t = np.arange(0.0, self.endTime, self.dt) maxNumTimesteps = np.size(self.t) self.stateOverTime = np.zeros((maxNumTimesteps + 1, 4)) self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays)) self.controlInputData = np.zeros((maxNumTimesteps + 1, 2)) self.numTimesteps = maxNumTimesteps # self.runBatchSimulation() if launchApp: self.setupPlayback() @staticmethod def loadFromFile(filename): filename = "data/" + filename + ".out" sim = Simulator(autoInitialize=False, verbose=False) my_shelf = shelve.open(filename) sim.options = my_shelf["options"] sim.initialize() sim.simulationData = my_shelf["simulationData"] sim.stateOverTime = np.array(my_shelf["stateOverTime"]) sim.raycastData = np.array(my_shelf["raycastData"]) sim.controlInputData = np.array(my_shelf["controlInputData"]) sim.numTimesteps = my_shelf["numTimesteps"] sim.idxDict = my_shelf["idxDict"] sim.counter = my_shelf["counter"] my_shelf.close() return sim
import os import math from director.consoleapp import ConsoleApp from director import ioUtils from director import segmentation from director import segmentationroutines from director import applogic from director import visualization as vis from director import roboturdf app = ConsoleApp() # create a view view = app.createView() segmentation._defaultSegmentationView = view robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True) segmentationroutines.SegmentationContext.initWithRobot(robotStateModel) # Move robot to near to table: robotStateJointController.q [5] = math.radians(120) robotStateJointController.q[0] = -1.5 robotStateJointController.q[1] = 2 robotStateJointController.q[2] = 0.95 robotStateJointController.push() # load poly data dataDir = app.getTestingDataDirectory() polyData = ioUtils.readPolyData(os.path.join(dataDir, 'tabletop/table-and-bin-scene.vtp')) vis.showPolyData(polyData, 'pointcloud snapshot')
from director.consoleapp import ConsoleApp from director import lcmspy from director import lcmUtils from director import simpletimer as st app = ConsoleApp() app.setupGlobals(globals()) if app.getTestingInteractiveEnabled(): app.showPythonConsole() lcmspy.findLCMModulesInSysPath() timer = st.SimpleTimer() stats = {} channelToMsg = {} items = {} def item(r, c): rowDict = items.setdefault(r, {}) try: return rowDict[c] except KeyError: i = QtGui.QTableWidgetItem('') table.setItem(r, c, i) rowDict[c] = i return i
timeNow = time.time() elapsed = timeNow - startTime if elapsed > 1.0: view.forceRender() print '%d samples/sec' % (sampleCount / elapsed), '%d total samples' % totalSampleCount startTime = timeNow sampleCount = 0 if app.getTestingEnabled(): assert badSampleCount == 0 app.quit() app = ConsoleApp() app.setupGlobals(globals()) view = app.createView() view.show() robotSystem = robotsystem.create(view, planningOnly=True) view.resetCamera() if robotSystem.ikPlanner.planningMode == 'pydrake': robotSystem.ikPlanner.plannerPub._setupLocalServer() runTest() elif robotSystem.ikPlanner.planningMode == 'matlabdrake': robotSystem.ikServer.connectStartupCompleted(onMatlabStartup) robotSystem.startIkServer()
d.addSphere(pickedPoint, radius=0.01) d.addLine(pickedPoint, np.array(pickedPoint) + 0.1 * np.array(normal), radius=0.005) obj = vis.updatePolyData(d.getPolyData(), 'link selection', color=[0,1,0]) obj.actor.SetPickable(False) self.linkName = linkName self.pickedPoint = pickedPoint def onMousePress(self, displayPoint, modifiers=None): print self.linkName, self.pickedPoint ########################### app = ConsoleApp() app.setupGlobals(globals()) view = app.createView() view.show() view.resize(1080, 768) robotModel, jointController = roboturdf.loadRobotModel('robot model', view, parent='scene', color=roboturdf.getRobotGrayColor(), visible=True) robotModel.addToView(view) widget = LinkWidget(view, robotModel) widget.start() app.viewOptions.setProperty('Gradient background', False) app.viewOptions.setProperty('Background color', [1,1,1]) app.viewOptions.setProperty('Orientation widget', False)
class Simulator(object): def __init__(self, percentObsDensity=20, endTime=40, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True): self.verbose = verbose self.startSimTime = time.time() self.collisionThreshold = 1.0 self.randomSeed = 5 self.Sensor_rayLength = 8 self.percentObsDensity = percentObsDensity self.defaultControllerTime = 1000 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() self.initial_XVelocity = 0.0 self.initial_YVelocity = 0.0 self.running_sim = True self.currentIdx = 0; if autoInitialize: self.initialize() def initializeOptions(self): self.options = dict() self.options['World'] = dict() self.options['World']['obstaclesInnerFraction'] = 0.98 self.options['World']['randomSeed'] = 40 self.options['World']['percentObsDensity'] = 25.0 self.options['World']['nonRandomWorld'] = True self.options['World']['circleRadius'] = 0.6 self.circleRadius = self.options['World']['circleRadius'] self.options['World']['scale'] = 2.0 self.options['Sensor'] = dict() self.options['Sensor']['rayLength'] = 10 self.options['Sensor']['numRays'] = 40 self.options['Car'] = dict() self.options['Car']['velocity'] = 20 self.options['dt'] = 0.05 self.options['runTime'] = dict() self.options['runTime']['defaultControllerTime'] = 100 def initializeColorMap(self): self.colorMap = dict() self.colorMap['default'] = [0,1,0] def initialize(self): self.dt = self.options['dt'] self.controllerTypeOrder = ['default'] self.Controller = ControllerObj() self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'], numRays=self.options['Sensor']['numRays']) self.Car = CarPlant(controller=self.Controller, velocity=self.options['Car']['velocity']) self.Controller.initializeVelocity(self.Car.v) self.ActionSet = ActionSetObj() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName('world')) self.world = World.buildCircleWorld(percentObsDensity=self.options['World']['percentObsDensity'], circleRadius=self.options['World']['circleRadius'], nonRandom=self.options['World']['nonRandomWorld'], scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction']) om.removeFromObjectModel(om.findObjectByName('robot')) self.robot, self.frame = World.buildRobot() self.locator = World.buildCellLocator(self.world.visObj.polyData) #will need to set locator for purple trajectories #self.Sensor.setLocator(self.locator) self.frame = self.robot.getChildFrame() self.frame.setProperty('Scale', 3) #self.frame.setProperty('Visible', False) #self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.defaultControllerTime = self.options['runTime']['defaultControllerTime'] self.Car.setFrame(self.frame) print "Finished initialization" def terminalEuclideanCostForTrajectories(self): # access the trajectories x_traj = self.ActionSet.p_x_trajectories y_traj = self.ActionSet.p_y_trajectories final_x_positions = x_traj[:,-1] final_y_positions = y_traj[:,-1] #access the global goal global_goal_x = self.globalGoal.global_goal_x global_goal_y = self.globalGoal.global_goal_y euclideans_vector = [] # def getKey(item): # return item[1] for i in xrange(self.ActionSet.num_x_bins): for j in xrange(self.ActionSet.num_y_bins): distance = np.sqrt((final_x_positions[i] - global_goal_x)**2 + (final_y_positions[j] - global_goal_y)**2) euclideans_vector.append(distance) #sorted_ranks_with_indices = sorted(ranks_with_indices,key=getKey) return np.array(euclideans_vector) def CheckIfCollisionFreeTrajectoryIndex(self, traj_index): # accessing the right trajectory x_trajectory_to_check = self.ActionSet.p_x_trajectories[traj_index[0]] y_trajectory_to_check = self.ActionSet.p_y_trajectories[traj_index[1]] #check if anything is too close to the center of the circles #print self.world.list_of_circles for i in range(len(x_trajectory_to_check)): point = ( x_trajectory_to_check[i], y_trajectory_to_check[i] ) if not self.CheckIfCollisionFreePoint(point): #print "trajectory wasn't free" return False return True def CheckIfCollisionFreePoint(self, point): for circle_center in self.world.list_of_circles: #print "circle centers first" #print circle_center[0], circle_center[1] #print self.circleRadius #print point[0], point[1] #print (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2 if ( (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2 < self.circleRadius**2): #print "I found a point in a circle" return False if point[0] > self.world.Xmax: #print "I would have gone past top wall" return False if point[0] < self.world.Xmin: #print "I would have gone below bottom wall" return False if point[1] > self.world.Ymax: #print "I would have gone to the right of the side wall" return False if point[1] < self.world.Ymin: #print "I would have gone to the left of the side wall" return False return True def computeProbabilitiesOfCollisionAllTrajectories(self, currentRaycastIntersectionLocations, speed_allowed_matrix): probability_vector = [] indices_vector = [] for x_index in xrange(self.ActionSet.num_x_bins): for y_index in xrange(self.ActionSet.num_y_bins): if not speed_allowed_matrix[x_index, y_index]: probability_vector.append(1.0) else: probability_of_collision = self.computeProbabilityOfCollisionOneTrajectory(x_index, y_index, currentRaycastIntersectionLocations) probability_vector.append(probability_of_collision) indices_vector.append([x_index, y_index]) return np.array(probability_vector), indices_vector def computeProbabilityOfCollisionOneTrajectory(self, x_index, y_index, currentRaycastIntersectionLocations): probability_no_collision = 1 for time_step_index, time_step_value in enumerate(self.ActionSet.t_vector): for obstacle_center in currentRaycastIntersectionLocations: probability_of_collision_step_k_obstacle_j = self.computeProbabilityOfCollisionOneStepOneObstacle(x_index, y_index, time_step_index, time_step_value, obstacle_center) probability_no_collision_step_k_obstacle_j = 1 - probability_of_collision_step_k_obstacle_j probability_no_collision = probability_no_collision * probability_no_collision_step_k_obstacle_j return float(1.0 - probability_no_collision) def computeProbabilityOfCollisionOneStepOneObstacle(self, x_index, y_index, time_step_index, time_step_value, obstacle_center): volume = 4.18 Sigma_sensor = np.zeros((3,3)) np.fill_diagonal(Sigma_sensor, self.sigma_sensor) variance_x = (2.5 + abs(self.current_initial_velocity_x*0.1))*time_step_value+0.01 variance_y = (2.5 + abs(self.current_initial_velocity_y*0.1))*time_step_value+0.01 variance_z = (1.5)*time_step_value+0.01 Sigma_robot = np.zeros((3,3)) np.fill_diagonal(Sigma_sensor, [variance_x, variance_y, variance_z]) Sigma_C = Sigma_robot + Sigma_sensor denominator = np.sqrt(np.linalg.det(2*np.pi*Sigma_C)) x = self.ActionSet.p_x_trajectories[x_index, time_step_index] y = self.ActionSet.p_y_trajectories[y_index, time_step_index] z = 0 x_robot = np.array([x, y, z]) obstacle_center = np.array(obstacle_center) exponent = - 0.5 * np.matrix((x_robot - obstacle_center)) * np.matrix(np.linalg.inv(Sigma_C)) * np.matrix((x_robot - obstacle_center)).T return volume / denominator * np.exp(exponent) def computeJerkVector(self,controlInput): jerk_vector = [] for x_index in xrange(self.ActionSet.num_x_bins): for y_index in xrange(self.ActionSet.num_y_bins): last_x_accel = controlInput[0] last_y_accel = controlInput[1] this_x_accel = self.ActionSet.a_x[x_index] this_y_accel = self.ActionSet.a_y[y_index] jerk = np.sqrt( (last_x_accel - this_x_accel)**2 + (last_y_accel - this_y_accel)**2 ) jerk_vector.append(jerk) return np.array(jerk_vector) def identifySpeedAllowedTrajectories(self): speed_allowed_matrix = np.ones((self.ActionSet.num_x_bins, self.ActionSet.num_y_bins)) # for x_index in xrange(self.ActionSet.num_x_bins): # for y_index in xrange(self.ActionSet.num_y_bins): # this_x_accel = self.ActionSet.a_x[x_index] # this_y_accel = self.ActionSet.a_y[y_index] # if np.sqrt( (self.current_initial_velocity_x - this_x_accel)**2 + (self.current_initial_velocity_y - this_y_accel)**2) < self.speed_max: # speed_allowed_matrix[x_index,y_index] = 1 return speed_allowed_matrix def runSingleSimulation(self, controllerType='default', simulationCutoff=None): self.Car.setCarState(0.0,0.0,self.initial_XVelocity,self.initial_YVelocity) self.setRobotFrameState(0.0,0.0,0.0) currentCarState = np.copy(self.Car.state) nextCarState = np.copy(self.Car.state) self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2]) self.Sensor.setLocator(self.locator) firstRaycast = self.Sensor.raycastAll(self.frame) firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame) nextRaycast = np.zeros(self.Sensor.numRays) # record the reward data runData = dict() startIdx = self.counter controlInput = [0,0] while (self.counter < self.numTimesteps - 1): idx = self.counter currentTime = self.t[idx] self.stateOverTime[idx,:] = currentCarState x = self.stateOverTime[idx,0] y = self.stateOverTime[idx,1] self.setRobotFrameState(x,y,0.0) # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2]) currentRaycastIntersectionLocations = self.Sensor.raycastLocationsOnlyOfIntersections(self.frame) if self.checkInCollision(currentCarState[0], currentCarState[1], currentRaycastIntersectionLocations): break if controllerType not in self.colorMap.keys(): print raise ValueError("controller of type " + controllerType + " not supported") # compute all trajectories from action set self.ActionSet.computeAllPositions(currentCarState[0], currentCarState[1], currentCarState[2],currentCarState[3]) self.current_initial_velocity_x = currentCarState[2] self.current_initial_velocity_y = currentCarState[3] speed_allowed_matrix = self.identifySpeedAllowedTrajectories() probability_vector, indices_list = self.computeProbabilitiesOfCollisionAllTrajectories(currentRaycastIntersectionLocations, speed_allowed_matrix) euclideans_vector = self.terminalEuclideanCostForTrajectories() jerk_vector = self.computeJerkVector(controlInput) # k_collision_cost = 10 # k_terminal_cost = 1 # k_jerk = 0.1 # sum_vector = probability_vector*k_collision_cost + euclideans_vector/np.max(euclideans_vector)*k_terminal_cost + k_jerk*jerk_vector/np.max(jerk_vector) # min_action_index_in_vector = np.argmin(sum_vector) # x_index_to_use = indices_list[min_action_index_in_vector][0] # y_index_to_use = indices_list[min_action_index_in_vector][1] # convert to probability no collision probability_vector = np.ones(len(probability_vector)) - probability_vector # convert distances to amount of progress current_distance = np.sqrt((currentCarState[0] - self.globalGoal.global_goal_x)**2 + (currentCarState[1] - self.globalGoal.global_goal_y)**2) #print "euclideans vector", euclideans_vector euclidean_progress_vector = np.ones(len(euclideans_vector))*current_distance - euclideans_vector #print "euclidean progress vector", euclidean_progress_vector reward_vector = euclidean_progress_vector - 0.1*jerk_vector/np.max(jerk_vector) #print "reward_vector", reward_vector expected_reward = np.multiply(probability_vector, reward_vector) max_action_index_in_vector = np.argmax(expected_reward) x_index_to_use = indices_list[max_action_index_in_vector][0] y_index_to_use = indices_list[max_action_index_in_vector][1] self.actionIndicesOverTime[idx,:] = [x_index_to_use, y_index_to_use] x_accel = self.ActionSet.a_x[x_index_to_use] y_accel = self.ActionSet.a_y[y_index_to_use] controlInput = [x_accel, y_accel] self.controlInputData[idx] = controlInput nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt) x = nextCarState[0] y = nextCarState[1] theta = nextCarState[2] self.setRobotFrameState(x,y,theta) #bookkeeping currentCarState = nextCarState self.counter+=1 # break if we are in collision if self.counter >= simulationCutoff: break # fill in the last state by hand self.stateOverTime[self.counter,:] = currentCarState self.actionIndicesOverTime[self.counter,:] = [x_index_to_use, y_index_to_use] # this just makes sure we don't get stuck in an infinite loop. if startIdx == self.counter: self.counter += 1 return runData def setNumpyRandomSeed(self, seed=1): np.random.seed(seed) def runBatchSimulation(self, endTime=None, dt=0.05): # for use in playback self.dt = self.options['dt'] self.endTime = self.defaultControllerTime # used to be the sum of the other times as well self.t = np.arange(0.0, self.endTime, dt) maxNumTimesteps = np.size(self.t) self.stateOverTime = np.zeros((maxNumTimesteps+1, 4)) self.actionIndicesOverTime = np.zeros((maxNumTimesteps+1, 2)) self.controlInputData = np.zeros((maxNumTimesteps+1,2)) self.numTimesteps = maxNumTimesteps self.controllerTypeOrder = ['default'] self.counter = 0 self.simulationData = [] self.initializeStatusBar() self.idxDict = dict() numRunsCounter = 0 self.idxDict['default'] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.defaultControllerTime/dt, self.numTimesteps) while ((self.counter - loopStartIdx < self.defaultControllerTime/dt) and self.counter < self.numTimesteps-1): self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(controllerType='default', simulationCutoff=simCutoff) runData['startIdx'] = startIdx runData['controllerType'] = "default" runData['duration'] = self.counter - runData['startIdx'] runData['endIdx'] = self.counter runData['runNumber'] = numRunsCounter numRunsCounter+=1 self.simulationData.append(runData) # BOOKKEEPING # truncate stateOverTime, raycastData, controlInputs to be the correct size self.numTimesteps = self.counter + 1 self.stateOverTime = self.stateOverTime[0:self.counter+1, :] self.actionIndicesOverTime = self.actionIndicesOverTime[0:self.counter+1, :] self.controlInputData = self.controlInputData[0:self.counter+1] self.endTime = 1.0*self.counter/self.numTimesteps*self.endTime def initializeStatusBar(self): self.numTicks = 10 self.nextTickComplete = 1.0 / float(self.numTicks) self.nextTickIdx = 1 print "Simulation percentage complete: (", self.numTicks, " # is complete)" def printStatusBar(self): fractionDone = float(self.counter) / float(self.numTimesteps) if fractionDone > self.nextTickComplete: self.nextTickIdx += 1 self.nextTickComplete += 1.0 / self.numTicks timeSoFar = time.time() - self.startSimTime estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0 print "#" * self.nextTickIdx, "-" * (self.numTicks - self.nextTickIdx), "estimated", estimatedTimeLeft_minutes, "minutes left" def setCollisionFreeInitialState(self): tol = 5 while True: x = 0.0 y = -5.0 theta = 0 #+ np.random.uniform(0,2*np.pi,1)[0] * 0.01 self.Car.setCarState(x,y,theta) self.setRobotFrameState(x,y,theta) print "In loop" if not self.checkInCollision(): break return x,y,theta def setRandomCollisionFreeInitialState(self): tol = 5 while True: x = np.random.uniform(self.world.Xmin+tol, self.world.Xmax-tol, 1)[0] y = np.random.uniform(self.world.Ymin+tol, self.world.Ymax-tol, 1)[0] #theta = np.random.uniform(0,2*np.pi,1)[0] theta = 0 #always forward self.Car.setCarState(x,y,self.initial_XVelocity,self.initial_YVelocity) self.setRobotFrameState(x,y,theta) if not self.checkInCollision(): break self.firstRandomCollisionFreeInitialState_x = x self.firstRandomCollisionFreeInitialState_y = y return x,y,0,0 def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0/self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) self.max_velocity = 20.0 sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged) sliderXVelocity.setMaximum(self.max_velocity) sliderXVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderXVelocity) sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged) sliderYVelocity.setMaximum(self.max_velocity) sliderYVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderYVelocity) randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal') randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton) l.addWidget(randomGlobalGoalButton) drawActionSetButton = QtGui.QPushButton('Draw Action Set') drawActionSetButton.connect('clicked()', self.onDrawActionSetButton) l.addWidget(drawActionSetButton) runSimButton = QtGui.QPushButton('Simulate') runSimButton.connect('clicked()', self.onRunSimButton) l.addWidget(runSimButton) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider l.addWidget(playButton) l.addWidget(slider) toggleSensorNoiseButton = QtGui.QPushButton('Toggle Sensor Noise') toggleSensorNoiseButton.connect('clicked()', self.onToggleSensorNoise) l.addWidget(toggleSensorNoiseButton) showObstaclesButton = QtGui.QPushButton('Show Obsatacles') showObstaclesButton.connect('clicked()', self.onShowObstacles) l.addWidget(showObstaclesButton) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() # need to connect frames with DrawActionSet self.frame.connectFrameModified(self.updateDrawActionSet) self.updateDrawActionSet(self.frame) self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2,0,-1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter/elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.app.start() def run(self, launchApp=True): self.sphere_toggle = False self.sigma_sensor = 0.5 self.speed_max = 10.0 self.running_sim = True self.onRandomGlobalGoalButton() self.counter = 1 self.runBatchSimulation() self.running_sim = False if launchApp: self.setupPlayback() def onToggleSensorNoise(self): self.sphere_toggle = not self.sphere_toggle def onShowObstacles(self): print "time to show obstacles" A = World.buildCircleWorld(percentObsDensity=self.options['World']['percentObsDensity'], circleRadius=self.options['World']['circleRadius'], nonRandom=self.options['World']['nonRandomWorld'], scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction'], alpha=1.0) def drawFirstIntersections(self, frame, firstRaycast): origin = np.array(frame.transform.GetPosition()) d = DebugData() firstRaycastLocations = self.Sensor.invertRaycastsToLocations(self.frame, firstRaycast) for i in xrange(self.Sensor.numRays): endpoint = firstRaycastLocations[i,:] if firstRaycast[i] >= self.Sensor.rayLength - 0.1: d.addLine(origin, endpoint, color=[0,1,0]) else: d.addLine(origin, endpoint, color=[1,0,0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255') self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) self.locator = self.LineSegmentLocator self.Sensor.setLocator(self.LineSegmentLocator) def updateDrawIntersection(self, frame, locator="None"): if locator=="None": locator = self.locator if self.sphere_toggle: sphere_radius=self.sigma_sensor else: sphere_radius=0.0 origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() d_sphere=DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast(locator, origin, origin + rayTransformed*self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1,0,0]) d_sphere.addSphere(intersection, radius=sphere_radius, color=[1,0,0]) else: d.addLine(origin, origin+rayTransformed*self.Sensor.rayLength, color=colorMaxRange) d_sphere.addSphere(origin, radius=0.0, color=[0,1,0]) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255') vis.updatePolyData(d_sphere.getPolyData(), 'spheres', colorByName='RGB255', alpha=0.3) def getControllerTypeFromCounter(self, counter): name = self.controllerTypeOrder[0] for controllerType in self.controllerTypeOrder[1:]: if counter >= self.idxDict[controllerType]: name = controllerType return name def setRobotFrameState(self, x, y, theta): t = vtk.vtkTransform() t.Translate(x,y,0.0) t.RotateZ(np.degrees(theta)) self.robot.getChildFrame().copyFrame(t) # returns true if we are in collision def checkInCollision(self, x, y, raycastLocations): return False # carState = [x,y,0] # for raycastLocation in raycastLocations: # if np.linalg.norm(carState - raycastLocation) < 0.3: # return True # return False def updateDrawActionSet(self, frame): origin = np.array(frame.transform.GetPosition()) if not self.running_sim: self.ActionSet.computeAllPositions(self.stateOverTime[self.currentIdx,0], self.stateOverTime[self.currentIdx,1], self.stateOverTime[self.currentIdx,2],self.stateOverTime[self.currentIdx,3]) #self.ActionSet.drawActionSetFinal() self.ActionSet.drawActionSetFull() self.drawChosenFunnel() def drawChosenFunnel(self): velocity_initial_x = self.stateOverTime[self.currentIdx,2] velocity_initial_y = self.stateOverTime[self.currentIdx,3] variance_x = 2.5 + abs(velocity_initial_x*0.1) variance_y = 2.5 + abs(velocity_initial_y*0.1) variance_z = 1.5 x_index = self.actionIndicesOverTime[self.currentIdx,0] y_index = self.actionIndicesOverTime[self.currentIdx,1] for i in xrange(0,10): time = 0.5/10.0*i x_center = self.ActionSet.p_x_trajectories[x_index, i] y_center = self.ActionSet.p_y_trajectories[y_index, i] z_center = 0.0 World.buildEllipse(i, [x_center,y_center,z_center], variance_x*time, variance_y*time, variance_z*time, alpha=0.3) def tick(self): #print timer.elapsed #simulate(t.elapsed) x = np.sin(time.time()) y = np.cos(time.time()) self.setRobotFrameState(x,y,0.0) if (time.time() - self.playTime) > self.endTime: self.playTimer.stop() def tick2(self): newtime = time.time() - self.playTime print time.time() - self.playTime x = np.sin(newtime) y = np.cos(newtime) self.setRobotFrameState(x,y,0.0) # just increment the slider, stop the timer if we get to the end def playTimerCallback(self): self.sliderMovedByPlayTimer = True currentIdx = self.slider.value nextIdx = currentIdx + 1 self.slider.setSliderPosition(nextIdx) if currentIdx >= self.sliderMax: print "reached end of tape, stopping playTime" self.playTimer.stop() def onXVelocityChanged(self, value): self.initial_XVelocity = value print "initial x velocity changed to ", value def onYVelocityChanged(self, value): self.initial_YVelocity = -value print "initial y velocity changed to ", -value def onSliderChanged(self, value): if not self.sliderMovedByPlayTimer: self.playTimer.stop() numSteps = len(self.stateOverTime) idx = int(np.floor(numSteps*(1.0*value/self.sliderMax))) idx = min(idx, numSteps-1) self.currentIdx = idx x,y, xdot, ydot = self.stateOverTime[idx] self.setRobotFrameState(x,y,0) self.sliderMovedByPlayTimer = False def onRandomGlobalGoalButton(self): print "random global goal button pressed" self.globalGoal = World.buildGlobalGoal(scale=self.options['World']['scale']) def onDrawActionSetButton(self): print "drawing action set" #self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing) self.ActionSet.computeAllPositions(self.Car.state[0], self.Car.state[1], self.initial_XVelocity, self.initial_YVelocity) #self.ActionSet.drawActionSetFinal() self.ActionSet.drawActionSetFull() def onRunSimButton(self): self.running_sim = True self.runBatchSimulation() self.running_sim = False #self.saveToFile("latest") def onPlayButton(self): if self.playTimer.isActive(): self.onPauseButton() return print 'play' self.playTimer.start() self.playTime = time.time() def onPauseButton(self): print 'pause' self.playTimer.stop() def saveToFile(self, filename): # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime filename = 'data/' + filename + ".out" my_shelf = shelve.open(filename,'n') my_shelf['options'] = self.options my_shelf['simulationData'] = self.simulationData my_shelf['stateOverTime'] = self.stateOverTime my_shelf['controlInputData'] = self.controlInputData my_shelf['numTimesteps'] = self.numTimesteps my_shelf['idxDict'] = self.idxDict my_shelf['counter'] = self.counter my_shelf.close() @staticmethod def loadFromFile(filename): filename = 'data/' + filename + ".out" sim = Simulator(autoInitialize=False, verbose=False) my_shelf = shelve.open(filename) sim.options = my_shelf['options'] sim.initialize() sim.simulationData = my_shelf['simulationData'] sim.stateOverTime = np.array(my_shelf['stateOverTime']) sim.controlInputData = np.array(my_shelf['controlInputData']) sim.numTimesteps = my_shelf['numTimesteps'] sim.idxDict = my_shelf['idxDict'] sim.counter = my_shelf['counter'] my_shelf.close() return sim
def debugMain(): listener = DebugAtlasCommandListener() ConsoleApp.start()
class Simulator(object): """Simulator.""" def __init__(self, world): """Constructs the simulator. Args: world: World. """ self._robots = [] self._obstacles = [] self._world = world self._app = ConsoleApp() self._view = self._app.createView(useGrid=False) # performance tracker self._num_targets = 0 self._num_crashes = 0 self._run_ticks = 0 self._initialize() def _initialize(self): """Initializes the world.""" # Add world to view. om.removeFromObjectModel(om.findObjectByName("world")) vis.showPolyData(self._world.to_polydata(), "world") def _add_polydata(self, polydata, frame_name, color): """Adds polydata to the simulation. Args: polydata: Polydata. frame_name: Frame name. color: Color of object. Returns: Frame. """ om.removeFromObjectModel(om.findObjectByName(frame_name)) frame = vis.showPolyData(polydata, frame_name, color=color) vis.addChildFrame(frame) return frame def add_target(self, target): data = DebugData() center = [target[0], target[1], 1] axis = [0, 0, 1] # Upright cylinder. data.addCylinder(center, axis, 2, 3) om.removeFromObjectModel(om.findObjectByName("target")) self._add_polydata(data.getPolyData(), "target", [0, 0.8, 0]) def add_robot(self, robot): """Adds a robot to the simulation. Args: robot: Robot. """ color = [0.4, 0.85098039, 0.9372549] frame_name = "robot{}".format(len(self._robots)) frame = self._add_polydata(robot.to_polydata(), frame_name, color) self._robots.append((robot, frame)) self._update_moving_object(robot, frame) def add_obstacle(self, obstacle): """Adds an obstacle to the simulation. Args: obstacle: Obstacle. """ color = [1.0, 1.0, 1.0] frame_name = "obstacle{}".format(len(self._obstacles)) frame = self._add_polydata(obstacle.to_polydata(), frame_name, color) self._obstacles.append((obstacle, frame)) self._update_moving_object(obstacle, frame) def _update_moving_object(self, moving_object, frame): """Updates moving object's state. Args: moving_object: Moving object. frame: Corresponding frame. """ t = vtk.vtkTransform() t.Translate(moving_object.x, moving_object.y, 0.0) t.RotateZ(np.degrees(moving_object.theta)) frame.getChildFrame().copyFrame(t) def _update_sensor(self, sensor, frame_name): """Updates sensor's rays. Args: sensor: Sensor. frame_name: Frame name. """ vis.updatePolyData(sensor.to_polydata(), frame_name, colorByName="RGB255") def update_locator(self): """Updates cell locator.""" d = DebugData() d.addPolyData(self._world.to_polydata()) for obstacle, frame in self._obstacles: d.addPolyData(obstacle.to_positioned_polydata()) self.locator = vtk.vtkCellLocator() self.locator.SetDataSet(d.getPolyData()) self.locator.BuildLocator() def run(self, display): """Launches and displays the simulator. Args: display: Displays the simulator or not. """ if display: widget = QtGui.QWidget() layout = QtGui.QVBoxLayout(widget) layout.addWidget(self._view) widget.showMaximized() # Set camera. applogic.resetCamera(viewDirection=[0.2, 0, -1]) # Set timer. self._tick_count = 0 self._timer = TimerCallback(targetFps=120) self._timer.callback = self.tick self._timer.start() self._app.start() def tick(self): """Update simulation clock.""" self._tick_count += 1 self._run_ticks += 1 if self._tick_count >= 500: print("timeout") for robot, frame in self._robots: self.reset(robot, frame) need_update = False for obstacle, frame in self._obstacles: if obstacle.velocity != 0.: obstacle.move() self._update_moving_object(obstacle, frame) need_update = True if need_update: self.update_locator() for i, (robot, frame) in enumerate(self._robots): self._update_moving_object(robot, frame) for sensor in robot.sensors: sensor.set_locator(self.locator) robot.move() for sensor in robot.sensors: frame_name = "rays{}".format(i) self._update_sensor(sensor, frame_name) if sensor.has_collided(): self._num_crashes += 1 print("collided", min(d for d in sensor._distances if d > 0)) print("targets hit", self._num_targets) print("ticks lived", self._run_ticks) print("deaths", self._num_crashes) self._run_ticks = 0 self._num_targets = 0 new_target = self.generate_position() for robot, frame in self._robots: robot.set_target(new_target) self.add_target(new_target) self.reset(robot, frame) if robot.at_target(): self._num_targets += 1 self._tick_count = 0 new_target = self.generate_position() for robot, frame in self._robots: robot.set_target(new_target) self.add_target(new_target) def generate_position(self): return tuple(np.random.uniform(-75, 75, 2)) def set_safe_position(self, robot): while True: robot.x, robot.y = self.generate_position() robot.theta = np.random.uniform(0, 2 * np.pi) if min(robot.sensors[0].distances) >= 0.30: return def reset(self, robot, frame_name): self._tick_count = 0 self.set_safe_position(robot) self._update_moving_object(robot, frame_name) robot._ctrl.save()
factory = robotsystem.RobotSystemFactory() options = factory.getDisabledOptions() options.useDirectorConfig = True options.useRobotState = True options.usePlanning = True options.usePlayback = True options.useAffordances = True options.usePlannerPublisher = True options.useTeleop = True robotSystem = factory.construct(view=view, options=options) return robotSystem app = ConsoleApp() app.setupGlobals(globals()) view = app.createView() robotSystem = makeRobotSystem(view) robotSystem.ikPlanner.planningMode = 'pydrake' robotSystem.teleopPanel.widget.show() #testIkPlan() #constraintSet = ikplanner.ConstraintSet(robotSystem.ikPlanner, buildConstraints(), endPoseName='user_end', startPoseName='q_nom') #fields = constraintSet.runIk() #print fields
class Simulator(object): """Simulator.""" def __init__(self, world, size): """Constructs the simulator. Args: world: World. """ self._robots = [] self._obstacles = [] self._commonships = [] self._world = world self._app = ConsoleApp() self._view = self._app.createView(useGrid=False) # performance tracker self._num_targets = 0 self._num_crashes = 0 self._run_ticks = 0 self._worldsize = size self._play_count = 0 self._initialize() def _initialize(self): """Initializes the world.""" # Add world to view. om.removeFromObjectModel(om.findObjectByName("world")) vis.showPolyData(self._world.to_polydata(), "world") def _add_polydata(self, polydata, frame_name, color): """Adds polydata to the simulation. Args: polydata: Polydata. frame_name: Frame name. color: Color of object. Returns: Frame. """ om.removeFromObjectModel(om.findObjectByName(frame_name)) frame = vis.showPolyData(polydata, frame_name, color=color) vis.addChildFrame(frame) return frame def add_target(self, target): data = DebugData() center = [target[0], target[1], 1] axis = [0, 0, 1] # Upright cylinder. data.addCylinder(center, axis, 2, 3) om.removeFromObjectModel(om.findObjectByName("target")) self._add_polydata(data.getPolyData(), "target", [0, 0.8, 0]) def add_robot(self, robot): """Adds a robot to the simulation. Args: robot: Robot. """ color = [0.4, 0.85098039, 0.9372549] frame_name = "robot{}".format(len(self._robots)) frame = self._add_polydata(robot.to_polydata(), frame_name, color) self._robots.append((robot, frame)) self._update_moving_object(robot, frame) def add_commonship(self, commonship): color = [1, 1, 1] frame_name = "commonship{}".format(len(self._commonships)) frame = self._add_polydata(commonship.to_polydata(), frame_name, color) self._commonships.append((commonship, frame)) self._update_moving_object(commonship, frame) def add_obstacle(self, obstacle): """Adds an obstacle to the simulation. Args: obstacle: Obstacle. """ color = [1.0, 1.0, 1.0] frame_name = "obstacle{}".format(len(self._obstacles)) frame = self._add_polydata(obstacle.to_polydata(), frame_name, color) self._obstacles.append((obstacle, frame)) self._update_moving_object(obstacle, frame) def _update_moving_object(self, moving_object, frame): """Updates moving object's state. Args: moving_object: Moving object. frame: Corresponding frame. """ t = vtk.vtkTransform() t.Translate(moving_object.x, moving_object.y, 0.0) t.RotateZ(np.degrees(moving_object.theta)) frame.getChildFrame().copyFrame(t) def _update_sensor(self, sensor, frame_name): """Updates sensor's rays. Args: sensor: Sensor. frame_name: Frame name. """ vis.updatePolyData(sensor.to_polydata(), frame_name, colorByName="RGB255") def _get_line(self, x, y, num, color): data = DebugData() data.addLine(x, y, radius=0.7, color=[1, 1, 1]) polydata = data.getPolyData() self._add_polydata(polydata, "line" + str(num), color) return polydata def clear_locator(self): d = DebugData() for ship, frame in self._commonships: d.addPolyData(ship.to_positioned_polydata()) self.locator = vtk.vtkCellLocator() self.locator.SetDataSet(d.getPolyData()) self.locator.BuildLocator() def update_locator(self): """Updates cell locator.""" d = DebugData() size = self._worldsize #d.addPolyData(self._world.to_polydata()) for robot, frame in self._robots: d.addPolyData( self._get_line( [robot._initPos[0], robot._initPos[1] + 51, 0], [robot._target[0] + 30, robot._target[1] + 51, 0], 1, color=[1, 1, 1])) d.addPolyData( self._get_line( [robot._initPos[0], robot._initPos[1] - 51, 0], [robot._target[0] + 30, robot._target[1] - 51, 0], 2, color=[1, 1, 1])) for ship, frame in self._commonships: d.addPolyData(ship.to_positioned_polydata()) self.locator = vtk.vtkCellLocator() self.locator.SetDataSet(d.getPolyData()) self.locator.BuildLocator() def run(self, display): """Launches and displays the simulator. Args: display: Displays the simulator or not. """ if display: widget = QtGui.QWidget() layout = QtGui.QVBoxLayout(widget) layout.addWidget(self._view) widget.showMaximized() # Set camera. applogic.resetCamera(viewDirection=[0.2, 0, -1]) # Set timer. self._tick_count = 0 self._timer = TimerCallback(targetFps=200) self._timer.callback = self.tick self._timer.start() self._app.start() def tick(self): """Update simulation clock.""" self._tick_count += 1 self._run_ticks += 1 if self._tick_count >= 500: print("timeout") for robot, frame in self._robots: self.reset(robot, frame) need_update = False for obstacle, frame in self._obstacles: if obstacle.velocity != 0.: obstacle.move() self._update_moving_object(obstacle, frame) need_update = True for commonship, frame in self._commonships: self._update_moving_object(commonship, frame) commonship.move() need_update = True size = self._worldsize if commonship.x > size / 2 + 40 or commonship.x < -size / 2 - 40 or commonship.y > size / 2 + 40 or commonship.y < -size / 2 - 40: ship.velocity = 0 if need_update: self.update_locator() for i, (robot, frame) in enumerate(self._robots): self._update_moving_object(robot, frame) for sensor in robot.sensors: sensor.set_locator(self.locator) robot.move() size = self._worldsize for sensor in robot.sensors: frame_name = "rays{}".format(i) self._update_sensor(sensor, frame_name) if sensor.has_collided(): self._num_crashes += 1 print("collided", min(d for d in sensor._distances if d > 0)) print("targets hit", self._num_targets) print("ticks lived", self._run_ticks) print("deaths", self._num_crashes) self._run_ticks = 0 self._num_targets = 0 # new_target = self.generate_position() # for robot, frame in self._robots: # robot.set_target(new_target) # self.add_target(new_target) self.reset(robot, frame) if robot.x > size / 2 + 2 or robot.x < -size / 2: self._run_ticks = 0 self.reset(robot, frame) if robot.at_target(): self._run_ticks = 0 self.reset(robot, frame) # self._num_targets += 1 # self._tick_count = 0 # new_target = self.generate_position() # for robot, frame in self._robots: # robot.set_target(new_target) # self.add_target(new_target) def generate_position(self): size = self._worldsize return tuple(np.random.uniform(-2 * size / 8, 2 * size / 8, 2)) def set_safe_position(self, robot): while True: size = self._worldsize for ship, frame in self._commonships: #ship.x,ship.y = self.generate_position() randomCommonship(ship) vis.updatePolyData(ship.to_polydata(), "commonship0") randn = np.random.choice([0, 1, 2, 3, 4, 5, 6]) if randn == 0 or randn == 6: self.set_meeting(robot, ship, size) elif randn == 1 or randn == 2: self.set_catch(robot, ship, size) else: self.set_side(robot, ship, size) #ship.velocity=0 theta = (robot.theta + 2 * np.pi) % (2 * np.pi) if theta > 0 and theta <= np.pi / 2: x = size / 2 y = robot.y + (size / 2 - robot.x) * np.tan(robot.theta) robot.set_target((x, y, robot.theta)) self.add_target((x, y)) # print("target",x,y,robot.theta) elif theta > np.pi / 2 and theta <= np.pi: x = -size / 2 y = robot.y + (-size / 2 - robot.x) * np.tan(robot.theta) robot.set_target((x, y, robot.theta)) self.add_target((x, y)) # print("target",x,y,robot.theta) elif theta > np.pi and theta <= np.pi / 2 * 3: x = -size / 2 y = robot.y + (-size / 2 - robot.x) * np.tan(robot.theta) robot.set_target((x, y, robot.theta)) self.add_target((x, y)) # print("target",x,y,robot.theta) elif theta > np.pi / 2 * 3 and theta <= np.pi * 2: x = size / 2 y = robot.y + (size / 2 - robot.x) * np.tan(robot.theta) robot.set_target((x, y, robot.theta)) self.add_target((x, y)) # print("target",x,y,robot.theta) robot.set_initPos((robot.x, robot.y)) robot.attach_sensor(RaySensor()) if min(robot.sensors[0].distances) >= 0.30: return def set_meeting(self, robot, ship, size): robot.x, robot.y = self.generate_position() robot.x = -size / 2 + 0.5 + np.random.uniform(0, 5) dx_1 = size / 2 - robot.x dy_1 = size / 2 - 50 - robot.y theta_1 = np.arctan2(dy_1, dx_1) dx_2 = size / 2 - robot.x dy_2 = -size / 2 + 50 - robot.y theta_2 = np.arctan2(dy_2, dx_2) robot.theta = np.random.uniform(theta_2, theta_1) ship.theta = (robot.theta + np.pi) % (2 * np.pi) dist = size - np.random.uniform(0, 5) - 5 ship.x = robot.x + dist ship.y = robot.y + dist * np.tan(robot.theta) ship.velocity = np.random.uniform(30, 50) #ship.x = def set_catch(self, robot, ship, size): randn = np.random.choice([0, 1]) if randn == 0: robot.x, robot.y = self.generate_position() robot.x = -size / 2 + 0.5 + np.random.uniform(0, 5) dx_1 = size / 2 - robot.x dy_1 = size / 2 - 50 - robot.y theta_1 = np.arctan2(dy_1, dx_1) dx_2 = size / 2 - robot.x dy_2 = -size / 2 + 50 - robot.y theta_2 = np.arctan2(dy_2, dx_2) robot.theta = np.random.uniform(theta_2, theta_1) ship.theta = robot.theta ship.velocity = np.random.uniform(5, 20) dist = np.random.uniform(40, 50) ship.x = robot.x + dist ship.y = robot.y + dist * np.tan(robot.theta) if randn == 1: ship.x, ship.y = self.generate_position() ship.x = -size / 2 + 0.5 + np.random.uniform(0, 5) dx_1 = size / 2 - ship.x dy_1 = size / 2 - 50 - ship.y theta_1 = np.arctan2(dy_1, dx_1) dx_2 = size / 2 - ship.x dy_2 = -size / 2 + 50 - ship.y theta_2 = np.arctan2(dy_2, dx_2) robot.theta = np.random.uniform(theta_2, theta_1) ship.theta = robot.theta ship.velocity = np.random.uniform(60, 80) dist = np.random.uniform(40, 50) robot.x = ship.x + dist robot.y = ship.y + dist * np.tan(robot.theta) def set_side(self, robot, ship, size): sx, sy = tuple(np.random.uniform(-size / 16, size / 16, 2)) dx_1 = sx + size / 2 dy_1 = sy - size / 2 + 50 theta_1 = np.arctan2(dy_1, dx_1) dx_2 = size / 2 - sx dy_2 = -size / 2 + 50 - sy theta_2 = np.arctan2(dy_2, dx_2) if theta_2 > theta_1: theta_1 = theta_2 dx_3 = sx + size / 2 dy_3 = sy + size / 2 - 50 theta_3 = np.arctan2(dy_3, dx_3) dx_4 = size / 2 - sx dy_4 = size / 2 - 50 - sy theta_4 = np.arctan2(dy_4, dx_4) if theta_4 < theta_3: theta_3 = theta_4 robot.theta = np.random.uniform(theta_1, theta_3) theta_n1 = 68.0 / 180.0 * np.pi ship.theta = np.random.uniform(robot.theta + theta_n1, 2 * np.pi + robot.theta - theta_n1) theta_1 = np.pi / 2 - robot.theta theta_2 = np.pi / 2 - ship.theta dist1 = np.random.uniform(3 * size / 8, 7 * size / 16) y1 = dist1 * np.cos(theta_1) x1 = dist1 * np.sin(theta_1) robot.x = sx - x1 robot.y = sy - y1 t = dist1 / 40.0 dist2 = np.random.uniform(3 * size / 8, 7 * size / 16) y2 = dist2 * np.cos(theta_2) x2 = dist2 * np.sin(theta_2) ship.x = sx - x2 ship.y = sy - y2 ship.velocity = dist2 / t def reset(self, robot, frame_name): self._tick_count = 0 robot.init_angle() self.clear_locator() om.removeFromObjectModel(om.findObjectByName("line1")) om.removeFromObjectModel(om.findObjectByName("line2")) self._play_count += 1 self.set_safe_position(robot) print("count:", self._play_count) self._update_moving_object(robot, frame_name) robot._ctrl.save()
from director.consoleapp import ConsoleApp from director import visualization as vis from director import roboturdf from director import jointcontrol import argparse def getArgs(): parser = argparse.ArgumentParser() parser.add_argument("--urdf", type=str, default=None, help="urdf filename to load") args, unknown = parser.parse_known_args() return args app = ConsoleApp() view = app.createView() args = getArgs() if args.urdf: robotModel = roboturdf.openUrdf(args.urdf, view) jointNames = robotModel.model.getJointNames() jointController = jointcontrol.JointController([robotModel], jointNames=jointNames) else: robotModel, jointController = roboturdf.loadRobotModel("robot model", view) print "urdf file:", robotModel.getProperty("Filename")
def main(): atlasdriver.init() panel = atlasdriverpanel.AtlasDriverPanel(atlasdriver.driver) panel.widget.show() ConsoleApp.start()
class AtlasCommandPanel(object): def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotSystem = robotsystem.create(self.view) self.config = drcargs.getDirectorConfig() jointGroups = self.config['teleopJointGroups'] self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups) self.jointCommandPanel = JointCommandPanel(self.robotSystem) self.jointCommandPanel.ui.speedSpinBox.setEnabled(False) self.jointCommandPanel.ui.mirrorArmsCheck.setChecked(self.jointTeleopPanel.mirrorArms) self.jointCommandPanel.ui.mirrorLegsCheck.setChecked(self.jointTeleopPanel.mirrorLegs) self.jointCommandPanel.ui.resetButton.connect('clicked()', self.resetJointTeleopSliders) self.jointCommandPanel.ui.mirrorArmsCheck.connect('clicked()', self.mirrorJointsChanged) self.jointCommandPanel.ui.mirrorLegsCheck.connect('clicked()', self.mirrorJointsChanged) self.widget = QtGui.QWidget() gl = QtGui.QGridLayout(self.widget) gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan gl.addWidget(self.view, 0, 1, 1, 1) gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1) gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1) gl.setRowStretch(0,1) gl.setColumnStretch(1,1) #self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan) lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) def onSingleJointPositionGoal(self, msg): jointPositionGoal = msg.joint_position jointName = msg.joint_name allowedJointNames = ['l_leg_aky','l_arm_lwy'] if not (jointName in allowedJointNames): print 'Position goals are not allowed for this joint' print 'ignoring this position goal' print 'use the sliders instead' return if (jointName == 'l_arm_lwy') and (not self.jointCommandPanel.steeringControlEnabled): print 'Steering control not enabled' print 'ignoring steering command' return if (jointName == 'l_leg_aky') and (not self.jointCommandPanel.throttleControlEnabled): print 'Throttle control not enabled' print 'ignoring throttle command' return jointIdx = self.jointTeleopPanel.toJointIndex(joint_name) self.jointTeleopPanel.endPose[jointIdx] = jointPositionGoal self.jointTeleopPanel.updateSliders() self.jointTeleopPanel.sliderChanged(jointName) def onRobotPlan(self, msg): playback = planplayback.PlanPlayback() playback.interpolationMethod = 'pchip' poseTimes, poses = playback.getPlanPoses(msg) f = playback.getPoseInterpolator(poseTimes, poses) jointController = self.robotSystem.teleopJointController timer = simpletimer.SimpleTimer() def setPose(pose): jointController.setPose('plan_playback', pose) self.jointTeleopPanel.endPose = pose self.jointTeleopPanel.updateSliders() commandStream.setGoalPose(pose) def updateAnimation(): tNow = timer.elapsed() if tNow > poseTimes[-1]: pose = poses[-1] setPose(pose) return False pose = f(tNow) setPose(pose) self.animationTimer = TimerCallback() self.animationTimer.targetFps = 60 self.animationTimer.callback = updateAnimation self.animationTimer.start() def mirrorJointsChanged(self): self.jointTeleopPanel.mirrorLegs = self.jointCommandPanel.ui.mirrorLegsCheck.checked self.jointTeleopPanel.mirrorArms = self.jointCommandPanel.ui.mirrorArmsCheck.checked def resetJointTeleopSliders(self): self.jointTeleopPanel.resetPoseToRobotState()
from director.consoleapp import ConsoleApp from director import visualization as vis import director.objectmodel as om import PythonQt from PythonQt import QtCore, QtGui import director.tasks.robottasks as rt from director.tasks.taskmanagerwidget import TaskWidgetManager from director import robotsystem from director.fieldcontainer import FieldContainer ############################### app = ConsoleApp() app.setupGlobals(globals()) app.showPythonConsole() view = app.createView() view.show() useRobotSystem = True if useRobotSystem: robotSystem = robotsystem.create(view) rt.robotSystem = robotSystem
class Simulator(object): def __init__(self, percentObsDensity=20, endTime=40, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True): self.verbose = verbose self.startSimTime = time.time() self.collisionThreshold = 0.4 self.randomSeed = 5 self.percentObsDensity = percentObsDensity self.defaultControllerTime = 1000 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() if autoInitialize: self.initialize() self.XVelocity_drawing = 0.0 self.YVelocity_drawing = 0.0 def initializeOptions(self): self.options = dict() self.options['World'] = dict() self.options['World']['obstaclesInnerFraction'] = 0.98 self.options['World']['randomSeed'] = 40 self.options['World']['percentObsDensity'] = 0.0 self.options['World']['nonRandomWorld'] = True self.options['World']['circleRadius'] = 1.0 self.options['World']['scale'] = 1 self.options['Car'] = dict() self.options['Car']['velocity'] = 4.0 self.options['dt'] = 0.05 self.options['runTime'] = dict() self.options['runTime']['defaultControllerTime'] = 100 def setDefaultOptions(self): defaultOptions = dict() defaultOptions['World'] = dict() defaultOptions['World']['obstaclesInnerFraction'] = 0.98 defaultOptions['World']['randomSeed'] = 40 defaultOptions['World']['percentObsDensity'] = 30 defaultOptions['World']['nonRandomWorld'] = True defaultOptions['World']['circleRadius'] = 1.75 defaultOptions['World']['scale'] = 2.5 defaultOptions['Car'] = dict() defaultOptions['Car']['velocity'] = 20 defaultOptions['dt'] = 0.05 defaultOptions['runTime'] = dict() defaultOptions['runTime']['defaultControllerTime'] = 100 for k in defaultOptions: self.options.setdefault(k, defaultOptions[k]) for k in defaultOptions: if not isinstance(defaultOptions[k], dict): continue for j in defaultOptions[k]: self.options[k].setdefault(j, defaultOptions[k][j]) def initializeColorMap(self): self.colorMap = dict() self.colorMap['default'] = [0,1,0] def initialize(self): self.dt = self.options['dt'] self.controllerTypeOrder = ['default'] self.setDefaultOptions() self.Controller = ControllerObj() self.Car = CarPlant(controller=self.Controller, velocity=self.options['Car']['velocity']) self.Controller.initializeVelocity(self.Car.v) self.ActionSet = ActionSetObj() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName('world')) self.world = World.buildLineSegmentTestWorld(percentObsDensity=self.options['World']['percentObsDensity'], circleRadius=self.options['World']['circleRadius'], nonRandom=self.options['World']['nonRandomWorld'], scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction']) om.removeFromObjectModel(om.findObjectByName('robot')) self.robot, self.frame = World.buildRobot() self.frame = self.robot.getChildFrame() self.frame.setProperty('Scale', 3) #self.frame.setProperty('Visible', False) #self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.defaultControllerTime = self.options['runTime']['defaultControllerTime'] self.Car.setFrame(self.frame) print "Finished initialization" def runSingleSimulation(self, controllerType='default', simulationCutoff=None): #self.setRandomCollisionFreeInitialState() self.setInitialStateAtZero() currentCarState = np.copy(self.Car.state) nextCarState = np.copy(self.Car.state) self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2]) # DELETED FIRST RAYCAST AND RAYCAST LOCATIONS # record the reward data runData = dict() startIdx = self.counter thisRunIndex = 0 NMaxSteps = 100 while (self.counter < self.numTimesteps - 1): thisRunIndex += 1 idx = self.counter currentTime = self.t[idx] self.stateOverTime[idx,:] = currentCarState x = self.stateOverTime[idx,0] y = self.stateOverTime[idx,1] self.setRobotFrameState(x,y,0.0) # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2]) if controllerType in ["default"]: controlInput, controlInputIdx = self.Controller.computeControlInput(currentCarState, currentTime, self.frame, randomize=False) self.controlInputData[idx] = controlInput nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt) print "NEXTCARSTATE is ", nextCarState x = nextCarState[0] y = nextCarState[1] self.setRobotFrameState(x,y,0.0) if controllerType in ["default", "defaultRandom"]: nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState, currentTime, self.frame, randomize=False) #bookkeeping self.counter+=1 # break if we are in collision if self.counter >= simulationCutoff: break # fill in the last state by hand self.stateOverTime[self.counter,:] = currentCarState # this just makes sure we don't get stuck in an infinite loop. if startIdx == self.counter: self.counter += 1 return runData def setNumpyRandomSeed(self, seed=1): np.random.seed(seed) def runBatchSimulation(self, endTime=None, dt=0.05): self.controllerTypeOrder = ['default'] self.counter = 0 self.simulationData = [] self.initializeStatusBar() self.idxDict = dict() numRunsCounter = 0 self.idxDict['default'] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.defaultControllerTime/dt, self.numTimesteps) while ((self.counter - loopStartIdx < self.defaultControllerTime/dt) and self.counter < self.numTimesteps-1): self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(controllerType='default', simulationCutoff=simCutoff) runData['startIdx'] = startIdx runData['controllerType'] = "default" runData['duration'] = self.counter - runData['startIdx'] runData['endIdx'] = self.counter runData['runNumber'] = numRunsCounter numRunsCounter+=1 self.simulationData.append(runData) # BOOKKEEPING # truncate stateOverTime, raycastData, controlInputs to be the correct size self.numTimesteps = self.counter + 1 self.stateOverTime = self.stateOverTime[0:self.counter+1, :] self.controlInputData = self.controlInputData[0:self.counter+1] self.endTime = 1.0*self.counter/self.numTimesteps*self.endTime def initializeStatusBar(self): self.numTicks = 10 self.nextTickComplete = 1.0 / float(self.numTicks) self.nextTickIdx = 1 print "Simulation percentage complete: (", self.numTicks, " # is complete)" def printStatusBar(self): fractionDone = float(self.counter) / float(self.numTimesteps) if fractionDone > self.nextTickComplete: self.nextTickIdx += 1 self.nextTickComplete += 1.0 / self.numTicks timeSoFar = time.time() - self.startSimTime estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0 print "#" * self.nextTickIdx, "-" * (self.numTicks - self.nextTickIdx), "estimated", estimatedTimeLeft_minutes, "minutes left" def setCollisionFreeInitialState(self): tol = 5 while True: x = 0.0 y = -5.0 theta = 0 #+ np.random.uniform(0,2*np.pi,1)[0] * 0.01 self.Car.setCarState(x,y,0.0,0.0) self.setRobotFrameState(x,y,theta) print "In loop" if not self.checkInCollision(): break return x,y,theta def setInitialStateAtZero(self): x = 0.0 y = 0.0 theta = 0.0 self.Car.setCarState(x,y,0.0,0.0) self.setRobotFrameState(x,y,theta) return x,y,theta def setRandomCollisionFreeInitialState(self): tol = 5 while True: x = np.random.uniform(self.world.Xmin+tol, self.world.Xmax-tol, 1)[0] y = np.random.uniform(self.world.Ymin+tol, self.world.Ymax-tol, 1)[0] theta = np.random.uniform(0,2*np.pi,1)[0] self.Car.setCarState(x,y,theta) self.setRobotFrameState(x,y,theta) if not self.checkInCollision(): break return x,y,theta def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0/self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) self.max_velocity = 20.0 sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged) sliderXVelocity.setMaximum(self.max_velocity) sliderXVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderXVelocity) sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged) sliderYVelocity.setMaximum(self.max_velocity) sliderYVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderYVelocity) randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal') randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton) l.addWidget(randomGlobalGoalButton) randomObstaclesButton = QtGui.QPushButton('Initialize Random Obstacles') randomObstaclesButton.connect('clicked()', self.onRandomObstaclesButton) l.addWidget(randomObstaclesButton) drawActionSetButton = QtGui.QPushButton('Draw Action Set') drawActionSetButton.connect('clicked()', self.onDrawActionSetButton) l.addWidget(drawActionSetButton) runSimButton = QtGui.QPushButton('Simulate') runSimButton.connect('clicked()', self.onRunSimButton) l.addWidget(runSimButton) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider # slider4 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider4.setMaximum(self.sliderMax) # l.addWidget(slider4) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider6 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider6.setMaximum(self.sliderMax) # l.addWidget(slider6) # slider7 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider7.setMaximum(self.sliderMax) # l.addWidget(slider7) l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() # We need to connectFrameModified with updateDrawActionSet # self.frame.connectFrameModified(self.updateDrawIntersection) # self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2,0,-1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter/elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.onRandomObstaclesButton() self.app.start() def getControllerTypeFromCounter(self, counter): name = self.controllerTypeOrder[0] for controllerType in self.controllerTypeOrder[1:]: if counter >= self.idxDict[controllerType]: name = controllerType return name def setRobotFrameState(self, x, y, theta): t = vtk.vtkTransform() t.Translate(x,y,0.0) t.RotateZ(np.degrees(theta)) self.robot.getChildFrame().copyFrame(t) # returns true if we are in collision def checkInCollision(self, raycastDistance=None): # We need to re-evaluate this return False # if raycastDistance[(len(raycastDistance)+1)/2] < self.collisionThreshold: # return True # else: # return False def tick(self): #print timer.elapsed #simulate(t.elapsed) x = np.sin(time.time()) y = np.cos(time.time()) self.setRobotFrameState(x,y,0.0) if (time.time() - self.playTime) > self.endTime: self.playTimer.stop() def tick2(self): newtime = time.time() - self.playTime print time.time() - self.playTime x = np.sin(newtime) y = np.cos(newtime) self.setRobotFrameState(x,y,0.0) # just increment the slider, stop the timer if we get to the end def playTimerCallback(self): self.sliderMovedByPlayTimer = True currentIdx = self.slider.value nextIdx = currentIdx + 1 self.slider.setSliderPosition(nextIdx) if currentIdx >= self.sliderMax: print "reached end of tape, stopping playTime" self.playTimer.stop() def onSliderChanged(self, value): if not self.sliderMovedByPlayTimer: self.playTimer.stop() numSteps = len(self.stateOverTime) idx = int(np.floor(numSteps*(1.0*value/self.sliderMax))) idx = min(idx, numSteps-1) x,y, xdot, ydot = self.stateOverTime[idx] self.setRobotFrameState(x,y,0) self.sliderMovedByPlayTimer = False def onXVelocityChanged(self, value): self.XVelocity_drawing = value self.onDrawActionSetButton() print "x velocity changed to ", value def onYVelocityChanged(self, value): print value self.YVelocity_drawing = -value self.onDrawActionSetButton() print "y velocity changed to ", -value def onRandomObstaclesButton(self): print "random obstacles button pressed" self.setInitialStateAtZero() self.world = World.buildLineSegmentTestWorld(percentObsDensity=8.0, circleRadius=self.options['World']['circleRadius'], nonRandom=False, scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction']) self.locator = World.buildCellLocator(self.world.visObj.polyData) def onRandomGlobalGoalButton(self): print "random global goal button pressed" self.globalGoal = World.buildGlobalGoal() def onDrawActionSetButton(self): print "drawing action set" #self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing) self.ActionSet.computeAllPositions(self.XVelocity_drawing,self.YVelocity_drawing) #self.ActionSet.drawActionSetFinal() self.ActionSet.drawActionSetFull() def onRunSimButton(self): self.runBatchSimulation() self.saveToFile("latest") def onPlayButton(self): if self.playTimer.isActive(): self.onPauseButton() return print 'play' self.playTimer.start() self.playTime = time.time() def onPauseButton(self): print 'pause' self.playTimer.stop() def saveToFile(self, filename): # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime filename = 'data/' + filename + ".out" my_shelf = shelve.open(filename,'n') my_shelf['options'] = self.options my_shelf['simulationData'] = self.simulationData my_shelf['stateOverTime'] = self.stateOverTime my_shelf['controlInputData'] = self.controlInputData my_shelf['numTimesteps'] = self.numTimesteps my_shelf['idxDict'] = self.idxDict my_shelf['counter'] = self.counter my_shelf.close() def run(self, launchApp=True): self.counter = 1 # for use in playback self.dt = self.options['dt'] self.endTime = self.defaultControllerTime # used to be the sum of the other times as well self.t = np.arange(0.0, self.endTime, self.dt) maxNumTimesteps = np.size(self.t) self.stateOverTime = np.zeros((maxNumTimesteps+1, 4)) self.controlInputData = np.zeros((maxNumTimesteps+1,2)) self.numTimesteps = maxNumTimesteps self.runBatchSimulation() if launchApp: self.setupPlayback() @staticmethod def loadFromFile(filename): filename = 'data/' + filename + ".out" sim = Simulator(autoInitialize=False, verbose=False) my_shelf = shelve.open(filename) sim.options = my_shelf['options'] sim.initialize() sim.simulationData = my_shelf['simulationData'] sim.stateOverTime = np.array(my_shelf['stateOverTime']) sim.raycastData = np.array( my_shelf['raycastData']) sim.controlInputData = np.array(my_shelf['controlInputData']) sim.numTimesteps = my_shelf['numTimesteps'] sim.idxDict = my_shelf['idxDict'] sim.counter = my_shelf['counter'] my_shelf.close() return sim
assert not checkGraspFrame(goalFrame, side) frame = teleopPanel.endEffectorTeleop.newReachTeleop(goalFrame, side) assert checkGraspFrame(goalFrame, side) teleopPanel.ui.planButton.click() assert playbackPanel.plan is not None teleopPanel.ikPlanner.useCollision = True teleopPanel.ui.planButton.click() assert playbackPanel.plan is not None frame.setProperty('Edit', True) app.startTestingModeQuitTimer() app = ConsoleApp() app.setupGlobals(globals()) view = app.createView() robotsystem.create(view, globals()) playbackPanel = playbackpanel.PlaybackPanel(planPlayback, playbackRobotModel, playbackJointController, robotStateModel, robotStateJointController, manipPlanner) teleopPanel = teleoppanel.TeleopPanel(robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController, ikPlanner, manipPlanner,
from director.consoleapp import ConsoleApp from director import roboturdf from director import jointcontrol from director import planplayback from director import playbackpanel from director import robotplanlistener from director import robotviewbehaviors from director import pointcloudlcm from director import cameraview from director import lcmUtils from PythonQt import QtCore, QtGui import drc as lcmdrc import bot_core as lcmbotcore # create the application app = ConsoleApp() view = app.createView() # load robot state model robotStateModel, robotStateJointController = roboturdf.loadRobotModel( 'robot model', view, colorMode='Textures') # listen for robot state updates robotStateJointController.addLCMUpdater('EST_ROBOT_STATE') # reload urdf from model publisher lcm roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)]) # load playback model playbackRobotModel, playbackJointController = roboturdf.loadRobotModel(
from director.consoleapp import ConsoleApp from director import robotsystem app = ConsoleApp() app.setupGlobals(globals()) if app.getTestingInteractiveEnabled(): app.showPythonConsole() view = app.createView() view.show() robotsystem.create(view, globals()) app.start()
class Simulator(object): def __init__(self, percentObsDensity=20, endTime=40, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True): self.verbose = verbose self.startSimTime = time.time() self.collisionThreshold = 0.4 self.randomSeed = 5 self.Sensor_rayLength = 8 self.percentObsDensity = percentObsDensity self.defaultControllerTime = 1000 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() if autoInitialize: self.initialize() def initializeOptions(self): self.options = dict() self.options['World'] = dict() self.options['World']['obstaclesInnerFraction'] = 0.98 self.options['World']['randomSeed'] = 40 self.options['World']['percentObsDensity'] = 10 self.options['World']['nonRandomWorld'] = True self.options['World']['circleRadius'] = 1.0 self.options['World']['scale'] = 1 self.options['Sensor'] = dict() self.options['Sensor']['rayLength'] = 20 self.options['Sensor']['numRays'] = 21 self.options['Car'] = dict() self.options['Car']['velocity'] = 4.0 self.options['dt'] = 0.05 self.options['runTime'] = dict() self.options['runTime']['defaultControllerTime'] = 100 def setDefaultOptions(self): defaultOptions = dict() defaultOptions['World'] = dict() defaultOptions['World']['obstaclesInnerFraction'] = 0.98 defaultOptions['World']['randomSeed'] = 40 defaultOptions['World']['percentObsDensity'] = 30 defaultOptions['World']['nonRandomWorld'] = True defaultOptions['World']['circleRadius'] = 1.75 defaultOptions['World']['scale'] = 2.5 defaultOptions['Sensor'] = dict() defaultOptions['Sensor']['rayLength'] = 20 defaultOptions['Sensor']['numRays'] = 41 defaultOptions['Car'] = dict() defaultOptions['Car']['velocity'] = 20 defaultOptions['dt'] = 0.05 defaultOptions['runTime'] = dict() defaultOptions['runTime']['defaultControllerTime'] = 100 for k in defaultOptions: self.options.setdefault(k, defaultOptions[k]) for k in defaultOptions: if not isinstance(defaultOptions[k], dict): continue for j in defaultOptions[k]: self.options[k].setdefault(j, defaultOptions[k][j]) def initializeColorMap(self): self.colorMap = dict() self.colorMap['default'] = [0,1,0] def initialize(self): self.dt = self.options['dt'] self.controllerTypeOrder = ['default'] self.setDefaultOptions() self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'], numRays=self.options['Sensor']['numRays']) self.SensorApproximator = SensorApproximatorObj(numRays=self.options['Sensor']['numRays'], circleRadius=self.options['World']['circleRadius'], ) self.Controller = ControllerObj(self.Sensor, self.SensorApproximator) self.Car = CarPlant(controller=self.Controller, velocity=self.options['Car']['velocity']) self.Controller.initializeVelocity(self.Car.v) # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName('world')) self.world = World.buildLineSegmentTestWorld(percentObsDensity=self.options['World']['percentObsDensity'], circleRadius=self.options['World']['circleRadius'], nonRandom=self.options['World']['nonRandomWorld'], scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction']) om.removeFromObjectModel(om.findObjectByName('robot')) self.robot, self.frame = World.buildRobot() self.locator = World.buildCellLocator(self.world.visObj.polyData) self.Sensor.setLocator(self.locator) self.frame = self.robot.getChildFrame() self.frame.setProperty('Scale', 3) #self.frame.setProperty('Visible', False) #self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.defaultControllerTime = self.options['runTime']['defaultControllerTime'] self.Car.setFrame(self.frame) print "Finished initialization" def runSingleSimulation(self, controllerType='default', simulationCutoff=None): #self.setRandomCollisionFreeInitialState() self.setInitialStateAtZero() currentCarState = np.copy(self.Car.state) nextCarState = np.copy(self.Car.state) self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2]) firstRaycast = self.Sensor.raycastAll(self.frame) firstRaycastLocations = self.Sensor.raycastAllLocations(self.frame) self.LineSegmentWorld = World.buildLineSegmentWorld(firstRaycastLocations) self.LineSegmentLocator = World.buildCellLocator(self.LineSegmentWorld.visObj.polyData) self.Sensor.setLocator(self.LineSegmentLocator) nextRaycast = np.zeros(self.Sensor.numRays) # record the reward data runData = dict() startIdx = self.counter thisRunIndex = 0 NMaxSteps = 100 while (self.counter < self.numTimesteps - 1): thisRunIndex += 1 idx = self.counter currentTime = self.t[idx] self.stateOverTime[idx,:] = currentCarState x = self.stateOverTime[idx,0] y = self.stateOverTime[idx,1] theta = self.stateOverTime[idx,2] self.setRobotFrameState(x,y,theta) # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2]) currentRaycast = self.Sensor.raycastAll(self.frame) print "current Raycast ", currentRaycast self.raycastData[idx,:] = currentRaycast S_current = (currentCarState, currentRaycast) if controllerType not in self.colorMap.keys(): print raise ValueError("controller of type " + controllerType + " not supported") if controllerType in ["default", "defaultRandom"]: controlInput, controlInputIdx = self.Controller.computeControlInput(currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=False) self.controlInputData[idx] = controlInput nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt) x = nextCarState[0] y = nextCarState[1] theta = nextCarState[2] self.setRobotFrameState(x,y,theta) nextRaycast = self.Sensor.raycastAll(self.frame) # Compute the next control input S_next = (nextCarState, nextRaycast) if controllerType in ["default", "defaultRandom"]: nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState, currentTime, self.frame, raycastDistance=firstRaycast, randomize=False) #bookkeeping currentCarState = nextCarState currentRaycast = nextRaycast self.counter+=1 # break if we are in collision if self.checkInCollision(nextRaycast): if self.verbose: print "Had a collision, terminating simulation" break if thisRunIndex > NMaxSteps: print "was safe during N steps" break if self.counter >= simulationCutoff: break # fill in the last state by hand self.stateOverTime[self.counter,:] = currentCarState self.raycastData[self.counter,:] = currentRaycast # this just makes sure we don't get stuck in an infinite loop. if startIdx == self.counter: self.counter += 1 return runData def setNumpyRandomSeed(self, seed=1): np.random.seed(seed) def runBatchSimulation(self, endTime=None, dt=0.05): # for use in playback self.dt = self.options['dt'] self.endTime = self.defaultControllerTime # used to be the sum of the other times as well self.t = np.arange(0.0, self.endTime, dt) maxNumTimesteps = np.size(self.t) self.stateOverTime = np.zeros((maxNumTimesteps+1, 3)) self.raycastData = np.zeros((maxNumTimesteps+1, self.Sensor.numRays)) self.controlInputData = np.zeros(maxNumTimesteps+1) self.numTimesteps = maxNumTimesteps self.controllerTypeOrder = ['default'] self.counter = 0 self.simulationData = [] self.initializeStatusBar() self.idxDict = dict() numRunsCounter = 0 self.idxDict['default'] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.defaultControllerTime/dt, self.numTimesteps) while ((self.counter - loopStartIdx < self.defaultControllerTime/dt) and self.counter < self.numTimesteps-1): self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(controllerType='default', simulationCutoff=simCutoff) runData['startIdx'] = startIdx runData['controllerType'] = "default" runData['duration'] = self.counter - runData['startIdx'] runData['endIdx'] = self.counter runData['runNumber'] = numRunsCounter numRunsCounter+=1 self.simulationData.append(runData) # BOOKKEEPING # truncate stateOverTime, raycastData, controlInputs to be the correct size self.numTimesteps = self.counter + 1 self.stateOverTime = self.stateOverTime[0:self.counter+1, :] self.raycastData = self.raycastData[0:self.counter+1, :] self.controlInputData = self.controlInputData[0:self.counter+1] self.endTime = 1.0*self.counter/self.numTimesteps*self.endTime def initializeStatusBar(self): self.numTicks = 10 self.nextTickComplete = 1.0 / float(self.numTicks) self.nextTickIdx = 1 print "Simulation percentage complete: (", self.numTicks, " # is complete)" def printStatusBar(self): fractionDone = float(self.counter) / float(self.numTimesteps) if fractionDone > self.nextTickComplete: self.nextTickIdx += 1 self.nextTickComplete += 1.0 / self.numTicks timeSoFar = time.time() - self.startSimTime estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0 print "#" * self.nextTickIdx, "-" * (self.numTicks - self.nextTickIdx), "estimated", estimatedTimeLeft_minutes, "minutes left" def setCollisionFreeInitialState(self): tol = 5 while True: x = 0.0 y = -5.0 theta = 0 #+ np.random.uniform(0,2*np.pi,1)[0] * 0.01 self.Car.setCarState(x,y,theta) self.setRobotFrameState(x,y,theta) print "In loop" if not self.checkInCollision(): break return x,y,theta def setInitialStateAtZero(self): x = 0.0 y = 0.0 theta = 0.0 self.Car.setCarState(x,y,theta) self.setRobotFrameState(x,y,theta) return x,y,theta def setRandomCollisionFreeInitialState(self): tol = 5 while True: x = np.random.uniform(self.world.Xmin+tol, self.world.Xmax-tol, 1)[0] y = np.random.uniform(self.world.Ymin+tol, self.world.Ymax-tol, 1)[0] theta = np.random.uniform(0,2*np.pi,1)[0] self.Car.setCarState(x,y,theta) self.setRobotFrameState(x,y,theta) if not self.checkInCollision(): break return x,y,theta def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0/self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2,0,-1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter/elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.app.start() def run(self, launchApp=True): self.counter = 1 self.runBatchSimulation() if launchApp: self.setupPlayback() def updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast(self.LineSegmentLocator, origin, origin + rayTransformed*self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1,0,0]) else: d.addLine(origin, origin+rayTransformed*self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255') #camera = self.view.camera() #camera.SetFocalPoint(frame.transform.GetPosition()) #camera.SetPosition(frame.transform.TransformPoint((-30,0,10))) def getControllerTypeFromCounter(self, counter): name = self.controllerTypeOrder[0] for controllerType in self.controllerTypeOrder[1:]: if counter >= self.idxDict[controllerType]: name = controllerType return name def setRobotFrameState(self, x, y, theta): t = vtk.vtkTransform() t.Translate(x,y,0.0) t.RotateZ(np.degrees(theta)) self.robot.getChildFrame().copyFrame(t) # returns true if we are in collision def checkInCollision(self, raycastDistance=None): if raycastDistance is None: self.setRobotFrameState(self.Car.state[0],self.Car.state[1],self.Car.state[2]) raycastDistance = self.Sensor.raycastAll(self.frame) if np.min(raycastDistance) < self.collisionThreshold: return True else: return False # if raycastDistance[(len(raycastDistance)+1)/2] < self.collisionThreshold: # return True # else: # return False def tick(self): #print timer.elapsed #simulate(t.elapsed) x = np.sin(time.time()) y = np.cos(time.time()) self.setRobotFrameState(x,y,0.0) if (time.time() - self.playTime) > self.endTime: self.playTimer.stop() def tick2(self): newtime = time.time() - self.playTime print time.time() - self.playTime x = np.sin(newtime) y = np.cos(newtime) self.setRobotFrameState(x,y,0.0) # just increment the slider, stop the timer if we get to the end def playTimerCallback(self): self.sliderMovedByPlayTimer = True currentIdx = self.slider.value nextIdx = currentIdx + 1 self.slider.setSliderPosition(nextIdx) if currentIdx >= self.sliderMax: print "reached end of tape, stopping playTime" self.playTimer.stop() def onSliderChanged(self, value): if not self.sliderMovedByPlayTimer: self.playTimer.stop() numSteps = len(self.stateOverTime) idx = int(np.floor(numSteps*(1.0*value/self.sliderMax))) idx = min(idx, numSteps-1) x,y,theta = self.stateOverTime[idx] self.setRobotFrameState(x,y,theta) self.sliderMovedByPlayTimer = False def onPlayButton(self): if self.playTimer.isActive(): self.onPauseButton() return print 'play' self.playTimer.start() self.playTime = time.time() def onPauseButton(self): print 'pause' self.playTimer.stop() def saveToFile(self, filename): # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime filename = 'data/' + filename + ".out" my_shelf = shelve.open(filename,'n') my_shelf['options'] = self.options my_shelf['simulationData'] = self.simulationData my_shelf['stateOverTime'] = self.stateOverTime my_shelf['raycastData'] = self.raycastData my_shelf['controlInputData'] = self.controlInputData my_shelf['numTimesteps'] = self.numTimesteps my_shelf['idxDict'] = self.idxDict my_shelf['counter'] = self.counter my_shelf.close() @staticmethod def loadFromFile(filename): filename = 'data/' + filename + ".out" sim = Simulator(autoInitialize=False, verbose=False) my_shelf = shelve.open(filename) sim.options = my_shelf['options'] sim.initialize() sim.simulationData = my_shelf['simulationData'] sim.stateOverTime = np.array(my_shelf['stateOverTime']) sim.raycastData = np.array( my_shelf['raycastData']) sim.controlInputData = np.array(my_shelf['controlInputData']) sim.numTimesteps = my_shelf['numTimesteps'] sim.idxDict = my_shelf['idxDict'] sim.counter = my_shelf['counter'] my_shelf.close() return sim
from director import vtkAll as vtk from director import applogic from director import visualization as vis from director.timercallback import TimerCallback vtkRos = vtk.vtkRosInit() vtkRos.Start() reader = vtk.vtkRosGridMapSubscriber() reader.Start() print reader import time app = ConsoleApp() view = app.createView() def spin(): polyData = vtk.vtkPolyData() reader.GetMesh(polyData) vis.updatePolyData(polyData, 'mesh') print "Number of points (a)", polyData.GetNumberOfPoints() if (polyData.GetNumberOfPoints() == 0): return polyDataPC = vtk.vtkPolyData() reader.GetPointCloud(polyDataPC) vis.updatePolyData(polyDataPC, 'output') print "Number of points (b)", polyDataPC.GetNumberOfPoints()
from director.consoleapp import ConsoleApp app = ConsoleApp() view = app.createView() view.showMaximized() app.start()
from director import applogic from director import visualization as vis from director import vtkNumpy as vnp from director import objectmodel as om from director import screengrabberpanel from director import cameracontrol from director import filterUtils from director.fieldcontainer import FieldContainer from director.shallowCopy import shallowCopy from director.debugVis import DebugData from director import vtkAll as vtk import numpy as np import shutil app = ConsoleApp() # create a view view = app.createView() segmentation._defaultSegmentationView = view # load poly data dataDir = app.getTestingDataDirectory() filename = os.path.join(dataDir, 'terrain/tilted_steps_lidar.pcd') #filename = os.path.join(dataDir, 'BigBIRD/tapatio_hot_sauce/meshes/poisson.ply') #filename = os.path.join(dataDir, 'BigBIRD/cheez_it_white_cheddar/meshes/poisson.ply') name = 'terrain' polyData = ioUtils.readPolyData(filename) vis.showPolyData(polyData, name, visible=False)
d.addLine(origin, origin + rayTransformed * rayLength) color = [0, 1, 0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color) ######################### numRays = 20 angleMin = -np.pi / 2 angleMax = np.pi / 2 angleGrid = np.linspace(angleMin, angleMax, numRays) rays = np.zeros((3, numRays)) rays[0, :] = np.cos(angleGrid) rays[1, :] = np.sin(angleGrid) app = ConsoleApp() view = app.createView() world = buildWorld() robot = buildRobot() locator = buildCellLocator(world.polyData) frame = robot.getChildFrame() frame.setProperty('Edit', True) frame.widget.HandleRotationEnabledOff() rep = frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frame.connectFrameModified(updateIntersection)
from director import affordanceurdf from director import objectmodel as om from director import visualization as vis from director import lcmUtils from director import ioUtils from director.debugVis import DebugData from director.timercallback import TimerCallback from director import segmentation from director.uuidutil import newUUID from director import geometryencoder from director import sceneloader import drc as lcmdrc import os import json from director.utime import getUtime app = ConsoleApp() app.setupGlobals(globals()) app.showPythonConsole() view = app.createView() view.show() robotsystem.create(view, globals()) def affordanceFromDescription(desc): affordanceManager.collection.updateDescription(desc) return affordanceManager.getAffordanceById(desc['uuid']) def newBox():
class Simulator(object): def __init__(self, percentObsDensity=20, endTime=40, randomizeControl=False, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, supervisedTrainingTime=500, autoInitialize=True, verbose=True, sarsaType="discrete"): self.verbose = verbose self.randomizeControl = randomizeControl self.startSimTime = time.time() self.collisionThreshold = 1.3 self.randomSeed = 5 self.Sarsa_numInnerBins = 4 self.Sarsa_numOuterBins = 4 self.Sensor_rayLength = 8 self.sarsaType = sarsaType self.percentObsDensity = percentObsDensity self.supervisedTrainingTime = 10 self.learningRandomTime = 10 self.learningEvalTime = 10 self.defaultControllerTime = 10 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() # view = app.createView(useGrid=False) self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() if autoInitialize: self.initialize() def initializeOptions(self): self.options = dict() self.options['Reward'] = dict() self.options['Reward']['actionCost'] = 0.1 self.options['Reward']['collisionPenalty'] = 100.0 self.options['Reward']['raycastCost'] = 20.0 self.options['SARSA'] = dict() self.options['SARSA']['type'] = "discrete" self.options['SARSA']['lam'] = 0.7 self.options['SARSA']['alphaStepSize'] = 0.2 self.options['SARSA']['useQLearningUpdate'] = False self.options['SARSA']['epsilonGreedy'] = 0.2 self.options['SARSA']['burnInTime'] = 500 self.options['SARSA']['epsilonGreedyExponent'] = 0.3 self.options['SARSA'][ 'exponentialDiscountFactor'] = 0.05 #so gamma = e^(-rho*dt) self.options['SARSA']['numInnerBins'] = 5 self.options['SARSA']['numOuterBins'] = 4 self.options['SARSA']['binCutoff'] = 0.5 self.options['World'] = dict() self.options['World']['obstaclesInnerFraction'] = 0.85 self.options['World']['randomSeed'] = 40 self.options['World']['percentObsDensity'] = 7.5 self.options['World']['nonRandomWorld'] = True self.options['World']['circleRadius'] = 1.75 self.options['World']['scale'] = 1.0 self.options['Sensor'] = dict() self.options['Sensor']['rayLength'] = 10 self.options['Sensor']['numRays'] = 20 self.options['Car'] = dict() self.options['Car']['velocity'] = 12 self.options['dt'] = 0.05 self.options['runTime'] = dict() self.options['runTime']['supervisedTrainingTime'] = 10 self.options['runTime']['learningRandomTime'] = 10 self.options['runTime']['learningEvalTime'] = 10 self.options['runTime']['defaultControllerTime'] = 10 def setDefaultOptions(self): defaultOptions = dict() defaultOptions['Reward'] = dict() defaultOptions['Reward']['actionCost'] = 0.1 defaultOptions['Reward']['collisionPenalty'] = 100.0 defaultOptions['Reward']['raycastCost'] = 20.0 defaultOptions['SARSA'] = dict() defaultOptions['SARSA']['type'] = "discrete" defaultOptions['SARSA']['lam'] = 0.7 defaultOptions['SARSA']['alphaStepSize'] = 0.2 defaultOptions['SARSA']['useQLearningUpdate'] = False defaultOptions['SARSA']['useSupervisedTraining'] = True defaultOptions['SARSA']['epsilonGreedy'] = 0.2 defaultOptions['SARSA']['burnInTime'] = 500 defaultOptions['SARSA']['epsilonGreedyExponent'] = 0.3 defaultOptions['SARSA'][ 'exponentialDiscountFactor'] = 0.05 #so gamma = e^(-rho*dt) defaultOptions['SARSA']['numInnerBins'] = 5 defaultOptions['SARSA']['numOuterBins'] = 4 defaultOptions['SARSA']['binCutoff'] = 0.5 defaultOptions['SARSA']['forceDriveStraight'] = True defaultOptions['World'] = dict() defaultOptions['World']['obstaclesInnerFraction'] = 0.85 defaultOptions['World']['randomSeed'] = 40 defaultOptions['World']['percentObsDensity'] = 7.5 defaultOptions['World']['nonRandomWorld'] = True defaultOptions['World']['circleRadius'] = 1.75 defaultOptions['World']['scale'] = 1.0 defaultOptions['Sensor'] = dict() defaultOptions['Sensor']['rayLength'] = 10 defaultOptions['Sensor']['numRays'] = 20 defaultOptions['Car'] = dict() defaultOptions['Car']['velocity'] = 12 defaultOptions['dt'] = 0.05 defaultOptions['runTime'] = dict() defaultOptions['runTime']['supervisedTrainingTime'] = 10 defaultOptions['runTime']['learningRandomTime'] = 10 defaultOptions['runTime']['learningEvalTime'] = 10 defaultOptions['runTime']['defaultControllerTime'] = 10 for k in defaultOptions: self.options.setdefault(k, defaultOptions[k]) for k in defaultOptions: if not isinstance(defaultOptions[k], dict): continue for j in defaultOptions[k]: self.options[k].setdefault(j, defaultOptions[k][j]) def initializeColorMap(self): self.colorMap = dict() self.colorMap['defaultRandom'] = [0, 0, 1] self.colorMap['learnedRandom'] = [1.0, 0.54901961, 0.] # this is orange self.colorMap['learned'] = [0.58039216, 0.0, 0.82745098] # this is yellow self.colorMap['default'] = [0, 1, 0] def initialize(self): self.dt = self.options['dt'] self.controllerTypeOrder = [ 'defaultRandom', 'learnedRandom', 'learned', 'default' ] self.setDefaultOptions() self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'], numRays=self.options['Sensor']['numRays']) self.Controller = ControllerObj(self.Sensor) self.Car = CarPlant(controller=self.Controller, velocity=self.options['Car']['velocity']) self.Reward = Reward( self.Sensor, collisionThreshold=self.collisionThreshold, actionCost=self.options['Reward']['actionCost'], collisionPenalty=self.options['Reward']['collisionPenalty'], raycastCost=self.options['Reward']['raycastCost']) self.setSARSA() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName('world')) self.world = World.buildCircleWorld( percentObsDensity=self.options['World']['percentObsDensity'], circleRadius=self.options['World']['circleRadius'], nonRandom=self.options['World']['nonRandomWorld'], scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World'] ['obstaclesInnerFraction']) om.removeFromObjectModel(om.findObjectByName('robot')) self.robot, self.frame = World.buildRobot() self.locator = World.buildCellLocator(self.world.visObj.polyData) self.Sensor.setLocator(self.locator) self.frame = self.robot.getChildFrame() self.frame.setProperty('Scale', 3) self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.supervisedTrainingTime = self.options['runTime'][ 'supervisedTrainingTime'] self.learningRandomTime = self.options['runTime']['learningRandomTime'] self.learningEvalTime = self.options['runTime']['learningEvalTime'] self.defaultControllerTime = self.options['runTime'][ 'defaultControllerTime'] self.Car.setFrame(self.frame) print "Finished initialization" def setSARSA(self, type=None): if type is None: type = self.options['SARSA']['type'] if type == "discrete": self.Sarsa = SARSADiscrete( sensorObj=self.Sensor, actionSet=self.Controller.actionSet, collisionThreshold=self.collisionThreshold, alphaStepSize=self.options['SARSA']['alphaStepSize'], useQLearningUpdate=self.options['SARSA']['useQLearningUpdate'], lam=self.options['SARSA']['lam'], numInnerBins=self.options['SARSA']['numInnerBins'], numOuterBins=self.options['SARSA']['numOuterBins'], binCutoff=self.options['SARSA']['binCutoff'], burnInTime=self.options['SARSA']['burnInTime'], epsilonGreedy=self.options['SARSA']['epsilonGreedy'], epsilonGreedyExponent=self.options['SARSA'] ['epsilonGreedyExponent'], forceDriveStraight=self.options['SARSA']['forceDriveStraight']) elif type == "continuous": self.Sarsa = SARSAContinuous( sensorObj=self.Sensor, actionSet=self.Controller.actionSet, lam=self.options['SARSA']['lam'], alphaStepSize=self.options['SARSA']['alphaStepSize'], collisionThreshold=self.collisionThreshold, burnInTime=self.options['SARSA']['burnInTime'], epsilonGreedy=self.options['SARSA']['epsilonGreedy'], epsilonGreedyExponent=self.options['SARSA'] ['epsilonGreedyExponent']) else: raise ValueError( "sarsa type must be either discrete or continuous") def runSingleSimulation(self, updateQValues=True, controllerType='default', simulationCutoff=None): if self.verbose: print "using QValue based controller = ", useQValueController self.setCollisionFreeInitialState() currentCarState = np.copy(self.Car.state) nextCarState = np.copy(self.Car.state) self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2]) currentRaycast = self.Sensor.raycastAll(self.frame) nextRaycast = np.zeros(self.Sensor.numRays) self.Sarsa.resetElibilityTraces() # record the reward data runData = dict() reward = 0 discountedReward = 0 avgReward = 0 startIdx = self.counter while (self.counter < self.numTimesteps - 1): idx = self.counter currentTime = self.t[idx] self.stateOverTime[idx, :] = currentCarState x = self.stateOverTime[idx, 0] y = self.stateOverTime[idx, 1] theta = self.stateOverTime[idx, 2] self.setRobotFrameState(x, y, theta) # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2]) currentRaycast = self.Sensor.raycastAll(self.frame) self.raycastData[idx, :] = currentRaycast S_current = (currentCarState, currentRaycast) if controllerType not in self.colorMap.keys(): print raise ValueError("controller of type " + controllerType + " not supported") if controllerType in ["default", "defaultRandom"]: if controllerType == "defaultRandom": controlInput, controlInputIdx = self.Controller.computeControlInput( currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=True) else: controlInput, controlInputIdx = self.Controller.computeControlInput( currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=False) if controllerType in ["learned", "learnedRandom"]: if controllerType == "learned": randomizeControl = False else: randomizeControl = True counterForGreedyDecay = self.counter - self.idxDict[ "learnedRandom"] controlInput, controlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy( S_current, counter=counterForGreedyDecay, randomize=randomizeControl) self.emptyQValue[idx] = emptyQValue if emptyQValue and self.options['SARSA'][ 'useSupervisedTraining']: controlInput, controlInputIdx = self.Controller.computeControlInput( currentCarState, currentTime, self.frame, raycastDistance=currentRaycast, randomize=False) self.controlInputData[idx] = controlInput nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt) # want to compute nextRaycast so we can do the SARSA algorithm x = nextCarState[0] y = nextCarState[1] theta = nextCarState[2] self.setRobotFrameState(x, y, theta) nextRaycast = self.Sensor.raycastAll(self.frame) # Compute the next control input S_next = (nextCarState, nextRaycast) if controllerType in ["default", "defaultRandom"]: if controllerType == "defaultRandom": nextControlInput, nextControlInputIdx = self.Controller.computeControlInput( nextCarState, currentTime, self.frame, raycastDistance=nextRaycast, randomize=True) else: nextControlInput, nextControlInputIdx = self.Controller.computeControlInput( nextCarState, currentTime, self.frame, raycastDistance=nextRaycast, randomize=False) if controllerType in ["learned", "learnedRandom"]: if controllerType == "learned": randomizeControl = False else: randomizeControl = True counterForGreedyDecay = self.counter - self.idxDict[ "learnedRandom"] nextControlInput, nextControlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy( S_next, counter=counterForGreedyDecay, randomize=randomizeControl) if emptyQValue and self.options['SARSA'][ 'useSupervisedTraining']: nextControlInput, nextControlInputIdx = self.Controller.computeControlInput( nextCarState, currentTime, self.frame, raycastDistance=nextRaycast, randomize=False) # if useQValueController: # nextControlInput, nextControlInputIdx, emptyQValue = self.Sarsa.computeGreedyControlPolicy(S_next) # # if not useQValueController or emptyQValue: # nextControlInput, nextControlInputIdx = self.Controller.computeControlInput(nextCarState, currentTime, self.frame, # raycastDistance=nextRaycast) # compute the reward reward = self.Reward.computeReward(S_next, controlInput) self.rewardData[idx] = reward discountedReward += self.Sarsa.gamma**(self.counter - startIdx) * reward avgReward += reward ###### SARSA update if updateQValues: self.Sarsa.sarsaUpdate(S_current, controlInputIdx, reward, S_next, nextControlInputIdx) #bookkeeping currentCarState = nextCarState currentRaycast = nextRaycast self.counter += 1 # break if we are in collision if self.checkInCollision(nextRaycast): if self.verbose: print "Had a collision, terminating simulation" break if self.counter >= simulationCutoff: break # fill in the last state by hand self.stateOverTime[self.counter, :] = currentCarState self.raycastData[self.counter, :] = currentRaycast # return the total reward avgRewardNoCollisionPenalty = avgReward - reward avgReward = avgReward * 1.0 / max(1, self.counter - startIdx) avgRewardNoCollisionPenalty = avgRewardNoCollisionPenalty * 1.0 / max( 1, self.counter - startIdx) # this extra multiplication is so that it is in the same "units" as avgReward runData['discountedReward'] = discountedReward * (1 - self.Sarsa.gamma) runData['avgReward'] = avgReward runData['avgRewardNoCollisionPenalty'] = avgRewardNoCollisionPenalty # this just makes sure we don't get stuck in an infinite loop. if startIdx == self.counter: self.counter += 1 return runData def setNumpyRandomSeed(self, seed=1): np.random.seed(seed) def runBatchSimulation(self, endTime=None, dt=0.05): # for use in playback self.dt = self.options['dt'] self.Sarsa.setDiscountFactor(dt) self.endTime = self.supervisedTrainingTime + self.learningRandomTime + self.learningEvalTime + self.defaultControllerTime self.t = np.arange(0.0, self.endTime, dt) maxNumTimesteps = np.size(self.t) self.stateOverTime = np.zeros((maxNumTimesteps + 1, 3)) self.raycastData = np.zeros((maxNumTimesteps + 1, self.Sensor.numRays)) self.controlInputData = np.zeros(maxNumTimesteps + 1) self.rewardData = np.zeros(maxNumTimesteps + 1) self.emptyQValue = np.zeros(maxNumTimesteps + 1, dtype='bool') self.numTimesteps = maxNumTimesteps self.controllerTypeOrder = [ 'defaultRandom', 'learnedRandom', 'learned', 'default' ] self.counter = 0 self.simulationData = [] self.initializeStatusBar() self.idxDict = dict() numRunsCounter = 0 # three while loops for different phases of simulation, supervisedTraining, learning, default self.idxDict['defaultRandom'] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.supervisedTrainingTime / dt, self.numTimesteps) while ((self.counter - loopStartIdx < self.supervisedTrainingTime / dt) and self.counter < self.numTimesteps): self.printStatusBar() useQValueController = False startIdx = self.counter runData = self.runSingleSimulation(updateQValues=True, controllerType='defaultRandom', simulationCutoff=simCutoff) runData['startIdx'] = startIdx runData['controllerType'] = "defaultRandom" runData['duration'] = self.counter - runData['startIdx'] runData['endIdx'] = self.counter runData['runNumber'] = numRunsCounter numRunsCounter += 1 self.simulationData.append(runData) self.idxDict['learnedRandom'] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.learningRandomTime / dt, self.numTimesteps) while ((self.counter - loopStartIdx < self.learningRandomTime / dt) and self.counter < self.numTimesteps): self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(updateQValues=True, controllerType='learnedRandom', simulationCutoff=simCutoff) runData['startIdx'] = startIdx runData['controllerType'] = "learnedRandom" runData['duration'] = self.counter - runData['startIdx'] runData['endIdx'] = self.counter runData['runNumber'] = numRunsCounter numRunsCounter += 1 self.simulationData.append(runData) self.idxDict['learned'] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.learningEvalTime / dt, self.numTimesteps) while ((self.counter - loopStartIdx < self.learningEvalTime / dt) and self.counter < self.numTimesteps): self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(updateQValues=False, controllerType='learned', simulationCutoff=simCutoff) runData['startIdx'] = startIdx runData['controllerType'] = "learned" runData['duration'] = self.counter - runData['startIdx'] runData['endIdx'] = self.counter runData['runNumber'] = numRunsCounter numRunsCounter += 1 self.simulationData.append(runData) self.idxDict['default'] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.defaultControllerTime / dt, self.numTimesteps) while ((self.counter - loopStartIdx < self.defaultControllerTime / dt) and self.counter < self.numTimesteps - 1): self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(updateQValues=False, controllerType='default', simulationCutoff=simCutoff) runData['startIdx'] = startIdx runData['controllerType'] = "default" runData['duration'] = self.counter - runData['startIdx'] runData['endIdx'] = self.counter runData['runNumber'] = numRunsCounter numRunsCounter += 1 self.simulationData.append(runData) # BOOKKEEPING # truncate stateOverTime, raycastData, controlInputs to be the correct size self.numTimesteps = self.counter + 1 self.stateOverTime = self.stateOverTime[0:self.counter + 1, :] self.raycastData = self.raycastData[0:self.counter + 1, :] self.controlInputData = self.controlInputData[0:self.counter + 1] self.rewardData = self.rewardData[0:self.counter + 1] self.endTime = 1.0 * self.counter / self.numTimesteps * self.endTime def initializeStatusBar(self): self.numTicks = 10 self.nextTickComplete = 1.0 / float(self.numTicks) self.nextTickIdx = 1 print "Simulation percentage complete: (", self.numTicks, " # is complete)" def printStatusBar(self): fractionDone = float(self.counter) / float(self.numTimesteps) if fractionDone > self.nextTickComplete: self.nextTickIdx += 1 self.nextTickComplete += 1.0 / self.numTicks timeSoFar = time.time() - self.startSimTime estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0 print "#" * self.nextTickIdx, "-" * ( self.numTicks - self.nextTickIdx ), "estimated", estimatedTimeLeft_minutes, "minutes left" def setCollisionFreeInitialState(self): tol = 5 while True: x = np.random.uniform(self.world.Xmin + tol, self.world.Xmax - tol, 1)[0] y = np.random.uniform(self.world.Ymin + tol, self.world.Ymax - tol, 1)[0] theta = np.random.uniform(0, 2 * np.pi, 1)[0] self.Car.setCarState(x, y, theta) self.setRobotFrameState(x, y, theta) if not self.checkInCollision(): break # if self.checkInCollision(): # print "IN COLLISION" # else: # print "COLLISION FREE" return x, y, theta def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0 / self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) l.addWidget(panel) w.showMaximized() self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2, 0, -1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter / elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.app.start() def run(self, launchApp=True): self.counter = 1 self.runBatchSimulation() # self.Sarsa.plotWeights() if launchApp: self.setupPlayback() def updateDrawIntersection(self, frame): origin = np.array(frame.transform.GetPosition()) #print "origin is now at", origin d = DebugData() sliderIdx = self.slider.value controllerType = self.getControllerTypeFromCounter(sliderIdx) colorMaxRange = self.colorMap[controllerType] # if the QValue was empty then color it green if self.emptyQValue[sliderIdx]: colorMaxRange = [1, 1, 0] # this is yellow for i in xrange(self.Sensor.numRays): ray = self.Sensor.rays[:, i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) #print "rayTransformed is", rayTransformed intersection = self.Sensor.raycast( self.locator, origin, origin + rayTransformed * self.Sensor.rayLength) if intersection is not None: d.addLine(origin, intersection, color=[1, 0, 0]) else: d.addLine(origin, origin + rayTransformed * self.Sensor.rayLength, color=colorMaxRange) vis.updatePolyData(d.getPolyData(), 'rays', colorByName='RGB255') #camera = self.view.camera() #camera.SetFocalPoint(frame.transform.GetPosition()) #camera.SetPosition(frame.transform.TransformPoint((-30,0,10))) def getControllerTypeFromCounter(self, counter): name = self.controllerTypeOrder[0] for controllerType in self.controllerTypeOrder[1:]: if counter >= self.idxDict[controllerType]: name = controllerType return name def setRobotFrameState(self, x, y, theta): t = vtk.vtkTransform() t.Translate(x, y, 0.0) t.RotateZ(np.degrees(theta)) self.robot.getChildFrame().copyFrame(t) # returns true if we are in collision def checkInCollision(self, raycastDistance=None): if raycastDistance is None: self.setRobotFrameState(self.Car.state[0], self.Car.state[1], self.Car.state[2]) raycastDistance = self.Sensor.raycastAll(self.frame) if np.min(raycastDistance) < self.collisionThreshold: return True else: return False def tick(self): #print timer.elapsed #simulate(t.elapsed) x = np.sin(time.time()) y = np.cos(time.time()) self.setRobotFrameState(x, y, 0.0) if (time.time() - self.playTime) > self.endTime: self.playTimer.stop() def tick2(self): newtime = time.time() - self.playTime print time.time() - self.playTime x = np.sin(newtime) y = np.cos(newtime) self.setRobotFrameState(x, y, 0.0) # just increment the slider, stop the timer if we get to the end def playTimerCallback(self): self.sliderMovedByPlayTimer = True currentIdx = self.slider.value nextIdx = currentIdx + 1 self.slider.setSliderPosition(nextIdx) if currentIdx >= self.sliderMax: print "reached end of tape, stopping playTime" self.playTimer.stop() def onSliderChanged(self, value): if not self.sliderMovedByPlayTimer: self.playTimer.stop() numSteps = len(self.stateOverTime) idx = int(np.floor(numSteps * (1.0 * value / self.sliderMax))) idx = min(idx, numSteps - 1) x, y, theta = self.stateOverTime[idx] self.setRobotFrameState(x, y, theta) self.sliderMovedByPlayTimer = False def onPlayButton(self): if self.playTimer.isActive(): self.onPauseButton() return print 'play' self.playTimer.start() self.playTime = time.time() def onPauseButton(self): print 'pause' self.playTimer.stop() def computeRunStatistics(self): numRuns = len(self.simulationData) runStart = np.zeros(numRuns) runDuration = np.zeros(numRuns) grid = np.arange(1, numRuns + 1) discountedReward = np.zeros(numRuns) avgReward = np.zeros(numRuns) avgRewardNoCollisionPenalty = np.zeros(numRuns) idxMap = dict() for controllerType, color in self.colorMap.iteritems(): idxMap[controllerType] = np.zeros(numRuns, dtype=bool) for idx, val in enumerate(self.simulationData): runStart[idx] = val['startIdx'] runDuration[idx] = val['duration'] discountedReward[idx] = val['discountedReward'] avgReward[idx] = val['avgReward'] avgRewardNoCollisionPenalty[idx] = val[ 'avgRewardNoCollisionPenalty'] controllerType = val['controllerType'] idxMap[controllerType][idx] = True def plotRunData(self, controllerTypeToPlot=None, showPlot=True, onlyLearned=False): if controllerTypeToPlot == None: controllerTypeToPlot = self.colorMap.keys() if onlyLearned: controllerTypeToPlot = ['learnedRandom', 'learned'] numRuns = len(self.simulationData) runStart = np.zeros(numRuns) runDuration = np.zeros(numRuns) grid = np.arange(1, numRuns + 1) discountedReward = np.zeros(numRuns) avgReward = np.zeros(numRuns) avgRewardNoCollisionPenalty = np.zeros(numRuns) idxMap = dict() for controllerType, color in self.colorMap.iteritems(): idxMap[controllerType] = np.zeros(numRuns, dtype=bool) for idx, val in enumerate(self.simulationData): runStart[idx] = val['startIdx'] runDuration[idx] = val['duration'] discountedReward[idx] = val['discountedReward'] avgReward[idx] = val['avgReward'] avgRewardNoCollisionPenalty[idx] = val[ 'avgRewardNoCollisionPenalty'] controllerType = val['controllerType'] idxMap[controllerType][idx] = True # usedQValueController[idx] = (val['controllerType'] == "QValue") # usedDefaultController[idx] = (val['controllerType'] == "default") # usedDefaultRandomController[idx] = (val['controllerType'] == "defaultRandom") # usedQValueRandomController[idx] = (val['controllerType'] == "QValueRandom") self.runStatistics = dict() dataMap = { 'duration': runDuration, 'discountedReward': discountedReward, 'avgReward': avgReward, 'avgRewardNoCollisionPenalty': avgRewardNoCollisionPenalty } def computeRunStatistics(dataMap): for controllerType, idx in idxMap.iteritems(): d = dict() for dataName, dataSet in dataMap.iteritems(): # average the appropriate values in dataset d[dataName] = np.sum( dataSet[idx]) / (1.0 * np.size(dataSet[idx])) self.runStatistics[controllerType] = d computeRunStatistics(dataMap) if not showPlot: return plt.figure() # # idxDefaultRandom = np.where(usedDefaultRandomController==True)[0] # idxQValueController = np.where(usedQValueController==True)[0] # idxQValueControllerRandom = np.where(usedQValueControllerRandom==True)[0] # idxDefault = np.where(usedDefaultController==True)[0] # # plotData = dict() # plotData['defaultRandom'] = {'idx': idxDefaultRandom, 'color': 'b'} # plotData['QValue'] = {'idx': idxQValueController, 'color': 'y'} # plotData['default'] = {'idx': idxDefault, 'color': 'g'} def scatterPlot(dataToPlot): for controllerType in controllerTypeToPlot: idx = idxMap[controllerType] plt.scatter(grid[idx], dataToPlot[idx], color=self.colorMap[controllerType]) def barPlot(dataName): plt.title(dataName) barWidth = 0.5 barCounter = 0 index = np.arange(len(controllerTypeToPlot)) for controllerType in controllerTypeToPlot: val = self.runStatistics[controllerType] plt.bar(barCounter, val[dataName], barWidth, color=self.colorMap[controllerType], label=controllerType) barCounter += 1 plt.xticks(index + barWidth / 2.0, controllerTypeToPlot) plt.subplot(4, 1, 1) plt.title('run duration') scatterPlot(runDuration) # for controllerType, idx in idxMap.iteritems(): # plt.scatter(grid[idx], runDuration[idx], color=self.colorMap[controllerType]) # plt.scatter(runStart[idxDefaultRandom], runDuration[idxDefaultRandom], color='b') # plt.scatter(runStart[idxQValueController], runDuration[idxQValueController], color='y') # plt.scatter(runStart[idxDefault], runDuration[idxDefault], color='g') plt.xlabel('run #') plt.ylabel('episode duration') plt.subplot(2, 1, 2) plt.title('discounted reward') scatterPlot(discountedReward) # for key, val in plotData.iteritems(): # plt.scatter(grid[idx], discountedReward[idx], color=self.colorMap[controllerType]) # plt.subplot(3,1,3) # plt.title("average reward") # scatterPlot(avgReward) # for key, val in plotData.iteritems(): # plt.scatter(grid[val['idx']],avgReward[val['idx']], color=val['color']) # plt.subplot(4,1,4) # plt.title("average reward no collision penalty") # scatterPlot(avgRewardNoCollisionPenalty) # # for key, val in plotData.iteritems(): # # plt.scatter(grid[val['idx']],avgRewardNoCollisionPenalty[val['idx']], color=val['color']) ## plot summary statistics plt.figure() plt.subplot(2, 1, 1) barPlot("duration") plt.subplot(2, 1, 2) barPlot("discountedReward") # plt.subplot(3,1,3) # barPlot("avgReward") # # plt.subplot(4,1,4) # barPlot("avgRewardNoCollisionPenalty") plt.show() def plotMultipleRunData(self, simList, toPlot=['duration', 'discountedReward'], controllerType='learned'): plt.figure() numPlots = len(toPlot) grid = np.arange(len(simList)) def plot(fieldToPlot, plotNum): plt.subplot(numPlots, 1, plotNum) plt.title(fieldToPlot) val = 0 * grid barWidth = 0.5 barCounter = 0 for idx, sim in enumerate(simList): value = sim.runStatistics[controllerType][fieldToPlot] plt.bar(idx, value, barWidth) counter = 1 for fieldToPlot in toPlot: plot(fieldToPlot, counter) counter += 1 plt.show() def saveToFile(self, filename): # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime filename = 'data/' + filename + ".out" my_shelf = shelve.open(filename, 'n') my_shelf['options'] = self.options if self.options['SARSA']['type'] == "discrete": my_shelf['SARSA_QValues'] = self.Sarsa.QValues my_shelf['simulationData'] = self.simulationData my_shelf['stateOverTime'] = self.stateOverTime my_shelf['raycastData'] = self.raycastData my_shelf['controlInputData'] = self.controlInputData my_shelf['emptyQValue'] = self.emptyQValue my_shelf['numTimesteps'] = self.numTimesteps my_shelf['idxDict'] = self.idxDict my_shelf['counter'] = self.counter my_shelf.close() @staticmethod def loadFromFile(filename): filename = 'data/' + filename + ".out" sim = Simulator(autoInitialize=False, verbose=False) my_shelf = shelve.open(filename) sim.options = my_shelf['options'] sim.initialize() if sim.options['SARSA']['type'] == "discrete": sim.Sarsa.QValues = np.array(my_shelf['SARSA_QValues']) sim.simulationData = my_shelf['simulationData'] # sim.runStatistics = my_shelf['runStatistics'] sim.stateOverTime = np.array(my_shelf['stateOverTime']) sim.raycastData = np.array(my_shelf['raycastData']) sim.controlInputData = np.array(my_shelf['controlInputData']) sim.emptyQValue = np.array(my_shelf['emptyQValue']) sim.numTimesteps = my_shelf['numTimesteps'] sim.idxDict = my_shelf['idxDict'] sim.counter = my_shelf['counter'] my_shelf.close() return sim
from director import roboturdf from director import jointcontrol from director import planplayback from director import playbackpanel from director import robotplanlistener from director import robotviewbehaviors from director import pointcloudlcm from director import cameraview from director import lcmUtils from PythonQt import QtCore, QtGui import drc as lcmdrc import bot_core as lcmbotcore # create the application app = ConsoleApp() view = app.createView() # load robot state model robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot model', view, colorMode='Textures') # listen for robot state updates robotStateJointController.addLCMUpdater('EST_ROBOT_STATE') # reload urdf from model publisher lcm roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)]) # load playback model playbackRobotModel, playbackJointController = roboturdf.loadRobotModel('playback robot model', view, color=roboturdf.getRobotOrangeColor(), visible=False) # initialize the playback panel
class AtlasCommandPanel(object): def __init__(self): self.app = ConsoleApp() self.view = self.app.createView() self.robotSystem = robotsystem.create(self.view) self.config = drcargs.getDirectorConfig() jointGroups = self.config['teleopJointGroups'] self.jointTeleopPanel = JointTeleopPanel(self.robotSystem, jointGroups) self.jointCommandPanel = JointCommandPanel(self.robotSystem) self.jointCommandPanel.ui.speedSpinBox.setEnabled(False) self.jointCommandPanel.ui.mirrorArmsCheck.setChecked( self.jointTeleopPanel.mirrorArms) self.jointCommandPanel.ui.mirrorLegsCheck.setChecked( self.jointTeleopPanel.mirrorLegs) self.jointCommandPanel.ui.resetButton.connect( 'clicked()', self.resetJointTeleopSliders) self.jointCommandPanel.ui.mirrorArmsCheck.connect( 'clicked()', self.mirrorJointsChanged) self.jointCommandPanel.ui.mirrorLegsCheck.connect( 'clicked()', self.mirrorJointsChanged) self.widget = QtGui.QWidget() gl = QtGui.QGridLayout(self.widget) gl.addWidget(self.app.showObjectModel(), 0, 0, 4, 1) # row, col, rowspan, colspan gl.addWidget(self.view, 0, 1, 1, 1) gl.addWidget(self.jointCommandPanel.widget, 1, 1, 1, 1) gl.addWidget(self.jointTeleopPanel.widget, 0, 2, -1, 1) gl.setRowStretch(0, 1) gl.setColumnStretch(1, 1) #self.sub = lcmUtils.addSubscriber('COMMITTED_ROBOT_PLAN', lcmdrc.robot_plan_t, self.onRobotPlan) lcmUtils.addSubscriber('STEERING_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) lcmUtils.addSubscriber('THROTTLE_COMMAND_POSITION_GOAL', lcmdrc.joint_position_goal_t, self.onSingleJointPositionGoal) def onSingleJointPositionGoal(self, msg): jointPositionGoal = msg.joint_position jointName = msg.joint_name allowedJointNames = ['l_leg_aky', 'l_arm_lwy'] if not (jointName in allowedJointNames): print 'Position goals are not allowed for this joint' print 'ignoring this position goal' print 'use the sliders instead' return if (jointName == 'l_arm_lwy') and ( not self.jointCommandPanel.steeringControlEnabled): print 'Steering control not enabled' print 'ignoring steering command' return if (jointName == 'l_leg_aky') and ( not self.jointCommandPanel.throttleControlEnabled): print 'Throttle control not enabled' print 'ignoring throttle command' return jointIdx = self.jointTeleopPanel.toJointIndex(joint_name) self.jointTeleopPanel.endPose[jointIdx] = jointPositionGoal self.jointTeleopPanel.updateSliders() self.jointTeleopPanel.sliderChanged(jointName) def onRobotPlan(self, msg): playback = planplayback.PlanPlayback() playback.interpolationMethod = 'pchip' poseTimes, poses = playback.getPlanPoses(msg) f = playback.getPoseInterpolator(poseTimes, poses) jointController = self.robotSystem.teleopJointController timer = simpletimer.SimpleTimer() def setPose(pose): jointController.setPose('plan_playback', pose) self.jointTeleopPanel.endPose = pose self.jointTeleopPanel.updateSliders() commandStream.setGoalPose(pose) def updateAnimation(): tNow = timer.elapsed() if tNow > poseTimes[-1]: pose = poses[-1] setPose(pose) return False pose = f(tNow) setPose(pose) self.animationTimer = TimerCallback() self.animationTimer.targetFps = 60 self.animationTimer.callback = updateAnimation self.animationTimer.start() def mirrorJointsChanged(self): self.jointTeleopPanel.mirrorLegs = self.jointCommandPanel.ui.mirrorLegsCheck.checked self.jointTeleopPanel.mirrorArms = self.jointCommandPanel.ui.mirrorArmsCheck.checked def resetJointTeleopSliders(self): self.jointTeleopPanel.resetPoseToRobotState()
class Simulator(object): def __init__(self, percentObsDensity=20, endTime=40, nonRandomWorld=False, circleRadius=0.7, worldScale=1.0, autoInitialize=True, verbose=True): self.verbose = verbose self.startSimTime = time.time() self.collisionThreshold = 1.0 self.randomSeed = 5 self.percentObsDensity = percentObsDensity self.defaultControllerTime = 1000 self.nonRandomWorld = nonRandomWorld self.circleRadius = circleRadius self.worldScale = worldScale # create the visualizer object self.app = ConsoleApp() self.view = self.app.createView(useGrid=False) self.initializeOptions() self.initializeColorMap() self.initial_XVelocity = 0.0 self.initial_YVelocity = 0.0 self.running_sim = True self.currentIdx = 0; if autoInitialize: self.initialize() def initializeOptions(self): self.options = dict() self.options['World'] = dict() self.options['World']['obstaclesInnerFraction'] = 0.98 self.options['World']['randomSeed'] = 40 self.options['World']['percentObsDensity'] = 15.0 self.options['World']['nonRandomWorld'] = True self.options['World']['circleRadius'] = 0.6 self.circleRadius = self.options['World']['circleRadius'] self.options['World']['scale'] = 2.0 self.options['Car'] = dict() self.options['Car']['velocity'] = 20 self.options['dt'] = 0.05 self.options['runTime'] = dict() self.options['runTime']['defaultControllerTime'] = 100 def setDefaultOptions(self): defaultOptions = dict() defaultOptions['World'] = dict() defaultOptions['World']['obstaclesInnerFraction'] = 0.98 defaultOptions['World']['randomSeed'] = 40 defaultOptions['World']['percentObsDensity'] = 30 defaultOptions['World']['nonRandomWorld'] = True defaultOptions['World']['circleRadius'] = 1.75 defaultOptions['World']['scale'] = 2.5 defaultOptions['Car'] = dict() defaultOptions['Car']['velocity'] = 20 defaultOptions['dt'] = 0.05 defaultOptions['runTime'] = dict() defaultOptions['runTime']['defaultControllerTime'] = 100 for k in defaultOptions: self.options.setdefault(k, defaultOptions[k]) for k in defaultOptions: if not isinstance(defaultOptions[k], dict): continue for j in defaultOptions[k]: self.options[k].setdefault(j, defaultOptions[k][j]) def initializeColorMap(self): self.colorMap = dict() self.colorMap['default'] = [0,1,0] def initialize(self): self.dt = self.options['dt'] self.controllerTypeOrder = ['default'] self.setDefaultOptions() self.Controller = ControllerObj() self.Car = CarPlant(controller=self.Controller, velocity=self.options['Car']['velocity']) self.Controller.initializeVelocity(self.Car.v) self.ActionSet = ActionSetObj() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName('world')) self.world = World.buildCircleWorld(percentObsDensity=self.options['World']['percentObsDensity'], circleRadius=self.options['World']['circleRadius'], nonRandom=self.options['World']['nonRandomWorld'], scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction']) om.removeFromObjectModel(om.findObjectByName('robot')) self.robot, self.frame = World.buildRobot() self.locator = World.buildCellLocator(self.world.visObj.polyData) #will need to set locator for purple trajectories #self.Sensor.setLocator(self.locator) self.frame = self.robot.getChildFrame() self.frame.setProperty('Scale', 3) #self.frame.setProperty('Visible', False) #self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.defaultControllerTime = self.options['runTime']['defaultControllerTime'] self.Car.setFrame(self.frame) print "Finished initialization" def rankTrajectories(self): # access the trajectories x_traj = self.ActionSet.p_x_trajectories y_traj = self.ActionSet.p_y_trajectories final_x_positions = x_traj[:,-1] final_y_positions = y_traj[:,-1] #access the global goal global_goal_x = self.globalGoal.global_goal_x global_goal_y = self.globalGoal.global_goal_y ranks_with_indices = [] def getKey(item): return item[1] final_distances_squared = np.zeros((len(final_x_positions),len(final_y_positions))) for i in xrange(len(final_x_positions)): for j in xrange(len(final_y_positions)): distance = (final_x_positions[i] - global_goal_x)**2 + (final_y_positions[j] - global_goal_y)**2 ranks_with_indices.append(((i,j), distance)) sorted_ranks_with_indices = sorted(ranks_with_indices,key=getKey) return sorted_ranks_with_indices def CheckIfCollisionFreeTrajectoryIndex(self, traj_index): # accessing the right trajectory x_trajectory_to_check = self.ActionSet.p_x_trajectories[traj_index[0]] y_trajectory_to_check = self.ActionSet.p_y_trajectories[traj_index[1]] #check if anything is too close to the center of the circles #print self.world.list_of_circles for i in range(len(x_trajectory_to_check)): point = ( x_trajectory_to_check[i], y_trajectory_to_check[i] ) if not self.CheckIfCollisionFreePoint(point): #print "trajectory wasn't free" return False return True def CheckIfCollisionFreePoint(self, point): for circle_center in self.world.list_of_circles: #print "circle centers first" #print circle_center[0], circle_center[1] #print self.circleRadius #print point[0], point[1] #print (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2 if ( (circle_center[0] - point[0])**2 + (circle_center[1]-point[1])**2 < self.circleRadius**2): #print "I found a point in a circle" return False if point[0] > self.world.Xmax: #print "I would have gone past top wall" return False if point[0] < self.world.Xmin: #print "I would have gone below bottom wall" return False if point[1] > self.world.Ymax: #print "I would have gone to the right of the side wall" return False if point[1] < self.world.Ymin: #print "I would have gone to the left of the side wall" return False return True def runSingleSimulation(self, controllerType='default', simulationCutoff=None): self.Car.setCarState(0.0,0.0,self.initial_XVelocity,self.initial_YVelocity) self.setRobotFrameState(0.0,0.0,0.0) currentCarState = np.copy(self.Car.state) nextCarState = np.copy(self.Car.state) self.setRobotFrameState(currentCarState[0], currentCarState[1], currentCarState[2]) # record the reward data runData = dict() startIdx = self.counter while (self.counter < self.numTimesteps - 1): idx = self.counter currentTime = self.t[idx] self.stateOverTime[idx,:] = currentCarState x = self.stateOverTime[idx,0] y = self.stateOverTime[idx,1] self.setRobotFrameState(x,y,0.0) # self.setRobotState(currentCarState[0], currentCarState[1], currentCarState[2]) if controllerType not in self.colorMap.keys(): print raise ValueError("controller of type " + controllerType + " not supported") # compute all trajectories from action set self.ActionSet.computeAllPositions(currentCarState[0], currentCarState[1],currentCarState[2],currentCarState[3]) sorted_ranks_with_indices = self.rankTrajectories() #method #inputs: trajectories, global goal #outputs: sorted list of indexes by desirability global goal #method #input: trajectory #output: is collision free or not for traj in sorted_ranks_with_indices: if self.CheckIfCollisionFreeTrajectoryIndex(traj[0]): traj_to_use_index = traj[0] break traj_to_use_index = traj[0] x_index_to_use = traj_to_use_index[0] y_index_to_use = traj_to_use_index[1] x_accel = self.ActionSet.a_x[x_index_to_use] y_accel = self.ActionSet.a_y[y_index_to_use] controlInput = [x_accel, y_accel] self.controlInputData[idx] = controlInput nextCarState = self.Car.simulateOneStep(controlInput=controlInput, dt=self.dt) x = nextCarState[0] y = nextCarState[1] theta = nextCarState[2] self.setRobotFrameState(x,y,theta) #bookkeeping currentCarState = nextCarState self.counter+=1 # break if we are in collision if self.counter >= simulationCutoff: break # fill in the last state by hand self.stateOverTime[self.counter,:] = currentCarState # this just makes sure we don't get stuck in an infinite loop. if startIdx == self.counter: self.counter += 1 return runData def setNumpyRandomSeed(self, seed=1): np.random.seed(seed) def runBatchSimulation(self, endTime=None, dt=0.05): # for use in playback self.dt = self.options['dt'] self.endTime = self.defaultControllerTime # used to be the sum of the other times as well self.t = np.arange(0.0, self.endTime, dt) maxNumTimesteps = np.size(self.t) self.stateOverTime = np.zeros((maxNumTimesteps+1, 4)) self.controlInputData = np.zeros((maxNumTimesteps+1,2)) self.numTimesteps = maxNumTimesteps self.controllerTypeOrder = ['default'] self.counter = 0 self.simulationData = [] self.initializeStatusBar() self.idxDict = dict() numRunsCounter = 0 self.idxDict['default'] = self.counter loopStartIdx = self.counter simCutoff = min(loopStartIdx + self.defaultControllerTime/dt, self.numTimesteps) while ((self.counter - loopStartIdx < self.defaultControllerTime/dt) and self.counter < self.numTimesteps-1): self.printStatusBar() startIdx = self.counter runData = self.runSingleSimulation(controllerType='default', simulationCutoff=simCutoff) runData['startIdx'] = startIdx runData['controllerType'] = "default" runData['duration'] = self.counter - runData['startIdx'] runData['endIdx'] = self.counter runData['runNumber'] = numRunsCounter numRunsCounter+=1 self.simulationData.append(runData) # BOOKKEEPING # truncate stateOverTime, raycastData, controlInputs to be the correct size self.numTimesteps = self.counter + 1 self.stateOverTime = self.stateOverTime[0:self.counter+1, :] self.controlInputData = self.controlInputData[0:self.counter+1] self.endTime = 1.0*self.counter/self.numTimesteps*self.endTime def initializeStatusBar(self): self.numTicks = 10 self.nextTickComplete = 1.0 / float(self.numTicks) self.nextTickIdx = 1 print "Simulation percentage complete: (", self.numTicks, " # is complete)" def printStatusBar(self): fractionDone = float(self.counter) / float(self.numTimesteps) if fractionDone > self.nextTickComplete: self.nextTickIdx += 1 self.nextTickComplete += 1.0 / self.numTicks timeSoFar = time.time() - self.startSimTime estimatedTimeLeft_sec = (1 - fractionDone) * timeSoFar / fractionDone estimatedTimeLeft_minutes = estimatedTimeLeft_sec / 60.0 print "#" * self.nextTickIdx, "-" * (self.numTicks - self.nextTickIdx), "estimated", estimatedTimeLeft_minutes, "minutes left" def setCollisionFreeInitialState(self): tol = 5 while True: x = 0.0 y = -5.0 theta = 0 #+ np.random.uniform(0,2*np.pi,1)[0] * 0.01 self.Car.setCarState(x,y,theta) self.setRobotFrameState(x,y,theta) print "In loop" if not self.checkInCollision(): break return x,y,theta def setRandomCollisionFreeInitialState(self): tol = 5 while True: x = np.random.uniform(self.world.Xmin+tol, self.world.Xmax-tol, 1)[0] y = np.random.uniform(self.world.Ymin+tol, self.world.Ymax-tol, 1)[0] #theta = np.random.uniform(0,2*np.pi,1)[0] theta = 0 #always forward self.Car.setCarState(x,y,self.initial_XVelocity,self.initial_YVelocity) self.setRobotFrameState(x,y,theta) if not self.checkInCollision(): break self.firstRandomCollisionFreeInitialState_x = x self.firstRandomCollisionFreeInitialState_y = y return x,y,0,0 def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0/self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) self.max_velocity = 20.0 sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged) sliderXVelocity.setMaximum(self.max_velocity) sliderXVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderXVelocity) sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged) sliderYVelocity.setMaximum(self.max_velocity) sliderYVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderYVelocity) randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal') randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton) l.addWidget(randomGlobalGoalButton) drawActionSetButton = QtGui.QPushButton('Draw Action Set') drawActionSetButton.connect('clicked()', self.onDrawActionSetButton) l.addWidget(drawActionSetButton) runSimButton = QtGui.QPushButton('Simulate') runSimButton.connect('clicked()', self.onRunSimButton) l.addWidget(runSimButton) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() # need to connect frames with DrawActionSet self.frame.connectFrameModified(self.updateDrawActionSet) self.updateDrawActionSet(self.frame) # self.frame.connectFrameModified(self.updateDrawIntersection) # self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2,0,-1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter/elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.app.start() def run(self, launchApp=True): self.running_sim = True self.onRandomGlobalGoalButton() self.counter = 1 self.runBatchSimulation() self.running_sim = False print "my state over time is", self.stateOverTime if launchApp: self.setupPlayback() def getControllerTypeFromCounter(self, counter): name = self.controllerTypeOrder[0] for controllerType in self.controllerTypeOrder[1:]: if counter >= self.idxDict[controllerType]: name = controllerType return name def setRobotFrameState(self, x, y, theta): t = vtk.vtkTransform() t.Translate(x,y,0.0) t.RotateZ(np.degrees(theta)) self.robot.getChildFrame().copyFrame(t) # returns true if we are in collision def checkInCollision(self): return False def updateDrawActionSet(self, frame): origin = np.array(frame.transform.GetPosition()) if not self.running_sim: self.ActionSet.computeAllPositions(self.stateOverTime[self.currentIdx,0], self.stateOverTime[self.currentIdx,1], self.stateOverTime[self.currentIdx,2],self.stateOverTime[self.currentIdx,3]) #self.ActionSet.drawActionSetFinal() self.ActionSet.drawActionSetFull() def tick(self): #print timer.elapsed #simulate(t.elapsed) x = np.sin(time.time()) y = np.cos(time.time()) self.setRobotFrameState(x,y,0.0) if (time.time() - self.playTime) > self.endTime: self.playTimer.stop() def tick2(self): newtime = time.time() - self.playTime print time.time() - self.playTime x = np.sin(newtime) y = np.cos(newtime) self.setRobotFrameState(x,y,0.0) # just increment the slider, stop the timer if we get to the end def playTimerCallback(self): self.sliderMovedByPlayTimer = True currentIdx = self.slider.value nextIdx = currentIdx + 1 self.slider.setSliderPosition(nextIdx) if currentIdx >= self.sliderMax: print "reached end of tape, stopping playTime" self.playTimer.stop() def onXVelocityChanged(self, value): self.initial_XVelocity = value print "initial x velocity changed to ", value def onYVelocityChanged(self, value): self.initial_YVelocity = -value print "initial y velocity changed to ", -value def onSliderChanged(self, value): if not self.sliderMovedByPlayTimer: self.playTimer.stop() numSteps = len(self.stateOverTime) idx = int(np.floor(numSteps*(1.0*value/self.sliderMax))) idx = min(idx, numSteps-1) self.currentIdx = idx x,y, xdot, ydot = self.stateOverTime[idx] self.setRobotFrameState(x,y,0) self.sliderMovedByPlayTimer = False def onRandomGlobalGoalButton(self): print "random global goal button pressed" self.globalGoal = World.buildGlobalGoal(scale=self.options['World']['scale']) def onDrawActionSetButton(self): print "drawing action set" #self.ActionSet.computeFinalPositions(self.XVelocity_drawing,self.YVelocity_drawing) self.ActionSet.computeAllPositions(self.Car.state[0], self.Car.state[1], self.initial_XVelocity, self.initial_YVelocity) #self.ActionSet.drawActionSetFinal() self.ActionSet.drawActionSetFull() def onRunSimButton(self): self.running_sim = True self.runBatchSimulation() print "my state over time is", self.stateOverTime self.running_sim = False #self.saveToFile("latest") def onPlayButton(self): if self.playTimer.isActive(): self.onPauseButton() return print 'play' self.playTimer.start() self.playTime = time.time() def onPauseButton(self): print 'pause' self.playTimer.stop() def saveToFile(self, filename): # should also save the run data if it is available, i.e. stateOverTime, rewardOverTime filename = 'data/' + filename + ".out" my_shelf = shelve.open(filename,'n') my_shelf['options'] = self.options my_shelf['simulationData'] = self.simulationData my_shelf['stateOverTime'] = self.stateOverTime my_shelf['controlInputData'] = self.controlInputData my_shelf['numTimesteps'] = self.numTimesteps my_shelf['idxDict'] = self.idxDict my_shelf['counter'] = self.counter my_shelf.close() @staticmethod def loadFromFile(filename): filename = 'data/' + filename + ".out" sim = Simulator(autoInitialize=False, verbose=False) my_shelf = shelve.open(filename) sim.options = my_shelf['options'] sim.initialize() sim.simulationData = my_shelf['simulationData'] sim.stateOverTime = np.array(my_shelf['stateOverTime']) sim.controlInputData = np.array(my_shelf['controlInputData']) sim.numTimesteps = my_shelf['numTimesteps'] sim.idxDict = my_shelf['idxDict'] sim.counter = my_shelf['counter'] my_shelf.close() return sim