def make_global_ap_relative(scene: Scene, project: Project, ap: ProjectActionPoint, parent_id: str) -> None: """ Transforms (in place) global AP into a relative one with given parent (can be object or another AP). :param scene: :param project: :param ap: :param parent_id: :return: """ assert project.scene_id == scene.id if parent_id in scene.object_ids: new_parent_pose = scene.object(parent_id).pose elif parent_id in project.action_points_ids: parent_ap = project.action_point(parent_id) if parent_ap.parent: make_global_ap_relative(scene, project, ap, parent_ap.parent) new_parent_pose = Pose(parent_ap.position, Orientation()) else: raise Arcor2Exception("Unknown parent_id.") ap.position = make_pose_rel(new_parent_pose, Pose(ap.position, Orientation())).position for ori in ap.orientations: ori.orientation = make_orientation_rel(new_parent_pose.orientation, ori.orientation) ap.parent = parent_id
def goto_pose(self, pose: Pose, linear: bool = True, relative: bool = False) -> None: """Commands the YuMi to goto the given pose. Parameters ---------- pose : RigidTransform linear : bool, optional If True, will use MoveL in RAPID to ensure linear path. Otherwise use MoveJ in RAPID, which does not ensure linear path. Defaults to True relative : bool, optional If True, will use goto_pose_relative by computing the delta pose from current pose to target pose. Defaults to False """ if relative: cur_pose = self.get_pose() delta_pose = Pose.from_tr_matrix(cur_pose.inversed().as_tr_matrix() * pose.as_tr_matrix()) rot = np.rad2deg(quaternion.as_euler_angles(delta_pose.orientation.as_quaternion())) self.goto_pose_delta(delta_pose.position, rot) else: body = self._get_pose_body(pose) if linear: cmd = CmdCodes.goto_pose_linear else: cmd = CmdCodes.goto_pose req = self._construct_req(cmd, body) self._request(req, timeout=self._motion_timeout)
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_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 make_relative_ap_global(scene: Scene, project: Project, ap: ProjectActionPoint) -> None: """ Transforms (in place) relative AP into a global one. :param scene: :param project: :param ap: :return: """ if not ap.parent: return if ap.parent in scene.object_ids: old_parent_pose = scene.object(ap.parent).pose elif ap.parent in project.action_points_ids: old_parent_pose = Pose(project.action_point(ap.parent).position, Orientation()) else: raise Arcor2Exception("AP has unknown parent_id.") ap.position = make_pose_abs(old_parent_pose, Pose(ap.position, Orientation())).position for ori in ap.orientations: ori.orientation = make_orientation_abs(old_parent_pose.orientation, ori.orientation) if ap.parent in project.action_points_ids: parent_ap = project.action_point(ap.parent) if parent_ap.parent: ap.parent = parent_ap.parent make_relative_ap_global(scene, project, ap) ap.parent = None
def make_pose_rel(parent: Pose, child: Pose) -> Pose: """ :param parent: e.g. scene object :param child: e.g. action point :return: relative pose """ p = Pose() p.position = make_position_rel(parent.position, child.position).rotated(parent.orientation, True) p.orientation = make_orientation_rel(parent.orientation, child.orientation) return p
def make_pose_abs(parent: Pose, child: Pose) -> Pose: """ :param parent: e.g. scene object :param child: e.g. action point :return: absolute pose """ p = Pose() p.position = child.position.rotated(parent.orientation) p.position = make_position_abs(parent.position, p.position) p.orientation = make_orientation_abs(parent.orientation, child.orientation) return p
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_generic_with_pose(start_processes: None) -> None: obj = GenericWithPose("id", "name", Pose(), Box("boxId", 0.1, 0.1, 0.1)) assert obj.id in scene_service.collision_ids() obj.pose = Pose() obj.enabled = False assert obj.id not in scene_service.collision_ids() obj.enabled = True assert obj.id in scene_service.collision_ids() obj.cleanup() assert not scene_service.collision_ids()
def test_generic_with_pose(start_processes: None) -> None: obj = CollisionObject("id", "name", Pose(), Box("boxId", 0.1, 0.1, 0.1)) assert obj.id in scene_service.collision_ids() obj.pose = Pose() obj.enabled = False assert obj.id not in scene_service.collision_ids() obj.enabled = True assert obj.id in scene_service.collision_ids() scene_service.start() assert obj.id in scene_service.collision_ids() scene_service.stop() # after stop, all collisions are removed by the Scene service assert not scene_service.collision_ids()
def put_ik() -> RespT: """Get the current state. --- put: description: Get the current state. tags: - Robot requestBody: content: application/json: schema: $ref: Pose responses: 200: description: Ok content: application/json: schema: type: array items: $ref: Joint 403: description: Not started """ assert _dobot is not None if not isinstance(request.json, dict): raise FlaskException("Body should be a JSON dict containing Pose.", error_code=400) pose = Pose.from_dict(request.json) return jsonify(_dobot.inverse_kinematics(pose))
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 main() -> None: dm = DobotMagician("id", "name", Pose(), DobotSettings("/dev/ttyUSB0")) joints = dm.robot_joints() pose = dm.get_end_effector_pose("default") print(pose) print(joints) print(quaternion.as_euler_angles(pose.orientation.as_quaternion())) print("--- Forward Kinematics -----------------------------") fk_pose = dm.forward_kinematics("", joints) dx = pose.position.x - fk_pose.position.x dy = pose.position.y - fk_pose.position.y dz = pose.position.z - fk_pose.position.z print("Position error: {:+.09f}".format(math.sqrt(dx**2 + dy**2 + dz**2))) print("dx: {:+.06f}".format(dx)) print("dy: {:+.06f}".format(dy)) print("dz: {:+.06f}".format(dz)) print(fk_pose.orientation) print("--- Inverse Kinematics -----------------------------") ik_joints = dm.inverse_kinematics("", pose) assert len(ik_joints) == len(joints) for idx, (joint, ik_joint) in enumerate(zip(joints, ik_joints)): print("j{}: {:+.06f}".format(idx + 1, joint.value - ik_joint.value))
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 put_eef_pose() -> RespT: """Set the EEF pose. --- put: description: Set the EEF pose. tags: - Robot parameters: - in: query name: moveType schema: type: string enum: - JUMP - LINEAR - JOINTS required: true description: Move type - name: velocity in: query schema: type: number format: float minimum: 0 maximum: 100 - name: acceleration in: query schema: type: number format: float minimum: 0 maximum: 100 requestBody: content: application/json: schema: $ref: Pose responses: 200: description: Ok content: application/json: schema: $ref: Pose 403: description: Not started """ assert _dobot is not None if not isinstance(request.json, dict): raise FlaskException("Body should be a JSON dict containing Pose.", error_code=400) pose = Pose.from_dict(request.json) move_type: str = request.args.get("moveType", "jump") velocity = float(request.args.get("velocity", default=50.0)) acceleration = float(request.args.get("acceleration", default=50.0)) _dobot.move(pose, MoveType(move_type), velocity, acceleration) return jsonify("ok")
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 put_ik() -> RespT: """Get the current state. --- put: description: Get the current state. tags: - Robot requestBody: content: application/json: schema: $ref: Pose responses: 200: description: Ok content: application/json: schema: type: array items: $ref: Joint 403: description: Not started """ assert _dobot is not None pose = Pose.from_dict(request.json) return jsonify(_dobot.inverse_kinematics(pose))
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 main() -> None: yumi = YuMi("", "", Pose(), YumiSettings("192.168.104.107")) left_joints = yumi.robot_joints(arm_id="left") right_joints = yumi.robot_joints(arm_id="right") print(yumi.forward_kinematics("", left_joints, "left")) print(yumi.forward_kinematics("", right_joints, "right")) pose_l = yumi.get_end_effector_pose("", "left") pose_r = yumi.get_end_effector_pose("", "right") print(yumi.inverse_kinematics("", pose_l, None, False, arm_id="left")) print(yumi.inverse_kinematics("", pose_r, None, False, arm_id="right")) pose_l_2 = copy.deepcopy(pose_l) pose_r_2 = copy.deepcopy(pose_r) pose_l_2.position.z += 0.01 pose_r_2.position.z += 0.01 while True: yumi.move_to_pose("", pose_l, 0.5, arm_id="left") yumi.move_to_pose("", pose_l_2, 0.5, arm_id="left") yumi.move_to_pose("", pose_r, 0.5, arm_id="right") yumi.move_to_pose("", pose_r_2, 0.5, arm_id="right")
def make_pose_rel_to_parent(scene: Scene, project: Project, pose: Pose, parent_id: str) -> Pose: """ Transforms global Pose into Pose that is relative to a given parent (can be object or AP). :param scene: :param project: :param pose: :param parent_id: :return: """ if parent_id in scene.object_ids: parent_pose = scene.object(parent_id).pose elif parent_id in project.action_points_ids: parent_ap = project.action_point(parent_id) if parent_ap.parent: pose = make_pose_rel_to_parent(scene, project, pose, parent_ap.parent) parent_pose = Pose(parent_ap.position, Orientation()) else: raise Arcor2Exception("Unknown parent_id.") return make_pose_rel(parent_pose, pose)
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 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 put_start() -> RespT: """Start the robot. --- put: description: Start the robot. tags: - State parameters: - in: query name: port schema: type: string default: /dev/dobot description: Dobot port - in: query name: model schema: type: string enum: - magician - m1 required: true description: Dobot model requestBody: content: application/json: schema: $ref: Pose responses: 204: description: Ok 403: description: Already started """ if started(): return "Already started.", 403 model: str = request.args.get("model", default="magician") port: str = request.args.get("port", default="/dev/dobot") if not isinstance(request.json, dict): raise FlaskException("Body should be a JSON dict containing Pose.", error_code=400) pose = Pose.from_dict(request.json) mapping: dict[str, type[Dobot]] = { "magician": DobotMagician, "m1": DobotM1 } global _dobot _dobot = mapping[model](pose, port, _mock) return Response(status=204)
def value(cls, type_defs: TypesDict, scene: Scene, project: Project, action_id: str, parameter_id: str) -> Pose: action = project.action(action_id) param = action.parameter(parameter_id) ori_id: str = cls.param_value(param) ap, ori = project.ap_and_orientation(ori_id) return Pose(ap.position, ori.orientation)
def parameter_value( cls, type_defs: TypesDict, scene: CScene, project: CProject, action_id: str, parameter_id: str ) -> Pose: try: ap, ori = project.bare_ap_and_orientation(cls.orientation_id(project, action_id, parameter_id)) except Arcor2Exception as e: raise ParameterPluginException("Failed to get scene/project data.") from e return Pose(ap.position, ori.orientation)
def __init__(self, obj_id: str, name: str, pose: Pose, settings: Optional[Settings] = None) -> None: super().__init__(obj_id, name, pose, settings) self._hand_teaching: dict[str, bool] = {self.Arms.left: False, self.Arms.right: False} self._poses: dict[str, dict[str, Pose]] = {} for arm, eefs in self.EEF.items(): self._poses[arm] = {} for eef in eefs: self._poses[arm][eef] = Pose()
def get_end_effector_pose(self) -> Pose: if self.simulator: return self.forward_kinematics(self.robot_joints()) try: pos = self._dobot.get_pose() # in mm except DobotApiException as e: raise DobotException("Failed to get pose.") from e p = Pose() p.position.x = pos.position.x / 1000.0 p.position.y = pos.position.y / 1000.0 p.position.z = pos.position.z / 1000.0 p.orientation = self.ROTATE_EEF * Orientation.from_rotation_vector( z=math.radians(pos.position.r)) self._handle_pose_out(p) return tr.make_pose_abs(self.pose, p)
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_pose_rel(parent: Pose, child: Pose) -> Pose: """ :param parent: e.g. scene object :param child: e.g. action point :return: relative pose """ return Pose( (child.position - parent.position).rotated(parent.orientation, inverse=True), Orientation.from_quaternion(parent.orientation.as_quaternion().inverse() * child.orientation.as_quaternion()), )
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