def _createMatlabClient(self):
hostname = drcargs.args().matlab_host
if hostname is not None:
return matlab.MatlabSocketClient(host=hostname)
else:
return matlab.MatlabPipeClient()
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 __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 getDirectorConfig():
with open(drcargs.args().directorConfigFile) as directorConfigFile:
directorConfig = json.load(directorConfigFile)
directorConfigDirectory = os.path.dirname(os.path.abspath(directorConfigFile.name))
directorConfig['fixedPointFile'] = os.path.join(directorConfigDirectory, directorConfig['fixedPointFile'])
urdfConfig = directorConfig['urdfConfig']
for key, urdf in list(urdfConfig.items()):
urdfConfig[key] = os.path.join(directorConfigDirectory, urdf)
return directorConfig
def getDirectorConfig():
with open(drcargs.args().directorConfigFile) as directorConfigFile:
directorConfig = json.load(directorConfigFile)
directorConfigDirectory = os.path.dirname(os.path.abspath(directorConfigFile.name))
directorConfig['fixedPointFile'] = os.path.join(directorConfigDirectory, directorConfig['fixedPointFile'])
urdfConfig = directorConfig['urdfConfig']
for key, urdf in list(urdfConfig.items()):
urdfConfig[key] = os.path.join(directorConfigDirectory, urdf)
return directorConfig
def getRobotStateJointNames():
global _robotStateJointNames
if _robotStateJointNames:
return _robotStateJointNames
else:
with open(drcargs.args().directorConfigFile) as directorConfigFile:
_robotStateJointNames = json.load(directorConfigFile)['robotStateJointNames']
return _robotStateJointNames
def getDrakePoseJointNames():
global _drakePoseJointNames
if _drakePoseJointNames:
return _drakePoseJointNames
else:
with open(drcargs.args().directorConfigFile) as directorConfigFile:
_drakePoseJointNames = json.load(directorConfigFile)['drakeJointNames']
return _drakePoseJointNames
def getRobotStateJointNames():
global _robotStateJointNames
if _robotStateJointNames:
return _robotStateJointNames
else:
with open(drcargs.args().directorConfigFile) as directorConfigFile:
_robotStateJointNames = json.load(
directorConfigFile)['robotStateJointNames']
return _robotStateJointNames
def getDrakePoseJointNames():
global _drakePoseJointNames
if _drakePoseJointNames:
return _drakePoseJointNames
else:
with open(drcargs.args().directorConfigFile) as directorConfigFile:
_drakePoseJointNames = json.load(
directorConfigFile)['drakeJointNames']
return _drakePoseJointNames
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 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__(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
from ddapp.tasks import robottasks as rt from ddapp.tasks import taskmanagerwidget from ddapp.tasks.descriptions import loadTaskDescriptions import drc as lcmdrc from collections import OrderedDict import functools import math import numpy as np from ddapp.debugVis import DebugData from ddapp 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 ddapp 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])):
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
import ddapp.objectmodel as om
import ddapp.visualization as vis
import ddapp.vtkAll as vtk
from ddapp import jointcontrol
from ddapp import getDRCBaseDir
from ddapp import lcmUtils
from ddapp import filterUtils
from ddapp import transformUtils
from ddapp import drcargs
import drc as lcmdrc
import math
import numpy as np
import json
with open(drcargs.args().directorConfigFile) as directorConfigFile:
directorConfig = json.load(directorConfigFile)
directorConfigDirectory = os.path.dirname(os.path.abspath(directorConfigFile.name))
fixedPointFile = os.path.join(directorConfigDirectory, directorConfig['fixedPointFile'])
urdfConfig = directorConfig['urdfConfig']
for key, urdf in list(urdfConfig.items()):
urdfConfig[key] = os.path.join(directorConfigDirectory, urdf)
handCombinations = directorConfig['handCombinations']
numberOfHands = len(handCombinations)
headLink = directorConfig['headLink']
def getRobotGrayColor():
return QtGui.QColor(177, 180, 190)
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 ddapp.tasks import robottasks as rt from ddapp.tasks import taskmanagerwidget from ddapp.tasks.descriptions import loadTaskDescriptions import drc as lcmdrc from collections import OrderedDict import functools import math import numpy as np from ddapp.debugVis import DebugData from ddapp 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 ###############################################################################
