From 198c9de935f87ed378f98c8eb70a50c4f689aa32 Mon Sep 17 00:00:00 2001 From: kvablack Date: Mon, 3 Aug 2026 20:31:00 +0000 Subject: [PATCH] fr3 --- CMakeLists.txt | 8 + README.md | 26 +- docs/fr3.md | 101 +++++ native/pi_control/CMakeLists.txt | 27 ++ .../include/pi_command_line_args.hpp | 26 ++ .../pi_control/include/pi_device_config.hpp | 3 + native/pi_control/include/pi_device_fr3.hpp | 42 ++ native/pi_control/include/pi_driver_fr3.hpp | 63 +++ .../pi_control/include/pi_fr3_controller.hpp | 55 +++ native/pi_control/include/pi_robotiq.hpp | 89 ++++ native/pi_control/src/pi_algo.cpp | 5 +- .../pi_control/src/pi_command_line_args.cpp | 69 ++- native/pi_control/src/pi_device.cpp | 5 + native/pi_control/src/pi_device_fr3.cpp | 203 +++++++++ native/pi_control/src/pi_driver.cpp | 7 +- native/pi_control/src/pi_driver_fr3.cpp | 410 ++++++++++++++++++ native/pi_control/src/pi_fr3_controller.cpp | 87 ++++ native/pi_control/src/pi_robotiq.cpp | 203 +++++++++ native/pi_control/tests/CMakeLists.txt | 3 + .../pi_control/tests/test_fr3_controller.cpp | 38 ++ native/pi_control/tests/test_robotiq.cpp | 24 + pyproject.toml | 4 +- scripts/build_deps.sh | 68 ++- scripts/install_build_deps_ubuntu.sh | 7 + scripts/patches/libfranka-0.21.3-static.patch | 13 + src/openpi_control/__init__.py | 6 + src/openpi_control/config.py | 112 ++++- src/openpi_control/models/arms/FR3/FR3.json | 26 ++ .../models/arms/FR3/FR3_01.json | 11 + .../models/effectors/Robotiq/Robotiq.json | 15 + .../models/effectors/Robotiq/Robotiq_01.json | 9 + src/openpi_control/native.py | 59 ++- src/openpi_control/urdf_inertial.py | 2 + tests/test_config.py | 48 +- tests/test_native_backend.py | 56 +++ uv.lock | 2 +- 36 files changed, 1908 insertions(+), 24 deletions(-) create mode 100644 docs/fr3.md create mode 100644 native/pi_control/include/pi_device_fr3.hpp create mode 100644 native/pi_control/include/pi_driver_fr3.hpp create mode 100644 native/pi_control/include/pi_fr3_controller.hpp create mode 100644 native/pi_control/include/pi_robotiq.hpp create mode 100644 native/pi_control/src/pi_device_fr3.cpp create mode 100644 native/pi_control/src/pi_driver_fr3.cpp create mode 100644 native/pi_control/src/pi_fr3_controller.cpp create mode 100644 native/pi_control/src/pi_robotiq.cpp create mode 100644 native/pi_control/tests/test_fr3_controller.cpp create mode 100644 native/pi_control/tests/test_robotiq.cpp create mode 100644 scripts/patches/libfranka-0.21.3-static.patch create mode 100644 src/openpi_control/models/arms/FR3/FR3.json create mode 100644 src/openpi_control/models/arms/FR3/FR3_01.json create mode 100644 src/openpi_control/models/effectors/Robotiq/Robotiq.json create mode 100644 src/openpi_control/models/effectors/Robotiq/Robotiq_01.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 83225ac..4c9bee8 100644 --- a/CMakeLists.txt +++ b/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") diff --git a/README.md b/README.md index 4c12ffa..43323d2 100644 --- a/README.md +++ b/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 diff --git a/docs/fr3.md b/docs/fr3.md new file mode 100644 index 0000000..181df8c --- /dev/null +++ b/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. diff --git a/native/pi_control/CMakeLists.txt b/native/pi_control/CMakeLists.txt index 9f6c3d7..96e8a35 100644 --- a/native/pi_control/CMakeLists.txt +++ b/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() diff --git a/native/pi_control/include/pi_command_line_args.hpp b/native/pi_control/include/pi_command_line_args.hpp index e9f13ed..82aa24c 100644 --- a/native/pi_control/include/pi_command_line_args.hpp +++ b/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). diff --git a/native/pi_control/include/pi_device_config.hpp b/native/pi_control/include/pi_device_config.hpp index c15c1d7..290d693 100644 --- a/native/pi_control/include/pi_device_config.hpp +++ b/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. diff --git a/native/pi_control/include/pi_device_fr3.hpp b/native/pi_control/include/pi_device_fr3.hpp new file mode 100644 index 0000000..d818618 --- /dev/null +++ b/native/pi_control/include/pi_device_fr3.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include + +#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 = nullptr, + std::shared_ptr 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& 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 driver_fr3_; + std::unique_ptr robotiq_; + float robotiq_default_speed_ = 1.0f; + float robotiq_default_force_ = 1.0f; +}; diff --git a/native/pi_control/include/pi_driver_fr3.hpp b/native/pi_control/include/pi_driver_fr3.hpp new file mode 100644 index 0000000..516feb1 --- /dev/null +++ b/native/pi_control/include/pi_driver_fr3.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "pi_driver.hpp" +#include "pi_fr3_controller.hpp" + +struct FR3DriverState { + uint64_t sequence = 0; + uint64_t monotonic_ns = 0; + std::array q{}; + std::array dq{}; + std::array torque{}; + std::array commanded_q{}; + bool valid = false; + bool faulted = false; + std::string fault; +}; + +class DriverFR3 final : public Driver { + public: + DriverFR3(Device* device, const CommandLineArgs& cla); + ~DriverFR3() override; + + ReturnCode open(int baud_rate) override; + ReturnCode close() override; + FR3DriverState state() const; + ReturnCode set_target(const std::array& target); + ReturnCode hold(); + ReturnCode move_to_ready(); + + private: + struct Impl; + ReturnCode start_controller(); + void run_controller(); + void apply_pending_command(); + ReturnCode initialize_controller_state(); + + CommandLineArgs cla_; + std::unique_ptr impl_; + mutable std::mutex state_mutex_; + mutable std::mutex pending_controller_mutex_; + FR3DriverState state_; + FR3ControllerLimits limits_; + FR3Controller controller_; + std::array pending_target_{}; + std::array reset_pose_{}; + bool target_pending_ = false; + bool hold_pending_ = false; + std::thread thread_; + std::atomic running_{false}; + std::atomic stop_requested_{false}; + std::mutex controller_lifecycle_mutex_; + std::condition_variable controller_lifecycle_cv_; + bool controller_started_ = false; + bool controller_exited_ = true; +}; diff --git a/native/pi_control/include/pi_fr3_controller.hpp b/native/pi_control/include/pi_fr3_controller.hpp new file mode 100644 index 0000000..e46b537 --- /dev/null +++ b/native/pi_control/include/pi_fr3_controller.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include + +struct FR3ControllerGains { + std::array cartesian_stiffness{400, 400, 400, 15, 15, 15}; + std::array cartesian_damping{37, 37, 37, 2, 2, 2}; + std::array joint_stiffness{40, 30, 50, 25, 35, 25, 10}; + std::array joint_damping{4, 6, 5, 5, 3, 2, 1}; +}; + +struct FR3ControllerLimits { + std::array cartesian_lower{-1, -1, -1}; + std::array cartesian_upper{1, 1, 1}; + std::array joint_lower{-2.65, -1.68, -2.80, -2.95, -2.70, 0.45, -2.90}; + std::array joint_upper{2.65, 1.68, 2.80, -0.16, 2.70, 4.40, 2.90}; + std::array velocity{2.075, 2.075, 2.075, 2.075, 2.51, 2.51, 2.51}; + std::array torque{86, 86, 86, 86, 11.5, 11.5, 11.5}; + double joint_margin = 0.2; + double velocity_margin = 0.5; + double cartesian_margin = 0.05; + double joint_stiffness = 50; + double velocity_stiffness = 20; + double cartesian_stiffness = 200; +}; + +struct FR3ControllerInput { + std::array q{}; + std::array dq{}; + std::array coriolis{}; + std::array flange_jacobian{}; + std::array end_effector_jacobian{}; + std::array end_effector_position{}; + double elbow_velocity = 0.0; +}; + +class FR3Controller { + public: + explicit FR3Controller(FR3ControllerGains gains = {}, FR3ControllerLimits limits = {}); + + void set_target(const std::array& target_position); + void hold(const std::array& measured_position); + std::array compute(const FR3ControllerInput& input); + const std::array& commanded_position() const { return target_position_; } + + private: + static void add_soft_limit(double value, double lower, double upper, double margin, + double stiffness, double& output); + + FR3ControllerGains gains_; + FR3ControllerLimits limits_; + std::array target_position_{}; + bool initialized_ = false; + bool position_command_active_ = false; +}; diff --git a/native/pi_control/include/pi_robotiq.hpp b/native/pi_control/include/pi_robotiq.hpp new file mode 100644 index 0000000..9872d51 --- /dev/null +++ b/native/pi_control/include/pi_robotiq.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +enum class RobotiqBackend { RTU, TCP }; + +struct RobotiqConfig { + RobotiqBackend backend = RobotiqBackend::RTU; + std::string endpoint; + int tcp_port = 502; + int baud_rate = 115200; + int slave_id = 9; + int poll_frequency_hz = 50; + int response_timeout_ms = 200; + uint8_t open_raw = 3; + uint8_t closed_raw = 230; +}; + +struct RobotiqState { + bool connected = false; + bool activated = false; + bool ever_activated = false; + bool moving = false; + float position = 1.0f; // 0 closed, 1 open. + float velocity = 0.0f; + float effort = 0.0f; + float current = 0.0f; + float target = 1.0f; + uint8_t fault = 0; +}; + +class RobotiqTransport { + public: + explicit RobotiqTransport(RobotiqConfig config); + ~RobotiqTransport(); + + bool start(); + void stop(); + bool activate(); + void set_target(float position, float speed, float force); + void hold(); + RobotiqState state() const; + + static uint8_t position_to_raw(float normalized_open, uint8_t open_raw, uint8_t closed_raw) { + const float value = closed_raw - std::clamp(normalized_open, 0.0f, 1.0f) * + static_cast(closed_raw - open_raw); + return static_cast(std::lround(value)); + } + static float raw_to_position(uint8_t raw, uint8_t open_raw, uint8_t closed_raw) { + if (closed_raw <= open_raw) return 0.0f; + const float normalized_open = + (static_cast(closed_raw) - static_cast(raw)) / + static_cast(closed_raw - open_raw); + return std::clamp(normalized_open, 0.0f, 1.0f); + } + static uint8_t normalized_to_raw(float value, bool preserve_regrasp = false) { + const long raw = std::lround(255.0f * std::clamp(value, 0.0f, 1.0f)); + return static_cast(preserve_regrasp && raw == 0 && value > 0 ? 1 : raw); + } + static bool has_operational_fault(const RobotiqState& state) { + return state.ever_activated && state.fault != 0; + } + + private: + struct Impl; + void run(); + + RobotiqConfig config_; + std::unique_ptr impl_; + mutable std::mutex mutex_; + std::condition_variable condition_; + std::thread thread_; + std::atomic running_{false}; + RobotiqState state_; + float target_position_ = 1.0f; + float target_speed_ = 1.0f; + float target_force_ = 1.0f; + uint64_t command_generation_ = 0; + bool activation_requested_ = false; + bool hold_requested_ = false; +}; diff --git a/native/pi_control/src/pi_algo.cpp b/native/pi_control/src/pi_algo.cpp index e531e8e..fc7d602 100644 --- a/native/pi_control/src/pi_algo.cpp +++ b/native/pi_control/src/pi_algo.cpp @@ -86,7 +86,10 @@ Algo* Algo::new_algo(Device* p_device, const DeviceConfig* p_config_model, const } Algo* p_algo = nullptr; - if (algo_type == p_config_model->val_algo_type_algo) { + if (algo_type == p_config_model->val_algo_type_none) { + PI_INFO("Algo", InfoLevel::HELPFUL_1, "No external algorithm requested"); + return nullptr; + } else if (algo_type == p_config_model->val_algo_type_algo) { p_algo = new Algo(p_device, cla); PI_INFO("Algo", InfoLevel::HELPFUL_1, "Created base Algo class instance"); #ifdef ENABLE_ALGO_PINO diff --git a/native/pi_control/src/pi_command_line_args.cpp b/native/pi_control/src/pi_command_line_args.cpp index c891e65..876f46f 100644 --- a/native/pi_control/src/pi_command_line_args.cpp +++ b/native/pi_control/src/pi_command_line_args.cpp @@ -163,6 +163,33 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { "Explicit URDF path")( OPT_PARENT_LIVENESS_FD, po::value()->default_value(-1), "Inherited read end of the supervising process's liveness pipe")( + OPT_FR3_ADDRESS, po::value()->default_value(""), + "FR3 controller hostname or IP address")( + OPT_FR3_RESET_POSE, + po::value()->default_value("0,-0.785398,0,-2.356194,0,1.570796,0.785398"), + "Seven comma-separated FR3 reset joint positions in radians")( + OPT_ROBOTIQ_TRANSPORT, po::value()->default_value(""), + "Robotiq transport: rtu or tcp")( + OPT_ROBOTIQ_ENDPOINT, po::value()->default_value(""), + "Robotiq serial device or TCP hostname/address")( + OPT_ROBOTIQ_PORT, po::value()->default_value(502), + "Robotiq Modbus TCP port")( + OPT_ROBOTIQ_BAUD_RATE, po::value()->default_value(115200), + "Robotiq Modbus RTU baud rate")( + OPT_ROBOTIQ_SLAVE_ID, po::value()->default_value(9), + "Robotiq Modbus slave id")( + OPT_ROBOTIQ_POLL_FREQUENCY, po::value()->default_value(50), + "Robotiq status polling frequency in Hz")( + OPT_ROBOTIQ_TIMEOUT_MS, po::value()->default_value(200), + "Robotiq Modbus response timeout in milliseconds")( + OPT_ROBOTIQ_MIN_POSITION_RAW, po::value()->default_value(3), + "Robotiq raw register value for fully open")( + OPT_ROBOTIQ_MAX_POSITION_RAW, po::value()->default_value(230), + "Robotiq raw register value for fully closed")( + OPT_ROBOTIQ_DEFAULT_SPEED, po::value()->default_value(1.0f), + "Robotiq speed for position commands, normalized to [0, 1]")( + OPT_ROBOTIQ_DEFAULT_FORCE, po::value()->default_value(1.0f), + "Robotiq force for position commands, normalized to [0, 1]")( OPT_MOVE_TO_READY_VEL_RAD_S_NORMAL, po::value()->default_value(MOVE_TO_READY_VEL_RAD_S_NORMAL), "Healthy move-to-ready angular speed (rad/s). Used by startup, command-driven " @@ -313,6 +340,8 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { control_port_name = vm[OPT_CONTROL_PORT].as(); PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Control port name: %s", control_port_name.c_str()); + } else if (device_model == "FR3") { + control_port_name.clear(); } else { PI_ERROR("--%s is not set", OPT_CONTROL_PORT); exit(2); @@ -604,6 +633,19 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { OPT_PARENT_LIVENESS_FD, parent_liveness_fd); exit(2); } + fr3_address = vm[OPT_FR3_ADDRESS].as(); + fr3_reset_pose = vm[OPT_FR3_RESET_POSE].as(); + robotiq_transport = vm[OPT_ROBOTIQ_TRANSPORT].as(); + robotiq_endpoint = vm[OPT_ROBOTIQ_ENDPOINT].as(); + robotiq_port = vm[OPT_ROBOTIQ_PORT].as(); + robotiq_baud_rate = vm[OPT_ROBOTIQ_BAUD_RATE].as(); + robotiq_slave_id = vm[OPT_ROBOTIQ_SLAVE_ID].as(); + robotiq_poll_frequency = vm[OPT_ROBOTIQ_POLL_FREQUENCY].as(); + robotiq_timeout_ms = vm[OPT_ROBOTIQ_TIMEOUT_MS].as(); + robotiq_min_position_raw = vm[OPT_ROBOTIQ_MIN_POSITION_RAW].as(); + robotiq_max_position_raw = vm[OPT_ROBOTIQ_MAX_POSITION_RAW].as(); + robotiq_default_speed = vm[OPT_ROBOTIQ_DEFAULT_SPEED].as(); + robotiq_default_force = vm[OPT_ROBOTIQ_DEFAULT_FORCE].as(); if (!topic_live_command.empty()) { topic_joint = topic_live_command; } @@ -616,8 +658,9 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { PI_ERROR("Standalone state/live/direct/lifecycle/status topics are all required"); exit(2); } - if (arm_model_config.empty() || arm_instance_config.empty() || urdf_path.empty()) { - PI_ERROR("Explicit --arm_model_config, --arm_instance_config, and --urdf_path are required"); + if (arm_model_config.empty() || arm_instance_config.empty() || + (device_model != "FR3" && urdf_path.empty())) { + PI_ERROR("Explicit arm model/instance configs are required; non-FR3 arms also require a URDF"); exit(2); } if (!effector_model.empty() && @@ -625,6 +668,28 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { PI_ERROR("Attached effectors require explicit model and instance configuration paths"); exit(2); } + if (device_model == "FR3") { + if (role != Role::FOLLOWER || fr3_address.empty()) { + PI_ERROR("FR3 requires follower role and --%s", OPT_FR3_ADDRESS); + exit(2); + } + if (!robotiq_transport.empty()) { + if ((robotiq_transport != "rtu" && robotiq_transport != "tcp") || robotiq_endpoint.empty()) { + PI_ERROR("Robotiq requires transport rtu/tcp and a non-empty endpoint"); + exit(2); + } + if (robotiq_port < 1 || robotiq_port > 65535 || robotiq_baud_rate <= 0 || + robotiq_slave_id < 0 || robotiq_slave_id > 247 || robotiq_poll_frequency <= 0 || + robotiq_timeout_ms <= 0 || robotiq_min_position_raw < 0 || + robotiq_max_position_raw > 255 || robotiq_min_position_raw >= robotiq_max_position_raw || + !std::isfinite(robotiq_default_speed) || robotiq_default_speed < 0.0f || + robotiq_default_speed > 1.0f || !std::isfinite(robotiq_default_force) || + robotiq_default_force < 0.0f || robotiq_default_force > 1.0f) { + PI_ERROR("Invalid Robotiq transport parameters"); + exit(2); + } + } + } // Unified move-to-ready / emergency-recovery options. All have defaults so existing // call sites are unaffected. Velocity-bounded stepping (step = max_vel * loop_dt) makes diff --git a/native/pi_control/src/pi_device.cpp b/native/pi_control/src/pi_device.cpp index c24f765..6c2f1aa 100644 --- a/native/pi_control/src/pi_device.cpp +++ b/native/pi_control/src/pi_device.cpp @@ -10,6 +10,7 @@ #include "pi_device.hpp" #include "pi_device_arm_can.hpp" #include "pi_device_arm_serial.hpp" +#include "pi_device_fr3.hpp" #include "pi_device_config.hpp" #include "pi_device_effector_can.hpp" #include "pi_device_effector_controller.hpp" @@ -582,6 +583,10 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf p_device = new DeviceArmSerial(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmSerial for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); + } else if (arm_type == cfg_model.val_arm_type_fr3) { + p_device = new DeviceFR3(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceFR3 for %s_%s", + cla.device_model.c_str(), cla.device_id.c_str()); } else { PI_ERROR("Invalid arm type: %s", arm_type.c_str()); return nullptr; diff --git a/native/pi_control/src/pi_device_fr3.cpp b/native/pi_control/src/pi_device_fr3.cpp new file mode 100644 index 0000000..e20903d --- /dev/null +++ b/native/pi_control/src/pi_device_fr3.cpp @@ -0,0 +1,203 @@ +#include "pi_device_fr3.hpp" + +#include +#include +#include +#include + +DeviceFR3::DeviceFR3(const CommandLineArgs& cla) : Device(cla) { + dof_ = 7; + dof_total_ = cla.robotiq_transport.empty() ? 7 : 8; + servo_num_ = 7; + servo_num_total_ = dof_total_; + type_ = DeviceType::ARM; +} + +DeviceFR3::~DeviceFR3() = default; + +ReturnCode DeviceFR3::init(const CommandLineArgs& cla, int argc, char** argv, + std::shared_ptr topic, std::shared_ptr driver) { + ReturnCode result = Device::init(cla, argc, argv, std::move(topic), std::move(driver)); + if (result != ReturnCode::SUCCESS) return result; + driver_fr3_ = std::dynamic_pointer_cast(p_driver_); + if (!driver_fr3_) return ReturnCode::NOT_INITIALIZED; + if (!cla.robotiq_transport.empty()) { + RobotiqConfig config; + config.backend = cla.robotiq_transport == "tcp" ? RobotiqBackend::TCP : RobotiqBackend::RTU; + config.endpoint = cla.robotiq_endpoint; + config.tcp_port = cla.robotiq_port; + config.baud_rate = cla.robotiq_baud_rate; + config.slave_id = cla.robotiq_slave_id; + config.poll_frequency_hz = cla.robotiq_poll_frequency; + config.response_timeout_ms = cla.robotiq_timeout_ms; + config.open_raw = static_cast(cla.robotiq_min_position_raw); + config.closed_raw = static_cast(cla.robotiq_max_position_raw); + robotiq_ = std::make_unique(std::move(config)); + robotiq_default_speed_ = cla.robotiq_default_speed; + robotiq_default_force_ = cla.robotiq_default_force; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceFR3::start(int baud_rate) { + ReturnCode result = Device::start(baud_rate); + if (result != ReturnCode::SUCCESS) return result; + const auto arm_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3); + while (!driver_fr3_->state().valid && std::chrono::steady_clock::now() < arm_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + if (!driver_fr3_->state().valid) return ReturnCode::NO_RESPONSE; + if (robotiq_) { + if (!robotiq_->start()) return ReturnCode::NO_RESPONSE; + const auto gripper_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2); + while (!robotiq_->state().connected && std::chrono::steady_clock::now() < gripper_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + if (!robotiq_->state().connected) return ReturnCode::NO_RESPONSE; + } + if (cla_.dont_go_to_home_pos) { + driver_fr3_->hold(); + is_ready_ = true; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceFR3::stop() { + const ReturnCode result = Device::stop(); + if (robotiq_) robotiq_->stop(); + return result; +} + +ReturnCode DeviceFR3::park_safely() { + if (driver_fr3_) driver_fr3_->hold(); + if (robotiq_) robotiq_->hold(); + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceFR3::apply_action(const MsgJoints& msg) { + if (msg.joints_.size() != static_cast(dof_total_)) return ReturnCode::INVALID_PARAM; + std::array target{}; + for (size_t i = 0; i < target.size(); ++i) { + const float value = msg.joints_[i].curr_pos_; + if (!std::isfinite(value)) return ReturnCode::INVALID_PARAM; + target[i] = value; + } + if (robotiq_) { + const float position = msg.joints_[7].curr_pos_; + const auto gripper = robotiq_->state(); + if (!std::isfinite(position) || position < 0.0f || position > 1.0f) return ReturnCode::INVALID_PARAM; + if (!gripper.connected) return ReturnCode::NO_RESPONSE; + if (!gripper.activated || RobotiqTransport::has_operational_fault(gripper)) { + return ReturnCode::HARDWARE_FAULT; + } + } + ReturnCode result = driver_fr3_->set_target(target); + if (result == ReturnCode::SUCCESS && robotiq_) { + robotiq_->set_target(msg.joints_[7].curr_pos_, robotiq_default_speed_, robotiq_default_force_); + } + return result; +} + +ReturnCode DeviceFR3::get_observation(MsgJoints& msg) { + const auto state = driver_fr3_->state(); + if (!state.valid) return ReturnCode::NOT_INITIALIZED; + const uint64_t now = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(); + const float age_ms = now >= state.monotonic_ns + ? static_cast(now - state.monotonic_ns) / 1000000.0f + : -1.0f; + for (size_t i = 0; i < 7; ++i) { + msg.add_joint_info(static_cast(state.q[i]), static_cast(state.dq[i]), + static_cast(state.torque[i]), 0.0f, 0.0f, age_ms); + } + if (robotiq_) { + const auto gripper = robotiq_->state(); + msg.add_joint_info(gripper.position, gripper.velocity, gripper.effort, 0.0f, + gripper.current, -1.0f); + if (!gripper.connected) return ReturnCode::NO_RESPONSE; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceFR3::process_follower_msg(const MsgJoints& msg) { return apply_action(msg); } + +ReturnCode DeviceFR3::read_hardware_values() { + const auto state = driver_fr3_->state(); + if (state.faulted) { + PI_ERROR("HARDWARE FAULT: %s", state.fault.c_str()); + return ReturnCode::HARDWARE_FAULT; + } + if (!state.valid) return ReturnCode::NOT_INITIALIZED; + if (robotiq_) { + const auto gripper = robotiq_->state(); + if (!gripper.connected) return ReturnCode::NO_RESPONSE; + if (RobotiqTransport::has_operational_fault(gripper)) { + PI_ERROR("HARDWARE FAULT: Robotiq reported fault 0x%02x", gripper.fault); + return ReturnCode::HARDWARE_FAULT; + } + } + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceFR3::write_hardware_values() { return ReturnCode::SUCCESS; } + +ReturnCode DeviceFR3::move_to_ready_position() { + ReturnCode result = driver_fr3_->move_to_ready(); + if (result != ReturnCode::SUCCESS) return result; + if (robotiq_) { + if (!robotiq_->state().activated && !robotiq_->activate()) { + driver_fr3_->hold(); + return ReturnCode::HARDWARE_FAULT; + } + robotiq_->set_target(1.0f, robotiq_default_speed_, robotiq_default_force_); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (std::chrono::steady_clock::now() < deadline) { + const auto gripper = robotiq_->state(); + if (!gripper.connected) { + driver_fr3_->hold(); + robotiq_->hold(); + return ReturnCode::NO_RESPONSE; + } + if (RobotiqTransport::has_operational_fault(gripper)) { + driver_fr3_->hold(); + robotiq_->hold(); + return ReturnCode::HARDWARE_FAULT; + } + if (gripper.activated && gripper.position >= 0.98f) { + is_ready_ = true; + return ReturnCode::SUCCESS; + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + PI_ERROR("FR3 move-to-ready timed out waiting for Robotiq to open"); + driver_fr3_->hold(); + robotiq_->hold(); + return ReturnCode::HARDWARE_FAULT; + } + is_ready_ = true; + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceFR3::operate_as_leader() { return ReturnCode::NOT_SUPPORTED; } +ReturnCode DeviceFR3::operate_as_follower() { return ReturnCode::SUCCESS; } + +ReturnCode DeviceFR3::get_servo_ids(std::vector& servo_ids) { + servo_ids.clear(); + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceFR3::set_control_mode(Role target_role, ControlModeIntent intent) { + (void)intent; + return target_role == Role::FOLLOWER ? ReturnCode::SUCCESS : ReturnCode::NOT_SUPPORTED; +} + +ReturnCode DeviceFR3::runtime_hold() { + if (rejects_direct_commands()) return ReturnCode::BUSY; + clear_command_buffers_for_move_to_ready(); + return ReturnCode::SUCCESS; +} + +void DeviceFR3::clear_command_buffers_for_move_to_ready() { + if (driver_fr3_) driver_fr3_->hold(); + if (robotiq_) robotiq_->hold(); +} diff --git a/native/pi_control/src/pi_driver.cpp b/native/pi_control/src/pi_driver.cpp index 6e5e10a..e4189fc 100644 --- a/native/pi_control/src/pi_driver.cpp +++ b/native/pi_control/src/pi_driver.cpp @@ -9,6 +9,7 @@ #include "pi_driver_can_mit.hpp" #include "pi_driver_arx_encoder.hpp" #include "pi_driver_ft.hpp" +#include "pi_driver_fr3.hpp" #include "pi_driver_trossen.hpp" #include "pi_servo.hpp" @@ -56,8 +57,12 @@ std::shared_ptr Driver::new_driver(Device* p_device, const DeviceConfig* p_driver = std::make_shared(p_device, cla); PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created FeeTech driver (DriverFt)"); + } else if (driver_type == p_config->val_driver_type_fr3) { + p_driver = std::make_shared(p_device, cla); + PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created libfranka driver (DriverFR3)"); + } else { - PI_ERROR("Unsupported driver type: '%s' (supported types: CAN, CAN_ENCODER, TROSSEN_ETHERNET, FEETECH)", + PI_ERROR("Unsupported driver type: '%s' (supported types: CAN, CAN_ENCODER, TROSSEN_ETHERNET, FEETECH, FR3)", driver_type.c_str()); return nullptr; } diff --git a/native/pi_control/src/pi_driver_fr3.cpp b/native/pi_control/src/pi_driver_fr3.cpp new file mode 100644 index 0000000..5dfc648 --- /dev/null +++ b/native/pi_control/src/pi_driver_fr3.cpp @@ -0,0 +1,410 @@ +#include "pi_driver_fr3.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include "pi_info.hpp" + +namespace { + +template +std::array parse_array(const std::string& value, const char* name) { + std::array result{}; + std::stringstream stream(value); + std::string item; + size_t index = 0; + while (std::getline(stream, item, ',') && index < result.size()) { + size_t consumed = 0; + result[index] = std::stod(item, &consumed); + if (consumed != item.size() || !std::isfinite(result[index])) { + throw std::invalid_argument(std::string(name) + " contains an invalid value"); + } + ++index; + } + if (index != result.size() || std::getline(stream, item, ',')) { + throw std::invalid_argument(std::string(name) + " has the wrong number of values"); + } + return result; +} + +uint64_t monotonic_ns() { + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +class ResetMotionGenerator { + public: + explicit ResetMotionGenerator(double speed_factor, const std::array& goal) + : goal_(goal) { + for (size_t i = 0; i < 7; ++i) { + max_velocity_[i] *= speed_factor; + max_start_acceleration_[i] *= speed_factor; + max_goal_acceleration_[i] *= speed_factor; + } + } + + franka::JointPositions operator()(const franka::RobotState& state, franka::Duration period) { + if (!initialized_) { + start_ = state.q_d; + for (size_t i = 0; i < 7; ++i) delta_[i] = goal_[i] - start_[i]; + synchronize(); + initialized_ = true; + return franka::JointPositions(start_); + } + time_ += period.toSec(); + std::array delta_desired{}; + const bool trajectory_finished = desired_values(time_, delta_desired); + std::array positions{}; + for (size_t i = 0; i < 7; ++i) positions[i] = start_[i] + delta_desired[i]; + bool motion_finished = false; + if (trajectory_finished) { + positions = goal_; + double max_error = 0; + for (size_t i = 0; i < 7; ++i) max_error = std::max(max_error, std::abs(state.q[i] - goal_[i])); + settle_time_ += period.toSec(); + motion_finished = max_error < 0.01 || settle_time_ > 4.0; + } + franka::JointPositions result(positions); + result.motion_finished = motion_finished; + return result; + } + + private: + bool desired_values(double time, std::array& desired) const { + std::array finished{}; + for (size_t i = 0; i < 7; ++i) { + const int sign = delta_[i] < 0 ? -1 : (delta_[i] > 0 ? 1 : 0); + const double constant_duration = second_sync_[i] - first_sync_[i]; + const double goal_ramp_duration = finish_sync_[i] - second_sync_[i]; + if (std::abs(delta_[i]) < 1e-6) { + desired[i] = 0; + finished[i] = true; + } else if (time < first_sync_[i]) { + desired[i] = -max_velocity_sync_[i] * sign / std::pow(first_sync_[i], 3) * + (0.5 * time - first_sync_[i]) * std::pow(time, 3); + } else if (time < second_sync_[i]) { + desired[i] = first_position_[i] + (time - first_sync_[i]) * max_velocity_sync_[i] * sign; + } else if (time < finish_sync_[i]) { + desired[i] = delta_[i] + 0.5 * + (1.0 / std::pow(goal_ramp_duration, 3) * + (time - first_sync_[i] - 2 * goal_ramp_duration - constant_duration) * + std::pow(time - first_sync_[i] - constant_duration, 3) + + (2 * time - 2 * first_sync_[i] - goal_ramp_duration - 2 * constant_duration)) * + max_velocity_sync_[i] * sign; + } else { + desired[i] = delta_[i]; + finished[i] = true; + } + } + return std::all_of(finished.begin(), finished.end(), [](bool value) { return value; }); + } + + void synchronize() { + std::array reachable_velocity = max_velocity_; + std::array finish{}; + for (size_t i = 0; i < 7; ++i) { + const int sign = delta_[i] < 0 ? -1 : (delta_[i] > 0 ? 1 : 0); + if (std::abs(delta_[i]) <= 1e-6) continue; + const double threshold = 0.75 * std::pow(max_velocity_[i], 2) / max_start_acceleration_[i] + + 0.75 * std::pow(max_velocity_[i], 2) / max_goal_acceleration_[i]; + if (std::abs(delta_[i]) < threshold) { + reachable_velocity[i] = std::sqrt(4.0 / 3.0 * delta_[i] * sign * + max_start_acceleration_[i] * max_goal_acceleration_[i] / + (max_start_acceleration_[i] + max_goal_acceleration_[i])); + } + const double first = 1.5 * reachable_velocity[i] / max_start_acceleration_[i]; + const double goal_ramp = 1.5 * reachable_velocity[i] / max_goal_acceleration_[i]; + finish[i] = first / 2 + goal_ramp / 2 + std::abs(delta_[i]) / reachable_velocity[i]; + } + const double synchronized_finish = *std::max_element(finish.begin(), finish.end()); + for (size_t i = 0; i < 7; ++i) { + if (std::abs(delta_[i]) <= 1e-6) continue; + const int sign = delta_[i] < 0 ? -1 : 1; + const double a = 0.75 * (max_goal_acceleration_[i] + max_start_acceleration_[i]); + const double b = -synchronized_finish * max_goal_acceleration_[i] * max_start_acceleration_[i]; + const double c = std::abs(delta_[i]) * max_goal_acceleration_[i] * max_start_acceleration_[i]; + max_velocity_sync_[i] = (-b - std::sqrt(std::max(0.0, b * b - 4 * a * c))) / (2 * a); + first_sync_[i] = 1.5 * max_velocity_sync_[i] / max_start_acceleration_[i]; + const double goal_ramp = 1.5 * max_velocity_sync_[i] / max_goal_acceleration_[i]; + finish_sync_[i] = first_sync_[i] / 2 + goal_ramp / 2 + std::abs(delta_[i] / max_velocity_sync_[i]); + second_sync_[i] = finish_sync_[i] - goal_ramp; + first_position_[i] = max_velocity_sync_[i] * sign * 0.5 * first_sync_[i]; + } + } + + const std::array goal_; + std::array start_{}; + std::array delta_{}; + std::array max_velocity_sync_{}; + std::array first_sync_{}; + std::array second_sync_{}; + std::array finish_sync_{}; + std::array first_position_{}; + double time_ = 0; + double settle_time_ = 0; + bool initialized_ = false; + std::array max_velocity_{2, 2, 2, 2, 2.5, 2.5, 2.5}; + std::array max_start_acceleration_{5, 5, 5, 5, 5, 5, 5}; + std::array max_goal_acceleration_{5, 5, 5, 5, 5, 5, 5}; +}; + +void configure_collision_behavior(franka::Robot& robot) { + robot.setCollisionBehavior( + {{40, 40, 40, 40, 40, 40, 40}}, {{40, 40, 40, 40, 40, 40, 40}}, + {{40, 40, 40, 40, 40, 40, 40}}, {{40, 40, 40, 40, 40, 40, 40}}, + {{40, 40, 40, 40, 40, 40}}, {{40, 40, 40, 40, 40, 40}}, + {{40, 40, 40, 40, 40, 40}}, {{40, 40, 40, 40, 40, 40}}); +} + +} // namespace + +struct DriverFR3::Impl { + std::unique_ptr robot; +}; + +DriverFR3::DriverFR3(Device* device, const CommandLineArgs& cla) + : Driver(device, cla), cla_(cla), impl_(std::make_unique()), controller_({}, limits_), + reset_pose_(parse_array<7>(cla.fr3_reset_pose, "FR3 reset pose")) {} + +DriverFR3::~DriverFR3() { close(); } + +ReturnCode DriverFR3::initialize_controller_state() { + const auto initial = impl_->robot->readOnce(); + { + std::lock_guard lock(pending_controller_mutex_); + controller_.hold(initial.q); + target_pending_ = false; + hold_pending_ = false; + } + { + std::lock_guard lock(state_mutex_); + state_.q = initial.q; + state_.dq = initial.dq; + state_.torque = initial.tau_J; + state_.commanded_q = initial.q; + state_.valid = true; + state_.faulted = false; + state_.fault.clear(); + } + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFR3::open(int baud_rate) { + (void)baud_rate; + try { + impl_->robot = std::make_unique(cla_.fr3_address, franka::RealtimeConfig::kIgnore); + configure_collision_behavior(*impl_->robot); + initialize_controller_state(); + const ReturnCode result = start_controller(); + if (result != ReturnCode::SUCCESS) { + PI_ERROR("FR3 controller failed before its first control cycle"); + impl_->robot.reset(); + } + return result; + } catch (const std::exception& error) { + PI_ERROR("Failed to start FR3: %s", error.what()); + return ReturnCode::FAIL; + } +} + +ReturnCode DriverFR3::start_controller() { + { + std::lock_guard lock(controller_lifecycle_mutex_); + controller_started_ = false; + controller_exited_ = false; + } + stop_requested_ = false; + running_ = true; + try { + thread_ = std::thread(&DriverFR3::run_controller, this); + } catch (...) { + running_ = false; + { + std::lock_guard lock(controller_lifecycle_mutex_); + controller_exited_ = true; + } + controller_lifecycle_cv_.notify_all(); + throw; + } + + // Do not expose a controller as ready until libfranka has invoked its first + // control callback. This guarantees that a subsequent close() cannot send + // stop() before the motion it intends to stop has actually begun. + std::unique_lock lock(controller_lifecycle_mutex_); + controller_lifecycle_cv_.wait(lock, [this] { + return controller_started_ || controller_exited_; + }); + const bool started = controller_started_; + lock.unlock(); + + if (!started && thread_.joinable()) { + thread_.join(); + } + return started ? ReturnCode::SUCCESS : ReturnCode::FAIL; +} + +ReturnCode DriverFR3::close() { + running_ = false; + stop_requested_ = true; + if (impl_->robot && thread_.joinable()) { + try { + impl_->robot->stop(); + } catch (const std::exception& error) { + PI_WARN("FR3 stop failed: %s", error.what()); + } + } + if (thread_.joinable()) thread_.join(); + impl_->robot.reset(); + return ReturnCode::SUCCESS; +} + +FR3DriverState DriverFR3::state() const { + std::lock_guard lock(state_mutex_); + return state_; +} + +ReturnCode DriverFR3::set_target(const std::array& target) { + for (size_t i = 0; i < target.size(); ++i) { + if (!std::isfinite(target[i]) || target[i] < limits_.joint_lower[i] || target[i] > limits_.joint_upper[i]) { + return ReturnCode::INVALID_PARAM; + } + } + std::lock_guard lock(pending_controller_mutex_); + pending_target_ = target; + target_pending_ = true; + hold_pending_ = false; + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFR3::hold() { + if (!state().valid) return ReturnCode::NOT_INITIALIZED; + std::lock_guard lock(pending_controller_mutex_); + hold_pending_ = true; + target_pending_ = false; + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFR3::move_to_ready() { + close(); + try { + impl_->robot = std::make_unique(cla_.fr3_address, franka::RealtimeConfig::kIgnore); + impl_->robot->automaticErrorRecovery(); + configure_collision_behavior(*impl_->robot); + ResetMotionGenerator motion_generator(0.2, reset_pose_); + impl_->robot->control([&](const franka::RobotState& state, franka::Duration period) { + return motion_generator(state, period); + }); + initialize_controller_state(); + const ReturnCode result = start_controller(); + if (result != ReturnCode::SUCCESS) { + PI_ERROR("FR3 controller failed to restart after move-to-ready"); + impl_->robot.reset(); + return ReturnCode::HARDWARE_FAULT; + } + return result; + } catch (const std::exception& error) { + PI_ERROR("FR3 move-to-ready failed: %s", error.what()); + return ReturnCode::HARDWARE_FAULT; + } +} + +void DriverFR3::apply_pending_command() { + std::unique_lock lock(pending_controller_mutex_, std::try_to_lock); + if (!lock.owns_lock()) return; + if (hold_pending_) { + std::array measured{}; + { + std::lock_guard state_lock(state_mutex_); + measured = state_.q; + } + controller_.hold(measured); + hold_pending_ = false; + } else if (target_pending_) { + controller_.set_target(pending_target_); + target_pending_ = false; + } +} + +void DriverFR3::run_controller() { + try { + auto model = impl_->robot->loadModel(); + if (stop_requested_) { + running_ = false; + } else { + impl_->robot->control( + [this, &model](const franka::RobotState& robot_state, franka::Duration) { + bool notify_started = false; + { + std::lock_guard lock(controller_lifecycle_mutex_); + if (!controller_started_) { + controller_started_ = true; + notify_started = true; + } + } + if (notify_started) { + controller_lifecycle_cv_.notify_all(); + } + + // close() normally preempts an active loop with Robot::stop(). If + // stop was requested in the narrow interval before control() took + // ownership, finish the first cycle cooperatively instead. + if (stop_requested_) { + return franka::MotionFinished(franka::Torques(robot_state.tau_J_d)); + } + + FR3ControllerInput input; + input.q = robot_state.q; + input.dq = robot_state.dq; + input.coriolis = model.coriolis(robot_state); + input.flange_jacobian = + model.zeroJacobian(franka::Frame::kFlange, robot_state); + input.end_effector_jacobian = + model.zeroJacobian(franka::Frame::kEndEffector, robot_state); + input.end_effector_position = {robot_state.O_T_EE[12], robot_state.O_T_EE[13], + robot_state.O_T_EE[14]}; + input.elbow_velocity = robot_state.delbow_c[0]; + apply_pending_command(); + const auto torque = controller_.compute(input); + { + std::unique_lock lock(state_mutex_, std::try_to_lock); + if (lock.owns_lock()) { + state_.sequence++; + state_.monotonic_ns = monotonic_ns(); + state_.q = robot_state.q; + state_.dq = robot_state.dq; + state_.torque = robot_state.tau_J; + state_.commanded_q = controller_.commanded_position(); + state_.valid = true; + state_.faulted = false; + } + } + franka::Torques command(torque); + if (stop_requested_) { + return franka::MotionFinished(command); + } + return command; + }, + true, 100.0); + } + } catch (const std::exception& error) { + std::lock_guard lock(state_mutex_); + if (!stop_requested_) { + state_.faulted = true; + state_.fault = error.what(); + PI_ERROR("HARDWARE FAULT: FR3 controller stopped: %s", error.what()); + } + } + running_ = false; + { + std::lock_guard lock(controller_lifecycle_mutex_); + controller_exited_ = true; + } + controller_lifecycle_cv_.notify_all(); +} diff --git a/native/pi_control/src/pi_fr3_controller.cpp b/native/pi_control/src/pi_fr3_controller.cpp new file mode 100644 index 0000000..56aff73 --- /dev/null +++ b/native/pi_control/src/pi_fr3_controller.cpp @@ -0,0 +1,87 @@ +#include "pi_fr3_controller.hpp" + +#include +#include +#include + +FR3Controller::FR3Controller(FR3ControllerGains gains, FR3ControllerLimits limits) + : gains_(gains), limits_(limits) {} + +void FR3Controller::set_target(const std::array& target_position) { + target_position_ = target_position; + initialized_ = true; + position_command_active_ = true; +} + +void FR3Controller::hold(const std::array& measured_position) { + target_position_ = measured_position; + initialized_ = true; + position_command_active_ = false; +} + +void FR3Controller::add_soft_limit(double value, double lower, double upper, double margin, + double stiffness, double& output) { + const double upper_violation = value - upper; + const double lower_violation = lower - value; + if (upper_violation > 0 || lower_violation > 0) { + throw std::runtime_error("FR3 safety hard limit exceeded"); + } + if (upper_violation > -margin) { + output -= stiffness * (margin + upper_violation); + } else if (lower_violation > -margin) { + output += stiffness * (margin + lower_violation); + } +} + +std::array FR3Controller::compute(const FR3ControllerInput& input) { + if (!initialized_) hold(input.q); + + std::array position_error{}; + std::array velocity_error{}; + std::array torque = input.coriolis; + for (size_t i = 0; i < 7; ++i) { + position_error[i] = target_position_[i] - input.q[i]; + velocity_error[i] = -input.dq[i]; + torque[i] += gains_.joint_stiffness[i] * position_error[i] + + gains_.joint_damping[i] * velocity_error[i]; + } + + // Tuned hybrid joint-space impedance term from the FR3 implementation. + if (position_command_active_) { + for (size_t row = 0; row < 6; ++row) { + double cartesian_position_error = 0; + double cartesian_velocity_error = 0; + for (size_t joint = 0; joint < 7; ++joint) { + const double jacobian = input.flange_jacobian[row + 6 * joint]; + cartesian_position_error += jacobian * position_error[joint]; + cartesian_velocity_error += jacobian * velocity_error[joint]; + } + const double wrench = gains_.cartesian_stiffness[row] * cartesian_position_error + + gains_.cartesian_damping[row] * cartesian_velocity_error; + for (size_t joint = 0; joint < 7; ++joint) { + torque[joint] += input.flange_jacobian[row + 6 * joint] * wrench; + } + } + } + + if (std::abs(input.elbow_velocity) > 2.075) { + throw std::runtime_error("FR3 safety elbow velocity hard limit exceeded"); + } + std::array cartesian_force{}; + for (size_t axis = 0; axis < 3; ++axis) { + add_soft_limit(input.end_effector_position[axis], limits_.cartesian_lower[axis], + limits_.cartesian_upper[axis], limits_.cartesian_margin, + limits_.cartesian_stiffness, cartesian_force[axis]); + } + for (size_t joint = 0; joint < 7; ++joint) { + for (size_t axis = 0; axis < 3; ++axis) { + torque[joint] += input.end_effector_jacobian[axis + 6 * joint] * cartesian_force[axis]; + } + add_soft_limit(input.q[joint], limits_.joint_lower[joint], limits_.joint_upper[joint], + limits_.joint_margin, limits_.joint_stiffness, torque[joint]); + add_soft_limit(input.dq[joint], -limits_.velocity[joint], limits_.velocity[joint], + limits_.velocity_margin, limits_.velocity_stiffness, torque[joint]); + torque[joint] = std::clamp(torque[joint], -limits_.torque[joint], limits_.torque[joint]); + } + return torque; +} diff --git a/native/pi_control/src/pi_robotiq.cpp b/native/pi_control/src/pi_robotiq.cpp new file mode 100644 index 0000000..13b1780 --- /dev/null +++ b/native/pi_control/src/pi_robotiq.cpp @@ -0,0 +1,203 @@ +#include "pi_robotiq.hpp" + +#include +#include +#include +#include +#include + +#include + +#include "pi_info.hpp" + +struct RobotiqTransport::Impl { + modbus_t* context = nullptr; +}; + +RobotiqTransport::RobotiqTransport(RobotiqConfig config) + : config_(std::move(config)), impl_(std::make_unique()) {} + +RobotiqTransport::~RobotiqTransport() { stop(); } + +bool RobotiqTransport::start() { + if (config_.endpoint.empty()) return false; + if (config_.backend == RobotiqBackend::RTU) { + impl_->context = modbus_new_rtu(config_.endpoint.c_str(), config_.baud_rate, 'N', 8, 1); + } else { + impl_->context = modbus_new_tcp(config_.endpoint.c_str(), config_.tcp_port); + } + if (!impl_->context) return false; + const uint32_t timeout_seconds = static_cast(config_.response_timeout_ms / 1000); + const uint32_t timeout_microseconds = static_cast(config_.response_timeout_ms % 1000) * 1000U; + if (modbus_set_slave(impl_->context, config_.slave_id) != 0 || + modbus_set_response_timeout(impl_->context, timeout_seconds, timeout_microseconds) != 0 || + modbus_connect(impl_->context) != 0) { + modbus_free(impl_->context); + impl_->context = nullptr; + return false; + } + { + std::lock_guard lock(mutex_); + state_ = {}; + } + running_ = true; + thread_ = std::thread(&RobotiqTransport::run, this); + return true; +} + +void RobotiqTransport::stop() { + if (running_) { + hold(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + running_ = false; + condition_.notify_all(); + if (thread_.joinable()) thread_.join(); + if (impl_->context) { + modbus_close(impl_->context); + modbus_free(impl_->context); + impl_->context = nullptr; + } + std::lock_guard lock(mutex_); + state_.connected = false; +} + +bool RobotiqTransport::activate() { + std::unique_lock lock(mutex_); + if (!running_) return false; + activation_requested_ = true; + condition_.notify_all(); + return condition_.wait_for(lock, std::chrono::seconds(15), [this] { + // Activation-required status codes are expected before gSTA reaches 3. + // Keep polling so those transitional codes do not abort activation. + return state_.activated || !running_; + }) && state_.activated && state_.fault == 0; +} + +void RobotiqTransport::set_target(float position, float speed, float force) { + std::lock_guard lock(mutex_); + target_position_ = std::clamp(position, 0.0f, 1.0f); + target_speed_ = std::clamp(speed, 0.0f, 1.0f); + target_force_ = std::clamp(force, 0.0f, 1.0f); + hold_requested_ = false; + ++command_generation_; + condition_.notify_all(); +} + +void RobotiqTransport::hold() { + std::lock_guard lock(mutex_); + hold_requested_ = true; + condition_.notify_all(); +} + +RobotiqState RobotiqTransport::state() const { + std::lock_guard lock(mutex_); + return state_; +} + +void RobotiqTransport::run() { + auto write_command = [this](uint8_t action, uint8_t position, uint8_t speed, uint8_t force) { + uint16_t registers[3]{static_cast(action << 8), position, + static_cast((speed << 8) | force)}; + return modbus_write_registers(impl_->context, 0x03E8, 3, registers) == 3; + }; + auto fail_transport = [this](const char* operation) { + PI_ERROR("Robotiq Modbus %s failed: %s", operation, modbus_strerror(errno)); + { + std::lock_guard lock(mutex_); + state_.connected = false; + state_.activated = false; + state_.moving = false; + } + running_ = false; + condition_.notify_all(); + }; + uint64_t applied_generation = UINT64_MAX; + uint8_t previous_raw = config_.open_raw; + auto previous_time = std::chrono::steady_clock::now(); + const auto period = std::chrono::milliseconds(1000 / std::max(1, config_.poll_frequency_hz)); + while (running_) { + bool activate = false; + bool hold = false; + float position; + float speed; + float force; + uint64_t generation; + { + std::lock_guard lock(mutex_); + activate = activation_requested_; + activation_requested_ = false; + hold = hold_requested_; + hold_requested_ = false; + position = target_position_; + speed = target_speed_; + force = target_force_; + generation = command_generation_; + } + if (activate) { + if (!write_command(0x00, 0, 0, 0)) { + fail_transport("activation reset write"); + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + if (!write_command(0x01, config_.open_raw, 0xff, 0x96)) { + fail_transport("activation write"); + break; + } + } + + std::array registers{}; + if (modbus_read_registers(impl_->context, 0x07D0, 3, registers.data()) != 3) { + fail_transport("read"); + break; + } + const uint8_t status = registers[0] >> 8; + const uint8_t fault = registers[1] >> 8; + const uint8_t raw = registers[2] >> 8; + const uint8_t current = registers[2] & 0xff; + const bool activated = (status & 0x01) && ((status >> 4) & 0x03) == 0x03; + const bool moving = ((status >> 3) & 0x01) && ((status >> 6) & 0x03) == 0; + const auto now = std::chrono::steady_clock::now(); + const float dt = std::chrono::duration(now - previous_time).count(); + const float normalized_position = raw_to_position(raw, config_.open_raw, config_.closed_raw); + const float previous_position = raw_to_position(previous_raw, config_.open_raw, config_.closed_raw); + { + std::lock_guard lock(mutex_); + state_.connected = true; + state_.activated = activated; + state_.ever_activated = state_.ever_activated || activated; + state_.moving = moving; + state_.position = normalized_position; + state_.velocity = dt > 0 ? (normalized_position - previous_position) / dt : 0; + state_.effort = 0.0f; + state_.current = static_cast(current) * 0.01f; + state_.target = position; + state_.fault = fault; + } + condition_.notify_all(); + previous_raw = raw; + previous_time = now; + + bool write_ok = true; + if (activated && hold) { + write_ok = write_command(0x01, raw, normalized_to_raw(speed), normalized_to_raw(force, true)); + applied_generation = generation; + } else if (activated && generation != applied_generation) { + write_ok = write_command(0x09, + position_to_raw(position, config_.open_raw, config_.closed_raw), + normalized_to_raw(speed), normalized_to_raw(force, true)); + applied_generation = generation; + } else if (activated && position < 0.05f && !moving) { + write_ok = write_command(0x09, config_.closed_raw, normalized_to_raw(speed), + normalized_to_raw(force, true)); + } + if (!write_ok) { + fail_transport("command write"); + break; + } + std::unique_lock lock(mutex_); + condition_.wait_for(lock, period, [this, generation] { + return !running_ || command_generation_ != generation || activation_requested_ || hold_requested_; + }); + } +} diff --git a/native/pi_control/tests/CMakeLists.txt b/native/pi_control/tests/CMakeLists.txt index e3f9aba..33ef168 100644 --- a/native/pi_control/tests/CMakeLists.txt +++ b/native/pi_control/tests/CMakeLists.txt @@ -36,6 +36,8 @@ add_executable(pi_control_tests test_posix_syscall_hooks.cpp test_servo_dm_status.cpp test_driver_base.cpp + test_fr3_controller.cpp + test_robotiq.cpp ${PI_CONTROL_ROOT}/src/can/math_ops.cpp ${PI_CONTROL_ROOT}/src/pi_filter.cpp ${PI_CONTROL_ROOT}/src/pi_stall_detect.cpp @@ -45,6 +47,7 @@ add_executable(pi_control_tests ${PI_CONTROL_ROOT}/src/pi_driver_can.cpp ${PI_CONTROL_ROOT}/src/pi_servo_dm_status.cpp ${PI_CONTROL_ROOT}/src/pi_info.cpp + ${PI_CONTROL_ROOT}/src/pi_fr3_controller.cpp ) target_include_directories(pi_control_tests PRIVATE ${PI_CONTROL_ROOT}/include) target_compile_features(pi_control_tests PRIVATE cxx_std_17) diff --git a/native/pi_control/tests/test_fr3_controller.cpp b/native/pi_control/tests/test_fr3_controller.cpp new file mode 100644 index 0000000..8a88984 --- /dev/null +++ b/native/pi_control/tests/test_fr3_controller.cpp @@ -0,0 +1,38 @@ +#include + +#include "pi_fr3_controller.hpp" + +TEST(FR3Controller, HoldsMeasuredPositionUntilFirstCommand) { + FR3Controller controller; + FR3ControllerInput input; + input.q = {0, -0.6, 0, -2.5, 0, 1.8, 0}; + controller.compute(input); + EXPECT_EQ(controller.commanded_position(), input.q); +} + +TEST(FR3Controller, PositionCommandUsesTunedImpedanceGains) { + FR3Controller controller; + FR3ControllerInput input; + input.q = {0, -0.6, 0, -2.5, 0, 1.8, 0}; + const std::array target{0.1, -0.6, 0, -2.5, 0, 1.8, 0}; + controller.set_target(target); + const auto torque = controller.compute(input); + EXPECT_NEAR(torque[0], 4.0, 1e-9); + EXPECT_EQ(controller.commanded_position(), target); +} + +TEST(FR3Controller, HoldDiscardsPreviousPositionTarget) { + FR3Controller controller; + FR3ControllerInput input; + input.q = {0, -0.6, 0, -2.5, 0, 1.8, 0}; + controller.set_target({0.1, -0.6, 0, -2.5, 0, 1.8, 0}); + controller.hold(input.q); + EXPECT_EQ(controller.commanded_position(), input.q); +} + +TEST(FR3Controller, RejectsHardJointLimitViolations) { + FR3Controller controller; + FR3ControllerInput input; + input.q = {3.0, -0.6, 0, -2.5, 0, 1.8, 0}; + EXPECT_THROW(controller.compute(input), std::runtime_error); +} diff --git a/native/pi_control/tests/test_robotiq.cpp b/native/pi_control/tests/test_robotiq.cpp new file mode 100644 index 0000000..b4eccbe --- /dev/null +++ b/native/pi_control/tests/test_robotiq.cpp @@ -0,0 +1,24 @@ +#include + +#include "pi_robotiq.hpp" + +TEST(RobotiqCalibration, PublicPositionIsZeroClosedOneOpen) { + EXPECT_EQ(RobotiqTransport::position_to_raw(0.0f, 3, 230), 230); + EXPECT_EQ(RobotiqTransport::position_to_raw(1.0f, 3, 230), 3); + EXPECT_FLOAT_EQ(RobotiqTransport::raw_to_position(3, 3, 230), 1.0f); + EXPECT_FLOAT_EQ(RobotiqTransport::raw_to_position(230, 3, 230), 0.0f); +} + +TEST(RobotiqCalibration, PositiveForceNeverDisablesRegrasp) { + EXPECT_EQ(RobotiqTransport::normalized_to_raw(0.001f, true), 1); + EXPECT_EQ(RobotiqTransport::normalized_to_raw(0.0f, true), 0); +} + +TEST(RobotiqStatus, ActivationRequiredIsNotAnOperationalFault) { + RobotiqState state; + state.fault = 0x07; + EXPECT_FALSE(RobotiqTransport::has_operational_fault(state)); + + state.ever_activated = true; + EXPECT_TRUE(RobotiqTransport::has_operational_fault(state)); +} diff --git a/pyproject.toml b/pyproject.toml index 16db0b2..8a0b72d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,8 +4,8 @@ build-backend = "scikit_build_core.build" [project] name = "openpi-control" -version = "0.1.4" -description = "OpenPI native control runtime for YAM and ARX robot arms" +version = "0.1.5" +description = "OpenPI native control runtime for YAM, ARX, and FR3 robot arms" readme = "README.md" requires-python = ">=3.11" authors = [{ name = "OpenPI Control maintainers" }] diff --git a/scripts/build_deps.sh b/scripts/build_deps.sh index 048a5bf..95b194f 100755 --- a/scripts/build_deps.sh +++ b/scripts/build_deps.sh @@ -11,7 +11,7 @@ if [[ "${EUID}" -eq 0 ]]; then exit 2 fi -for command_name in cmake git; do +for command_name in autoreconf cmake git make; do if ! command -v "${command_name}" >/dev/null 2>&1; then echo "Missing ${command_name}; first run sudo ./scripts/install_build_deps_ubuntu.sh" >&2 exit 2 @@ -32,6 +32,12 @@ cppzmq_ref="${CPPZMQ_GIT_REF:-v4.9.0}" trossen_arm_source="${deps_root}/trossen_arm" trossen_arm_prefix="${trossen_arm_source}/install" trossen_arm_ref="${TROSSEN_ARM_GIT_REF:-v1.10.0}" +libfranka_source="${deps_root}/libfranka" +libfranka_prefix="${libfranka_source}/install" +libfranka_ref="${LIBFRANKA_GIT_REF:-0.21.3}" +libmodbus_source="${deps_root}/libmodbus" +libmodbus_prefix="${libmodbus_source}/install" +libmodbus_ref="${LIBMODBUS_GIT_REF:-v3.1.12}" build_jobs="${BUILD_JOBS:-2}" # A cached clone built from a different pinned ref must not be reused @@ -52,6 +58,8 @@ verify_cached_ref() { verify_cached_ref "${pinocchio_source}" "${pinocchio_ref}" verify_cached_ref "${cppzmq_source}" "${cppzmq_ref}" verify_cached_ref "${trossen_arm_source}" "${trossen_arm_ref}" +verify_cached_ref "${libfranka_source}" "${libfranka_ref}" +verify_cached_ref "${libmodbus_source}" "${libmodbus_ref}" if ! [[ "${build_jobs}" =~ ^[1-9][0-9]*$ ]]; then echo "BUILD_JOBS must be a positive integer; got ${build_jobs@Q}" >&2 @@ -97,6 +105,50 @@ else cmake --install "${pinocchio_source}/build" fi +if [[ -f "${libfranka_prefix}/lib/libfranka.a" ]]; then + echo "libfranka ${libfranka_ref} already installed at ${libfranka_prefix}; skipping." +else + if [[ ! -d "${libfranka_source}/.git" ]]; then + git clone --branch "${libfranka_ref}" --depth 1 --recurse-submodules --shallow-submodules \ + https://github.com/frankarobotics/libfranka.git "${libfranka_source}" + fi + + libfranka_patch="${repo_root}/scripts/patches/libfranka-0.21.3-static.patch" + if git -C "${libfranka_source}" apply --check "${libfranka_patch}"; then + git -C "${libfranka_source}" apply "${libfranka_patch}" + elif ! git -C "${libfranka_source}" apply --reverse --check "${libfranka_patch}"; then + echo "The pinned libfranka source does not accept ${libfranka_patch}." >&2 + exit 1 + fi + + cmake -S "${libfranka_source}" -B "${libfranka_source}/build" \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX="${libfranka_prefix}" \ + -DCMAKE_PREFIX_PATH="${pinocchio_prefix}" \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_TESTS=OFF \ + -DGENERATE_PYLIBFRANKA=OFF + cmake --build "${libfranka_source}/build" --parallel "${build_jobs}" + cmake --install "${libfranka_source}/build" +fi + +if [[ -f "${libmodbus_prefix}/lib/libmodbus.a" ]]; then + echo "libmodbus ${libmodbus_ref} already installed at ${libmodbus_prefix}; skipping." +else + if [[ ! -d "${libmodbus_source}/.git" ]]; then + git clone --branch "${libmodbus_ref}" --depth 1 \ + https://github.com/stephane/libmodbus.git "${libmodbus_source}" + fi + + ( + cd "${libmodbus_source}" + ./autogen.sh + ./configure --prefix="${libmodbus_prefix}" --enable-static --disable-shared + make -j"${build_jobs}" + make install + ) +fi + if [[ -f "${trossen_arm_prefix}/lib/libtrossen_arm.a" ]]; then echo "libtrossen_arm ${trossen_arm_ref} already installed at ${trossen_arm_prefix}; skipping." else @@ -126,6 +178,14 @@ if [[ ! -f "${trossen_arm_prefix}/lib/libtrossen_arm.a" ]]; then echo "libtrossen_arm installation is missing libtrossen_arm.a: ${trossen_arm_prefix}" >&2 exit 1 fi +if [[ ! -f "${libfranka_prefix}/lib/libfranka.a" ]]; then + echo "libfranka installation is missing libfranka.a: ${libfranka_prefix}" >&2 + exit 1 +fi +if [[ ! -f "${libmodbus_prefix}/lib/libmodbus.a" ]]; then + echo "libmodbus installation is missing libmodbus.a: ${libmodbus_prefix}" >&2 + exit 1 +fi cat < None: ) -ArmConnection = SocketCanConnection | EthernetConnection | SerialConnection +@dataclass(frozen=True, slots=True) +class FR3Connection: + """A Franka Emika FR3 controller at a fixed IPv4 address.""" + + address: str + reset_pose_rad: tuple[float, ...] = ( + 0.0, + -0.6283185307, + 0.0, + -2.5132741229, + 0.0, + 1.8849555922, + 0.0, + ) + + def __post_init__(self) -> None: + try: + ipaddress.IPv4Address(self.address) + except ValueError as err: + raise ConfigurationError(f"invalid FR3 IPv4 address {self.address!r}") from err + pose = tuple(float(value) for value in self.reset_pose_rad) + if len(pose) != 7 or any(not math.isfinite(value) for value in pose): + raise ConfigurationError("FR3 reset_pose_rad must contain seven finite joint positions") + object.__setattr__(self, "reset_pose_rad", pose) + + +class RobotiqTransport(StrEnum): + RTU = "rtu" + TCP = "tcp" + + +@dataclass(frozen=True, slots=True) +class RobotiqConnection: + """Robotiq 2F gripper using true Modbus RTU or Modbus TCP.""" + + transport: RobotiqTransport + endpoint: str + port: int = 502 + baud_rate: int = 115200 + slave_id: int = 9 + poll_frequency_hz: int = 50 + timeout_s: float = 0.2 + open_position_raw: int = 3 + closed_position_raw: int = 230 + default_speed: float = 1.0 + default_force: float = 1.0 + + @classmethod + def rtu(cls, device: str, **kwargs: object) -> RobotiqConnection: + return cls(RobotiqTransport.RTU, device, **kwargs) + + @classmethod + def tcp(cls, address: str, *, port: int = 502, **kwargs: object) -> RobotiqConnection: + return cls(RobotiqTransport.TCP, address, port=port, **kwargs) + + def __post_init__(self) -> None: + try: + transport = RobotiqTransport(self.transport) + except ValueError as err: + raise ConfigurationError("Robotiq transport must be 'rtu' or 'tcp'") from err + object.__setattr__(self, "transport", transport) + if transport is RobotiqTransport.RTU: + if not self.endpoint.startswith("/dev/"): + raise ConfigurationError("Robotiq RTU endpoint must be a /dev path") + else: + try: + ipaddress.IPv4Address(self.endpoint) + except ValueError as err: + raise ConfigurationError( + f"invalid Robotiq TCP IPv4 address {self.endpoint!r}" + ) from err + if not 1 <= self.port <= 65535: + raise ConfigurationError("Robotiq TCP port must be between 1 and 65535") + if self.baud_rate <= 0 or not 0 <= self.slave_id <= 247: + raise ConfigurationError("Robotiq baud_rate and slave_id are invalid") + if self.poll_frequency_hz <= 0 or self.timeout_s <= 0: + raise ConfigurationError("Robotiq polling frequency and timeout must be positive") + if not 0 <= self.open_position_raw < self.closed_position_raw <= 255: + raise ConfigurationError("Robotiq raw positions must satisfy 0 <= open < closed <= 255") + for name in ("default_speed", "default_force"): + value = getattr(self, name) + if not math.isfinite(value) or not 0.0 <= value <= 1.0: + raise ConfigurationError(f"Robotiq {name} must be in [0, 1]") + + +ArmConnection = SocketCanConnection | EthernetConnection | SerialConnection | FR3Connection def connection_for_interface(interface: str) -> ArmConnection: @@ -139,7 +227,7 @@ def has_inputs(self) -> bool: class ResolvedArmAssets: model_config: Path instance_config: Path - urdf: Path + urdf: Path | None effector_model_config: Path | None effector_instance_config: Path | None @@ -166,14 +254,16 @@ def resolve_model_assets( arm_dir = root / "arms" / model model_config = arm_dir / f"{model}.json" instance = instance_config or arm_dir / f"{model}_01.json" - resolved_urdf = urdf or arm_dir / f"{model}.urdf" + resolved_urdf = urdf or (None if model == "FR3" else arm_dir / f"{model}.urdf") eff_model: Path | None = None eff_instance: Path | None = None if effector_model: eff_dir = root / "effectors" / effector_model eff_model = eff_dir / f"{effector_model}.json" eff_instance = effector_instance_config or eff_dir / f"{effector_model}_01.json" - required = [model_config, Path(instance), Path(resolved_urdf)] + required = [model_config, Path(instance)] + if resolved_urdf is not None: + required.append(Path(resolved_urdf)) if eff_model is not None and eff_instance is not None: required.extend([eff_model, Path(eff_instance)]) missing = [str(path) for path in required if not path.is_file()] @@ -182,7 +272,7 @@ def resolve_model_assets( return ResolvedArmAssets( model_config=model_config, instance_config=Path(instance), - urdf=Path(resolved_urdf), + urdf=Path(resolved_urdf) if resolved_urdf is not None else None, effector_model_config=eff_model, effector_instance_config=Path(eff_instance) if eff_instance else None, ) @@ -197,6 +287,7 @@ class ArmConfig: connection: ArmConnection instance_config: Path | None = None effector_model: str | None = None + effector_connection: RobotiqConnection | None = None effector_instance_config: Path | None = None urdf: Path | None = None # First contact after the arm has sat idle can exceed a minute of native @@ -238,6 +329,17 @@ def __post_init__(self) -> None: f"unsupported effector {self.effector_model!r}; supported effectors: " f"{', '.join(SUPPORTED_EFFECTORS)}" ) + if self.model == "FR3": + if not isinstance(self.connection, FR3Connection): + raise ConfigurationError("FR3 requires an FR3Connection") + if self.effector_model != "Robotiq" or self.effector_connection is None: + raise ConfigurationError("FR3 requires a Robotiq effector and connection") + elif isinstance(self.connection, FR3Connection): + raise ConfigurationError("FR3Connection can only be used with the FR3 model") + if self.effector_model == "Robotiq" and self.model != "FR3": + raise ConfigurationError("the Robotiq effector is only supported on FR3") + if self.effector_connection is not None and self.effector_model != "Robotiq": + raise ConfigurationError("effector_connection is only valid for a Robotiq effector") if self.connect_timeout_s <= 0: raise ConfigurationError("connect_timeout_s must be positive") if self.leader_gravity_compensation is None: diff --git a/src/openpi_control/models/arms/FR3/FR3.json b/src/openpi_control/models/arms/FR3/FR3.json new file mode 100644 index 0000000..1f7c74e --- /dev/null +++ b/src/openpi_control/models/arms/FR3/FR3.json @@ -0,0 +1,26 @@ +{ + "config_version": "1.3.1", + "device_model": "FR3", + "device_type": "arm", + "arm_type": "fr3", + "topic_type": "ZMQ", + "driver_type": "FR3", + "algo_type": "None", + "planning_type": "None", + "read_only": false, + "catalog": { + "display_label": "FR3: Franka Emika FR3", + "order": 9, + "default_effector_model": "Robotiq", + "port_type": "Ethernet" + }, + "joints": [ + {"joint_id": 0, "joint_name": "fr3_joint1"}, + {"joint_id": 1, "joint_name": "fr3_joint2"}, + {"joint_id": 2, "joint_name": "fr3_joint3"}, + {"joint_id": 3, "joint_name": "fr3_joint4"}, + {"joint_id": 4, "joint_name": "fr3_joint5"}, + {"joint_id": 5, "joint_name": "fr3_joint6"}, + {"joint_id": 6, "joint_name": "fr3_joint7"} + ] +} diff --git a/src/openpi_control/models/arms/FR3/FR3_01.json b/src/openpi_control/models/arms/FR3/FR3_01.json new file mode 100644 index 0000000..e23fe0f --- /dev/null +++ b/src/openpi_control/models/arms/FR3/FR3_01.json @@ -0,0 +1,11 @@ +{ + "config_version": "1.3.1", + "device_model": "FR3", + "device_type": "arm", + "arm_type": "fr3", + "spring_effect": false, + "gravity_compensation": true, + "follower_gravity_compensation": true, + "base_rpy": [0, 0, 0], + "reset_pose_rad": [0, -0.785398, 0, -2.356194, 0, 1.570796, 0.785398] +} diff --git a/src/openpi_control/models/effectors/Robotiq/Robotiq.json b/src/openpi_control/models/effectors/Robotiq/Robotiq.json new file mode 100644 index 0000000..693ab1d --- /dev/null +++ b/src/openpi_control/models/effectors/Robotiq/Robotiq.json @@ -0,0 +1,15 @@ +{ + "config_version": "1.3.1", + "device_model": "Robotiq", + "device_type": "effector", + "effector_type": "modbus", + "topic_type": "ZMQ", + "driver_type": "MODBUS", + "algo_type": "None", + "planning_type": "None", + "catalog": { + "display_label": "Robotiq 2F (Modbus RTU or TCP)", + "order": 9 + }, + "joints": [{"joint_id": 7, "joint_name": "robotiq_gripper"}] +} diff --git a/src/openpi_control/models/effectors/Robotiq/Robotiq_01.json b/src/openpi_control/models/effectors/Robotiq/Robotiq_01.json new file mode 100644 index 0000000..4b29ae4 --- /dev/null +++ b/src/openpi_control/models/effectors/Robotiq/Robotiq_01.json @@ -0,0 +1,9 @@ +{ + "config_version": "1.3.1", + "device_model": "Robotiq", + "device_type": "effector", + "effector_type": "modbus", + "control_mode": "position", + "open_position": 1.0, + "closed_position": 0.0 +} diff --git a/src/openpi_control/native.py b/src/openpi_control/native.py index b03fc53..2008e31 100644 --- a/src/openpi_control/native.py +++ b/src/openpi_control/native.py @@ -25,8 +25,10 @@ ArmConfig, ArmConnection, EthernetConnection, + FR3Connection, InputLayout, ResolvedArmAssets, + RobotiqTransport, SerialConnection, ) from .exceptions import ( @@ -117,6 +119,9 @@ def native_executable() -> Path: def validate_connection(connection: ArmConnection) -> None: + if isinstance(connection, FR3Connection): + # libfranka performs the authoritative protocol/firmware handshake. + return if isinstance(connection, EthernetConnection): if not trossen_eth.reachable(connection.ip): raise ConnectionUnavailableError( @@ -328,6 +333,17 @@ def connect(self, config: ArmConfig, role: ArmRole, topics: ArmTopics) -> ArmCap "physical native control is supported on Linux only" ) validate_connection(config.connection) + if isinstance(config.connection, FR3Connection): + if role is not ArmRole.FOLLOWER: + raise ConfigurationError("FR3 is supported as a follower only") + if config.effector_connection is None: + raise ConfigurationError("FR3 requires a Robotiq effector connection") + if config.effector_connection.transport is RobotiqTransport.RTU: + gripper_device = Path(config.effector_connection.endpoint) + if not gripper_device.exists(): + raise ConnectionUnavailableError( + f"Robotiq serial device {str(gripper_device)!r} does not exist" + ) if role is ArmRole.FOLLOWER and config.is_read_only(): raise ConfigurationError( f"model {config.model!r} is read-only (leader-only, no actuation) " @@ -373,7 +389,9 @@ def _connect_prepared( # node a merged URDF whose end link inertial is replaced with the effector # mass model (zero mass when no effector). A caller-supplied URDF is # trusted as-is and skips merging. - if config.urdf is None: + if isinstance(config.connection, FR3Connection): + urdf_path = None + elif config.urdf is None: urdf_path = prepare_merged_urdf( assets, model=config.model, effector_model=config.effector_model ) @@ -389,7 +407,38 @@ def _connect_prepared( # The native node takes the bus identity as an opaque string: a SocketCAN # interface name, the controller IPv4 address for Ethernet drivers, or # the tty device path for serial buses (which also need the catalog baud). - if isinstance(config.connection, EthernetConnection): + if isinstance(config.connection, FR3Connection): + assert config.effector_connection is not None + gripper = config.effector_connection + connection_args = [ + "--fr3_address", + config.connection.address, + "--fr3_reset_pose", + ",".join(f"{value:g}" for value in config.connection.reset_pose_rad), + "--robotiq_transport", + gripper.transport.value, + "--robotiq_endpoint", + gripper.endpoint, + "--robotiq_port", + str(gripper.port), + "--robotiq_baud_rate", + str(gripper.baud_rate), + "--robotiq_slave_id", + str(gripper.slave_id), + "--robotiq_poll_frequency", + str(gripper.poll_frequency_hz), + "--robotiq_timeout_ms", + str(round(gripper.timeout_s * 1000)), + "--robotiq_min_position_raw", + str(gripper.open_position_raw), + "--robotiq_max_position_raw", + str(gripper.closed_position_raw), + "--robotiq_default_speed", + f"{gripper.default_speed:g}", + "--robotiq_default_force", + f"{gripper.default_force:g}", + ] + elif isinstance(config.connection, EthernetConnection): connection_args = ["--control_port", config.connection.ip] elif isinstance(config.connection, SerialConnection): connection_args = [ @@ -422,7 +471,7 @@ def _connect_prepared( # which this node does not ship; force the Pinocchio implementation. # Effector configs declaring "Algo" keep priority over this flag. "--algo_type", - "Pinocchio", + "None" if isinstance(config.connection, FR3Connection) else "Pinocchio", "--topic_state", topics.state, "--topic_live_command", @@ -437,8 +486,6 @@ def _connect_prepared( str(assets.model_config), "--arm_instance_config", str(assets.instance_config), - "--urdf_path", - str(urdf_path), "--force_feedback", "-1", # Connect is passive: the arm holds its current pose instead of @@ -447,6 +494,8 @@ def _connect_prepared( "--dont_go_to_home_pos", *connection_args, ] + if urdf_path is not None: + args.extend(["--urdf_path", str(urdf_path)]) if self._paired_follower_state_topic: args.extend(["--paired_follower_state_topic", self._paired_follower_state_topic]) if config.torq_rescale is not None: diff --git a/src/openpi_control/urdf_inertial.py b/src/openpi_control/urdf_inertial.py index fbd341e..451b9dd 100644 --- a/src/openpi_control/urdf_inertial.py +++ b/src/openpi_control/urdf_inertial.py @@ -53,6 +53,8 @@ def prepare_merged_urdf( ConfigurationError: If the effector mass model is missing or malformed, or the base URDF has no end link. """ + if assets.urdf is None: + raise ConfigurationError(f"model {model!r} does not use a URDF") base_text = assets.urdf.read_text(encoding="utf-8") if effector_model: if assets.effector_model_config is None: diff --git a/tests/test_config.py b/tests/test_config.py index 6bc7945..b5d0835 100644 --- a/tests/test_config.py +++ b/tests/test_config.py @@ -7,7 +7,9 @@ ArmConfig, ConfigurationError, EthernetConnection, + FR3Connection, InputLayout, + RobotiqConnection, SafetyLimits, SerialConnection, SocketCanConnection, @@ -19,18 +21,56 @@ @pytest.mark.parametrize("model", SUPPORTED_MODELS) def test_physical_model_catalog_is_complete(model: str) -> None: - config = ArmConfig("arm", model, SocketCanConnection("test")) + config = ( + ArmConfig( + "arm", + "FR3", + FR3Connection("192.168.1.10"), + effector_model="Robotiq", + effector_connection=RobotiqConnection.tcp("192.168.1.11"), + ) + if model == "FR3" + else ArmConfig("arm", model, SocketCanConnection("test")) + ) assets = config.resolve_assets() assert assets.model_config.is_file() assert assets.instance_config.is_file() - assert assets.urdf.is_file() + if model == "FR3": + assert assets.urdf is None + else: + assert assets.urdf is not None and assets.urdf.is_file() # SO101 (SO-ARM100/101) is the catalog's only 5-DOF arm; everything else is 6-DOF. - assert len(config.joint_names()) == (5 if model == "SO101" else 6) + assert len(config.joint_names()) == (7 if model == "FR3" else 5 if model == "SO101" else 6) # The native node is always launched with --algo_type Pinocchio, which overrides # any config value except "Algo" (config keeps priority for "Algo"). Arm configs # must therefore never declare "Algo": robot-test 1.1.1 configs say "KDL" # (overridden to Pinocchio), openpi-tuned configs say "Pinocchio" directly. - assert json.loads(assets.model_config.read_text())["algo_type"] in ("Pinocchio", "KDL") + expected_algos = ("None",) if model == "FR3" else ("Pinocchio", "KDL") + assert json.loads(assets.model_config.read_text())["algo_type"] in expected_algos + + +def test_fr3_requires_typed_arm_and_gripper_connections() -> None: + gripper = RobotiqConnection.rtu("/dev/serial/by-id/usb-robotiq") + config = ArmConfig( + "follower", + "FR3", + FR3Connection("192.168.1.10"), + effector_model="Robotiq", + effector_connection=gripper, + ) + assert config.joint_names() == tuple(f"fr3_joint{i}" for i in range(1, 8)) + assert gripper.endpoint.startswith("/dev/serial/by-id/") + with pytest.raises(ConfigurationError, match="FR3 requires"): + ArmConfig("follower", "FR3", SocketCanConnection("can0")) + + +def test_robotiq_supports_true_rtu_and_tcp_endpoints() -> None: + rtu = RobotiqConnection.rtu("/dev/ttyUSB0", baud_rate=115200) + tcp = RobotiqConnection.tcp("192.168.1.11", port=502) + assert rtu.transport.value == "rtu" + assert tcp.transport.value == "tcp" + with pytest.raises(ConfigurationError, match="/dev path"): + RobotiqConnection.rtu("192.168.1.11") def test_connection_for_interface_dispatches_on_the_interface_form() -> None: diff --git a/tests/test_native_backend.py b/tests/test_native_backend.py index b5ad54f..cc058db 100644 --- a/tests/test_native_backend.py +++ b/tests/test_native_backend.py @@ -25,8 +25,10 @@ ArmMode, ArmRole, ArmSession, + FR3Connection, InputLayout, PositionCommand, + RobotiqConnection, SafetyLimits, SocketCanConnection, ) @@ -155,6 +157,60 @@ def make_backend_config() -> ArmConfig: return ArmConfig("native-test", "Yam", SocketCanConnection("lo")) +@pytest.mark.parametrize( + ("gripper", "expected_transport", "expected_endpoint"), + [ + (RobotiqConnection.rtu("/dev/null"), "rtu", "/dev/null"), + (RobotiqConnection.tcp("192.168.1.12", port=1502), "tcp", "192.168.1.12"), + ], +) +def test_native_backend_forwards_fr3_and_robotiq_arguments( + monkeypatch, + gripper: RobotiqConnection, + expected_transport: str, + expected_endpoint: str, +) -> None: + captured_args: list[str] = [] + backend = NativeArmBackend() + config = ArmConfig( + "fr3-follower", + "FR3", + FR3Connection("192.168.1.10"), + effector_model="Robotiq", + effector_connection=gripper, + ) + + def capture_spawn(args, **_kwargs): + captured_args.extend(args) + raise RuntimeError("captured native arguments") + + monkeypatch.setattr("openpi_control.native.platform.system", lambda: "Linux") + monkeypatch.setattr("openpi_control.native.native_executable", lambda: Path(sys.executable)) + monkeypatch.setattr("openpi_control.native.subprocess.Popen", capture_spawn) + + try: + with pytest.raises(RuntimeError, match="captured native arguments"): + backend.connect( + config, + ArmRole.FOLLOWER, + topics_for(f"fr3-{expected_transport}", "fr3-follower"), + ) + finally: + backend.close() + + def value_after(option: str) -> str: + return captured_args[captured_args.index(option) + 1] + + assert value_after("--device_model") == "FR3" + assert value_after("--fr3_address") == "192.168.1.10" + assert value_after("--robotiq_transport") == expected_transport + assert value_after("--robotiq_endpoint") == expected_endpoint + assert value_after("--robotiq_port") == ("1502" if expected_transport == "tcp" else "502") + assert value_after("--algo_type") == "None" + assert "--control_port" not in captured_args + assert "--urdf_path" not in captured_args + + @pytest.mark.parametrize("safety_torque_mode", [False, True]) def test_native_backend_forwards_safety_torque_mode_only_when_enabled( monkeypatch, safety_torque_mode: bool diff --git a/uv.lock b/uv.lock index a49b250..0694452 100644 --- a/uv.lock +++ b/uv.lock @@ -333,7 +333,7 @@ wheels = [ [[package]] name = "openpi-control" -version = "0.1.4" +version = "0.1.5" source = { editable = "." } dependencies = [ { name = "numpy", version = "2.4.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.12'" },