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 setUp(self):
GraspitCommander.clearWorld()
def setUp(self):
GraspitCommander.clearWorld()