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 main():
app = ConsoleApp()
camVis = CameraVisualizer()
camVis.createUI()
camVis.showUI()
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.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 __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):
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 __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()
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
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 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)
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()
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
def debugMain():
listener = DebugAtlasCommandListener()
ConsoleApp.start()
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 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()
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
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()
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