def test_make_pose_rel_and_abs_again() -> None: parent = Pose(Position(), Orientation(0, 0, 1, 0)) child_to_be = Pose(Position(1, 0, 0)) child = make_pose_rel(parent, child_to_be) assert child == Pose(Position(-1, 0, 0), Orientation(0, 0, -1, 0)) assert make_pose_abs(parent, child) == child_to_be
def test_obj_relative_ap_global() -> None: scene = Scene("s1") # object rotated 90° clock-wise so1 = SceneObject( "so1", "WhatEver", Pose(Position(1, 0, 0), Orientation(0, 0, -0.707, 0.707))) scene.objects.append(so1) cached_scene = CachedScene(scene) project = Project("p1", scene.id) ap1 = ActionPoint("ap1", Position(1, 0, 0), parent=so1.id) project.action_points.append(ap1) ap2 = ActionPoint("ap2", Position(1, 0, 0), parent=ap1.id) no1 = NamedOrientation("o1", Orientation()) ap2.orientations.append(no1) project.action_points.append(ap2) cached_project = CachedProject(project) make_relative_ap_global(cached_scene, cached_project, ap2) check_ap(ap2) assert ap2.position == Position(1, -2, 0) assert so1.pose assert no1.orientation == so1.pose.orientation make_global_ap_relative(cached_scene, cached_project, ap2, ap1.id) assert ap2.position == Position(1, 0, 0) assert no1.orientation == Orientation() check_ap(ap2)
def test_make_pose_abs_3() -> None: parent = Pose(Position(1, 1, 0), Orientation(0, 0, 0.707, 0.707)) child = Pose(Position(-1, 1, 0), Orientation()) assert make_pose_abs(parent, child) == Pose(Position(0, 0, 0), Orientation(0, 0, 0.707, 0.707))
def test_make_relative_ap_global_and_relative_again(): scene = Scene("s1", "s1") scene.objects.append( SceneObject("so1", "so1", "WhatEver", Pose(Position(3, 0, 0), Orientation()))) project = Project("p1", "p1", "s1") project.action_points.append( ProjectActionPoint("ap1", "ap1", Position(-1, 0, 0), parent="so1")) project.action_points.append( ProjectActionPoint("ap2", "ap2", Position(-1, 0, 0), parent="ap1")) ap3 = ProjectActionPoint("ap3", "ap3", Position(-1, 0, 0), parent="ap2") project.action_points.append(ap3) make_relative_ap_global(scene, project, ap3) assert ap3.parent is None assert ap3.position.x == .0 make_global_ap_relative(scene, project, ap3, "ap2") assert ap3.parent == "ap2" assert ap3.position.x == -1 ap3.parent = "something_unknown" with pytest.raises(Arcor2Exception): make_relative_ap_global(scene, project, ap3)
def test_get_value() -> None: p = Pose(Position(1, 2, 3), Orientation(1, 0, 0, 0)) scene = Scene("s1") obj = SceneObject("test_name", TestObject.__name__) scene.objects.append(obj) project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position(1, 0, 0)) project.action_points.append(ap1) ap2 = ActionPoint("ap2", Position(), parent=ap1.id) project.action_points.append(ap2) ori1 = NamedOrientation("ori1", p.orientation) ap2.orientations.append(ori1) invalid_param_name = "invalid_param" ac1 = Action( "ac1", f"{obj.id}/{TestObject.action.__name__}", parameters=[ ActionParameter(param_name, PosePlugin.type_name(), json.dumps(ori1.id)), ActionParameter(invalid_param_name, PosePlugin.type_name(), json.dumps("non_sense")), ], ) ap1.actions.append(ac1) cscene = CachedScene(scene) cproject = CachedProject(project) with pytest.raises(Arcor2Exception): PosePlugin.parameter_value(type_defs, cscene, cproject, ac1.id, "non_sense") with pytest.raises(Arcor2Exception): PosePlugin.parameter_value(type_defs, cscene, cproject, "non_sense", param_name) with pytest.raises(ParameterPluginException): PosePlugin.parameter_value(type_defs, cscene, cproject, ac1.id, invalid_param_name) value = PosePlugin.parameter_value(type_defs, cscene, cproject, ac1.id, param_name) exe_value = PosePlugin.parameter_execution_value(type_defs, cscene, cproject, ac1.id, param_name) assert value == value assert value != exe_value
def make_position_abs(parent: Position, child: Position) -> Position: p = Position() p.x = child.x + parent.x p.y = child.y + parent.y p.z = child.z + parent.z return p
def test_blind_branch() -> None: scene = Scene("s1") obj = SceneObject("test_name", Test.__name__) scene.objects.append(obj) project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position()) project.action_points.append(ap1) ac1 = Action("ac1", f"{obj.id}/test", flows=[Flow(outputs=["bool_res"])]) ap1.actions.append(ac1) ac2 = Action("ac2", f"{obj.id}/test", flows=[Flow()]) ap1.actions.append(ac2) ac3 = Action("ac3", f"{obj.id}/test", flows=[Flow()]) ap1.actions.append(ac3) ac4 = Action("ac4", f"{obj.id}/test", flows=[Flow()]) ap1.actions.append(ac4) project.logic.append(LogicItem(LogicItem.START, ac1.id)) project.logic.append(LogicItem(ac1.id, ac2.id, ProjectLogicIf(f"{ac1.id}/default/0", json.dumps(True)))) project.logic.append(LogicItem(ac1.id, ac3.id, ProjectLogicIf(f"{ac1.id}/default/0", json.dumps(False)))) project.logic.append(LogicItem(ac2.id, ac4.id)) project.logic.append(LogicItem(ac4.id, LogicItem.END)) with pytest.raises(SourceException, match=f"Action {ac3.name} has no outputs."): program_src({Test.__name__: Test}, CachedProject(project), CachedScene(scene))
def test_constant() -> None: scene = Scene("s1") obj = SceneObject("test_name", Test.__name__) scene.objects.append(obj) project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position()) project.action_points.append(ap1) const_value = 1234 const = ProjectParameter("int_const", "integer", json.dumps(const_value)) project.parameters.append(const) ac1 = Action( "ac1", f"{obj.id}/test_par", flows=[Flow()], parameters=[ ActionParameter("param", ActionParameter.TypeEnum.CONSTANT, json.dumps(const.id)) ], ) ap1.actions.append(ac1) project.logic.append(LogicItem(LogicItem.START, ac1.id)) project.logic.append(LogicItem(ac1.id, LogicItem.END)) src = program_src({Test.__name__: Test}, CachedProject(project), CachedScene(scene)) assert f"{const.name} = {const_value}" in src assert f"test_name.{Test.test_par.__name__}({const.name}, an='ac1')" in src
def test_global_aps_cls() -> None: proj = Project("test", "scene_id") pos = Position(1, 2, 3) ap1 = ActionPoint("ap1", pos) ap1_o1 = NamedOrientation("o1", Orientation(0.707, 0, 0, 0.707)) ap1.orientations.append(ap1_o1) ap1_j1 = ProjectRobotJoints("j1", "robot", [Joint("whatever", 1234)]) ap1.robot_joints.append(ap1_j1) proj.action_points.append(ap1) os.environ["ARCOR2_PROJECT_PATH"] = "/tmp" import arcor2.resources # noqa my_name = "my_module" my_spec = importlib.util.spec_from_loader(my_name, loader=None) my_module = importlib.util.module_from_spec(my_spec) cproj = CachedProject(proj) src = global_action_points_class(cproj) exec(src, my_module.__dict__) sys.modules["my_module"] = my_module aps = my_module.ActionPoints(SimResources(cproj)) # type: ignore assert aps.ap1.position == pos assert aps.ap1.position is not pos assert aps.ap1.poses.o1 == Pose(ap1.position, ap1_o1.orientation) assert aps.ap1.poses.o1.orientation is not ap1_o1.orientation assert aps.ap1.joints.j1 == ap1_j1 assert aps.ap1.joints.j1 is not ap1_j1
def make_position_rel(parent: Position, child: Position) -> Position: p = Position() p.x = child.x - parent.x p.y = child.y - parent.y p.z = child.z - parent.z return p
def test_make_relative_ap_global_and_relative_again() -> None: scene = Scene("s1") so1 = SceneObject("so1", "WhatEver", Pose(Position(3, 0, 0), Orientation())) scene.objects.append(so1) cached_scene = CachedScene(scene) project = Project("p1", scene.id) ap1 = ActionPoint("ap1", Position(-1, 0, 0), parent=so1.id) project.action_points.append(ap1) ap2 = ActionPoint("ap2", Position(-1, 0, 0), parent=ap1.id) project.action_points.append(ap2) ap3 = ActionPoint( "ap3", Position(-1, 0, 0), parent=ap2.id, orientations=[NamedOrientation("bla", random_orientation())]) project.action_points.append(ap3) cached_project = CachedProject(project) assert ap3.parent ap3_parent = get_parent_pose(cached_scene, cached_project, ap3.parent) assert Pose(ap2.position, Orientation()) == ap3_parent.pose assert ap3_parent.parent_id == ap1.id make_relative_ap_global(cached_scene, cached_project, ap3) check_ap(ap3) assert ap3.parent is None assert ap3.position.x == 0.0 # type: ignore make_global_ap_relative(cached_scene, cached_project, ap3, ap2.id) check_ap(ap3) assert ap3.parent == ap2.id assert ap3.position.x == -1 ap3.parent = "something_unknown" with pytest.raises(Arcor2Exception): make_relative_ap_global(cached_scene, cached_project, ap3)
def project() -> UpdateableCachedProject: project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position()) project.action_points.append(ap1) ap1.actions.append(ac1) ap1.actions.append(ac2) ap1.actions.append(ac3) ap1.actions.append(ac4) return UpdateableCachedProject(project)
def test_patch_object_actions(monkeypatch, capsys) -> None: class MyObject(Generic): def action(self, pose: Pose, *, an: Optional[str] = None) -> None: pass action.__action__ = ActionMetadata() # type: ignore # @action tries to read from stdin sio = io.StringIO() sio.fileno = lambda: 0 # type: ignore # fake whatever fileno monkeypatch.setattr("sys.stdin", sio) obj_id = "123" pose = Pose(Position(0, 0, 0), Orientation(1, 0, 0, 0)) setattr(pose, AP_ID_ATTR, "pose") # set pose id (simulate pose declaration in scene json) my_obj = MyObject(obj_id, "") my_obj.action(pose) assert action._executed_action is None out_before, _ = capsys.readouterr() assert not out_before patch_object_actions(MyObject) setattr( MyObject, ACTION_NAME_ID_MAPPING_ATTR, {"name": "id"}) # this simulates what patch_with_action_mapping does my_obj.action(pose, an="name") assert action._executed_action is None out_after, _ = capsys.readouterr() arr = out_after.strip().split("\n") assert len(arr) == 2 before_evt = ActionStateBefore.from_json(arr[0]) after_evt = ActionStateAfter.from_json(arr[1]) assert before_evt.data.action_id == "id" assert after_evt.data.action_id == "id" assert before_evt.data.action_point_ids is not None assert "pose" in before_evt.data.action_point_ids with pytest.raises(Arcor2Exception): my_obj.action(pose) assert action._executed_action is None with pytest.raises(Arcor2Exception): my_obj.action(pose, an="unknown_action_name") assert action._executed_action is None
def estimate_camera_pose(camera_matrix: List[List[float]], dist_matrix: List[float], image: Image.Image, marker_size: float) -> Dict[int, Pose]: camera_matrix_arr, dist_matrix_arr, gray, corners, ids = detect_corners( camera_matrix, dist_matrix, image, refine=True) ret: Dict[int, Pose] = {} if np.all(ids is None): return ret # TODO do not perform estimation for un-configured markers rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, marker_size, camera_matrix_arr, dist_matrix_arr) rvec = rvec.reshape(len(ids), 3) tvec = tvec.reshape(len(ids), 3) if __debug__: backtorgb = cv2.cvtColor(gray, cv2.COLOR_GRAY2RGB) aruco.drawDetectedMarkers(backtorgb, corners) # Draw A square around the markers for idx in range(len(ids)): aruco.drawAxis(backtorgb, camera_matrix_arr, dist_matrix_arr, rvec[idx], tvec[idx], 0.15) cv2.imwrite("marker.jpg", backtorgb) for idx, mid in enumerate(ids): # convert pose of the marker wrt camera to pose of camera wrt marker # based on https://stackoverflow.com/a/51515560/3142796 marker_rot_matrix, _ = cv2.Rodrigues(rvec[idx]) assert np.allclose(np.linalg.inv(marker_rot_matrix), marker_rot_matrix.transpose()) assert math.isclose(np.linalg.det(marker_rot_matrix), 1) camera_rot_matrix = marker_rot_matrix.transpose() camera_trans_vector = np.matmul(-camera_rot_matrix, tvec[idx].reshape(3, 1)).flatten() o = Orientation.from_quaternion( quaternion.from_rotation_matrix(camera_rot_matrix)) ret[mid[0]] = Pose( Position(camera_trans_vector[0], camera_trans_vector[1], camera_trans_vector[2]), o) return ret
def _make_global_ap_relative(parent_id: str) -> None: parent = get_parent_pose(scene, project, parent_id) if parent.parent_id: _make_global_ap_relative(parent.parent_id) ap.position = make_pose_rel(parent.pose, Pose(ap.position, Orientation())).position for ori in project.ap_orientations(ap.id): ori.orientation = make_pose_rel(parent.pose, Pose(Position(), ori.orientation)).orientation if parent.pose.orientation != Orientation(): _update_childs(parent, ap.id)
def test_get_value() -> None: scene = Scene("s1", "s1") obj = SceneObject("test_name", TestObject.__name__) prj = ProjectRobotJoints("name", obj.id, [Joint("name", 0.333)]) scene.objects.append(obj) project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position(1, 0, 0)) ap1.robot_joints.append(prj) project.action_points.append(ap1) invalid_param_name = "invalid_param" act = Action( "ac1", f"{obj.id}/{TestObject.action.__name__}", parameters=[ ActionParameter(param_name, JointsPlugin.type_name(), json.dumps(prj.id)), ActionParameter(invalid_param_name, JointsPlugin.type_name(), json.dumps("non_sense")), ], ) ap1.actions.append(act) cscene = CachedScene(scene) cproject = CachedProject(project) with pytest.raises(Arcor2Exception): JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id, "non_sense") with pytest.raises(Arcor2Exception): JointsPlugin.parameter_value(type_defs, cscene, cproject, "non_sense", param_name) with pytest.raises(ParameterPluginException): JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id, invalid_param_name) value = JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id, param_name) exe_value = JointsPlugin.parameter_execution_value(type_defs, cscene, cproject, act.id, param_name) assert value == value assert value == exe_value
def test_get_value() -> None: img = Image.new("RGB", (320, 240)) scene = Scene("s1", "s1") obj = SceneObject("test_name", TestObject.__name__) scene.objects.append(obj) project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position(1, 0, 0)) project.action_points.append(ap1) invalid_param_name = "invalid_param" ac1 = Action( "ac1", f"{obj.id}/{TestObject.action.__name__}", parameters=[ ActionParameter(param_name, ImagePlugin.type_name(), ImagePlugin.value_to_json(img)), ActionParameter(invalid_param_name, ImagePlugin.type_name(), json.dumps("non_sense")), ], ) ap1.actions.append(ac1) cscene = CachedScene(scene) cproject = CachedProject(project) with pytest.raises(Arcor2Exception): ImagePlugin.parameter_value(type_defs, cscene, cproject, ac1.id, "non_sense") with pytest.raises(Arcor2Exception): ImagePlugin.parameter_value(type_defs, cscene, cproject, "non_sense", param_name) with pytest.raises(ParameterPluginException): ImagePlugin.parameter_value(type_defs, cscene, cproject, ac1.id, invalid_param_name) value = ImagePlugin.parameter_value(type_defs, cscene, cproject, ac1.id, param_name) exe_value = ImagePlugin.parameter_execution_value(type_defs, cscene, cproject, ac1.id, param_name) assert value == value assert value == exe_value
def message_to_pose(message: str) -> Pose: tokens = message.split() if len(tokens) != 7: raise YumiException("Invalid format for pose! Got:\n{0}".format(message)) pose_vals = [float(token) for token in tokens] q = pose_vals[3:] t = pose_vals[:3] try: return Pose( Position(t[0], t[1], t[2]) * MM_TO_METERS, Orientation.from_quaternion(quaternion.from_float_array(q)) ) except (IndexError, ValueError): raise YumiException("Invalid pose.")
def _make_relative_ap_global(_ap: BareActionPoint) -> None: if not _ap.parent: return parent = get_parent_pose(scene, project, _ap.parent) if parent.pose.orientation != Orientation(): _update_childs(parent, _ap.id) _ap.position = make_pose_abs(parent.pose, Pose(_ap.position, Orientation())).position for ori in project.ap_orientations(_ap.id): ori.orientation = make_pose_abs(parent.pose, Pose(Position(), ori.orientation)).orientation _ap.parent = parent.parent_id _make_relative_ap_global(_ap)
def test_get_value(self, val: str) -> None: scene = Scene("s1") obj = SceneObject("test_name", TestObject.__name__) scene.objects.append(obj) project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position()) project.action_points.append(ap1) invalid_param_name = "invalid_param" ac1 = Action( "ac1", f"{obj.id}/{TestObject.action.__name__}", parameters=[ ActionParameter(param_name, StringPlugin.type_name(), StringPlugin.value_to_json(val)), ActionParameter(invalid_param_name, StringPlugin.type_name(), json.dumps(666)), ], ) ap1.actions.append(ac1) cscene = CachedScene(scene) cproject = CachedProject(project) with pytest.raises(Arcor2Exception): StringPlugin.parameter_value({}, cscene, cproject, ac1.id, "non_sense") with pytest.raises(Arcor2Exception): StringPlugin.parameter_value({}, cscene, cproject, "non_sense", param_name) with pytest.raises(ParameterPluginException): StringPlugin.parameter_value({}, cscene, cproject, ac1.id, invalid_param_name) value = StringPlugin.parameter_value({}, cscene, cproject, ac1.id, param_name) exe_value = StringPlugin.parameter_execution_value({}, cscene, cproject, ac1.id, param_name) assert value == val assert value == exe_value
def test_patch_object_actions_without_mapping(monkeypatch, capsys) -> None: class MyObject(Generic): def action(self, pose: Pose, *, an: Optional[str] = None) -> None: pass action.__action__ = ActionMetadata() # type: ignore # @action tries to read from stdin sio = io.StringIO() sio.fileno = lambda: 0 # type: ignore # fake whatever fileno monkeypatch.setattr("sys.stdin", sio) obj_id = "123" pose = Pose(Position(0, 0, 0), Orientation(1, 0, 0, 0)) setattr(pose, AP_ID_ATTR, "pose") # set pose id (simulate pose declaration in scene json) my_obj = MyObject(obj_id, "") patch_object_actions(MyObject) # no mapping given my_obj.action(pose) # this should be ok assert action._executed_action is None out_after, _ = capsys.readouterr() assert out_after arr = out_after.strip().split("\n") assert len(arr) == 1 before_evt = ActionStateBefore.from_json(arr[0]) assert before_evt.data.action_id is None assert before_evt.data.parameters assert Pose.from_json(before_evt.data.parameters[0]) == pose assert before_evt.data.action_point_ids is not None assert "pose" in before_evt.data.action_point_ids # the 'an' should be just ignored # we are testing here that second execution of action is fine my_obj.action(pose, an="whatever") assert action._executed_action is None out_after2, _ = capsys.readouterr() assert out_after2 == out_after
def test_prev_result() -> None: scene = Scene("s1") obj = SceneObject("test_name", Test.__name__) scene.objects.append(obj) project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position()) project.action_points.append(ap1) ac1 = Action("ac1", f"{obj.id}/{Test.get_int.__name__}", flows=[Flow(outputs=["res"])]) ap1.actions.append(ac1) ac2 = Action( "ac2", f"{obj.id}/{Test.test_par.__name__}", flows=[Flow()], parameters=[ ActionParameter("param", ActionParameter.TypeEnum.LINK, json.dumps(f"{ac1.id}/default/0")) ], ) ap1.actions.append(ac2) project.logic.append(LogicItem(LogicItem.START, ac1.id)) project.logic.append(LogicItem(ac1.id, ac2.id)) project.logic.append(LogicItem(ac2.id, LogicItem.END)) src = program_src({Test.__name__: Test}, CachedProject(project), CachedScene(scene)) assert f"res = test_name.{Test.get_int.__name__}(an='{ac1.name}')" in src assert f"test_name.{Test.test_par.__name__}(res, an='{ac2.name}')" in src # test wrong order of logic project.logic.clear() project.logic.append(LogicItem(LogicItem.START, ac2.id)) project.logic.append(LogicItem(ac2.id, ac1.id)) project.logic.append(LogicItem(ac1.id, LogicItem.END)) with pytest.raises(SourceException): program_src({Test.__name__: Test}, CachedProject(project), CachedScene(scene))
def main() -> None: dm = DobotMagician( "id", "name", Pose(Position(1, -1, -3), Orientation(0, 0, 0.707, 0.707)), DobotSettings("/dev/ttyUSB0")) orig_pose = dm.get_end_effector_pose("default") val = float(sys.argv[1]) updated_pose = copy.deepcopy(orig_pose) updated_pose.position.x += val dm.move_to_pose("", updated_pose, 1.0) new_pose = dm.get_end_effector_pose("default") print(new_pose.position.x - orig_pose.position.x - val)
def forward_kinematics(self, joints: List[Joint]) -> Pose: """Computes forward kinematics. Outputs absolute pose. Inspired by DobotKinematics.py from open-dobot project. :param end_effector_id: Target end effector name :param joints: Input joint values :return: Pose of the given end effector """ self.validate_joints(joints) j1 = joints[0].value j2 = joints[1].value j3 = joints[2].value sj = j2 + j3 radius = ( self.link_2_length * math.cos(j2 - math.pi / 2) + self.link_3_length * math.cos(sj) + self.link_4_length ) x = radius * math.cos(j1) y = radius * math.sin(j1) z = ( self.link_2_length * math.cos(j2) + self.link_3_length * math.cos(sj + math.pi / 2) - self.end_effector_length ) pose = Pose( Position(x, y, z), Orientation.from_quaternion(quaternion.from_euler_angles(0, math.pi, joints[-1].value + j1)), ) if __debug__: self._check_orientation(pose) return tr.make_pose_abs(self.pose, pose)
def make_relative_ap_global(scene: CScene, project: CProject, ap: BareActionPoint) -> None: """Transforms (in place) relative AP into a global one. :param scene: :param project: :param ap: :return: """ if not ap.parent: return parent = get_parent_pose(scene, project, ap.parent) ap.position = make_pose_abs(parent.pose, Pose(ap.position, Orientation())).position for ori in project.ap_orientations(ap.id): ori.orientation = make_pose_abs(parent.pose, Pose(Position(), ori.orientation)).orientation ap.parent = parent.parent_id make_relative_ap_global(scene, project, ap)
import pytest from arcor2.data.common import Orientation, Pose, Position from arcor2_fit_demo.object_types.abstract_dobot import DobotException, DobotSettings from arcor2_fit_demo.object_types.dobot_magician import DobotMagician URL = os.getenv("ARCOR2_DOBOT_URL", "http://*****:*****@pytest.mark.skip(reason="Temporarily disabled (need to run the service).") @pytest.mark.parametrize( "pose", [ Pose( Position(0.24027805415562356, -0.033064804793467024, -0.10107283020019531), Orientation(-0.06832257459695434, 0.9976632827765306, -4.18355111448332e-18, 6.108925729395595e-17), ), Pose( Position(0.2570099061465494, -0.03512534022111209, 0.021290023803710936), Orientation(-0.0678616466931505, 0.9976947413453146, -4.155327420381767e-18, 6.10911835757343e-17), ), Pose( Position(0.21265726282945766, -0.04578391051491453, 0.10375743103027343), Orientation(-0.10583019614908788, 0.9943842162781173, -6.480230548355851e-18, 6.088847237938229e-17), ),
def test_update_object_pose(start_processes: None, ars: ARServer) -> None: upload_def(Box, BoxModel("Box", 0.1, 0.1, 0.1)) upload_def(DummyMultiArmRobot) test = "test" event(ars, events.c.ShowMainScreen) assert ars.call_rpc( rpc.s.NewScene.Request(get_id(), rpc.s.NewScene.Request.Args(test)), rpc.s.NewScene.Response).result assert len(event(ars, events.o.ChangedObjectTypes).data) == 2 event(ars, events.s.OpenScene) event(ars, events.s.SceneState) assert ars.call_rpc( rpc.s.AddObjectToScene.Request( get_id(), rpc.s.AddObjectToScene.Request.Args("obj", Box.__name__, Pose())), rpc.s.AddObjectToScene.Response, ).result scene_obj = event(ars, events.s.SceneObjectChanged).data assert ars.call_rpc( rpc.s.AddObjectToScene.Request( get_id(), rpc.s.AddObjectToScene.Request.Args("robot", DummyMultiArmRobot.__name__, Pose())), rpc.s.AddObjectToScene.Response, ).result scene_robot = event(ars, events.s.SceneObjectChanged).data new_pose = Pose(Position(1)) lock_object(ars, scene_obj.id) assert ars.call_rpc( rpc.s.UpdateObjectPose.Request( get_id(), rpc.s.UpdateObjectPose.Request.Args(scene_obj.id, new_pose)), rpc.s.UpdateObjectPose.Response, ).result assert event(ars, events.s.SceneObjectChanged).data.pose == new_pose unlock_object(ars, scene_obj.id) lock_object(ars, scene_robot.id) assert ars.call_rpc( rpc.s.UpdateObjectPose.Request( get_id(), rpc.s.UpdateObjectPose.Request.Args(scene_robot.id, new_pose)), rpc.s.UpdateObjectPose.Response, ).result assert event(ars, events.s.SceneObjectChanged).data.pose == new_pose unlock_object(ars, scene_robot.id) assert ars.call_rpc( rpc.s.StartScene.Request(get_id()), rpc.s.StartScene.Response, ).result assert event(ars, events.s.SceneState ).data.state == events.s.SceneState.Data.StateEnum.Starting assert event(ars, events.s.SceneState ).data.state == events.s.SceneState.Data.StateEnum.Started lock_object(ars, scene_obj.id) # with started scene, it should still be possible to manipulate the object assert ars.call_rpc( rpc.s.UpdateObjectPose.Request( get_id(), rpc.s.UpdateObjectPose.Request.Args(scene_obj.id, Pose())), rpc.s.UpdateObjectPose.Response, ).result assert event(ars, events.s.SceneObjectChanged).data.pose == Pose() unlock_object(ars, scene_obj.id) lock_object(ars, scene_robot.id) # ...but it should not be possible to manipulate the robot assert not ars.call_rpc( rpc.s.UpdateObjectPose.Request( get_id(), rpc.s.UpdateObjectPose.Request.Args(scene_robot.id, Pose())), rpc.s.UpdateObjectPose.Response, ).result unlock_object(ars, scene_robot.id)
def test_branched_output_2() -> None: scene = Scene("s1") obj = SceneObject("test_name", Test.__name__) scene.objects.append(obj) project = Project("p1", "s1") ap1 = ActionPoint("ap1", Position()) project.action_points.append(ap1) ac1 = Action("ac1", f"{obj.id}/test", flows=[Flow(outputs=["bool_res"])]) ap1.actions.append(ac1) ac2 = Action("ac2", f"{obj.id}/test", flows=[Flow()]) ap1.actions.append(ac2) ac3 = Action("ac3", f"{obj.id}/test", flows=[Flow()]) ap1.actions.append(ac3) ac4 = Action("ac4", f"{obj.id}/test", flows=[Flow(outputs=["bool2_res"])]) ap1.actions.append(ac4) ac5 = Action("ac5", f"{obj.id}/test", flows=[Flow()]) ap1.actions.append(ac5) ac6 = Action("ac6", f"{obj.id}/test", flows=[Flow()]) ap1.actions.append(ac6) project.logic.append(LogicItem(LogicItem.START, ac1.id)) project.logic.append( LogicItem(ac1.id, ac2.id, ProjectLogicIf(f"{ac1.id}/default/0", json.dumps(True)))) project.logic.append( LogicItem(ac1.id, ac4.id, ProjectLogicIf(f"{ac1.id}/default/0", json.dumps(False)))) project.logic.append(LogicItem(ac2.id, ac3.id)) project.logic.append(LogicItem(ac3.id, ac6.id)) project.logic.append( LogicItem(ac4.id, ac5.id, ProjectLogicIf(f"{ac4.id}/default/0", json.dumps(True)))) project.logic.append(LogicItem(ac5.id, ac6.id)) project.logic.append(LogicItem(ac6.id, LogicItem.END)) project.logic.append( LogicItem(ac4.id, LogicItem.END, ProjectLogicIf(f"{ac4.id}/default/0", json.dumps(False)))) src = program_src({Test.__name__: Test}, CachedProject(project), CachedScene(scene)) parse(src) """ bool_res = test_name.test(res.ac1) if (bool_res == False): bool2_res = test_name.test(res.ac4) if (bool2_res == False): continue elif (bool2_res == True): test_name.test(res.ac5) elif (bool_res == True): test_name.test(res.ac2) test_name.test(res.ac3) test_name.test(res.ac6) """ spl = src.splitlines() # it has to be robust against changed order of blocks ac1_idx = subs_index(spl, "bool_res = test_name.test(an='ac1')") if_bool_res_false_idx = subs_index(spl, "if (bool_res == False):") assert if_bool_res_false_idx > ac1_idx assert cntsp(spl[ac1_idx]) == cntsp(spl[if_bool_res_false_idx]) bool2_res_idx = subs_index(spl, "bool2_res = test_name.test(an='ac4')") assert bool2_res_idx > if_bool_res_false_idx assert cntsp(spl[if_bool_res_false_idx]) == cntsp(spl[bool2_res_idx]) - TAB if_bool_2_res_false_idx = subs_index(spl, "if (bool2_res == False):") assert cntsp(spl[if_bool_2_res_false_idx]) == cntsp(spl[bool2_res_idx]) assert if_bool_2_res_false_idx > bool2_res_idx assert "continue" in spl[if_bool_2_res_false_idx + 1] assert cntsp( spl[bool2_res_idx]) == cntsp(spl[if_bool_2_res_false_idx + 1]) - TAB if_bool_2_res_true_idx = subs_index(spl, "if (bool2_res == True):") assert if_bool_2_res_true_idx > bool2_res_idx assert "test_name.test(an='ac5')" in spl[if_bool_2_res_true_idx + 1] assert cntsp(spl[if_bool_2_res_true_idx]) == cntsp( spl[if_bool_2_res_true_idx + 1]) - TAB if_bool_res_true_idx = subs_index(spl, "if (bool_res == True):") assert if_bool_res_true_idx > ac1_idx assert cntsp(spl[ac1_idx]) == cntsp(spl[if_bool_res_true_idx]) assert "test_name.test(an='ac2')" in spl[if_bool_res_true_idx + 1] assert cntsp(spl[if_bool_res_true_idx]) == cntsp( spl[if_bool_res_true_idx + 1]) - TAB assert "test_name.test(an='ac3')" in spl[if_bool_res_true_idx + 2] assert cntsp(spl[if_bool_res_true_idx]) == cntsp( spl[if_bool_res_true_idx + 2]) - TAB ac6_idx = subs_index(spl, "test_name.test(an='ac6')") assert cntsp(spl[ac1_idx]) == cntsp(spl[ac6_idx]) assert ac6_idx > if_bool_2_res_false_idx assert ac6_idx > if_bool_2_res_true_idx
def get_calibration() -> RespT: """Get calibration (camera pose wrt. marker) --- put: description: Returns camera pose with respect to the origin. tags: - Camera parameters: - in: query name: inverse schema: type: boolean description: When set, the method returns pose of the origin wrt. the camera. - in: query name: fx schema: type: number format: float required: true description: unique ID - in: query name: fy schema: type: number format: float required: true description: unique ID - in: query name: cx schema: type: number format: float required: true description: unique ID - in: query name: cy schema: type: number format: float required: true description: unique ID - in: query name: distCoefs schema: type: array items: type: number format: float required: true description: unique ID requestBody: content: multipart/form-data: schema: type: object required: - image properties: # 'image' will be the field name in this multipart request image: type: string format: binary responses: 200: description: Ok content: application/json: schema: $ref: EstimatedPose 404: description: No marker found. content: application/json: schema: type: string """ file = request.files["image"] camera_matrix = camera_matrix_from_request() image = Image.open(file.stream) dist_matrix = dist_matrix_from_request() if _mock: time.sleep(0.5) quality = random.uniform(0, 1) pose = Pose( Position(random.uniform(-0.5, 0.5), random.uniform(-0.5, 0.5), random.uniform(0.2, 1))) else: poses = estimate_camera_pose(camera_matrix, dist_matrix, image, MARKER_SIZE) if not poses: return jsonify("No marker detected."), 404 quality_dict: dict[int, float] = {k: 0.0 for k, v in MARKERS.items()} known_markers: list[tuple[Pose, float]] = [] # apply configured marker offset from origin to the detected poses for marker_id in poses.keys(): try: cpose = MARKERS[marker_id] except KeyError: logger.debug(f"Detected un-configured marker id {marker_id}.") continue mpose = poses[marker_id] dist = math.sqrt(mpose.position.x**2 + mpose.position.y**2 + mpose.position.z**2) # the closer the theta is to pi, the higher quality we get theta = quaternion.as_spherical_coords( mpose.orientation.as_quaternion())[0] ori_marker_quality = normalize(abs(theta), MIN_THETA, MAX_THETA) # the closer the marker is, the higher quality we get dist_marker_quality = 1.0 - normalize(dist, MIN_DIST, MAX_DIST) marker_quality = (ori_marker_quality + dist_marker_quality) / 2 quality_dict[marker_id] = marker_quality known_markers.append((tr.make_pose_abs(cpose, mpose), marker_quality)) logger.debug(f"Known marker : {marker_id}") logger.debug(f"...original pose : {poses[marker_id]}") logger.debug(f"...transformed pose: {poses[marker_id]}") logger.debug(f"...dist quality : {dist_marker_quality:.3f}") logger.debug(f"...ori quality : {ori_marker_quality:.3f}") logger.debug(f"...overall quality : {marker_quality:.3f}") if not known_markers: return jsonify("No known marker detected."), 404 weights = [marker[1] for marker in known_markers] wsum = sum(weights) if wsum <= 0: logger.warning(f"Got invalid weights, probably bad input data.\n" f"Camera matrix: {camera_matrix}\n" f"Dist matrix: {dist_matrix}") return jsonify("Invalid input data."), 500 # combine all detections pose = Pose() for mpose, weight in known_markers: pose.position += mpose.position * weight pose.position *= 1.0 / wsum quaternions = np.array([ quaternion.as_float_array(km[0].orientation.as_quaternion()) for km in known_markers ]) pose.orientation.set_from_quaternion( quaternion.from_float_array( weighted_average_quaternions(quaternions, np.array(weights)))) quality = np.mean(list(quality_dict.values())) inverse = request.args.get("inverse", default="false") == "true" if inverse: logger.debug("Inverting the output pose.") pose = pose.inversed() return jsonify(EstimatedPose(pose, quality)), 200
def test_object_parameters(start_processes: None, ars: ARServer, scene: Scene) -> None: assert ars.call_rpc(rpc.s.OpenScene.Request(uid(), IdArgs(scene.id)), rpc.s.OpenScene.Response).result event(ars, events.s.OpenScene) assert ars.call_rpc( rpc.s.AddObjectToScene.Request( uid(), rpc.s.AddObjectToScene.Request.Args("ows", ObjectWithActions.__name__)), rpc.s.AddObjectToScene.Response, ).result obj = event(ars, events.s.SceneObjectChanged).data assert obj is not None assert ars.call_rpc( rpc.p.NewProject.Request( uid(), rpc.p.NewProject.Request.Args(scene.id, "Project name")), rpc.p.NewProject.Response, ).result event(ars, events.p.OpenProject) assert ars.call_rpc( rpc.p.AddActionPoint.Request( uid(), rpc.p.AddActionPoint.Request.Args("ap", Position())), rpc.p.AddActionPoint.Response, ).result ap = event(ars, events.p.ActionPointChanged).data assert ap is not None assert ars.call_rpc( rpc.p.AddAction.Request( uid(), rpc.p.AddAction.Request.Args( ap.id, "a1", f"{obj.id}/{ObjectWithActions.bool_action.__name__}", [], [Flow(outputs=["bool_result"])], ), ), rpc.p.AddAction.Response, ).result a1 = event(ars, events.p.ActionChanged).data assert a1 is not None assert ars.call_rpc( rpc.p.AddAction.Request( uid(), rpc.p.AddAction.Request.Args( ap.id, "a2", f"{obj.id}/{ObjectWithActions.str_action.__name__}", [], [Flow()], ), ), rpc.p.AddAction.Response, ).result a2 = event(ars, events.p.ActionChanged).data assert a2 is not None add_logic_item(ars, LogicItem.START, a1.id) add_logic_item( ars, a1.id, a2.id, ProjectLogicIf(f"{a1.id}/{FlowTypes.DEFAULT}/{0}", json.dumps(True))) add_logic_item(ars, a2.id, LogicItem.END) add_logic_item( ars, a1.id, LogicItem.END, ProjectLogicIf(f"{a1.id}/{FlowTypes.DEFAULT}/{0}", json.dumps(False))) # TODO try to add some invalid connections here? save_project(ars)