def __init__(self, model_file): """\ Constructor. @param model_file: The YAML file for the model. @type model_file: C{str} """ self.model, relevance = YAMLParser(model_file).experiment try: self.target = relevance['target'] #self.robot = self.model.scene['robot'] self.vision_graph = \ self.model.coverage_hypergraph(relevance['cell'], K=[2]) except KeyError: raise KeyError('incorrect experiment format')
class CameraSelector(object): """\ Camera selector class. """ def __init__(self, model_file): """\ Constructor. @param model_file: The YAML file for the model. @type model_file: C{str} """ self.model, relevance = YAMLParser(model_file).experiment try: self.target = relevance['target'] #self.robot = self.model.scene['robot'] self.vision_graph = \ self.model.coverage_hypergraph(relevance['cell'], K=[2]) except KeyError: raise KeyError('incorrect experiment format') def best_view(self, current, target_pose, robot_config, threshold=0.0): """\ TODO @param current: The current active camera ID. @type current: C{str} @param target_pose: The current pose of the target. @type target_pose: L{Pose} @param robot_config: The current state of the robotic arm. @type robot_config: C{list} of C{float} @param threshold: Hysteresis threshold. @type threshold: C{float} @return: The next camera to make active. @rtype: C{str} """ self.target.pose = target_pose self.target.mount = self.model[current] print('Received pose %s from camera %s.' % (self.target.pose, current)) #self.robot.config = robot_config candidates = dict.fromkeys(self.vision_graph.neighbors(current) | \ set([current])) for camera in candidates: candidates[camera] = self.model[camera].performance(self.target) print(candidates) candidates[current] += threshold return sorted(candidates.keys(), key=candidates.__getitem__)[-1]
class TestModel01(unittest.TestCase): """\ Test model 01. """ def setUp(self): self.model, self.tasks = YAMLParser('test/test01.yaml').experiment def test_strength(self): p1 = Point(0, 0, 1000) p2 = Point(0, 0, 1200) self.assertEqual(self.model.strength(p1, self.tasks['R1'].params), self.model['C'].strength(p1, self.tasks['R1'].params)) self.assertTrue(self.model.strength(p1, self.tasks['R1'].params)) self.assertFalse(self.model.strength(p2, self.tasks['R1'].params)) self.model['C'].set_absolute_pose(Pose(T=Point(1000, 0, 0))) self.assertFalse(self.model.strength(p1, self.tasks['R1'].params)) self.model['C'].set_absolute_pose(Pose(R=Rotation.from_axis_angle(pi, Point(1, 0, 0)))) self.assertFalse(self.model.strength(p1, self.tasks['R1'].params)) def test_performance(self): self.assertTrue(self.model.performance(self.tasks['R1']) > 0) self.assertEqual(self.model.performance(self.tasks['R2']), 0.0) self.model['C'].setparam('zS', 600.0) self.assertEqual(self.model.performance(self.tasks['R1']), 0.0) def test_occlusion_cache(self): key = self.model._update_occlusion_cache(self.tasks['R1'].params) self.assertTrue(all([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles])) self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P2'].triangles])) self.model['C'].set_absolute_pose(Pose(R=Rotation.from_axis_angle(-pi / 2.0, Point(1, 0, 0)))) key = self.model._update_occlusion_cache(self.tasks['R1'].params) self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles])) self.assertTrue(all([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P2'].triangles])) self.model['C'].set_absolute_pose(Pose(R=Rotation.from_axis_angle(pi, Point(1, 0, 0)))) key = self.model._update_occlusion_cache(self.tasks['R1'].params) self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles])) self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P2'].triangles])) self.model['C'].set_absolute_pose(Pose()) key = self.model._update_occlusion_cache(self.tasks['R1'].params) self.assertTrue(all([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles])) self.model['C'].setparam('zS', 600.0) key = self.model._update_occlusion_cache(self.tasks['R1'].params) self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P1'].triangles])) self.assertFalse(any([t.mapped_triangle() in self.model._occlusion_cache[key]['C'].values() for t in self.model['P2'].triangles])) def test_robot_occlusion(self): self.model['RV1A'].set_config([90.0, 72.0, 60.0, 0.0, 0.0, 0.0, 0.0]) self.assertEqual(self.model.performance(self.tasks['R1']), 0.0) self.model['RV1A'].set_config([110.0, 72.0, 60.0, 0.0, 0.0, 0.0, 0.0]) self.assertTrue(self.model.performance(self.tasks['R1']) > 0)
def setUp(self): self.model, self.tasks = YAMLParser('test/test01.yaml').experiment
def setUp(self): self.model, self.tasks = YAMLParser('test/posable.yaml').experiment self.value = 0