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 _createMatlabClient(self):
hostname = drcargs.args().matlab_host
if hostname is not None:
return matlab.MatlabSocketClient(host=hostname)
else:
return matlab.MatlabPipeClient()
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 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 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 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 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 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 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 ###############################################################################
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
###############################################################################
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 ###############################################################################
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)
def loadData():
for filename in drcargs.args().data_files:
openDataHandler.openGeometry(filename)
