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 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 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 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')