diff --git a/src/iiwa_config/config/api_endpoints.yaml b/src/iiwa_config/config/api_endpoints.yaml index cfa4948..71245ca 100644 --- a/src/iiwa_config/config/api_endpoints.yaml +++ b/src/iiwa_config/config/api_endpoints.yaml @@ -14,13 +14,14 @@ endpoints: - path: /robot/pose method: GET - type: tf + type: fk + ros_name: /compute_fk parent_frame: base_link child_frame: tcp summary: "Текущая декартова поза TCP" - description: "Позиция (м) и ориентация TCP относительно base_link через TF2. Углы Эйлера в конвенции KUKA ABC (ZYX): A=рыскание, B=тангаж, C=крен." + description: "Позиция (м) и ориентация TCP относительно base_link через /compute_fk. Углы Эйлера в конвенции KUKA ABC (ZYX): A=рыскание, B=тангаж, C=крен." tags: [robot] - timeout: 1.0 + timeout: 5.0 enabled: true - path: /robot/stop diff --git a/src/iiwa_config/config/moveit/joint_limits.yaml b/src/iiwa_config/config/moveit/joint_limits.yaml index f90f72c..15970a8 100644 --- a/src/iiwa_config/config/moveit/joint_limits.yaml +++ b/src/iiwa_config/config/moveit/joint_limits.yaml @@ -20,8 +20,8 @@ joint_limits: max_jerk: 85.0 joint2: has_position_limits: true - min_position: -2.09 - max_position: 2.09 + min_position: -2.10 + max_position: 2.10 has_velocity_limits: true max_velocity: 1.71 has_acceleration_limits: true @@ -40,8 +40,8 @@ joint_limits: max_jerk: 87.0 joint4: has_position_limits: true - min_position: -2.09 - max_position: 2.09 + min_position: -2.10 + max_position: 2.10 has_velocity_limits: true max_velocity: 2.27 has_acceleration_limits: true @@ -60,8 +60,8 @@ joint_limits: max_jerk: 122.0 joint6: has_position_limits: true - min_position: -2.09 - max_position: 2.09 + min_position: -2.10 + max_position: 2.10 has_velocity_limits: true max_velocity: 3.14 has_acceleration_limits: true diff --git a/src/iiwa_web/iiwa_web/dynamic_router.py b/src/iiwa_web/iiwa_web/dynamic_router.py index cb15547..d0596c6 100644 --- a/src/iiwa_web/iiwa_web/dynamic_router.py +++ b/src/iiwa_web/iiwa_web/dynamic_router.py @@ -176,6 +176,61 @@ def _quat_to_euler_zyx(x: float, y: float, z: float, w: float) -> tuple[float, f return roll, pitch, yaw # C, B, A +def _make_fk_handler(ep: EndpointDef): + ros_name = ep.ros_name + fk_link = ep.child_frame + base_frame = ep.parent_frame + timeout = ep.timeout + + def handler(): + from moveit_msgs.srv import GetPositionFK + + bridge = get_bridge() + + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + js = bridge.get_latest("/joint_states") + if js is not None: + break + time.sleep(_POLL_INTERVAL) + else: + raise HTTPException(503, "Timeout waiting for /joint_states") + + req = GetPositionFK.Request() + req.header.frame_id = base_frame + req.fk_link_names = [fk_link] + req.robot_state.joint_state = js + + try: + resp = bridge.call_service(GetPositionFK, ros_name, req, timeout) + except (RuntimeError, TimeoutError) as e: + raise HTTPException(503, str(e)) + + if not resp.pose_stamped: + raise HTTPException(503, f"FK вернул пустой результат для '{fk_link}'") + + pose = resp.pose_stamped[0].pose + t = pose.position + r = pose.orientation + roll, pitch, yaw = _quat_to_euler_zyx(r.x, r.y, r.z, r.w) + + return { + "position": {"x": t.x, "y": t.y, "z": t.z}, + "orientation": { + "quaternion": {"x": r.x, "y": r.y, "z": r.z, "w": r.w}, + "euler_rad": {"a": yaw, "b": pitch, "c": roll}, + "euler_deg": { + "a": math.degrees(yaw), + "b": math.degrees(pitch), + "c": math.degrees(roll), + }, + }, + "frame": {"parent": base_frame, "child": fk_link}, + } + + return handler + + def _make_tf_handler(ep: EndpointDef): parent = ep.parent_frame child = ep.child_frame @@ -238,13 +293,20 @@ def build_dynamic_router( if not ep.parent_frame or not ep.child_frame: raise ValueError(f"TF-эндпоинт '{ep.path}' требует parent_frame и child_frame") handler = _make_tf_handler(ep) + elif ep.type == "fk": + if not ep.ros_name or not ep.parent_frame or not ep.child_frame: + raise ValueError(f"FK-эндпоинт '{ep.path}' требует ros_name, parent_frame и child_frame") + handler = _make_fk_handler(ep) else: raise ValueError(f"Неизвестный тип эндпоинта: '{ep.type}'") + operation_id = ep.path.strip("/").replace("/", "_") + "_" + ep.method.lower() + router.add_api_route( ep.path, handler, methods=[ep.method], + operation_id=operation_id, summary=ep.summary or ep.description, description=ep.description, tags=ep.tags, diff --git a/src/iiwa_web/package.xml b/src/iiwa_web/package.xml index 80df4bf..45dab2e 100644 --- a/src/iiwa_web/package.xml +++ b/src/iiwa_web/package.xml @@ -10,8 +10,10 @@ python3-fastapi python3-uvicorn python3-multipart + python3-fastmcp tf2_ros tf2_py + moveit_msgs ament_copyright ament_flake8