def _inverse_kinematics(self, pose: Pose) -> list[Joint]: """Computes inverse kinematics. Works with robot-relative pose. Inspired by DobotKinematics.py from open-dobot project and DobotInverseKinematics.py from BenchBot. :param pose: IK target pose (relative to robot) :return: Inverse kinematics """ self._check_orientation(pose) # TODO this is probably not working properly (use similar solution as in _check_orientation?) _, _, yaw = quaternion.as_euler_angles( pose.orientation.as_quaternion()) x = pose.position.x y = pose.position.y z = pose.position.z + self.end_effector_length # pre-compute distances # radial position of end effector in the x-y plane r = math.sqrt(math.pow(x, 2) + math.pow(y, 2)) rho_sq = pow(r - self.link_4_length, 2) + pow(z, 2) rho = math.sqrt( rho_sq) # distance b/w the ends of the links joined at the elbow l2_sq = self.link_2_length**2 l3_sq = self.link_3_length**2 # law of cosines try: alpha = math.acos( (l2_sq + rho_sq - l3_sq) / (2.0 * self.link_2_length * rho)) gamma = math.acos((l2_sq + l3_sq - rho_sq) / (2.0 * self.link_2_length * self.link_3_length)) except ValueError: raise DobotException("Failed to compute IK.") beta = math.atan2(z, r - self.link_4_length) # joint angles baseAngle = math.atan2(y, x) rearAngle = math.pi / 2 - beta - alpha frontAngle = math.pi / 2 - gamma joints = [ Joint(Joints.J1, baseAngle), Joint(Joints.J2, rearAngle), Joint(Joints.J3, frontAngle), Joint(Joints.J4, -rearAngle - frontAngle), Joint(Joints.J5, yaw - baseAngle), ] self.validate_joints(joints) return joints
def __init__(self, pose: Pose, port: str = "/dev/dobot", simulator: bool = False) -> None: super(DobotMagician, self).__init__(pose, port, simulator) if self.simulator: self._joint_values = [ Joint(Joints.J1, -0.038), Joint(Joints.J2, 0.341), Joint(Joints.J3, 0.3632), Joint(Joints.J4, -0.704), Joint(Joints.J5, -0.568), ]
def move_to_calibration_pose(self) -> None: joint_values = [ # TODO define as pose Joint(Joints.J1, -0.0115), Joint(Joints.J2, 0.638), Joint(Joints.J3, -0.5486), Joint(Joints.J4, -0.0898), Joint(Joints.J5, 1.41726), ] self.move(self.forward_kinematics("", joint_values), MoveType.LINEAR, 50)
def main() -> None: # robots aubo = Aubo(uid("rbt"), "Whatever", Pose(), RobotSettings("http://127.0.0.1:13000", "aubo")) simatic = Aubo(uid("rbt"), "Whatever", Pose(), RobotSettings("http://127.0.0.1:13001", "simatic")) # objects with pose, with 'System' and 'Configurations' controllers barcode = Barcode(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:14000", "simulator")) search = Search(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:12000", "simulator")) ict = Ict(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:19000", "simulator")) # objects without pose, without 'System' and 'Configurations' controllers interaction = Interaction(uid("srv"), "Whatever", SimpleSettings("http://127.0.0.1:17000")) statistic = Statistic(uid("srv"), "Whatever", SimpleSettings("http://127.0.0.1:16000")) # Add @action decorator to all actions of each object using patch_object_actions. # It has to be called exactly once for each type! # Certain functions of Execution unit (as e.g. pausing script execution) would not work without this step. # Note: in a generated script, this is done within the Resources context manager. # Side effect: a lot of JSON data will be printed out to the console when running the script manually. patch_object_actions(Aubo) patch_object_actions(Barcode) patch_object_actions(Search) patch_object_actions(Ict) patch_object_actions(Interaction) patch_object_actions(Statistic) scene_service.delete_all_collisions() scene_service.upsert_collision(Box("box_id", 0.1, 0.1, 0.1), Pose()) scene_service.start() # this is normally done by auto-generated Resources class aubo.move("suction", Pose(), MoveTypeEnum.SIMPLE, safe=False) simatic.set_joints(ProjectRobotJoints("", "", [Joint("x", 0), Joint("y", 0)]), MoveTypeEnum.SIMPLE) barcode.scan() search.set_search_parameters(SearchEngineParameters(search_log_level=SearchLogLevel(LogLevel.DEBUG))) search.grab_image() interaction.add_notification("Test", NotificationLevelEnum.INFO) try: statistic.get_groups() except rest.RestHttpException as e: # service returned error code print(f"The error code is {e.error_code}.") except rest.RestException as e: # request totally failed (timeout, connection error, etc) print(str(e)) if ict.ready(): test = ict.test("OK") print(test) scene_service.stop() # this is normally done by auto-generated Resources class
def robot_joints(self, include_gripper: bool = False) -> list[Joint]: if self.simulator: return self._joint_values joints = self._dobot.get_pose().joints.in_radians() j3 = self._dobot.get_pose().joints.j3 / 1000 return [ Joint(Joints.J1, joints.j1), Joint(Joints.J2, joints.j2), Joint(Joints.J3, j3), Joint(Joints.J4, joints.j4), ]
def __init__(self, pose: Pose, port: str = "/dev/dobot", simulator: bool = False) -> None: super(DobotM1, self).__init__(pose, port, simulator) if self.simulator: self._joint_values = [ Joint(Joints.J1, 0), Joint(Joints.J2, 0), Joint(Joints.J3, 0), Joint(Joints.J4, 0) ]
def robot_joints(self) -> List[Joint]: if self.simulator: return self._joint_values joints = self._dobot.get_pose().joints.in_radians() return [ Joint(Joints.J1, joints.j1), Joint(Joints.J2, joints.j2), Joint(Joints.J3, joints.j3 - joints.j2), Joint(Joints.J4, -joints.j3), Joint(Joints.J5, joints.j4), ]
def put_fk() -> RespT: """Get the current state. --- put: description: Get the current state. tags: - Robot requestBody: content: application/json: schema: type: array items: $ref: Joint responses: 200: description: Ok content: application/json: schema: $ref: Pose 403: description: Not started """ assert _dobot is not None joints = [Joint.from_dict(j) for j in request.json] return jsonify(_dobot.forward_kinematics(joints))
def put_fk() -> RespT: """Get the current state. --- put: description: Get the current state. tags: - Robot requestBody: content: application/json: schema: type: array items: $ref: Joint responses: 200: description: Ok content: application/json: schema: $ref: Pose 403: description: Not started """ assert _dobot is not None if not isinstance(request.json, list): raise FlaskException("Body should be a JSON array containing joints.", error_code=400) joints = [Joint.from_dict(j) for j in request.json] return jsonify(_dobot.forward_kinematics(joints))
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 _response_to_joints(self, res: RawResponse) -> List[Joint]: tokens = res.message.split() if len(tokens) != self.JOINTS: raise YumiException("Invalid format for states! Got: \n{0}".format(res.message)) values = [math.radians(float(token)) for token in tokens] assert self.name return [Joint(f"yumi_joint_{i + 1}_{self.name[0]}", j) for i, j in enumerate(values)]
def robot_joints(self) -> List[Joint]: if self.settings.simulator: return [ Joint(Joints.J1, 0), Joint(Joints.J2, 0), Joint(Joints.J3, 0), Joint(Joints.J4, 0), Joint(Joints.J5, 0) ] joints = self._dobot.get_pose().joints.in_radians() return [ Joint(Joints.J1, joints.j1), Joint(Joints.J2, joints.j2), Joint(Joints.J3, joints.j3 - joints.j2), Joint(Joints.J4, -joints.j3), Joint(Joints.J5, joints.j4) ]
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
class DummyMultiArmRobot(MultiArmRobot): """Example class representing multiarm robot.""" _ABSTRACT = False class Arms(StrEnum): left: str = "left" right: str = "right" GRIPPERS: dict[str, set[str]] = { Arms.left: {"l_gripper_1", "l_gripper_2"}, Arms.right: {"r_gripper_1", "r_gripper_2"}, } SUCTIONS: dict[str, set[str]] = { Arms.left: {"l_suction_1", "l_suction_2"}, Arms.right: {"r_suction_1", "r_suction_2"}, } EEF: dict[str, set[str]] = {Arms.left: {"l_eef_1", "l_eef_2"}, Arms.right: {"r_eef_1", "r_eef_2"}} JOINTS: dict[str, list[Joint]] = {Arms.left: [Joint("l_joint_1", 1.0)], Arms.right: [Joint("r_joint_1", -1.0)]} 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_arm_ids(self) -> set[str]: return self.Arms.set() @staticmethod def _get_from_dict(d: dict[str, set[str]], arm_id: Optional[str] = None) -> set[str]: if arm_id is None: raise Arcor2Exception("Arm has to be specified.") try: return d[arm_id] except KeyError: raise Arcor2Exception("Unknown arm.") def get_end_effectors_ids(self, arm_id: Optional[str] = None) -> set[str]: return self._get_from_dict(self.EEF, arm_id) def get_end_effector_pose(self, end_effector: str, arm_id: Optional[str] = None) -> Pose: if end_effector not in self.get_end_effectors_ids(arm_id): raise Arcor2Exception("Unknown end effector.") assert arm_id return tr.make_pose_abs(self.pose, self._poses[arm_id][end_effector]) def robot_joints(self, include_gripper: bool = False, arm_id: Optional[str] = None) -> list[Joint]: """With no arm specified, returns all robot joints. Otherwise, returns joints for the given arm. :param arm_id: :return: """ if arm_id is None: return [item for sublist in self.JOINTS.values() for item in sublist] try: return self.JOINTS[arm_id] except KeyError: raise Arcor2Exception("Unknown arm.") def grippers(self, arm_id: Optional[str] = None) -> set[str]: return self._get_from_dict(self.GRIPPERS, arm_id) def suctions(self, arm_id: Optional[str] = None) -> set[str]: return self._get_from_dict(self.SUCTIONS, arm_id) def move_to_pose( self, end_effector_id: str, target_pose: Pose, speed: float, safe: bool = True, linear: bool = True, arm_id: Optional[str] = None, ) -> None: """Move given robot's end effector to the selected pose. :param end_effector_id: :param target_pose: :param speed: :param safe: :return: """ if end_effector_id not in self.get_end_effectors_ids(arm_id): raise Arcor2Exception("Unknown end effector.") assert arm_id speed = min(max(0.0, speed), 1.0) with self._move_lock: time.sleep(1.0 - speed) self._poses[arm_id][end_effector_id] = tr.make_pose_rel(self.pose, target_pose) def move_to_joints( self, target_joints: list[Joint], speed: float, safe: bool = True, arm_id: Optional[str] = None ) -> None: """Sets target joint values. :param target_joints: :param speed: :param safe: :return: """ if arm_id is None: if len(target_joints) != len(self.robot_joints()): raise Arcor2Exception("Joints for both arms have to be specified.") elif arm_id not in self.get_arm_ids(): raise Arcor2Exception("Unknown arm.") speed = min(max(0.0, speed), 1.0) with self._move_lock: time.sleep(1.0 - speed) def inverse_kinematics( self, end_effector_id: str, pose: Pose, start_joints: Optional[list[Joint]] = None, avoid_collisions: bool = True, arm_id: Optional[str] = None, ) -> list[Joint]: """Computes inverse kinematics. :param end_effector_id: IK target pose end-effector :param pose: IK target pose :param start_joints: IK start joints :param avoid_collisions: Return non-collision IK result if true :return: Inverse kinematics """ if end_effector_id not in self.get_end_effectors_ids(arm_id): raise Arcor2Exception("Unknown end effector.") assert arm_id is not None try: return self.JOINTS[arm_id] except KeyError: raise Arcor2Exception("Unknown arm.") def forward_kinematics(self, end_effector_id: str, joints: list[Joint], arm_id: Optional[str] = None) -> Pose: """Computes forward kinematics. :param end_effector_id: Target end effector name :param joints: Input joint values :return: Pose of the given end effector """ if end_effector_id not in self.get_end_effectors_ids(arm_id): raise Arcor2Exception("Unknown end effector.") return Pose() def get_hand_teaching_mode(self, arm_id: Optional[str] = None) -> bool: """ This is expected to be implemented if the robot supports set_hand_teaching_mode :return: """ if arm_id is None: raise Arcor2Exception("Arm has to be specified.") try: return self._hand_teaching[arm_id] except KeyError: raise Arcor2Exception("Unknown arm.") def set_hand_teaching_mode(self, enabled: bool, arm_id: Optional[str] = None) -> None: if arm_id is None: raise Arcor2Exception("Arm has to be specified.") try: state = self._hand_teaching[arm_id] except KeyError: raise Arcor2Exception("Unknown arm.") if state == enabled: raise Arcor2Exception("That's the current state.") self._hand_teaching[arm_id] = enabled
def test_value_to_json() -> None: prj = ProjectRobotJoints("name", "robot_id", [Joint("name", 0.333)]) assert JointsPlugin.value_to_json(prj) == prj.to_json()
def test_plugin_from_instance() -> None: assert plugin_from_instance(ProjectRobotJoints("name", "robot_id", [Joint("name", 0.333)])) is JointsPlugin