def setupPackagePaths():
searchPaths = [
'software/models/atlas_v3',
'software/models/atlas_v4',
'software/models/atlas_v5',
'software/models/valkyrie',
'software/models/val_description',
'software/models/valkyrie_original',
'software/models/lwr_defs',
'software/models/mit_gazebo_models/mit_robot',
'software/models/mit_gazebo_models/V1',
'software/models/common_components/multisense_sl',
'software/models/common_components/irobot_hand',
'software/models/common_components/handle_description',
'software/models/common_components/robotiq_hand_description',
'software/models/common_components/schunk_description',
'software/models/otdf',
]
for path in searchPaths:
PythonQt.dd.ddDrakeModel.addPackageSearchPath(os.path.join(getDRCBaseDir(), path))
environmentVariables = ['ROS_PACKAGE_PATH']
for e in environmentVariables:
paths = os.environ.get(e, '').split(':')
for path in paths:
for root, dirnames, filenames in os.walk(path):
if os.path.isfile(os.path.join(root, 'package.xml')) or os.path.isfile(os.path.join(root, 'manifest.xml')):
PythonQt.dd.ddDrakeModel.addPackageSearchPath(root)
def setupPackagePaths():
searchPaths = [
"software/models/atlas_v3",
"software/models/atlas_v4",
"software/models/atlas_v5",
"software/models/valkyrie",
"software/models/val_description",
"software/models/valkyrie_original",
"software/models/lwr_defs",
"software/models/mit_gazebo_models/mit_robot",
"software/models/mit_gazebo_models/V1",
"software/models/common_components/multisense_sl",
"software/models/common_components/irobot_hand",
"software/models/common_components/handle_description",
"software/models/common_components/robotiq_hand_description",
"software/models/common_components/schunk_description",
"software/models/otdf",
]
for path in searchPaths:
PythonQt.dd.ddDrakeModel.addPackageSearchPath(os.path.join(getDRCBaseDir(), path))
environmentVariables = ["ROS_PACKAGE_PATH"]
for e in environmentVariables:
paths = os.environ.get(e, "").split(":")
for path in paths:
for root, dirnames, filenames in os.walk(path):
if os.path.isfile(os.path.join(root, "package.xml")) or os.path.isfile(
os.path.join(root, "manifest.xml")
):
PythonQt.dd.ddDrakeModel.addPackageSearchPath(root)
def setupPackagePaths():
searchPaths = [
'software/models/atlas_v3',
'software/models/atlas_v4',
'software/models/atlas_v5',
'software/models/valkyrie',
'software/models/val_description',
'software/models/valkyrie_original',
'software/models/lwr_defs',
'software/models/mit_gazebo_models/mit_robot',
'software/models/mit_gazebo_models/V1',
'software/models/common_components/multisense_sl',
'software/models/common_components/irobot_hand',
'software/models/common_components/handle_description',
'software/models/common_components/robotiq_hand_description',
'software/models/common_components/schunk_description',
'software/models/otdf',
]
for path in searchPaths:
PythonQt.dd.ddDrakeModel.addPackageSearchPath(os.path.join(getDRCBaseDir(), path))
environmentVariables = ['ROS_PACKAGE_PATH']
for e in environmentVariables:
paths = os.environ.get(e, '').split(':')
for path in paths:
for root, dirnames, filenames in os.walk(path):
if os.path.isfile(os.path.join(root, 'package.xml')) or os.path.isfile(os.path.join(root, 'manifest.xml')):
PythonQt.dd.ddDrakeModel.addPackageSearchPath(root)
def __init__(self):
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
self.pointcloud = ioUtils.readPolyData(ddapp.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp')
self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None)
segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0, 0. , 0. , 0.0])))
self.originFrame = self.pointcloudPD.getChildFrame()
t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625, 0. , 0. , -0.34604951]))
self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-3.31840048, 0.36408685, -0.67413123]), array([ 0.93449475, 0. , 0. , -0.35597691]))
self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004, 0. , 0. , -0.34940972]))
self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572, 0. , 0. , 0.91450893]))
self.drillWallWalkFarthestSafeFrame = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519, 0. , 0. , -0.16124022]))
self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-1.16122318, 0.04723203, -0.67493468]), array([ 0.93163145, 0. , 0. , -0.36340451]))
self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497, 0. , 0. , -0.53906374]))
self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075, 0. , 0. , -0.16525575]))
self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977, 0. , 0. , -0.3299461 ]))
self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1., 0., 0., 0.]))
self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
def __init__(self):
pose = transformUtils.poseFromTransform(vtk.vtkTransform())
self.pointcloud = ioUtils.readPolyData(ddapp.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp')
self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None)
segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0, 0. , 0. , 0.0])))
self.originFrame = self.pointcloudPD.getChildFrame()
t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625, 0. , 0. , -0.34604951]))
self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-3.31840048, 0.36408685, -0.67413123]), array([ 0.93449475, 0. , 0. , -0.35597691]))
self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004, 0. , 0. , -0.34940972]))
self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572, 0. , 0. , 0.91450893]))
self.drillWallWalkFarthestSafeFrame = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519, 0. , 0. , -0.16124022]))
self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-1.16122318, 0.04723203, -0.67493468]), array([ 0.93163145, 0. , 0. , -0.36340451]))
self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497, 0. , 0. , -0.53906374]))
self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075, 0. , 0. , -0.16525575]))
self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977, 0. , 0. , -0.3299461 ]))
self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1., 0., 0., 0.]))
self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD)
self.frameSync = vis.FrameSync()
self.frameSync.addFrame(self.originFrame)
self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True)
self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True)
self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
def testDemo(self):
postureFile = os.path.join(ddapp.getDRCBaseDir(), 'software/control/matlab/planners/prone/data_KPT.mat')
self.nominalPosture = scipy.io.loadmat(postureFile)['q_sol'][:,0]
self.contacts = ['l_toe', 'l_knee', 'r_toe', 'r_knee', 'l_hand']
self.positionCosts = [
['l_hand', [0.061, 0.703, 0], 1.0],
['r_hand', [-0.0295, -1.09, 0], 4.0],
]
self.makeGoalFrames()
self.findPose()
def _sendStartupCommands(self):
if self.restarted:
return
commands = []
commands.append('\n%-------- startup --------\n')
commands.append('format long e')
commands.append('addpath_control')
commands.append(
"addpath([getenv('DRC_BASE'), '/software/ddapp/src/matlab'])")
commands.append("robotURDF = [getenv('DRC_BASE'), '/%s'];" %
os.path.relpath(self.robotURDF, ddapp.getDRCBaseDir()))
commands.append(
"fixed_point_file = [getenv('DRC_BASE'), '/%s'];" %
os.path.relpath(self.fixedPointFile, ddapp.getDRCBaseDir()))
commands.append("left_foot_link = '%s';" % self.leftFootLink)
commands.append("right_foot_link = '%s';" % self.rightFootLink)
commands.append("pelvis_link = '%s';" % self.pelvisLink)
commands.append('runIKServer')
commands.append('\n%------ startup end ------\n')
return self.comm.sendCommandsAsync(commands)
def testDemo(self):
postureFile = os.path.join(
ddapp.getDRCBaseDir(),
'software/control/matlab/planners/prone/data_KPT.mat')
self.nominalPosture = scipy.io.loadmat(postureFile)['q_sol'][:, 0]
self.contacts = ['l_toe', 'l_knee', 'r_toe', 'r_knee', 'l_hand']
self.positionCosts = [
['l_hand', [0.061, 0.703, 0], 1.0],
['r_hand', [-0.0295, -1.09, 0], 4.0],
]
self.makeGoalFrames()
self.findPose()
def onShowMapButton(self):
# reloads the map each time - in case its changed on disk:
#if (self.octomap_cloud is None):
filename = ddapp.getDRCBaseDir() + "/software/build/data/octomap.pcd"
self.octomap_cloud = io.readPolyData(filename) # c++ object called vtkPolyData
assert (self.octomap_cloud.GetNumberOfPoints() !=0 )
# clip point cloud to height - doesnt work very well yet. need to know robot's height
#self.octomap_cloud = segmentation.cropToLineSegment(self.octomap_cloud, np.array([0,0,-10]), np.array([0,0,3]) )
# access to z values
#points= vnp.getNumpyFromVtk(self.octomap_cloud, 'Points')
#zvalues = points[:,2]
# remove previous map:
folder = om.getOrCreateContainer("segmentation")
om.removeFromObjectModel(folder)
vis.showPolyData(self.octomap_cloud, 'prior map', alpha=1.0, color=[0,0,0.4])
def onShowMapButton(self):
# reloads the map each time - in case its changed on disk:
#if (self.octomap_cloud is None):
filename = ddapp.getDRCBaseDir() + "/software/build/data/octomap.pcd"
self.octomap_cloud = io.readPolyData(filename) # c++ object called vtkPolyData
assert (self.octomap_cloud.GetNumberOfPoints() !=0 )
# clip point cloud to height - doesnt work very well yet. need to know robot's height
#self.octomap_cloud = segmentation.cropToLineSegment(self.octomap_cloud, np.array([0,0,-10]), np.array([0,0,3]) )
# access to z values
#points= vnp.getNumpyFromVtk(self.octomap_cloud, 'Points')
#zvalues = points[:,2]
# remove previous map:
folder = om.getOrCreateContainer("segmentation")
om.removeFromObjectModel(folder)
vis.showPolyData(self.octomap_cloud, 'prior map', alpha=1.0, color=[0,0,0.4])
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
if not filename:
polyData = vtk.vtkPolyData()
else:
polyData = self.getMeshManager().get(filename)
if not polyData:
if not os.path.isabs(filename):
filename = os.path.join(ddapp.getDRCBaseDir(), filename)
if os.path.isfile(filename):
polyData = ioUtils.readPolyData(filename)
else:
# use axes as a placeholder mesh
d = DebugData()
d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
polyData = d.getPolyData()
self.setPolyData(polyData)
def updateGeometryFromProperties(self):
filename = self.getProperty('Filename')
if not filename:
polyData = vtk.vtkPolyData()
else:
polyData = self.getMeshManager().get(filename)
if not polyData:
if not os.path.isabs(filename):
filename = os.path.join(ddapp.getDRCBaseDir(), filename)
if os.path.isfile(filename):
polyData = ioUtils.readPolyData(filename)
else:
# use axes as a placeholder mesh
d = DebugData()
d.addFrame(vtk.vtkTransform(), scale=0.1, tubeRadius=0.005)
polyData = d.getPolyData()
self.setPolyData(polyData)
def _sendStartupCommands(self):
if self.restarted:
return
commands = []
commands.append('\n%-------- startup --------\n')
commands.append('format long e')
commands.append('addpath_control')
commands.append("addpath([getenv('DRC_BASE'), '/software/ddapp/src/matlab'])")
commands.append("robotURDF = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.robotURDF, ddapp.getDRCBaseDir()))
commands.append("fixed_point_file = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.fixedPointFile, ddapp.getDRCBaseDir()))
commands.append("left_foot_link = '%s';" % self.leftFootLink)
commands.append("right_foot_link = '%s';" % self.rightFootLink)
commands.append("pelvis_link = '%s';" % self.pelvisLink)
commands.append('runIKServer')
commands.append('\n%------ startup end ------\n')
return self.comm.sendCommandsAsync(commands)
def getHandUrdf(self):
urdfBase = os.path.join(getDRCBaseDir(), 'software/models/common_components')
return os.path.join(urdfBase, 'hand_factory', self.handUrdf)
import os
import ddapp
from ddapp.consoleapp import ConsoleApp
from ddapp import applogic
from ddapp import visualization as vis
from ddapp import otdfmodel
app = ConsoleApp()
view = app.createView()
filename = os.path.join(ddapp.getDRCBaseDir(), 'software/models/otdf/cinderblockstep.otdf')
otdfmodel.openOtdf(filename, view)
if app.getTestingInteractiveEnabled():
view.show()
app.showObjectModel()
app.start()
def getHandUrdf(self):
urdfBase = os.path.join(getDRCBaseDir(), 'software/models/common_components')
return os.path.join(urdfBase, 'hand_factory', self.handUrdf)
def getDefaultBotConfigFile(self):
return os.path.join(ddapp.getDRCBaseDir(), 'software/config/drc_robot.cfg')
def getDefaultValkyrieV2DirectorConfigFile(self):
return os.path.join(ddapp.getDRCBaseDir(),
'software/models/val_description/director_config.json')
def getDefaultAtlasV5DirectorConfigFile(self):
return os.path.join(ddapp.getDRCBaseDir(),
'software/models/atlas_v5/director_config.json')
def getDefaultKukaLWRConfigFile(self):
return os.path.join(ddapp.getDRCBaseDir(),
'software/models/lwr_defs/director_config.json')
def getDefaultValkyrieV2DirectorConfigFile(self):
return os.path.join(
ddapp.getDRCBaseDir(),
'software/models/val_description/director_config.json')
def getDefaultAtlasV5DirectorConfigFile(self):
return os.path.join(ddapp.getDRCBaseDir(),
'software/models/atlas_v5/director_config.json')
def getDefaultBotConfigFile(self):
return os.path.join(ddapp.getDRCBaseDir(),
'software/config/drc_robot.cfg')
def getDefaultKukaLWRConfigFile(self):
return os.path.join(ddapp.getDRCBaseDir(),
'software/models/lwr_defs/director_config.json')
