def setUp(self): from prpy.planning.chomp import DistanceFieldManager from openravepy import Environment self.env = Environment() self.env.Load('data/wamtest2.env.xml') self.robot = self.env.GetRobot('BarrettWAM') self.body = self.env.GetKinBody('mug-table') self.bodies = set(self.env.GetBodies()) self.module = CHOMPModuleMock(self.env) self.manager = DistanceFieldManager(self.module)
class DistanceFieldManagerTest(TestCase): def setUp(self): from prpy.planning.chomp import DistanceFieldManager from openravepy import Environment self.env = Environment() self.env.Load('data/wamtest2.env.xml') self.robot = self.env.GetRobot('BarrettWAM') self.body = self.env.GetKinBody('mug-table') self.bodies = set(self.env.GetBodies()) self.module = CHOMPModuleMock(self.env) self.manager = DistanceFieldManager(self.module) def test_GetGeometricState_ChangeEnabledStatusChangesState(self): with self.env: link = self.robot.GetLinks()[0] link.Enable(True) state_before = self.manager.get_geometric_state(self.robot) link.Enable(False) state_after = self.manager.get_geometric_state(self.robot) self.assertNotEquals(state_after, state_before) def test_GetGeometricState_ChangeInDOFValuesChangesState(self): with self.env: lower_limits, upper_limits = self.robot.GetDOFLimits() self.robot.SetDOFValues(lower_limits) state_before = self.manager.get_geometric_state(self.robot) self.robot.SetDOFValues(upper_limits) state_after = self.manager.get_geometric_state(self.robot) self.assertNotEquals(state_after, state_before) def test_GetGeometricState_ChangeInKinematicsGeometryHashChangesState(self): self.robot.GetKinematicsGeometryHash = lambda: 'mock_before' state_before = self.manager.get_geometric_state(self.robot) self.robot.GetKinematicsGeometryHash = lambda: 'mock_after' state_after = self.manager.get_geometric_state(self.robot) self.assertNotEquals(state_after, state_before) def test_GetCachePath_DifferentStatesProduceDifferentPaths(self): self.robot.GetKinematicsGeometryHash = lambda: 'mock_before' state_before = self.manager.get_geometric_state(self.robot) path_before = self.manager.get_cache_path(state_before) self.robot.GetKinematicsGeometryHash = lambda: 'mock_after' state_after = self.manager.get_geometric_state(self.robot) path_after = self.manager.get_cache_path(state_after) self.assertNotEquals(path_after, path_before) def test_Sync_InitiallyCreatesAllDistanceFields(self): self.manager.sync(self.robot) computed_bodies = [ args['kinbody'] for args in self.module.computedistancefield_args ] self.assertItemsEqual(self.bodies, computed_bodies) self.assertEqual(len(self.module.removefield_args), 0) def test_Sync_SyncTwiceDoesNothing(self): self.manager.sync(self.robot) del self.module.computedistancefield_args[:] del self.module.removefield_args[:] self.manager.sync(self.robot) self.assertEqual(len(self.module.computedistancefield_args), 0) self.assertEqual(len(self.module.removefield_args), 0) def test_Sync_IgnoresActiveRobotLinks(self): self.is_processed = False def computedistancefield(kinbody=None, cube_extent=None, aabb_padding=None, cache_filename=None, releasegil=False, **kw_args): if kinbody.GetName() == self.robot.GetName(): self.assertTrue(self.robot.GetLink('segway').IsEnabled()) self.assertTrue(self.robot.GetLink('wam0').IsEnabled()) self.assertTrue(self.robot.GetLink('wam1').IsEnabled()) self.assertTrue(self.robot.GetLink('wam2').IsEnabled()) self.assertTrue(self.robot.GetLink('wam3').IsEnabled()) self.assertFalse(self.robot.GetLink('wam4').IsEnabled()) self.assertFalse(self.robot.GetLink('wam5').IsEnabled()) self.assertFalse(self.robot.GetLink('wam6').IsEnabled()) self.assertFalse(self.robot.GetLink('wam7').IsEnabled()) self.assertFalse(self.robot.GetLink('handbase').IsEnabled()) self.assertFalse(self.robot.GetLink('Finger0-0').IsEnabled()) self.assertFalse(self.robot.GetLink('Finger0-1').IsEnabled()) self.assertFalse(self.robot.GetLink('Finger0-2').IsEnabled()) self.assertFalse(self.robot.GetLink('Finger1-0').IsEnabled()) self.assertFalse(self.robot.GetLink('Finger1-1').IsEnabled()) self.assertFalse(self.robot.GetLink('Finger1-2').IsEnabled()) self.assertFalse(self.robot.GetLink('Finger2-1').IsEnabled()) self.assertFalse(self.robot.GetLink('Finger2-2').IsEnabled()) self.is_processed = True dof_index = self.robot.GetJoint('Elbow').GetDOFIndex() self.robot.SetActiveDOFs([ dof_index ]) self.robot.Enable(True) self.module.computedistancefield = computedistancefield self.manager.sync(self.robot) self.assertTrue(self.is_processed) def test_Sync_RecomputesDistanceFieldIfStateChanges(self): self.manager.sync(self.robot) del self.module.computedistancefield_args[:] del self.module.removefield_args[:] # Change the geometry to invalidate the key. link = self.robot.GetLink('segway') link.SetGroupGeometries('test', []) link.SetGeometriesFromGroup('test') self.manager.sync(self.robot) self.assertEqual(len(self.module.computedistancefield_args), 1) self.assertEqual(self.module.computedistancefield_args[0]['kinbody'], self.robot) self.assertEqual(len(self.module.removefield_args), 1) self.assertEqual(self.module.removefield_args[0]['kinbody'], self.robot) self.assertLess(self.module.removefield_args[0]['__sequence__'], self.module.computedistancefield_args[0]['__sequence__'])