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