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 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 setUp(self):
GraspitCommander.clearWorld()
def testSimAnnPlannerOnPlannerMugWorld(self):
GraspitCommander.loadWorld("plannerMug")
r = GraspitCommander.planGrasps(max_steps=60000)
self.assertGreater(len(r.grasps), 0)
def testLoadInvalidWorld(self):
with self.assertRaises(LoadWorldException):
GraspitCommander.loadWorld("")
def testLoadWorld(self):
GraspitCommander.loadWorld("plannerMug")
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 testImportInvalidRobot(self):
with self.assertRaises(ImportRobotException):
GraspitCommander.importRobot("")
def testImportRobot(self):
GraspitCommander.importRobot("Barrett")
assert len(GraspitCommander.getRobots().ids) == 1
def testImportInvalidGraspableBody(self):
with self.assertRaises(ImportGraspableBodyException):
GraspitCommander.importGraspableBody("")
def testImportGraspableBody(self):
GraspitCommander.importGraspableBody("ashtray")
assert len(GraspitCommander.getGraspableBodies().ids) == 1
def testImportInvalidObstacle(self):
with self.assertRaises(ImportObstacleException):
GraspitCommander.importObstacle("")
def testImportObstacle(self):
GraspitCommander.importObstacle("floor")
assert len(GraspitCommander.getBodies().ids) == 1
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 testSaveImage(self):
GraspitCommander.loadWorld("plannerMug")
GraspitCommander.saveImage("test_img")
def testSaveWorld(self):
GraspitCommander.importRobot("Barrett")
GraspitCommander.importGraspableBody("ashtray")
GraspitCommander.saveWorld("test_world")
def setUp(self):
GraspitCommander.clearWorld()
def testToggleAllCollisions(self):
GraspitCommander.toggleAllCollisions(True)
def testLoadInvalidWorld(self):
with self.assertRaises(LoadWorldException):
GraspitCommander.loadWorld("")
def testLoadWorld(self):
GraspitCommander.loadWorld("plannerMug")
