def setupPackagePaths(): try: getDRCBaseDir() except KeyError: return searchPaths = [ 'software/models/atlas_v3', 'software/models/atlas_v4', 'software/models/atlas_v5', 'software/models/valkyrie', 'software/models/val_description', '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 getPackagePaths(): paths = [] try: getDRCBaseDir() except KeyError: pass else: paths += getBuiltinPackagePaths() paths += getEnvironmentPackagePaths() return paths
def updateGeometryFromProperties(self): filename = self.getProperty('Filename') scale = self.getProperty('Scale') 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(director.getDRCBaseDir(), filename) if os.path.isfile(filename): polyData = ioUtils.readPolyData(filename) if not scale == [1, 1, 1]: transform = vtk.vtkTransform() transform.Scale(scale) polyData = filterUtils.transformPolyData( polyData, transform) 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') scale = self.getProperty('Scale') 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(director.getDRCBaseDir(), filename) if os.path.isfile(filename): polyData = ioUtils.readPolyData(filename) if not scale == [1, 1, 1]: transform = vtk.vtkTransform() transform.Scale(scale) transformFilter = vtk.vtkTransformPolyDataFilter() transformFilter.SetInput(polyData) transformFilter.SetTransform(transform) transformFilter.Update() polyData = transformFilter.GetOutput() 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 getBuiltinPackagePaths(): searchPaths = [ "params/models/atlas_v3", "params/models/atlas_v4", "params/models/atlas_v5", "params/models/dual_arm_husky_description", "params/models/ur_description", "params/models/val_description", "params/models/hyq_description", "params/models/anymal_description", "params/models/anymal_b_description", "params/models/anymal_boxy_description", "params/models/lwr_defs", "params/models/husky_description", "params/models/mit_gazebo_models/mit_robot", "params/models/common_components/irobot_hand", "params/models/common_components/handle_description", "params/models/common_components/robotiq_hand_description", "params/models/common_components/schunk_description", "params/models/actuated_lidar_description", "params/models/dynamixel_description", "params/models/husky_description_rpg", "params/models/flir_ptu_description", "params/models/multisense_sl", "params/models/sick_description", "params/models/hokuyo_description", "params/models/otdf", ] baseDir = getDRCBaseDir() return [os.path.join(baseDir, path) for path in searchPaths]
def getHandUrdfFullPath(self): if director.getDRCBaseIsSet(): urdfBase = os.path.join( getDRCBaseDir(), 'software/models/common_components/hand_factory') else: urdfBase = drcargs.DirectorConfig.getDefaultInstance().dirname return os.path.join(urdfBase, self.handUrdf)
def testDemo(self): postureFile = os.path.join(director.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 __init__(self): pose = transformUtils.poseFromTransform(vtk.vtkTransform()) self.pointcloud = ioUtils.readPolyData(director.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 _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/director/src/matlab'])") commands.append( "robotURDF = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.robotURDF, director.getDRCBaseDir())) commands.append( "fixed_point_file = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.fixedPointFile, director.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 onShowMapButton(self): # reloads the map each time - in case its changed on disk: # if (self.octomap_cloud is None): filename = director.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 = director.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 getBuiltinPackagePaths(): searchPaths = [ 'software/models/atlas_v3', 'software/models/atlas_v4', 'software/models/atlas_v5', 'software/models/valkyrie', 'software/models/val_description', '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', ] baseDir = getDRCBaseDir() return [os.path.join(baseDir, path) for path in searchPaths]
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(director.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 getDefaultAtlasV5DirectorConfigFile(self): return os.path.join(director.getDRCBaseDir(), 'software/models/atlas_v5/director_config.json')
def getDefaultValkyrieSimpleDirectorConfigFile(self): return os.path.join( director.getDRCBaseDir(), 'software/models/val_description/director_config_simple.json')
def getHandUrdfFullPath(self): if director.getDRCBaseIsSet(): urdfBase = os.path.join(getDRCBaseDir(), 'software/models/common_components/hand_factory') else: urdfBase = drcargs.DirectorConfig.getDefaultInstance().dirname return os.path.join(urdfBase, self.handUrdf)
def getDefaultBotConfigFile(self): return os.path.join(director.getDRCBaseDir(), 'software/config/val/robot.cfg')
def getDefaultDualArmHuskyConfigFile(self): return os.path.join(director.getDRCBaseDir(), 'software/models/dual_arm_husky_description/director_config.json')
def getHandUrdf(self): urdfBase = os.path.join(getDRCBaseDir(), 'software/models/common_components') return os.path.join(urdfBase, 'hand_factory', self.handUrdf)
def getDefaultHyQDirectorConfigFile(self): return os.path.join(director.getDRCBaseDir(), "models/hyq_description/director_config.yaml")
def getDefaultValkyrieV1DirectorConfigFile(self): return os.path.join(director.getDRCBaseDir(), 'software/models/valkyrie/director_config.json')
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/director/src/matlab'])") commands.append("robotURDF = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.robotURDF, director.getDRCBaseDir())) commands.append("fixed_point_file = [getenv('DRC_BASE'), '/%s'];" % os.path.relpath(self.fixedPointFile, director.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)
import os import director from director.consoleapp import ConsoleApp from director import applogic from director import visualization as vis from director import otdfmodel app = ConsoleApp() view = app.createView() filename = os.path.join(director.getDRCBaseDir(), 'software/models/otdf/cinderblockstep.otdf') otdfmodel.openOtdf(filename, view) if app.getTestingInteractiveEnabled(): view.show() app.showObjectModel() app.start()
def getDefaultKukaLWRConfigFile(self): return os.path.join(director.getDRCBaseDir(), "models/lwr_defs/director_config.yaml")
def getDefaultAnymalDirectorConfigFile(self): return os.path.join(director.getDRCBaseDir(), "config/anymal/director_config.yaml")
def getDefaultKukaLWRConfigFile(self): return os.path.join(director.getDRCBaseDir(), 'software/models/lwr_defs/director_config.json')
def getDefaultValkyrieSimpleDirectorConfigFile(self): return os.path.join(director.getDRCBaseDir(), 'software/models/val_description/director_config_simple.json')
def getDefaultDualArmHuskyConfigFile(self): return os.path.join( director.getDRCBaseDir(), 'software/models/dual_arm_husky_description/director_config.json')
def getDefaultAtlasV5DirectorConfigFile(self): return os.path.join(director.getDRCBaseDir(), "models/atlas_v5/director_config.yaml")
def getEnvironmentPackagePaths(): packageMap = packagepath.PackageMap() packageMap.populateFromEnvironment(['ROS_PACKAGE_PATH']) return packageMap.map.values() def setupPackagePaths(): paths = getBuiltinPackagePaths() + getEnvironmentPackagePaths() for path in paths: PythonQt.dd.ddDrakeModel.addPackageSearchPath(path) try: getDRCBaseDir() except KeyError: pass else: setupPackagePaths() class HandFactory(object): def __init__(self, robotModel, defaultLeftHandType=None, defaultRightHandType=None): self.robotModel = robotModel self.defaultHandTypes = {} self.loaders = {}
def getDefaultHuskyConfigFile(self): return os.path.join(director.getDRCBaseDir(), "config/husky/director_config.yaml")