def showToolbarWidget(self):
if app.getMainWindow() is None:
return
if self.toolbarWidget:
self.execButton.setEnabled(True)
return
w = QtGui.QWidget()
l = QtGui.QHBoxLayout(w)
label = QtGui.QLabel('Walk plan:')
execButton = QtGui.QPushButton('')
execButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_MediaPlay))
clearButton = QtGui.QPushButton('')
clearButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_TrashIcon))
stopButton = QtGui.QPushButton('')
stopButton.setIcon(QtGui.QApplication.style().standardIcon(QtGui.QStyle.SP_MediaStop))
l.addWidget(label)
l.addWidget(execButton)
l.addWidget(stopButton)
l.addWidget(clearButton)
l.setContentsMargins(0, 0, 0, 0)
execButton.setShortcut(QtGui.QKeySequence('Ctrl+Return'))
execButton.connect('clicked()', self.onExecClicked)
clearButton.connect('clicked()', self.onClearClicked)
stopButton.connect('clicked()', self.sendStopWalking)
self.execButton = execButton
self.stopButton = stopButton
self.toolbarWidget = app.getMainWindow().toolBar().addWidget(w)
self.execButton.show()
def onFileSaveData():
obj = om.getActiveObject()
if not obj:
app.showErrorMessage('Please select an object', title='No object selected')
return
if isinstance(obj, otdfmodel.OtdfModelItem):
mainWindow = app.getMainWindow()
filename = QtGui.QFileDialog.getSaveFileName(mainWindow, "Save Data...", getDefaultDirectory(), 'OTDF (*.otdf)', 'OTDF (*.otdf)')
if not os.path.splitext(filename)[1]:
filename += '.otdf'
storeDefaultDirectory(filename)
otdfString = obj.parser.getUpdatedOtdf()
otdfFile = open(filename, 'w')
otdfFile.write(otdfString)
otdfFile.close()
elif hasattr(obj, 'polyData'):
mainWindow = app.getMainWindow()
fileFilters = "PLY (*.ply);;STL (*.stl);;VTP (*.vtp)";
filename = QtGui.QFileDialog.getSaveFileName(mainWindow, "Save Data...", getDefaultDirectory(), fileFilters, 'VTP (*.vtp)')
if not filename:
return
if not os.path.splitext(filename)[1]:
filename += '.vtp'
polyData = io.writePolyData(obj.polyData, filename)
else:
app.showErrorMessage('Please select an object that contains geometry data or an OTDF object', title='Invalid object selected')
return
storeDefaultDirectory(filename)
def notifyUserStatusBar(self):
if self.warningButton:
return
self.warningButton = QtGui.QPushButton('Spindle Stuck Warning')
self.warningButton.setStyleSheet("background-color:red")
app.getMainWindow().statusBar().insertPermanentWidget(
0, self.warningButton)
def onSaveScreenshot(self):
outDir = self.screenshotOutputDirectory()
if not self.ensureDirectoryIsWritable(outDir):
return
filename = os.path.join(outDir, 'Screenshot-' + self.dateTimeString() + '.png')
saveScreenshot(self.view, filename)
app.getMainWindow().statusBar().showMessage('Saved: ' + filename, 2000)
def onSaveScreenshot(self):
outDir = self.screenshotOutputDirectory()
if not self.ensureDirectoryIsWritable(outDir):
return
filename = os.path.join(outDir, "Screenshot-" + self.dateTimeString() + ".png")
saveScreenshot(self.view, filename)
app.getMainWindow().statusBar().showMessage("Saved: " + filename, 2000)
def onIkStartup(ikServer, startSuccess):
if startSuccess:
app.getMainWindow().statusBar().showMessage(
'Planning server started.', 2000)
else:
app.showErrorMessage(
'Error detected while starting the matlab planning server. '
'Please check the output console for more information.',
title='Error starting matlab')
def onFileSaveData():
obj = om.getActiveObject()
if not obj:
app.showErrorMessage("Please select an object",
title="No object selected")
return
if isinstance(obj, otdfmodel.OtdfModelItem):
mainWindow = app.getMainWindow()
filename = QtGui.QFileDialog.getSaveFileName(
mainWindow,
"Save Data...",
getDefaultDirectory(),
"OTDF (*.otdf)",
"OTDF (*.otdf)",
)
if not os.path.splitext(filename)[1]:
filename += ".otdf"
storeDefaultDirectory(filename)
otdfString = obj.parser.getUpdatedOtdf()
otdfFile = open(filename, "w")
otdfFile.write(otdfString)
otdfFile.close()
elif hasattr(obj, "polyData"):
mainWindow = app.getMainWindow()
fileFilters = "PLY (*.ply);;STL (*.stl);;VTP (*.vtp);;VTK (*.vtk)"
defaultFilter = "VTP (*.vtp)"
filename = QtGui.QFileDialog.getSaveFileName(
mainWindow,
"Save Data...",
getDefaultDirectory(),
fileFilters,
defaultFilter,
)
if not filename:
return
if not os.path.splitext(filename)[1]:
filename += ".vtp"
polyData = io.writePolyData(obj.polyData, filename)
else:
app.showErrorMessage(
"Please select an object that contains geometry data or an OTDF object",
title="Invalid object selected",
)
return
storeDefaultDirectory(filename)
def __init__(self):
self._folder_name = 'Hydroelastic Contact'
self._name = 'Hydroelastic Contact Visualizer'
self._enabled = False
self._sub = None
self.set_enabled(True)
# Visualization parameters
# TODO(SeanCurtis-TRI): Find some way to persist these settings across
# invocations of drake visualizer. Config file, environment settings,
# something.
self.color_map_mode = ColorMapModes.kFlameMap
self.min_pressure = 0
self.max_pressure = 10
self.show_contact_edges = True
self.show_pressure = True
self.max_pressure_observed = 0
menu_bar = applogic.getMainWindow().menuBar()
plugin_menu = get_sub_menu_or_make(menu_bar, '&Plugins')
contact_menu = get_sub_menu_or_make(plugin_menu, '&Contacts')
self.configure_action = contact_menu.addAction(
'Configure &Hydroelastic Contact Visualization')
self.configure_action.connect('triggered()', self.configure_via_dialog)
self.set_enabled(True)
def startRecording(self):
self.frameCount = 0
if not self.ensureDirectoryIsWritable(self.movieOutputDirectory()):
self.ui.recordMovieButton.checked = False
return
existingFiles = glob.glob(os.path.join(self.movieOutputDirectory(), "*.tiff"))
if len(existingFiles):
choice = QtGui.QMessageBox.question(
app.getMainWindow(),
"Continue?",
"There are existing image files in the output directory. They will be deleted prior to recording. Continue?",
QtGui.QMessageBox.Yes | QtGui.QMessageBox.No,
QtGui.QMessageBox.No,
)
if choice == QtGui.QMessageBox.No:
self.ui.recordMovieButton.checked = False
return
for fileToRemove in existingFiles:
os.remove(fileToRemove)
self.fpsCounter.tick()
self.startT = time.time()
interval = int(round(1000.0 / self.captureRate()))
self.recordTimer.setInterval(interval)
self.recordTimer.start()
def init():
mainWindow = app.getMainWindow()
mainWindow.connect('fileOpen()', onFileOpenDialog)
mainWindow.connect('fileSaveData()', onFileSaveData)
mainWindow.connect('fileExportUrdf()', onFileExportUrdf)
mainWindow.connect('openOnlineHelp()', onOpenOnlineHelp)
def startRecording(self):
self.frameCount = 0
if not self.ensureDirectoryIsWritable(self.movieOutputDirectory()):
self.ui.recordMovieButton.checked = False
return
existingFiles = glob.glob(
os.path.join(self.movieOutputDirectory(), '*.tiff'))
if len(existingFiles):
choice = QtGui.QMessageBox.question(
app.getMainWindow(), 'Continue?',
'There are existing image files in the output directory. They will be deleted prior to recording. Continue?',
QtGui.QMessageBox.Yes | QtGui.QMessageBox.No,
QtGui.QMessageBox.No)
if choice == QtGui.QMessageBox.No:
self.ui.recordMovieButton.checked = False
return
for fileToRemove in existingFiles:
os.remove(fileToRemove)
self.fpsCounter.tick()
self.startT = time.time()
interval = int(round(1000.0 / self.captureRate()))
self.recordTimer.setInterval(interval)
self.recordTimer.start()
def __init__(self):
self._folder_name = 'Hydroelastic Contact'
self._name = 'Hydroelastic Contact Visualizer'
self._enabled = False
self._sub = None
self.set_enabled(True)
# Visualization parameters
# TODO(SeanCurtis-TRI): Find some way to persist these settings across
# invocations of drake visualizer. Config file, environment settings,
# something.
self.color_map_mode = ColorMapModes.kFlameMap
self.min_pressure = 0
self.max_pressure = 10
self.show_contact_edges = True
self.show_pressure = True
self.max_pressure_observed = 0
self.show_spatial_force = True
self.show_traction_vectors = False
self.show_slip_velocity_vectors = False
# TODO(SeanCurtis-TRI): Make these variables settable through a dialog
# box.
self.magnitude_mode = ContactVisModes.kFixedLength
self.global_scale = 0.3
self.min_magnitude = 1e-4
menu_bar = applogic.getMainWindow().menuBar()
plugin_menu = get_sub_menu_or_make(menu_bar, '&Plugins')
contact_menu = get_sub_menu_or_make(plugin_menu, '&Contacts')
self.configure_action = contact_menu.addAction(
'Configure &Hydroelastic Contact Visualization')
self.configure_action.connect('triggered()', self.configure_via_dialog)
self.set_enabled(True)
def addRobotBehaviors(_robotSystem):
global robotSystem, robotModel, handFactory, footstepsDriver, neckDriver, robotLinkSelector
robotSystem = _robotSystem
robotModel = robotSystem.robotStateModel
handFactory = robotSystem.handFactory
footstepsDriver = robotSystem.footstepsDriver
neckDriver = robotSystem.neckDriver
if app.getMainWindow() is not None:
robotLinkSelector = RobotLinkSelector()
def onFileOpenDialog():
mainWindow = app.getMainWindow()
fileFilters = "Data Files (*.obj *.pcd *.ply *.stl *.vtk *.vtp *.wrl *.urdf *.otdf)";
filename = QtGui.QFileDialog.getOpenFileName(mainWindow, "Open...", getDefaultDirectory(), fileFilters)
if not filename:
return
storeDefaultDirectory(filename)
onOpenFile(filename)
def __init__(self, view, _robotSystem):
self.view = view
self.viewBehaviors = viewbehaviors.ViewBehaviors(view)
self.robotViewBehaviors = RobotViewEventFilter(self, view)
self.robotName = _robotSystem.robotName
self.robotSystem = _robotSystem
self.robotModel = self.robotSystem.robotStateModel
if app.getMainWindow() is not None:
self.robotLinkSelector = RobotLinkSelector()
viewbehaviors.registerContextMenuActions(self.getRobotActions)
def __init__(self, view, _robotSystem):
self.view = view
self.viewBehaviors = viewbehaviors.ViewBehaviors(view)
self.logCommander = KeyPressLogCommander(view)
self.robotViewBehaviors = RobotViewEventFilter(view)
global robotSystem, robotModel, handFactory, footstepsDriver, neckDriver, robotLinkSelector
robotSystem = _robotSystem
robotModel = robotSystem.robotStateModel
handFactory = robotSystem.handFactory
footstepsDriver = robotSystem.footstepsDriver
neckDriver = robotSystem.neckDriver
if app.getMainWindow() is not None:
robotLinkSelector = RobotLinkSelector()
def __init__(self, view, _robotSystem):
self.view = view
self.viewBehaviors = viewbehaviors.ViewBehaviors(view)
self.logCommander = KeyPressLogCommander(view)
self.robotViewBehaviors = RobotViewEventFilter(view)
global robotSystem, robotModel, handFactory, footstepsDriver, neckDriver, robotLinkSelector
robotSystem = _robotSystem
robotModel = robotSystem.robotStateModel
handFactory = robotSystem.handFactory
footstepsDriver = robotSystem.footstepsDriver
neckDriver = robotSystem.neckDriver
if app.getMainWindow() is not None:
robotLinkSelector = RobotLinkSelector()
def onFileOpen():
mainWindow = app.getMainWindow()
fileFilters = "Data Files (*.obj *.pcd *.ply *.stl *.vtk *.vtp *.urdf *.otdf)";
filename = QtGui.QFileDialog.getOpenFileName(mainWindow, "Open...", getDefaultDirectory(), fileFilters)
if not filename:
return
storeDefaultDirectory(filename)
if filename.lower().endswith('urdf'):
onOpenUrdf(filename)
elif filename.lower().endswith('otdf'):
onOpenOtdf(filename)
else:
onOpenGeometry(filename)
def onFileExportUrdf():
obj = om.getActiveObject()
if not obj or not isinstance(obj, otdfmodel.OtdfModelItem):
app.showErrorMessage('Please select an OTDF object', title='OTDF object not selected')
return
mainWindow = app.getMainWindow()
filename = QtGui.QFileDialog.getSaveFileName(mainWindow, "Save Data...", getDefaultDirectory(), 'URDF (*.urdf)', 'URDF (*.urdf)')
if not os.path.splitext(filename)[1]:
filename += '.urdf'
storeDefaultDirectory(filename)
urdfString = obj.parser.getUrdfFromOtdf()
urdfFile = open(filename, 'w')
urdfFile.write(urdfString)
urdfFile.close()
def __init__(self):
self._folder_name = 'Point Pair Contact Results'
self._name = "Contact Visualizer"
self._enabled = False
self._sub = None
# Visualization parameters
# TODO(SeanCurtis-TRI): Find some way to persist these settings across
# invocations of drake visualizer. Config file, environment settings,
# something.
self.magnitude_mode = ContactVisModes.kFixedLength
self.global_scale = 0.3
self.min_magnitude = 1e-4
menu_bar = applogic.getMainWindow().menuBar()
plugin_menu = get_sub_menu_or_make(menu_bar, '&Plugins')
contact_menu = get_sub_menu_or_make(plugin_menu, '&Contacts')
self.configure_action = contact_menu.addAction(
"&Configure Force Vector for Point Contacts")
self.configure_action.connect('triggered()', self.configure_via_dialog)
self.set_enabled(True)
self.update_screen_text()
def __init__(self):
self._folder_name = 'Point Pair Contact Results'
self._name = "Contact Visualizer"
self._enabled = False
self._sub = None
# Visualization parameters
# TODO(SeanCurtis-TRI): Find some way to persist these settings across
# invocations of drake visualizer. Config file, environment settings,
# something.
self.magnitude_mode = ContactVisModes.kFixedLength
self.global_scale = 0.3
self.min_magnitude = 1e-4
menu_bar = applogic.getMainWindow().menuBar()
# TODO(SeanCurtis): This would be better extracted out of *this* plugin
# and into some common plugin repository so that all plugins can place
# their menu functionality into a common menu.
plugin_name = '&Plugins'
plugin_menu = None
for a in menu_bar.actions():
if a.text == plugin_name:
plugin_menu = a
break
if plugin_menu is None:
plugin_menu = menu_bar.addMenu(plugin_name)
# TODO: I should probably test for the existence of this sub-menu and
# the action -- otherwise they'll stomp on each other (the new submenu
# implicitly replacing the old). More generally, we need a safe way
# to dynamically modify the menu based on arbitrary plugins.
contact_menu = plugin_menu.addMenu('&Contacts')
self.configure_action = contact_menu.addAction(
"&Configure Force Vector for Point Contacts")
self.configure_action.connect('triggered()', self.configure_via_dialog)
self.set_enabled(True)
self.update_screen_text()
def update(self, unused):
if (om.findObjectByName('measured cop').getProperty('Visible')
and self.robotStateJointController.lastRobotStateMessage):
if self.dialogVisible == False:
self.warningButton.setVisible(True)
app.getMainWindow().statusBar().insertPermanentWidget(
0, self.warningButton)
self.dialogVisible = True
self.leftInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_force_z > 500
self.rightInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_force_z > 500
if self.rightInContact or self.leftInContact:
lFootFt = [
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_torque_y, 0.0, 0.0, 0.0,
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_foot_force_z
]
rFootFt = [
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_torque_y, 0.0, 0.0, 0.0,
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_foot_force_z
]
lFootTransform = self.robotStateModel.getLinkFrame(
self.robotSystem.ikPlanner.leftFootLink)
rFootTransform = self.robotStateModel.getLinkFrame(
self.robotSystem.ikPlanner.rightFootLink)
rFootOrigin = np.array(
rFootTransform.TransformPoint([0, 0, -0.07645]))
lFootOrigin = np.array(
lFootTransform.TransformPoint([0, 0,
-0.07645])) # down to sole
measured_cop = self.ddDrakeWrapper.resolveCenterOfPressure(
self.robotStateModel.model,
[self.lFootFtFrameId, self.rFootFtFrameId],
lFootFt + rFootFt, [0., 0., 1.],
(self.rightInContact * rFootOrigin +
self.leftInContact * lFootOrigin) /
(self.leftInContact + self.rightInContact))
allFootContacts = np.empty([0, 2])
if self.rightInContact:
rFootContacts = np.array([
rFootTransform.TransformPoint(contact_point)
for contact_point in self.LONG_FOOT_CONTACT_POINTS
])
allFootContacts = np.concatenate(
(allFootContacts, rFootContacts[:, 0:2]))
if self.leftInContact:
lFootContacts = np.array([
lFootTransform.TransformPoint(contact_point)
for contact_point in self.LONG_FOOT_CONTACT_POINTS
])
allFootContacts = np.concatenate(
(allFootContacts, lFootContacts[:, 0:2]))
num_pts = allFootContacts.shape[0]
dist = self.ddDrakeWrapper.drakeSignedDistanceInsideConvexHull(
num_pts, allFootContacts.reshape(num_pts * 2, 1),
measured_cop[0:2])
inSafeSupportPolygon = dist >= 0
# map dist to color -- green if inside threshold, red if not
dist = min(max(0, dist), self.DESIRED_INTERIOR_DISTANCE
) / self.DESIRED_INTERIOR_DISTANCE
# nonlinear interpolation here to try to maintain saturation
r = int(255. - 255. * dist**4)
g = int(255. * dist**0.25)
b = 0
colorStatus = [r / 255., g / 255., b / 255.]
colorStatusString = 'rgb(%d, %d, %d)' % (r, g, b)
self.warningButton.setStyleSheet("background-color:" +
colorStatusString)
d = DebugData()
d.addSphere(measured_cop[0:3], radius=0.02)
vis.updatePolyData(d.getPolyData(),
'measured cop',
view=self.view,
parent='robot state model').setProperty(
'Color', colorStatus)
elif self.dialogVisible:
app.getMainWindow().statusBar().removeWidget(self.warningButton)
self.dialogVisible = False
import drc as lcmdrc import bot_core as lcmbotcore import atlas from collections import OrderedDict import functools import math import numpy as np from director.debugVis import DebugData from director import ioUtils as io drcargs.requireStrict() drcargs.args() app.startup(globals()) om.init(app.getMainWindow().objectTree(), app.getMainWindow().propertiesPanel()) actionhandlers.init() quit = app.quit exit = quit view = app.getDRCView() camera = view.camera() tree = app.getMainWindow().objectTree() orbit = cameracontrol.OrbitController(view) showPolyData = segmentation.showPolyData updatePolyData = segmentation.updatePolyData ###############################################################################
def loadFileAsMap(self):
fileFilters = "Map Files (*.obj *.pcd *.ply *.stl *.vtk *.vtp)";
filename = QtGui.QFileDialog.getOpenFileName(app.getMainWindow(), "Open...", actionhandlers.getDefaultDirectory(), fileFilters)
if not filename:
return
self.mappingPanel.loadMap(filename)
for model in self.associatedWidgets[robot]["models"]:
model.setProperty("Visible", robot == robotName)
for viewBehavior in self.associatedWidgets[robot]["viewbehaviors"]:
# Setting the enabled flag to false will cause the event filter not to filter any events, allowing them
# to pass to the event filter which is enabled, i.e. the one for the currently selected robot
viewBehavior.robotViewBehaviors.setEnabled(robot == robotName)
# Disable the submenu in the view menu for other robots. Without this items in the submenus are hidden
# but the submenu is not greyed out
app.findMenu(robot).setEnabled(robot == robotName)
drcargs.args()
app.startup(globals())
om.init(app.getMainWindow().objectTree(),
app.getMainWindow().propertiesPanel())
actionhandlers.init()
quit = app.quit
exit = quit
view = app.getDRCView()
camera = view.camera()
tree = app.getMainWindow().objectTree()
orbit = cameracontrol.OrbitController(view)
showPolyData = segmentation.showPolyData
updatePolyData = segmentation.updatePolyData
selector = RobotSelector()
# To hide the selector if there is only one robot we actually need to hide the action that is created by the
# toolbar's addwidget
def update(self, unused):
if (om.findObjectByName('measured cop').getProperty('Visible') and self.robotStateJointController.lastRobotStateMessage):
if self.dialogVisible == False:
self.warningButton.setVisible(True)
app.getMainWindow().statusBar().insertPermanentWidget(0, self.warningButton)
self.dialogVisible = True
self.leftInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_force_z > 500
self.rightInContact = self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_force_z > 500
if self.rightInContact or self.leftInContact:
lFootFt = [self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_torque_y,
0.0,
0.0,
0.0,
self.robotStateJointController.lastRobotStateMessage.force_torque.l_foot_force_z]
rFootFt = [self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_torque_x,
self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_torque_y,
0.0,
0.0,
0.0,
self.robotStateJointController.lastRobotStateMessage.force_torque.r_foot_force_z]
lFootTransform = self.robotStateModel.getLinkFrame( self.robotSystem.ikPlanner.leftFootLink )
rFootTransform = self.robotStateModel.getLinkFrame( self.robotSystem.ikPlanner.rightFootLink)
rFootOrigin = np.array(rFootTransform.TransformPoint([0, 0, -0.07645]))
lFootOrigin = np.array(lFootTransform.TransformPoint([0, 0, -0.07645])) # down to sole
measured_cop = self.ddDrakeWrapper.resolveCenterOfPressure(self.robotStateModel.model, [self.lFootFtFrameId, self.rFootFtFrameId],
lFootFt + rFootFt, [0., 0., 1.], (self.rightInContact*rFootOrigin+self.leftInContact*lFootOrigin)/(self.leftInContact + self.rightInContact))
allFootContacts = np.empty([0, 2])
if self.rightInContact:
rFootContacts = np.array([rFootTransform.TransformPoint(contact_point) for contact_point in self.LONG_FOOT_CONTACT_POINTS])
allFootContacts = np.concatenate((allFootContacts, rFootContacts[:, 0:2]))
if self.leftInContact:
lFootContacts = np.array([lFootTransform.TransformPoint(contact_point) for contact_point in self.LONG_FOOT_CONTACT_POINTS])
allFootContacts = np.concatenate((allFootContacts, lFootContacts[:, 0:2]))
num_pts = allFootContacts.shape[0]
dist = self.ddDrakeWrapper.drakeSignedDistanceInsideConvexHull(num_pts, allFootContacts.reshape(num_pts*2, 1), measured_cop[0:2])
inSafeSupportPolygon = dist >= 0
# map dist to color -- green if inside threshold, red if not
dist = min(max(0, dist), self.DESIRED_INTERIOR_DISTANCE) / self.DESIRED_INTERIOR_DISTANCE
# nonlinear interpolation here to try to maintain saturation
r = int(255. - 255. * dist**4 )
g = int(255. * dist**0.25 )
b = 0
colorStatus = [r/255., g/255., b/255.]
colorStatusString = 'rgb(%d, %d, %d)' % (r, g, b)
self.warningButton.setStyleSheet("background-color:"+colorStatusString)
d = DebugData()
d.addSphere(measured_cop[0:3], radius=0.02)
vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model').setProperty('Color', colorStatus)
elif self.dialogVisible:
app.getMainWindow().statusBar().removeWidget(self.warningButton)
self.dialogVisible = False
def notifyUserStatusBar(self):
if self.warningButton:
return
self.warningButton = QtGui.QPushButton("Spindle Stuck Warning")
self.warningButton.setStyleSheet("background-color:red")
app.getMainWindow().statusBar().insertPermanentWidget(0, self.warningButton)
def chooseDirectory(self):
return QtGui.QFileDialog.getExistingDirectory(
app.getMainWindow(), "Choose directory...", self.movieOutputDirectory()
)
def chooseDirectory(self):
return QtGui.QFileDialog.getExistingDirectory(
app.getMainWindow(), "Choose directory...",
self.movieOutputDirectory())
import bot_core as lcmbotcore import maps as lcmmaps import atlas from collections import OrderedDict import functools import math import numpy as np from director.debugVis import DebugData from director import ioUtils as io drcargs.requireStrict() drcargs.args() app.startup(globals()) om.init(app.getMainWindow().objectTree(), app.getMainWindow().propertiesPanel()) actionhandlers.init() quit = app.quit exit = quit view = app.getDRCView() camera = view.camera() tree = app.getMainWindow().objectTree() orbit = cameracontrol.OrbitController(view) showPolyData = segmentation.showPolyData updatePolyData = segmentation.updatePolyData ###############################################################################
def onIkStartup(ikServer, startSuccess):
if startSuccess:
app.getMainWindow().statusBar().showMessage('Planning server started.', 2000)
else:
app.showErrorMessage('Error detected while starting the matlab planning server. '
'Please check the output console for more information.', title='Error starting matlab')
self.lc.publish("PD_CONFIG", msg.encode())
time.sleep(self.ramp_up_time / 100.0)
# store previous kp kd gains
self.kp_ = msg.kp
self.kd_ = msg.kd
self.prev_pos_ = msg.desired_position
def setState_clicked(self):
self.lc.handle_timeout(100)
self.lc.handle_timeout(100) #twice to clear the buffer
def state_handler(self, channel, data):
msg = dairlib.lcmt_robot_output.decode(data)
for idx_msg, joint in enumerate(msg.position_names):
if joint in position_names:
idx = position_names.index(joint)
self.ledits[idx].setValue(msg.position[idx_msg])
self.values[idx] = msg.position[idx_msg]
print('message handled')
print(msg.position[6])
panel = ControllerGui()
app.addWidgetToDock(panel, QtCore.Qt.RightDockWidgetArea)
import director.openscope as scope
import subprocess
view = applogic.getMainWindow()
applogic.addShortcut(view, 'Ctrl+I', scope.startSignalScope)
import bot_core as lcmbotcore
import maps as lcmmaps
import atlas
from collections import OrderedDict
import functools
import math
import numpy as np
from director.debugVis import DebugData
from director import ioUtils as io
drcargs.requireStrict()
drcargs.args()
app.startup(globals())
om.init(app.getMainWindow().objectTree(),
app.getMainWindow().propertiesPanel())
actionhandlers.init()
quit = app.quit
exit = quit
view = app.getDRCView()
camera = view.camera()
tree = app.getMainWindow().objectTree()
orbit = cameracontrol.OrbitController(view)
showPolyData = segmentation.showPolyData
updatePolyData = segmentation.updatePolyData
###############################################################################
robotSystem = robotsystem.create(view)
