def prepTestDemoSequence(self): ''' Running this function should launch a full planning sequence to pick to objects, walk and drop. Requires footstep footstepPlanner ''' filename = os.path.expanduser('~/drc-testing-data/tabletop/table-and-bin-scene.vtp') scene = ioUtils.readPolyData(filename) vis.showPolyData(scene,"scene") #stanceFrame = transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 0, 123.0]) #self.teleportRobotToStanceFrame(stanceFrame) self.userFitTable() self.onSegmentTable( np.array([-1.72105646, 2.73210716, 0.79449952]), np.array([-1.67336452, 2.63351011, 0.78698605]) ) self.userFitBin() self.onSegmentBin( np.array([-0.02, 2.43, 0.61 ]), np.array([-0.40, 2.79, 0.61964661]) ) self.computeTableStanceFrame() self.computeBinStanceFrame()
def prepIhmcDemoSequenceFromFile(self): filename = os.path.expanduser('~/drc-testing-data/ihmc_table/ihmc_table.vtp') polyData = ioUtils.readPolyData( filename ) vis.showPolyData( polyData,'scene') self.prepIhmcDemoSequence()
def prepKukaTestDemoSequence(self, inputFile='~/drc-testing-data/tabletop/kinect_collision_environment.vtp'): filename = os.path.expanduser(inputFile) scene = ioUtils.readPolyData(filename) vis.showPolyData(scene,"scene") self.prepKukaLabScene()