fr3

CMakeLists.txt

@@ -23,6 +23,10 @@ set(OPENPI_CONTROL_CPPZMQ_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/cppzmq/install" CAC
     "cppzmq installation prefix used by openpi-control")
 set(OPENPI_CONTROL_TROSSEN_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/trossen_arm/install" CACHE PATH
     "libtrossen_arm installation prefix used by openpi-control")
+set(OPENPI_CONTROL_LIBFRANKA_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/libfranka/install" CACHE PATH
+    "libfranka installation prefix used by openpi-control")
+set(OPENPI_CONTROL_LIBMODBUS_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/libmodbus/install" CACHE PATH
+    "libmodbus installation prefix used by openpi-control")
 
 if(EXISTS "${OPENPI_CONTROL_PINOCCHIO_PREFIX}/lib/pkgconfig/pinocchio.pc")
   list(PREPEND CMAKE_PREFIX_PATH "${OPENPI_CONTROL_PINOCCHIO_PREFIX}")
@@ -43,6 +47,10 @@ if(EXISTS "${OPENPI_CONTROL_TROSSEN_PREFIX}/lib/libtrossen_arm.a")
   list(PREPEND CMAKE_PREFIX_PATH "${OPENPI_CONTROL_TROSSEN_PREFIX}")
 endif()
 
+if(EXISTS "${OPENPI_CONTROL_LIBFRANKA_PREFIX}/lib/libfranka.a")
+  list(PREPEND CMAKE_PREFIX_PATH "${OPENPI_CONTROL_LIBFRANKA_PREFIX}")
+endif()
+
 # Sanitizer plumbing for the test targets. Valid values: "" (off), "address,undefined",
 # "thread". Applied globally so the exercised pi_control sources are instrumented too.
 set(OPENPI_CONTROL_SANITIZER "" CACHE STRING "Comma-separated -fsanitize= list for test builds")

README.md

@@ -9,6 +9,7 @@ Python process via ZeroMQ.
 | `Yam` | `E_Yam` | `E_Yam_Handle` |
 | `ARX_L5` | `E_ARX` | `E_ARX` |
 | `ARX_X5` | `E_ARX` | `E_ARX` |
+| `FR3` | `Robotiq` | — |
 
 ```python
 from openpi_control import ArmConfig, ArmSession, PositionCommand, SocketCanConnection
@@ -27,10 +28,31 @@ with ArmSession() as session:
     follower.command(PositionCommand([0, 0, 0, 0, 0, 0], 1.0))
 ```
 
+FR3 uses the same `ArmSession`, `FollowerArm`, and `PositionCommand` API. Its
+connection selects one of the two real Robotiq Modbus transports:
+
+```python
+from openpi_control import FR3Connection, RobotiqConnection
+
+config = ArmConfig(
+    "follower",
+    "FR3",
+    FR3Connection("192.168.1.10"),
+    effector_model="Robotiq",
+    effector_connection=RobotiqConnection.rtu("/dev/serial/by-id/usb-robotiq"),
+    # Or: RobotiqConnection.tcp("192.168.1.11", port=502),
+)
+```
+
+See [docs/fr3.md](docs/fr3.md) for firmware, networking, controller, and
+hardware-validation details.
+
 ## Building from source
 
-Building requires CMake, a C++17 compiler, Eigen, Boost, ZeroMQ,
-cppzmq, and Pinocchio. It has only been tested on Ubuntu 22.04.
+Building requires CMake and a C++17 compiler. The dependency builder pins and
+builds Pinocchio, ZeroMQ, cppzmq, Trossen, libfranka 0.21.3, and libmodbus;
+the resulting libfranka and libmodbus archives are linked into
+`pi_control_node`. It has been tested on Ubuntu 22.04 and 24.04.
 
 ```bash
 sudo ./scripts/install_build_deps_ubuntu.sh

docs/fr3.md

@@ -0,0 +1,101 @@
+# FR3 and Robotiq control
+
+`openpi-control` owns one Franka Emika FR3 and one Robotiq 2F gripper through
+the same `ArmSession` and `FollowerArm` API used by the other follower arms.
+One `pi_control_node` process owns both hardware connections.
+
+## Requirements
+
+- libfranka is pinned to **0.21.3** and built from source by
+  `scripts/build_deps.sh`.
+- The FR3 must run Robot System version **5.9.0 or newer** (robot server
+  protocol 10).
+- The host must be able to reach the FR3 controller over Ethernet.
+- The gripper must use either true Modbus RTU over a serial device or true
+  Modbus TCP. RTU-over-TCP gateways are not treated as serial devices.
+
+A real-time kernel is not required. The driver always constructs libfranka
+with `franka::RealtimeConfig::kIgnore`; the torque callback still runs at the
+1 kHz rate owned by libfranka.
+
+## Python configuration
+
+For serial RTU:
+
+```python
+from openpi_control import ArmConfig, FR3Connection, RobotiqConnection
+
+config = ArmConfig(
+    "follower",
+    "FR3",
+    FR3Connection("192.168.1.10"),
+    effector_model="Robotiq",
+    effector_connection=RobotiqConnection.rtu(
+        "/dev/serial/by-id/usb-robotiq",
+        baud_rate=115200,
+        slave_id=9,
+    ),
+)
+```
+
+For Modbus TCP, only the gripper connection changes:
+
+```python
+effector_connection=RobotiqConnection.tcp("192.168.1.11", port=502)
+```
+
+The public gripper convention is `0.0 = fully closed` and `1.0 = fully open`.
+Raw Robotiq register calibration defaults to 3 (open) and 230 (closed).
+
+Connecting is passive: the arm holds its measured pose and the gripper is not
+activated. `move_to_ready()` performs internal FR3 error recovery, moves to the
+configured seven-joint reset pose, activates the gripper, and waits for it to
+open fully. There are no
+FR3-specific activation, recovery, velocity-command, read-only, or synthetic
+backend APIs.
+
+```python
+from openpi_control import ArmSession, PositionCommand
+
+with ArmSession() as session:
+    follower = session.add_follower(config)
+    session.connect()
+    follower.move_to_ready()
+    follower.command(
+        PositionCommand(
+            [0.0, -0.6283185307, 0.0, -2.5132741229, 0.0, 1.8849555922, 0.0],
+            1.0,
+        )
+    )
+```
+
+## Native controller
+
+Policy targets may arrive at a lower frequency while libfranka continues its
+1 kHz torque callback. Position targets remain active until replaced or held.
+The controller retains the ported, tuned hybrid joint/Cartesian impedance
+gains:
+
+- Cartesian stiffness: `400, 400, 400, 15, 15, 15`
+- Cartesian damping: `37, 37, 37, 2, 2, 2`
+- Joint stiffness: `40, 30, 50, 25, 35, 25, 10`
+- Joint damping: `4, 6, 5, 5, 3, 2, 1`
+
+Joint, velocity, Cartesian, torque, collision, owner-liveness, and command
+shape checks remain active. libfranka supplies the robot dynamics and
+kinematics, so the FR3 configuration does not require a packaged URDF.
+
+The dependency builder produces static `libfranka.a` and `libmodbus.a`
+archives. Both are linked directly into the packaged `pi_control_node`, just
+like the existing pinned native dependencies.
+
+## Hardware acceptance
+
+Perform the first actuating checks with the workspace clear and an operator at
+the E-stop:
+
+1. Connect and verify seven joint states plus one gripper state without motion.
+2. Verify the RTU or TCP gripper endpoint and the open/closed convention.
+3. Call `move_to_ready()` and verify the blocking reset motion and activation.
+4. Send low-rate position targets and verify the 1 kHz callback remains healthy.
+5. Terminate the Python owner and verify that the arm holds and gripper motion stops.

native/pi_control/CMakeLists.txt

@@ -34,8 +34,24 @@ set(CMAKE_INSTALL_RPATH "$ORIGIN/../libs")
 find_package(Boost CONFIG REQUIRED COMPONENTS program_options)
 find_package(PkgConfig REQUIRED)
 find_package(Eigen3 REQUIRED)
+if(NOT TARGET Eigen3::Eigen3 AND TARGET Eigen3::Eigen)
+  add_library(Eigen3::Eigen3 ALIAS Eigen3::Eigen)
+endif()
+find_package(fmt REQUIRED)
+find_package(Poco REQUIRED COMPONENTS Net Foundation)
+find_package(pinocchio REQUIRED)
+find_package(Franka 0.21.3 EXACT CONFIG REQUIRED)
 pkg_check_modules(PC_ZMQ REQUIRED libzmq)
 pkg_check_modules(PINOCCHIO REQUIRED pinocchio)
+pkg_check_modules(TINYXML2 REQUIRED IMPORTED_TARGET tinyxml2)
+add_library(TinyXML2::TinyXML2 ALIAS PkgConfig::TINYXML2)
+
+find_library(OPENPI_CONTROL_MODBUS_STATIC_LIBRARY NAMES libmodbus.a
+  PATHS "${OPENPI_CONTROL_LIBMODBUS_PREFIX}/lib" NO_DEFAULT_PATH REQUIRED)
+add_library(openpi_control_modbus STATIC IMPORTED)
+set_target_properties(openpi_control_modbus PROPERTIES
+  IMPORTED_LOCATION "${OPENPI_CONTROL_MODBUS_STATIC_LIBRARY}"
+  INTERFACE_INCLUDE_DIRECTORIES "${OPENPI_CONTROL_LIBMODBUS_PREFIX}/include/modbus")
 
 # Trossen iNerve controller SDK (DriverTrossen). Provisioned by
 # scripts/build_deps.sh from the pinned TrossenRobotics/trossen_arm release
@@ -81,6 +97,9 @@ set(COMMON_SOURCES
   src/pi_driver_can.cpp
   src/pi_driver_controller.cpp
   src/pi_driver_trossen.cpp
+  src/pi_driver_fr3.cpp
+  src/pi_fr3_controller.cpp
+  src/pi_robotiq.cpp
   # Serial bus-servo stack (protocol-neutral DriverSerial + FeeTech; a future
   # Dynamixel port adds pi_driver_dxl.cpp / pi_servo_dxl.cpp here).
   src/pi_driver_serial.cpp
@@ -96,6 +115,7 @@ set(COMMON_SOURCES
   src/pi_device_arm.cpp
   src/pi_device_arm_can.cpp
   src/pi_device_arm_serial.cpp
+  src/pi_device_fr3.cpp
   src/pi_device_effector.cpp
   src/pi_device_effector_can.cpp
   src/pi_device_effector_controller.cpp
@@ -137,7 +157,10 @@ target_link_libraries(${OPENPI_CONTROL_TARGET}
   ${PINOCCHIO_LDFLAGS}
   Boost::program_options
   pi_trossen_shim
+  Franka::Franka
+  openpi_control_modbus
 )
+target_link_options(${OPENPI_CONTROL_TARGET} PRIVATE "LINKER:--exclude-libs,ALL")
 
 # Use a transitive DT_RPATH instead of DT_RUNPATH. Libraries copied beside the
 # executable by the runtime-dependency install rule have no reason to know the
@@ -167,6 +190,8 @@ if(OPENPI_CONTROL_BUILD_TESTING)
     ${PINOCCHIO_LDFLAGS}
     Boost::program_options
     pi_trossen_shim
+    Franka::Franka
+    openpi_control_modbus
   )
   gtest_discover_tests(
     pi_topic_zmq_tests
@@ -228,6 +253,8 @@ if(OPENPI_CONTROL_BUILD_FUZZERS)
       ${PINOCCHIO_LDFLAGS}
       Boost::program_options
       pi_trossen_shim
+      Franka::Franka
+      openpi_control_modbus
     )
   endforeach()
 endif()

native/pi_control/include/pi_command_line_args.hpp

@@ -74,6 +74,19 @@
 #define OPT_URDF_PATH                             "urdf_path"
 // Read end of the supervising Python process's lifeline pipe.
 #define OPT_PARENT_LIVENESS_FD                    "parent_liveness_fd"
+#define OPT_FR3_ADDRESS                           "fr3_address"
+#define OPT_FR3_RESET_POSE                        "fr3_reset_pose"
+#define OPT_ROBOTIQ_TRANSPORT                     "robotiq_transport"
+#define OPT_ROBOTIQ_ENDPOINT                      "robotiq_endpoint"
+#define OPT_ROBOTIQ_PORT                          "robotiq_port"
+#define OPT_ROBOTIQ_BAUD_RATE                     "robotiq_baud_rate"
+#define OPT_ROBOTIQ_SLAVE_ID                      "robotiq_slave_id"
+#define OPT_ROBOTIQ_POLL_FREQUENCY                "robotiq_poll_frequency"
+#define OPT_ROBOTIQ_TIMEOUT_MS                    "robotiq_timeout_ms"
+#define OPT_ROBOTIQ_MIN_POSITION_RAW              "robotiq_min_position_raw"
+#define OPT_ROBOTIQ_MAX_POSITION_RAW              "robotiq_max_position_raw"
+#define OPT_ROBOTIQ_DEFAULT_SPEED                 "robotiq_default_speed"
+#define OPT_ROBOTIQ_DEFAULT_FORCE                 "robotiq_default_force"
 
 
 // Unified move-to-ready / emergency-recovery options. Every "move to ready" path (startup,
@@ -153,6 +166,19 @@ class CommandLineArgs {
     std::string effector_instance_config;
     std::string urdf_path;
     int parent_liveness_fd = -1;  ///< Inherited lifeline pipe; EOF means the Python owner exited.
+    std::string fr3_address;       ///< FR3 controller hostname or IP address.
+    std::string fr3_reset_pose;    ///< Seven comma-separated reset joint positions (rad).
+    std::string robotiq_transport; ///< Empty, "rtu", or "tcp".
+    std::string robotiq_endpoint;  ///< Serial device for RTU or hostname/address for TCP.
+    int robotiq_port = 502;        ///< Modbus TCP port.
+    int robotiq_baud_rate = 115200; ///< Modbus RTU baud rate.
+    int robotiq_slave_id = 9;      ///< Robotiq Modbus slave id.
+    int robotiq_poll_frequency = 50; ///< Gripper status polling frequency (Hz).
+    int robotiq_timeout_ms = 200;  ///< Modbus response timeout.
+    int robotiq_min_position_raw = 3; ///< Raw register value for fully open.
+    int robotiq_max_position_raw = 230; ///< Raw register value for fully closed.
+    float robotiq_default_speed = 1.0f; ///< Speed used by position-only public commands.
+    float robotiq_default_force = 1.0f; ///< Force used by position-only public commands.
 
     // Unified move-to-ready / emergency-recovery options. See macros above for descriptions.
     float move_to_ready_vel_rad_s_normal; ///< Healthy move-to-ready angular speed (rad/s).

native/pi_control/include/pi_device_config.hpp

@@ -46,6 +46,7 @@ class DeviceConfig {
     const std::string val_arm_type_can    = "can";      ///< Value for MIT-mode CAN arms.
     const std::string val_arm_type_controller = "controller";  ///< Value for arms managed by a whole-arm controller (DriverController).
     const std::string val_arm_type_serial  = "serial";   ///< Value for serial bus-servo arms (e.g. SO-ARM101).
+    const std::string val_arm_type_fr3     = "fr3";      ///< Value for Franka Emika FR3 arms.
 
     const std::string fn_effector_type                    = "effector_type";     ///< Field name for effector type.
     const std::string val_effector_type_can                   = "can";               ///< Value for MIT-mode CAN effectors.
@@ -68,11 +69,13 @@ class DeviceConfig {
     const std::string val_driver_type_can_encoder = "CAN_ENCODER"; ///< Value for driver type CAN read-only encoder (DriverArxEncoder).
     const std::string val_driver_type_trossen    = "TROSSEN_ETHERNET";  ///< Value for driver type Trossen iNerve controller over Ethernet (DriverTrossen).
     const std::string val_driver_type_ft         = "FEETECH";      ///< Value for driver type FeeTech SMS/STS serial bus (DriverFt).
+    const std::string val_driver_type_fr3        = "FR3";          ///< Value for libfranka FR3 driver.
     const std::string fn_controller_model        = "controller_model";  ///< Field name for the vendor controller model string (e.g. "wxai_v0").
 
     const std::string fn_algo_type            = "algo_type";   ///< Field name for algorithm type.
     const std::string val_algo_type_algo          = "Algo";        ///< Value for algorithm type Algo.
     const std::string val_algo_type_pinocchio     = "Pinocchio";   ///< Value for algorithm type Pinocchio.
+    const std::string val_algo_type_none          = "None";        ///< Value for devices with an internal controller.
 
     const std::string fn_base_rpy   = "base_rpy";    ///< Field name for base axes rotation (roll, pitch, yaw), radian.
 

native/pi_control/include/pi_device_fr3.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <memory>
+
+#include "pi_device.hpp"
+#include "pi_driver_fr3.hpp"
+#include "pi_robotiq.hpp"
+
+class DeviceFR3 final : public Device {
+   public:
+    explicit DeviceFR3(const CommandLineArgs& cla);
+    ~DeviceFR3() override;
+
+    ReturnCode init(const CommandLineArgs& cla, int argc, char** argv,
+                    std::shared_ptr<Topic> topic = nullptr,
+                    std::shared_ptr<Driver> driver = nullptr) override;
+    ReturnCode start(int baud_rate) override;
+    ReturnCode stop() override;
+    ReturnCode park_safely() override;
+    ReturnCode apply_action(const MsgJoints& msg) override;
+    ReturnCode get_observation(MsgJoints& msg) override;
+    ReturnCode process_follower_msg(const MsgJoints& msg) override;
+    ReturnCode read_hardware_values() override;
+    ReturnCode write_hardware_values() override;
+    ReturnCode move_to_ready_position() override;
+    ReturnCode operate_as_leader() override;
+    ReturnCode operate_as_follower() override;
+    ReturnCode get_servo_ids(std::vector<int>& servo_ids) override;
+    ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override;
+    ReturnCode runtime_hold() override;
+
+   protected:
+    void reset_ready_state_for_move_to_ready() override { is_ready_ = false; }
+    void clear_command_buffers_for_move_to_ready() override;
+    float get_ready_move_completion_ratio() const override { return is_ready_ ? 1.0f : 0.0f; }
+
+   private:
+    std::shared_ptr<DriverFR3> driver_fr3_;
+    std::unique_ptr<RobotiqTransport> robotiq_;
+    float robotiq_default_speed_ = 1.0f;
+    float robotiq_default_force_ = 1.0f;
+};