def processSingleBlock(robotStateModel, whichFile=0): if whichFile == 0: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/terrain_simple_ihmc.vtp")) vis.updatePolyData(polyData, "terrain_simple_ihmc.vtp", parent="continuous") else: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/terrain_flagstones_ihmc.vtp")) cwdemo.chosenTerrain = "stairs" cwdemo.supportContact = lcmdrc.footstep_params_t.SUPPORT_GROUPS_MIDFOOT_TOE vis.updatePolyData(polyData, "terrain_stairs_ihmc.vtp", parent="continuous") if drcargs.args().directorConfigFile.find("atlas") != -1: standingFootName = "l_foot" else: standingFootName = "leftFoot" standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent="continuous", visible=False) # Step 1: filter the data down to a box in front of the robot: polyData = cwdemo.getRecedingTerrainRegion(polyData, footstepsDriver.getFeetMidPoint(robotStateModel)) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if clusters is None: print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks, match_idx, groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def processSnippet(): obj = om.getOrCreateContainer('continuous') om.getOrCreateContainer('cont debug', obj) if (continuouswalkingDemo.processContinuousStereo): polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet_stereo.vtp')) polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) else: polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/block_snippet.vtp')) vis.updatePolyData( polyData, 'walking snapshot trimmed', parent='continuous') if drcargs.args().directorConfigFile.find('atlas') != -1: standingFootName = 'l_foot' else: standingFootName = 'leftFoot' standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if (clusters is None): print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks,match_idx,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def processSingleBlock(robotStateModel, whichFile=0): if (whichFile == 0): polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/terrain_simple_ihmc.vtp')) vis.updatePolyData( polyData, 'terrain_simple_ihmc.vtp', parent='continuous') else: polyData = ioUtils.readPolyData(os.path.join(dataDir, 'terrain/terrain_stairs_ihmc.vtp')) cwdemo.chosenTerrain = 'stairs' cwdemo.supportContact = lcmdrc.footstep_params_t.SUPPORT_GROUPS_MIDFOOT_TOE vis.updatePolyData( polyData, 'terrain_stairs_ihmc.vtp', parent='continuous') if drcargs.args().directorConfigFile.find('atlas') != -1: standingFootName = 'l_foot' else: standingFootName = 'leftFoot' standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent='continuous', visible=False) # Step 1: filter the data down to a box in front of the robot: polyData = cwdemo.getRecedingTerrainRegion(polyData, footstepsDriver.getFeetMidPoint(robotStateModel)) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if (clusters is None): print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks,match_idx,groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def _createMatlabClient(self): hostname = drcargs.args().matlab_host if hostname is not None: return matlab.MatlabSocketClient(host=hostname) else: return matlab.MatlabPipeClient()
def processSnippet(): obj = om.getOrCreateContainer("continuous") om.getOrCreateContainer("cont debug", obj) if continuouswalkingDemo.processContinuousStereo: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/block_snippet_stereo.vtp")) polyData = segmentation.applyVoxelGrid(polyData, leafSize=0.01) else: polyData = ioUtils.readPolyData(os.path.join(dataDir, "terrain/block_snippet.vtp")) vis.updatePolyData(polyData, "walking snapshot trimmed", parent="continuous") if drcargs.args().directorConfigFile.find("atlas") != -1: standingFootName = "l_foot" else: standingFootName = "leftFoot" standingFootFrame = robotStateModel.getLinkFrame(standingFootName) vis.updateFrame(standingFootFrame, standingFootName, parent="continuous", visible=False) # Step 2: find all the surfaces in front of the robot (about 0.75sec) clusters = segmentation.findHorizontalSurfaces(polyData) if clusters is None: print "No cluster found, stop walking now!" return # Step 3: find the corners of the minimum bounding rectangles blocks, match_idx, groundPlane = cwdemo.extractBlocksFromSurfaces(clusters, standingFootFrame) footsteps = cwdemo.placeStepsOnBlocks(blocks, groundPlane, standingFootName, standingFootFrame) cwdemo.drawFittedSteps(footsteps)
def __init__(self): self.images = {} self.imageUtimes = {} self.textures = {} self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread()) self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
def start(self): if self.reader is None: self.reader = drc.vtkMultisenseSource() self.reader.InitBotConfig(drcargs.args().config_file) self.reader.SetDistanceRange(0.25, 4.0) self.reader.Start() TimerCallback.start(self)
def __init__(self): self.images = {} self.imageUtimes = {} self.textures = {} self.imageRotations180 = {} self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread()) self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
def start(self): if self.reader is None: self.reader = drc.vtkLidarSource() self.reader.InitBotConfig(drcargs.args().config_file) self.reader.SetDistanceRange(0.25, 80.0) self.reader.SetHeightRange(-80.0, 80.0) self.reader.Start() TimerCallback.start(self)
def start(self): if self.reader is None: self.reader = drc.vtkLidarSource() self.reader.subscribe(self.channelName) self.reader.setCoordinateFrame(self.coordinateFrame) self.reader.InitBotConfig(drcargs.args().config_file) self.reader.SetDistanceRange(0.25, 80.0) self.reader.SetHeightRange(-80.0, 80.0) self.reader.Start() TimerCallback.start(self)
def start(self): if self.reader is None: self.reader = drc.vtkMultisenseSource() self.reader.InitBotConfig(drcargs.args().config_file) self.reader.SetDistanceRange(0.25, 4.0) self.reader.SetHeightRange(-80.0, 80.0) self.reader.Start() self.setIntensityRange(400, 4000) TimerCallback.start(self)
def init(view): global KinectQueue, _kinectItem, _kinectSource KinectQueue = PythonQt.dd.ddKinectLCM(lcmUtils.getGlobalLCMThread()) KinectQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file) _kinectSource = KinectSource(view, KinectQueue) _kinectSource.start() sensorsFolder = om.getOrCreateContainer('sensors') _kinectItem = KinectItem(_kinectSource) om.addToObjectModel(_kinectItem, sensorsFolder)
def init(view): global PointCloudQueue, _pointcloudItem, _pointcloudSource PointCloudQueue = PythonQt.dd.ddPointCloudLCM(lcmUtils.getGlobalLCMThread()) PointCloudQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file) _pointcloudSource = PointCloudSource(view, PointCloudQueue) _pointcloudSource.start() sensorsFolder = om.getOrCreateContainer('sensors') _pointcloudItem = PointCloudItem(_pointcloudSource) om.addToObjectModel(_pointcloudItem, sensorsFolder)
def initMainWindow(self, fields): organizationName = "RobotLocomotion" applicationName = "DirectorMainWindow" windowTitle = "Director App" if hasattr(fields, "organizationName"): organizationName = fields.organizationName if hasattr(fields, "applicationName"): applicationName = fields.applicationName if hasattr(fields, "windowTitle"): windowTitle = fields.windowTitle MainWindowApp.applicationInstance().setOrganizationName( organizationName) MainWindowApp.applicationInstance().setApplicationName(applicationName) app = MainWindowApp() app.mainWindow.setCentralWidget(fields.view) app.mainWindow.setWindowTitle(windowTitle) app.mainWindow.setWindowIcon(QtGui.QIcon(":/images/drake_logo.png")) sceneBrowserDock = app.addWidgetToDock( fields.objectModel.getTreeWidget(), QtCore.Qt.LeftDockWidgetArea, visible=True, ) propertiesDock = app.addWidgetToDock( app.wrapScrollArea(fields.objectModel.getPropertiesPanel()), QtCore.Qt.LeftDockWidgetArea, visible=True, ) app.addViewMenuSeparator() def toggleObjectModelDock(): newState = not sceneBrowserDock.visible sceneBrowserDock.setVisible(newState) propertiesDock.setVisible(newState) applogic.addShortcut(app.mainWindow, "F1", toggleObjectModelDock) # applogic.addShortcut(app.mainWindow, 'F8', app.showPythonConsole) return FieldContainer( app=app, mainWindow=app.mainWindow, sceneBrowserDock=sceneBrowserDock, propertiesDock=propertiesDock, toggleObjectModelDock=toggleObjectModelDock, commandLineArgs=drcargs.args(), )
def initMainWindow(self, fields): from director import viewcolors organizationName = 'RobotLocomotion' applicationName = 'DirectorMainWindow' windowTitle = 'Director App' if hasattr(fields, 'organizationName'): organizationName = fields.organizationName if hasattr(fields, 'applicationName'): applicationName = fields.applicationName if hasattr(fields, 'windowTitle'): windowTitle = fields.windowTitle MainWindowApp.applicationInstance().setOrganizationName(organizationName) MainWindowApp.applicationInstance().setApplicationName(applicationName) app = MainWindowApp() app.mainWindow.setCentralWidget(fields.view) app.mainWindow.setWindowTitle(windowTitle) app.mainWindow.setWindowIcon(QtGui.QIcon(':/images/drake_logo.png')) sceneBrowserDock = app.addWidgetToDock(fields.objectModel.getTreeWidget(), QtCore.Qt.LeftDockWidgetArea, visible=True) propertiesDock = app.addWidgetToDock(app.wrapScrollArea(fields.objectModel.getPropertiesPanel()), QtCore.Qt.LeftDockWidgetArea, visible=True) app.addViewMenuSeparator() def toggleObjectModelDock(): newState = not sceneBrowserDock.visible sceneBrowserDock.setVisible(newState) propertiesDock.setVisible(newState) applogic.addShortcut(app.mainWindow, 'F1', toggleObjectModelDock) #applogic.addShortcut(app.mainWindow, 'F8', app.showPythonConsole) return FieldContainer( app=app, mainWindow=app.mainWindow, sceneBrowserDock=sceneBrowserDock, propertiesDock=propertiesDock, toggleObjectModelDock=toggleObjectModelDock, commandLineArgs=drcargs.args() )
def init(view): global PointCloudQueue, _pointcloudItem, _pointcloudSource PointCloudQueue = PythonQt.dd.ddPointCloudLCM( lcmUtils.getGlobalLCMThread()) PointCloudQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file) _pointcloudSource = PointCloudSource(view, PointCloudQueue) _pointcloudSource.start() sensorsFolder = om.getOrCreateContainer('sensors') _pointcloudItem = PointCloudItem(_pointcloudSource) om.addToObjectModel(_pointcloudItem, sensorsFolder)
def init(view): global _multisenseItem global multisenseDriver sensorsFolder = om.getOrCreateContainer('sensors') m = MultiSenseSource(view) m.start() multisenseDriver = m _multisenseItem = MultisenseItem(m) om.addToObjectModel(_multisenseItem, sensorsFolder) queue = PythonQt.dd.ddPointCloudLCM(lcmUtils.getGlobalLCMThread()) queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file) lidarNames = queue.getLidarNames() for lidar in lidarNames: if queue.displayLidar(lidar): l = LidarSource(view, queue.getLidarChannelName(lidar), queue.getLidarCoordinateFrame(lidar), queue.getLidarFriendlyName(lidar), queue.getLidarIntensity(lidar)) l.start() lidarDriver = l _lidarItem = LidarItem(l) om.addToObjectModel(_lidarItem, sensorsFolder) useMapServer = hasattr(drc, 'vtkMapServerSource') if useMapServer: mapServerSource = MapServerSource(view, callbackFunc=view.render) mapsServerContainer = om.ObjectModelItem('Map Server', icon=om.Icons.Robot) mapsServerContainer.source = mapServerSource om.addToObjectModel(mapsServerContainer, parentObj=sensorsFolder) mapServerSource.start() else: mapServerSource = None spindleDebug = SpindleAxisDebug(multisenseDriver) spindleDebug.addToView(view) om.addToObjectModel(spindleDebug, sensorsFolder) return multisenseDriver, mapServerSource
def __init__(self, jointController, footstepDriver): self.jointController = jointController self.footstepDriver = footstepDriver loader = QtUiTools.QUiLoader() uifile = QtCore.QFile(':/ui/ddMapping.ui') assert uifile.open(uifile.ReadOnly) self.widget = loader.load(uifile) self.ui = WidgetDict(self.widget.children()) self.ui.startMappingButton.connect("clicked()", self.onStartMappingButton) self.ui.stopMappingButton.connect("clicked()", self.onStopMappingButton) self.ui.showMapButton.connect("clicked()", self.onShowMapButton) self.ui.hideMapButton.connect("clicked()", self.onHideMapButton) self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread()) self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
def __init__(self, view, _PointCloudQueue): self.view = view self.PointCloudQueue = _PointCloudQueue self.visible = True self.p = vtk.vtkPolyData() utime = PointCloudQueue.getPointCloudFromPointCloud(self.p) self.polyDataObj = vis.PolyDataItem('pointcloud source', shallowCopy(self.p), view) self.polyDataObj.actor.SetPickable(1) self.polyDataObj.initialized = False om.addToObjectModel(self.polyDataObj) self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread()) self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file) self.targetFps = 30 self.timerCallback = TimerCallback(targetFps=self.targetFps) self.timerCallback.callback = self._updateSource
def __init__(self, view, _KinectQueue): self.view = view self.KinectQueue = _KinectQueue self.visible = True self.p = vtk.vtkPolyData() utime = KinectQueue.getPointCloudFromKinect(self.p) self.polyDataObj = vis.PolyDataItem('kinect source', shallowCopy(self.p), view) self.polyDataObj.actor.SetPickable(1) self.polyDataObj.initialized = False om.addToObjectModel(self.polyDataObj) self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread()) self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file) self.targetFps = 30 self.timerCallback = TimerCallback(targetFps=self.targetFps) self.timerCallback.callback = self._updateSource
def main(globalsDict=None): if "--testing" not in sys.argv: drcargs.requireStrict() parser = drcargs.getGlobalArgParser().getParser() parser.add_argument( "--protocol", dest="visualizer_protocol", default="drake", type=str, help="Visualizer protocol (drake or json)" ) args = drcargs.args() knownProtocols = ("drake", "json") if args.visualizer_protocol not in knownProtocols: print print "Unrecognized visualizer protocol:", args.visualizer_protocol print "Available protocols:", ", ".join(knownProtocols) print sys.exit(1) appName = "Drake Visualizer" app = mainwindowapp.MainWindowAppFactory().construct( globalsDict=globalsDict, windowTitle=appName, applicationName=appName ) fact = mainwindowapp.MainWindowPanelFactory() options = fact.getDefaultOptions() options.useLCMGLRenderer = True if args.visualizer_protocol == "json": fact.setDependentOptions(options, useLCMVisualizer=True) elif args.visualizer_protocol == "drake": fact.setDependentOptions(options, useDrakeVisualizer=True) fact.construct(options, app=app.app, view=app.view) if globalsDict is not None: globalsDict.update(**dict(app)) app.app.start()
def searchFinalPose(self, constraints, side, eeName, eePose, nominalPoseName, capabilityMapFile, ikParameters): commands = [] commands.append('default_shrink_factor = %s;' % ikParameters.quasiStaticShrinkFactor) constraintNames = [] for constraintId, constraint in enumerate(constraints): if not constraint.enabled: continue constraint.getCommands(commands, constraintNames, suffix='_%d' % constraintId) commands.append('\n') commands.append('eeId = r.findLinkId(\'{:s}\');'.format(eeName)) commands.append('additional_constraints = {};') commands.append('goal_constraints = {};') commands.append( 'capability_map = CapabilityMap([\'{:s}\', \'/{:s}\']);'.format( os.path.dirname(drcargs.args().directorConfigFile), drcargs.getDirectorConfig()['capabilityMapFile'])) for constraint in constraintNames: commands.append( 'if isa({0:s}, \'Point2PointDistanceConstraint\') && {0:s}.body_a.idx == eeId ' '|| isa({0:s}, \'EulerConstraint\') && {0:s}.body == eeId ' 'goal_constraints = {{goal_constraints{{:}}, {0:s}}}; else ' 'additional_constraints = {{additional_constraints{{:}}, {0:s}}};end' .format(constraint)) commands.append('cost = Point(r.getPositionFrame(),10);') commands.append( 'for i = r.getNumBodies():-1:1 ' 'if all(r.getBody(i).parent > 0) && all(r.getBody(r.getBody(i).parent).position_num > 0) ' 'cost(r.getBody(r.getBody(i).parent).position_num) = ' 'cost(r.getBody(r.getBody(i).parent).position_num) + cost(r.getBody(i).position_num);end;end' ) commands.append('cost(1:6) = max(cost(7:end))/2;') commands.append('cost = cost/min(cost);') commands.append('Q = diag(cost);') commands.append('ikoptions = IKoptions(r);') commands.append( 'ikoptions = ikoptions.setMajorIterationsLimit({:d});'.format( ikParameters.majorIterationsLimit)) commands.append('ikoptions = ikoptions.setQ(Q);') commands.append( 'ikoptions = ikoptions.setMajorOptimalityTolerance({:f});'.format( ikParameters.majorOptimalityTolerance)) # commands.append('{:s}'.format(ConstraintBase.toColumnVectorString(xGoal))) commands.append( 'fpp = FinalPoseProblem(r, eeId, reach_start, {:s}, additional_constraints,' '{:s}, \'capabilitymap\', capability_map, \'ikoptions\', ikoptions, \'graspinghand\', \'{:s}\');' .format(ConstraintBase.toColumnVectorString(eePose), nominalPoseName, side)) commands.append('[x_goal, info] = fpp.findFinalPose();') self.comm.sendCommands(commands) info = self.comm.getFloatArray('info')[0] if info == 1: endPose = self.comm.getFloatArray('x_goal(8:end)') else: endPose = [] return endPose, info
from director import drcargs import drc as lcmdrc from bot_core.pose_t import pose_t from drc.robot_state_t import robot_state_t import functools import json from PythonQt import QtGui, QtCore _footMeshes = None _footMeshFiles = [] _robotType = 0 # 0 - any atlas, 1 - val v1, 2 - val v2 _pelvisLink = '' # pelvis _leftFootLink = '' # l_foot _rightFootLink = '' # r_foot with open(drcargs.args().directorConfigFile) as directorConfigFile: directorConfig = json.load(directorConfigFile) # dodgy use of filename to find valkyrie: if (directorConfigFile.name.find("valkyrie") > -1): _robotType = 1 if (directorConfigFile.name.find("val_description") > -1): _robotType = 2 directorConfigDirectory = os.path.dirname(os.path.abspath(directorConfigFile.name)) if 'leftFootMeshFiles' in directorConfig: _footMeshFiles.append( directorConfig['leftFootMeshFiles'] ) _footMeshFiles.append( directorConfig['rightFootMeshFiles'] ) for j in range(0,2): for i in range(len(_footMeshFiles[j])):
from director.tasks.descriptions import loadTaskDescriptions import drc as lcmdrc 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 searchFinalPose(self, constraints, side, eeName, eePose, nominalPoseName, capabilityMapFile, ikParameters): commands = [] commands.append('default_shrink_factor = %s;' % ikParameters.quasiStaticShrinkFactor) constraintNames = [] for constraintId, constraint in enumerate(constraints): if not constraint.enabled: continue constraint.getCommands(commands, constraintNames, suffix='_%d' % constraintId) commands.append('\n') commands.append('eeId = r.findLinkId(\'{:s}\');'.format(eeName)) commands.append('additional_constraints = {};') commands.append('goal_constraints = {};') commands.append('capability_map = CapabilityMap([\'{:s}\', \'/{:s}\']);'.format(os.path.dirname(drcargs.args().directorConfigFile), drcargs.getDirectorConfig()['capabilityMapFile'])) for constraint in constraintNames: commands.append('if isa({0:s}, \'Point2PointDistanceConstraint\') && {0:s}.body_a.idx == eeId ' '|| isa({0:s}, \'EulerConstraint\') && {0:s}.body == eeId ' 'goal_constraints = {{goal_constraints{{:}}, {0:s}}}; else ' 'additional_constraints = {{additional_constraints{{:}}, {0:s}}};end'.format(constraint)) commands.append('cost = Point(r.getPositionFrame(),10);') commands.append('for i = r.getNumBodies():-1:1 ' 'if all(r.getBody(i).parent > 0) && all(r.getBody(r.getBody(i).parent).position_num > 0) ' 'cost(r.getBody(r.getBody(i).parent).position_num) = ' 'cost(r.getBody(r.getBody(i).parent).position_num) + cost(r.getBody(i).position_num);end;end') commands.append('cost(1:6) = max(cost(7:end))/2;') commands.append('cost = cost/min(cost);') commands.append('Q = diag(cost);') commands.append('ikoptions = IKoptions(r);') commands.append('ikoptions = ikoptions.setMajorIterationsLimit({:d});'.format(ikParameters.majorIterationsLimit)) commands.append('ikoptions = ikoptions.setQ(Q);') commands.append('ikoptions = ikoptions.setMajorOptimalityTolerance({:f});' .format(ikParameters.majorOptimalityTolerance)) # commands.append('{:s}'.format(ConstraintBase.toColumnVectorString(xGoal))) commands.append('fpp = FinalPoseProblem(r, eeId, reach_start, {:s}, additional_constraints,' '{:s}, \'capabilitymap\', capability_map, \'ikoptions\', ikoptions, \'graspinghand\', \'{:s}\');'.format(ConstraintBase.toColumnVectorString(eePose), nominalPoseName, side)) commands.append('[x_goal, info] = fpp.findFinalPose();') self.comm.sendCommands(commands) info = self.comm.getFloatArray('info')[0] if info == 1: endPose = self.comm.getFloatArray('x_goal(8:end)') else: endPose = [] return endPose, info
app.getViewManager().hideView(view, False) 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()
def loadData(): for filename in drcargs.args().data_files: openDataHandler.openGeometry(filename)