From ea5cabe6ff9e9c1b828f591136e71687d8957356 Mon Sep 17 00:00:00 2001
From: Martino Russi <nopyeps@gmail.com>
Date: Fri, 31 Jul 2026 15:24:25 +0200
Subject: [PATCH] feat(openarm): add end-effector kinematics support

Expose optional URDF-based forward/inverse kinematics for the OpenArm
follower so it can be recorded/commanded in end-effector (Cartesian)
space, mirroring the SO-100 kinematics pattern.

- config: add optional urdf_path / target_frame_name fields
- robot: add arm_motor_names property and make_kinematics() helper that
  builds a RobotKinematics solver (lazy import; placo only needed when used)

No behavior change when urdf_path is unset (joint-space only).

Co-authored-by: Cursor <cursoragent@cursor.com>
---
 .../config_openarm_follower.py                |  8 +++++
 .../openarm_follower/openarm_follower.py      | 32 ++++++++++++++++++-
 2 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/src/lerobot/robots/openarm_follower/config_openarm_follower.py b/src/lerobot/robots/openarm_follower/config_openarm_follower.py
index 096ec320f..8e628f8cd 100644
--- a/src/lerobot/robots/openarm_follower/config_openarm_follower.py
+++ b/src/lerobot/robots/openarm_follower/config_openarm_follower.py
@@ -74,6 +74,14 @@ class OpenArmFollowerConfigBase:
     # Set to a positive scalar for all motors, or a dict mapping motor names to limits
     max_relative_target: float | dict[str, float] | None = None
 
+    # End-effector kinematics (optional). Point `urdf_path` at the OpenArm URDF and set
+    # `target_frame_name` to the end-effector link in that URDF to enable forward/inverse
+    # kinematics in Cartesian (EE) space -- via `lerobot.model.RobotKinematics` and the
+    # shared FK/IK processor steps. Left as None, the robot behaves exactly as before
+    # (joint space only). The URDF is user-supplied, so no large asset is vendored here.
+    urdf_path: str | None = None
+    target_frame_name: str | None = None
+
     # Camera configurations
     cameras: dict[str, CameraConfig] = field(default_factory=dict)
 
diff --git a/src/lerobot/robots/openarm_follower/openarm_follower.py b/src/lerobot/robots/openarm_follower/openarm_follower.py
index 00adf41ab..3e0e2c4c3 100644
--- a/src/lerobot/robots/openarm_follower/openarm_follower.py
+++ b/src/lerobot/robots/openarm_follower/openarm_follower.py
@@ -17,7 +17,7 @@
 import logging
 import time
 from functools import cached_property
-from typing import Any
+from typing import TYPE_CHECKING, Any
 
 from lerobot.cameras import make_cameras_from_configs
 from lerobot.lerobot_types import RobotAction, RobotObservation
@@ -25,6 +25,9 @@ from lerobot.motors import Motor, MotorCalibration, MotorNormMode
 from lerobot.motors.damiao import DamiaoMotorsBus
 from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
 
+if TYPE_CHECKING:
+    from lerobot.model import RobotKinematics
+
 from ..robot import Robot
 from ..utils import ensure_safe_goal_position
 from .config_openarm_follower import (
@@ -120,6 +123,33 @@ class OpenArmFollower(Robot):
         """Action features."""
         return self._motors_ft
 
+    @property
+    def arm_motor_names(self) -> list[str]:
+        """Arm joints forming the kinematic chain to the end-effector (excludes the gripper)."""
+        return [motor for motor in self.bus.motors if motor != "gripper"]
+
+    def make_kinematics(self) -> "RobotKinematics":
+        """Build a solver for end-effector forward/inverse kinematics.
+
+        Requires ``config.urdf_path`` (path to the OpenArm URDF) and
+        ``config.target_frame_name`` (the end-effector link in that URDF). Pair the returned
+        solver with the shared FK/IK processor steps in
+        ``lerobot.robots.so_follower.robot_kinematic_processor`` to record or command the arm
+        in end-effector (Cartesian) space instead of raw joint angles.
+        """
+        from lerobot.model import RobotKinematics
+
+        if self.config.urdf_path is None or self.config.target_frame_name is None:
+            raise ValueError(
+                "OpenArm end-effector kinematics require config.urdf_path and "
+                "config.target_frame_name (the OpenArm URDF and its end-effector link)."
+            )
+        return RobotKinematics(
+            urdf_path=self.config.urdf_path,
+            target_frame_name=self.config.target_frame_name,
+            joint_names=self.arm_motor_names,
+        )
+
     @property
     def is_connected(self) -> bool:
         """Check if robot is connected."""