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 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 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")
def testLoadInvalidWorld(self):
with self.assertRaises(LoadWorldException):
GraspitCommander.loadWorld("")
def testLoadWorld(self):
GraspitCommander.loadWorld("plannerMug")