def testStaticGraspExecution(self): GraspitCommander.loadWorld("plannerMug") GraspitCommander.approachToContact() GraspitCommander.autoGrasp() result = GraspitCommander.computeQuality() self.assertAlmostEqual(result.volume, 0.004336969, 4) self.assertAlmostEqual(result.epsilon, 0.046997464, 4)
def testFindInitialContact(self): # Test equivalence of FindInitialContact and ApproachContact # on the plannerMug world. GraspitCommander.loadWorld("plannerMug") GraspitCommander.findInitialContact() robot = GraspitCommander.getRobots().ids[0] pose_test = GraspitCommander.getRobot(robot).robot.pose GraspitCommander.clearWorld() GraspitCommander.loadWorld("plannerMug") GraspitCommander.approachToContact() robot = GraspitCommander.getRobots().ids[0] pose_target = GraspitCommander.getRobot(robot).robot.pose self.assertTrue(pose_test == pose_target)
def testDynamicGraspExecution(self): GraspitCommander.loadWorld("plannerMug") GraspitCommander.approachToContact() GraspitCommander.setDynamics(True) self.assertTrue(GraspitCommander.getDynamics()) GraspitCommander.autoGrasp() self.assertFalse(GraspitCommander.dynamicAutoGraspComplete()) while not GraspitCommander.dynamicAutoGraspComplete(): time.sleep(0.01) self.assertTrue(GraspitCommander.dynamicAutoGraspComplete()) GraspitCommander.setDynamics(False) result = GraspitCommander.computeQuality() self.assertAlmostEqual(result.volume, 0.005218228, 4) self.assertAlmostEqual(result.epsilon, 0.018258844, 4)
def testLoadInvalidWorld(self): with self.assertRaises(LoadWorldException): GraspitCommander.loadWorld("")
def testLoadWorld(self): GraspitCommander.loadWorld("plannerMug")
def testSimAnnPlannerOnPlannerMugWorld(self): GraspitCommander.loadWorld("plannerMug") r = GraspitCommander.planGrasps(max_steps=60000) self.assertGreater(len(r.grasps), 0)
def testSaveImage(self): GraspitCommander.loadWorld("plannerMug") GraspitCommander.saveImage("test_img")