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