diff --git a/CMakeLists.txt b/CMakeLists.txt index 00ac8ab..83225ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,8 @@ set(OPENPI_CONTROL_PINOCCHIO_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/pinocchio/instal "Pinocchio installation prefix used by openpi-control") set(OPENPI_CONTROL_CPPZMQ_PREFIX "${OPENPI_CONTROL_DEPS_DIR}/cppzmq/install" CACHE PATH "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") if(EXISTS "${OPENPI_CONTROL_PINOCCHIO_PREFIX}/lib/pkgconfig/pinocchio.pc") list(PREPEND CMAKE_PREFIX_PATH "${OPENPI_CONTROL_PINOCCHIO_PREFIX}") @@ -37,6 +39,10 @@ if(EXISTS "${OPENPI_CONTROL_CPPZMQ_PREFIX}/include/zmq.hpp") set(OPENPI_CONTROL_CPPZMQ_INCLUDE_DIR "${OPENPI_CONTROL_CPPZMQ_PREFIX}/include") endif() +if(EXISTS "${OPENPI_CONTROL_TROSSEN_PREFIX}/lib/libtrossen_arm.a") + list(PREPEND CMAKE_PREFIX_PATH "${OPENPI_CONTROL_TROSSEN_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/native/pi_control/CMakeLists.txt b/native/pi_control/CMakeLists.txt index 2651270..8b98965 100644 --- a/native/pi_control/CMakeLists.txt +++ b/native/pi_control/CMakeLists.txt @@ -37,6 +37,12 @@ find_package(Eigen3 REQUIRED) pkg_check_modules(PC_ZMQ REQUIRED libzmq) pkg_check_modules(PINOCCHIO REQUIRED pinocchio) +# Trossen iNerve controller SDK (DriverTrossen). Provisioned by +# scripts/build_deps.sh from the pinned TrossenRobotics/trossen_arm release +# (prebuilt static lib + headers); provides the imported STATIC target +# `libtrossen_arm`. +find_package(libtrossen_arm REQUIRED) + # Let CMake's runtime dependency scanner resolve local build dependencies # without hard-coding those directories into the installed executable. set(CMAKE_BUILD_RPATH ${PINOCCHIO_LIBRARY_DIRS} ${PC_ZMQ_LIBRARY_DIRS}) @@ -73,16 +79,27 @@ set(COMMON_SOURCES src/pi_driver_arx.cpp src/pi_driver_arx_encoder.cpp src/pi_driver_can.cpp + src/pi_driver_controller.cpp + src/pi_driver_trossen.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 + src/pi_driver_ft.cpp + src/pi_servo_ft.cpp src/pi_joint.cpp src/pi_servo.cpp + src/pi_servo_controller.cpp src/pi_servo_dm.cpp src/pi_servo_dm_status.cpp src/pi_servo_can_encoder.cpp src/pi_device.cpp src/pi_device_arm.cpp src/pi_device_arm_arx.cpp + src/pi_device_arm_nello.cpp src/pi_device_effector.cpp src/pi_device_effector_arx.cpp + src/pi_device_effector_controller.cpp + src/pi_device_effector_nello.cpp src/pi_device_config.cpp src/pi_algo.cpp src/pi_algo_pino.cpp @@ -95,12 +112,31 @@ set(COMMON_SOURCES ) +# --------------------------------------------------------------------------- +# libpi_trossen_shim: isolation boundary around the prebuilt libtrossen_arm.a. +# The vendor archive statically bundles its own pinocchio/urdfdom/tinyxml2; +# linking it into the node directly makes those symbols interpose with the +# node's shared pinocchio and mixes two pinocchio builds in one process +# (SIGSEGV in ModelTpl::addJoint during TrossenArmDriver::configure). The +# archive is therefore linked into this dedicated shared library and its +# symbols are hidden from the dynamic symbol table, so only the +# TrossenArmShim API (compiled here, not from the archive) is exported. +# --------------------------------------------------------------------------- +add_library(pi_trossen_shim SHARED src/pi_trossen_shim.cpp) +target_include_directories(pi_trossen_shim PRIVATE include) +target_link_libraries(pi_trossen_shim PRIVATE libtrossen_arm pthread) +# Mark every symbol pulled from static archives (i.e. libtrossen_arm.a and +# its bundled third-party copies) as LOCAL in the .so's dynamic symbol +# table. Internal references bind at link time; nothing leaks out. +target_link_options(pi_trossen_shim PRIVATE "LINKER:--exclude-libs,ALL") + add_executable(${OPENPI_CONTROL_TARGET} ${COMMON_SOURCES}) target_link_libraries(${OPENPI_CONTROL_TARGET} ${PC_ZMQ_LDFLAGS} ${PINOCCHIO_LDFLAGS} Boost::program_options + pi_trossen_shim ) # Use a transitive DT_RPATH instead of DT_RUNPATH. Libraries copied beside the @@ -129,6 +165,7 @@ if(OPENPI_CONTROL_BUILD_TESTING) ${PC_ZMQ_LDFLAGS} ${PINOCCHIO_LDFLAGS} Boost::program_options + pi_trossen_shim ) gtest_discover_tests( pi_topic_zmq_tests @@ -140,6 +177,11 @@ install(TARGETS ${OPENPI_CONTROL_TARGET} RUNTIME_DEPENDENCY_SET openpi_control_runtime_dependencies RUNTIME DESTINATION openpi_control/bin) +# The shim is a build product (not scanned as an external runtime dependency), +# so bundle it explicitly next to the other shared libraries; the node's +# $ORIGIN/../libs RPATH resolves it after wheel installation. +install(TARGETS pi_trossen_shim LIBRARY DESTINATION openpi_control/libs) + # Ship the node's complete non-system shared-library closure. The executable's # $ORIGIN/../libs RPATH above resolves these files after wheel installation; # a binary consumer therefore does not need Pinocchio, ZeroMQ, or Boost from @@ -184,6 +226,7 @@ if(OPENPI_CONTROL_BUILD_FUZZERS) ${PC_ZMQ_LDFLAGS} ${PINOCCHIO_LDFLAGS} Boost::program_options + pi_trossen_shim ) 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 0c31a5f..c145069 100644 --- a/native/pi_control/include/pi_command_line_args.hpp +++ b/native/pi_control/include/pi_command_line_args.hpp @@ -16,6 +16,7 @@ #define OPT_EFFECTOR_MODEL "effector_model" ///< Effector model option. #define OPT_EFFECTOR_ID "effector_id" ///< Effector ID option. #define OPT_CONTROL_PORT "control_port" ///< Control port option. +#define OPT_BAUD_RATE "baud_rate" ///< Baud rate option (serial buses). #define OPT_INFO_LEVEL "info_level" ///< Logging level option. #define OPT_INFO_GROUPS "info_groups" ///< Information groups option. #define OPT_TOPIC_JOINT "topic_joint" ///< Joint topic option. @@ -104,6 +105,7 @@ class CommandLineArgs { std::string topic_joystick; ///< Joystick topic name. std::string info_groups; ///< Information groups (comma-separated). std::string control_port_name; ///< Control port name. + int baud_rate; ///< Baud rate for serial buses (bps). int info_level; ///< Logging level. int dof_arm; ///< Arm DOF. int servo_num_arm; ///< Arm servo count. diff --git a/native/pi_control/include/pi_control.hpp b/native/pi_control/include/pi_control.hpp index 3ba174d..0fd953d 100644 --- a/native/pi_control/include/pi_control.hpp +++ b/native/pi_control/include/pi_control.hpp @@ -4,6 +4,7 @@ */ #pragma once +#define DEFAULT_BAUD_RATE 3000000 ///< Default baud rate (bits per second). #define DEFAULT_DOF_ARM 6 ///< Default arm DOF. #define DEFAULT_SERVO_NUM_ARM DEFAULT_DOF_ARM ///< Default number of servos in arm. #define DEFAULT_DOF_EFFECTOR 1 ///< Default effector DOF. @@ -104,6 +105,13 @@ // to the policy but never trip the 10 s dead detector. #define ARX_STALL_WARN_AGE_MS 250 ///< Frame age (ms) that triggers the warn-only stall log. +// Whole-arm controller (DriverController) stall watchdog. Vendor controller +// stacks (Trossen iNerve etc.) keep streaming the last command from a driver- +// internal daemon thread even when the host control loop hangs, so a separate +// watchdog thread idles the arm when the loop stops calling group read/write. +#define CONTROLLER_STALL_WATCHDOG_TIMEOUT_MS 1000 ///< Driver-interaction silence (ms) before the arm is idled. +#define CONTROLLER_STALL_WATCHDOG_PERIOD_MS 100 ///< Watchdog polling period (ms). + // ARX read-only joint encoder (DriverArxEncoder) CAN feedback decoding. // Per the ARX encoder CAN protocol: each joint encoder broadcasts a fixed 2-byte // mechanical angle at 200 Hz. raw = (data[0] << 8) | data[1], covering 0..16384 diff --git a/native/pi_control/include/pi_device_arm_nello.hpp b/native/pi_control/include/pi_device_arm_nello.hpp new file mode 100644 index 0000000..f5f5b28 --- /dev/null +++ b/native/pi_control/include/pi_device_arm_nello.hpp @@ -0,0 +1,42 @@ +/*! + * @file pi_device_arm_nello.hpp + * @brief Defines the DeviceArmNello class for Nello robotic arm device. + */ +#pragma once +#include "pi_device_arm.hpp" + +/*! + * @class DeviceArmNello + * @brief Nello robotic arm device implementation. + */ +class DeviceArmNello : public DeviceArm { + public: + /*! + * @brief Constructs a new DeviceArmNello instance. + * + * @param cla Command-line arguments containing device configuration parameters. + */ + DeviceArmNello(const CommandLineArgs& cla); + + // Destroys the DeviceArmNello instance. + ~DeviceArmNello(); + + /*! + * @brief Moves the arm to the ready position using Nello-specific movement sequence. + * + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode move_to_ready_position() override; + + /*! + * @brief Sets control mode for Nello arm. + * + * Nello: leader and follower require different servo operation modes. + * - NORMAL_OPERATION: follow the target_role policy. + * - READY_MOVE_OVERRIDE: force a safe position-based mode so the arm can move to home from current pose. + */ + ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; + + private: +}; + diff --git a/native/pi_control/include/pi_device_config.hpp b/native/pi_control/include/pi_device_config.hpp index 0ad8f56..94a56de 100644 --- a/native/pi_control/include/pi_device_config.hpp +++ b/native/pi_control/include/pi_device_config.hpp @@ -43,9 +43,13 @@ class DeviceConfig { const std::string fn_arm_type = "arm_type"; ///< Field name for arm type. const std::string val_arm_type_arx = "arx"; ///< Value for arm type arx. + const std::string val_arm_type_controller = "controller"; ///< Value for arms managed by a whole-arm controller (DriverController). + const std::string val_arm_type_nello = "nello"; ///< Value for arm type nello (serial bus-servo arms, e.g. SO-ARM101). const std::string fn_effector_type = "effector_type"; ///< Field name for effector type. const std::string val_effector_type_arx = "arx"; ///< Value for effector type arx. + const std::string val_effector_type_controller = "controller"; ///< Value for effectors managed by a whole-arm controller (DriverController). + const std::string val_effector_type_nello = "nello"; ///< Value for effector type nello (serial bus-servo grippers, e.g. SO-ARM101). const std::string val_effector_type_none = "None"; ///< Value for effector type none. const std::string fn_effector_control_mode = "control_mode"; ///< Field name for effector control mode. const std::string val_effector_control_mode_torque = "torque"; ///< Value for effector control mode torque. @@ -61,6 +65,9 @@ class DeviceConfig { const std::string fn_driver_type = "driver_type"; ///< Field name for driver type. const std::string val_driver_type_can = "CAN"; ///< Value for driver type CAN. 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 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. @@ -111,6 +118,8 @@ class DeviceConfig { const std::string val_servo_model_encos_A4310 = "Encos EC-A4310-P2-36"; ///< Encos EC-A4310-P2-36 (CAN). const std::string val_servo_model_can_passive_encoder = "CAN Passive Encoder"; ///< YAM teaching-handle trigger encoder (CAN request/response poll, read-only). const std::string val_servo_model_arx_encoder = "ARX Remote Encoder"; ///< ARX read-only joint encoder (CAN, 2-byte angle, no actuation). + const std::string val_servo_model_trossen_wxai = "Trossen WXAI Joint"; ///< One joint of a Trossen WidowX AI arm managed by the iNerve controller (Ethernet). + const std::string val_servo_model_ft_sts3215 = "FeeTech STS3215"; ///< FeeTech STS3215 (SO-ARM100/101, Serial); also covers Hiwonder HX-30HM/HX-10HM (identical SMS/STS protocol). const std::string fn_servo_id = "servo_id"; ///< Field name for servo ID. const std::string fn_servo_data_index = "data_index"; ///< Field name for servo data index. const std::string fn_servo_pos_min = "pos_min"; ///< Field name for servo position minimum (relative radian). @@ -132,6 +141,7 @@ class DeviceConfig { const std::string fn_servo_ka = "kA"; ///< Field name for servo current value to mA conversion constant. const std::string fn_servo_kv = "kV"; ///< Field name for servo velocity value to rpm conversion constant. const std::string fn_servo_resolution = "servo_resolution"; ///< Field name for servo resolution. + const std::string fn_servo_prof_accel = "prof_accel"; ///< Field name for acceleration for servo profile control. const std::string fn_servo_dir_invert = "dir_invert"; ///< Field name for servo direction invert: inverted = -1, not inverted = 1. const std::string fn_servo_zero_pos = "zero_pos"; ///< Field name for servo zero position (absolute radian). const std::string fn_servo_position_wrap_period = "position_wrap_period"; ///< Optional single-turn feedback wrap period (relative radian). @@ -203,6 +213,31 @@ class DeviceConfig { return ReturnCode::SUCCESS; } + /*! + * @brief Extracts a typed value for an optional field without warning when it is absent. + * + * Identical to get_field_value() except a missing field returns INVALID_PARAM silently. + * Use for fields that have a documented default so startup logs are not flooded with + * "is not defined" warnings for perfectly valid configs. + * @param json_data JSON object containing the configuration data. + * @param field_name Name of the field to extract (should use one of the fn_* constants). + * @param value Output parameter that will be populated with the extracted value. + */ + template + ReturnCode get_field_value_optional(const json& json_data, const std::string& field_name, T& value) const { + if (!json_data.contains(field_name)) { + return ReturnCode::INVALID_PARAM; + } + try { + value = json_data[field_name].get(); + } catch (const nlohmann::json::type_error& e) { + std::string what = e.what(); + PI_ERROR("Type Error: %s", what.c_str()); + return ReturnCode::INVALID_PARAM; + } + return ReturnCode::SUCCESS; + } + private: /*! * @brief Loads and parses a configuration file from the given file path. diff --git a/native/pi_control/include/pi_device_effector_controller.hpp b/native/pi_control/include/pi_device_effector_controller.hpp new file mode 100644 index 0000000..42ceef9 --- /dev/null +++ b/native/pi_control/include/pi_device_effector_controller.hpp @@ -0,0 +1,41 @@ +/*! + * @file pi_device_effector_controller.hpp + * @brief DeviceEffectorController class for grippers managed by a whole-arm controller. + */ +#pragma once + +#include "pi_device_effector_arx.hpp" + +/*! + * @class DeviceEffectorController + * @brief Effector device for grippers driven by a whole-arm controller (DriverController). + * + * Inherits the ARX torque-gripper motion logic (distance-to-torque via + * apply_torque_with_damping(), which ServoController maps to controller + * external efforts). Unlike ARX, controller-managed grippers need real + * leader/follower mode switching (position vs external effort on the vendor + * controller), so set_control_mode() restores the DeviceEffector base + * behavior of delegating to Joint::change_control_mode_for_{leader,follower}(). + */ +class DeviceEffectorController : public DeviceEffectorArx { + public: + /*! + * @brief Constructor. + * @param cla Command-line arguments. + */ + explicit DeviceEffectorController(const CommandLineArgs& cla); + + /*! + * @brief Destructor. + */ + ~DeviceEffectorController() override; + + /*! + * @brief Delegates mode switching to the joints (DeviceEffector base + * behavior), undoing the ARX no-op override. + * @param target_role Target role (LEADER or FOLLOWER). + * @param intent Control mode intent. + * @return ReturnCode indicating success or failure. + */ + ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; +}; diff --git a/native/pi_control/include/pi_device_effector_nello.hpp b/native/pi_control/include/pi_device_effector_nello.hpp new file mode 100644 index 0000000..aeffbf0 --- /dev/null +++ b/native/pi_control/include/pi_device_effector_nello.hpp @@ -0,0 +1,76 @@ +/*! + * @file pi_device_effector_nello.hpp + * @brief Nello effector device implementation. + */ +#pragma once +#include "pi_device_effector.hpp" + +/*! + * @brief Nello effector device implementation. + */ +class DeviceEffectorNello : public DeviceEffector { + public: + /*! + * @brief Constructor. + * @param cla Command-line arguments. + */ + DeviceEffectorNello(const CommandLineArgs& cla); + + /*! + * @brief Destructor. + */ + ~DeviceEffectorNello(); + + // + // Override functions + // + + /*! + * @brief Initializes the Nello effector device. + * @param cla Command-line arguments. + * @param argc Argument count. + * @param argv Argument values. + * @param p_topic Topic instance for communication. + * @param p_driver Driver instance for hardware communication. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode init(const CommandLineArgs& cla, int argc, char** argv, std::shared_ptr p_topic, + std::shared_ptr p_driver) override; + + /*! + * @brief Moves the effector to the ready position. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode move_to_ready_position() override; + + /*! + * @brief Moves a joint using torque control. + * @param p_joint Pointer to the joint. + * @param target_pos Target position (relative radians). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode move_joint_with_torque(Joint* p_joint, float target_pos) override; + + /*! + * @brief Sets control mode for Nello effector. + * + * Nello: leader and follower require different servo operation modes, and follower behavior also depends on + * effector control type (torque vs position). READY_MOVE_OVERRIDE forces a safe position-based mode via the + * base-class override flag. + */ + ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; + + /*! + * @brief Resets ready state so a commanded move-to-ready re-engages torque once. + */ + void reset_ready_state_for_move_to_ready() override { + DeviceEffector::reset_ready_state_for_move_to_ready(); + ready_move_torque_engaged_ = false; + } + + private: + /// True once the ready move has engaged servo torque; prevents per-cycle + /// enable(true) re-sends that race the leader-passive torque disable. + bool ready_move_torque_engaged_ = false; +}; + diff --git a/native/pi_control/include/pi_driver_controller.hpp b/native/pi_control/include/pi_driver_controller.hpp new file mode 100644 index 0000000..d483897 --- /dev/null +++ b/native/pi_control/include/pi_driver_controller.hpp @@ -0,0 +1,275 @@ +/*! + * @file pi_driver_controller.hpp + * @brief DriverController -- vendor-neutral base class for whole-arm robot controllers. + * + * Covers arms that are NOT driven servo-by-servo over CAN/serial but through a + * vendor controller box exposing a whole-arm API over Ethernet (Trossen iNerve, + * and structurally similar stacks like Franka FCI or UR RTDE). The class keeps + * the existing Driver group read/write contract so DeviceArm / DeviceEffector / + * Joint / Servo code runs unchanged: + * + * - group_read_hardware_values() -> one whole-arm state fetch into a cache; + * read_hardware_values(Servo*) copies the per-joint slice into ServoController. + * - ServoController::move()/apply_torque() queue per-joint commands here; + * group_write_hardware_values() flushes them as one whole-arm transaction. + * - Per-joint control modes (position / external effort) are buffered and + * applied lazily right before the next command flush, so a leader/follower + * switch costs one vendor configuration call instead of one per joint. + * + * Lifecycle safety (vendor-neutral): + * - open() is a template method: vendor_connect() -> joint count validation -> + * initial state read, so a half-configured controller can never enter the + * control loop. + * - make_safe() idles the whole arm exactly once (idempotent, thread-safe); + * close() and the stall watchdog both funnel into it. + * - arm_comm_loss_protection() starts a stall watchdog thread: if the host + * control loop stops calling group read/write while the vendor daemon keeps + * streaming the last command (host hang), the watchdog idles the arm. + */ + +#pragma once +#include +#include +#include +#include +#include +#include + +#include "pi_driver.hpp" + +class ServoController; + +/*! + * @brief Vendor-neutral base class for whole-arm controller drivers. + */ +class DriverController : public Driver { + public: + /*! + * @brief Per-joint control mode requested from the vendor controller. + */ + enum class JointCommandMode { + UNSET, ///< No mode selected yet (joint not started). + POSITION, ///< Controller-side position servoing (follower / ready moves). + EXTERNAL_EFFORT, ///< Torque on top of the controller's own gravity/friction compensation (leader). + }; + + /*! + * @brief Per-joint command buffered until the next group write flush. + */ + struct JointCommand { + JointCommandMode mode = JointCommandMode::UNSET; ///< Mode the command was queued for. + float position = 0; ///< Goal position (rad; gripper joints use meters). + float velocity_ff = 0; ///< Feedforward velocity (rad/s), 0 when unused. + float external_effort = 0; ///< External effort (Nm; gripper joints use N). + bool pending = false; ///< True when queued and not yet flushed. + }; + + /*! + * @brief Per-joint state cache filled by group_read_hardware_values(). + */ + struct JointState { + float position = 0; ///< Position (rad; gripper joints use meters). + float velocity = 0; ///< Velocity (rad/s). + float effort = 0; ///< Effort (Nm). + float temperature = 0; ///< Motor temperature (degrees Celsius). + bool valid = false; ///< True once at least one whole-arm read succeeded. + }; + + /*! + * @brief Constructor. + * @param p_device Pointer to the Device instance. + * @param cla Command-line arguments (control_port_name carries the controller IP address). + */ + DriverController(Device* p_device, const CommandLineArgs& cla); + + /*! + * @brief Destructor. Funnels into close() so the arm is idled even when the + * owning Device skips its stop path (idempotent). + */ + ~DriverController() override; + + /*! + * @brief Connects to the controller (template method around vendor_connect()). + * + * Fast-fails when the vendor reports fewer joints than the model config + * registered, and performs one initial whole-arm read so the position + * caches are valid before any servo is started. + * @param baud_rate Unused for Ethernet controllers (Driver interface compatibility). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode open(int baud_rate) override; + + /*! + * @brief Idles the arm and disconnects. Idempotent: safe to call repeatedly + * and from the destructor. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode close() override; + + /*! + * @brief Copies the cached whole-arm state slice of one joint into its ServoController. + * @param p_servo Pointer to the Servo instance (must be a ServoController). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode read_hardware_values(Servo* p_servo) override; + + /*! + * @brief Fetches the whole-arm state from the controller into the joint state cache. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode group_read_hardware_values() override; + + /*! + * @brief Applies pending mode changes, then flushes the queued per-joint + * commands as whole-arm vendor transactions. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode group_write_hardware_values() override; + + /*! + * @brief Starts the stall watchdog. Called once by the node right before + * the main control loop starts. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode arm_comm_loss_protection() override; + + /*! + * @brief Requests a control mode for one joint. Buffered; the vendor + * configuration call happens once in the next group write flush. + * @param data_index Joint slot (Servo::data_index_). + * @param mode Requested mode. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_joint_command_mode(int data_index, JointCommandMode mode); + + /*! + * @brief Queues a position command for one joint. + * @param data_index Joint slot (Servo::data_index_). + * @param position Goal position (rad; gripper joints use meters). + * @param velocity_ff Feedforward velocity (rad/s), 0 when unused. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode queue_position(int data_index, float position, float velocity_ff); + + /*! + * @brief Queues an external effort command for one joint. + * @param data_index Joint slot (Servo::data_index_). + * @param external_effort External effort (Nm; gripper joints use N). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode queue_external_effort(int data_index, float external_effort); + + /*! + * @brief Puts the whole arm in the vendor's safe passive state exactly once. + * + * Thread-safe and idempotent: park_safely() of every ServoController, close(), + * the stall watchdog, and the destructor all funnel into this method; only the + * first caller performs the vendor call. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode make_safe(); + + /*! + * @brief Number of joints the vendor controller reports (arm + gripper). + */ + int controller_joint_count() const { return joint_count_; } + + /*! + * @brief True once open() completed and close() has not run. + */ + bool is_open() const { return opened_; } + + /*! + * @brief True once at least one whole-arm state read succeeded. + */ + bool has_valid_state() const { return read_success_count_ > 0; } + + protected: + // ---- Vendor hooks (implemented by DriverTrossen and future controller drivers) ---- + + /*! + * @brief Establishes the connection to the controller and returns its joint count. + * @param address Controller network address (from --control_port_name). + * @param joint_count Output: number of joints the controller manages. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_connect(const std::string& address, int& joint_count) = 0; + + /*! + * @brief Tears the connection down. Must tolerate being called when the + * connection is already gone. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_disconnect() = 0; + + /*! + * @brief Reads the whole-arm state. + * @param states Output vector sized to controller_joint_count(). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_read_state(std::vector& states) = 0; + + /*! + * @brief Sends the pending commands (only entries with pending == true). + * @param commands Command buffer sized to controller_joint_count(). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_write_commands(const std::vector& commands) = 0; + + /*! + * @brief Applies the requested per-joint control modes on the controller. + * @param modes Mode vector sized to controller_joint_count(); UNSET entries + * must be mapped to the vendor's passive/idle mode. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_apply_command_modes(const std::vector& modes) = 0; + + /*! + * @brief Puts every joint in the vendor's safe passive state (e.g. idle with + * controller-side gravity compensation). Must not throw. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + virtual ReturnCode vendor_make_safe() = 0; + + private: + /*! + * @brief Validates a joint slot index against the vendor joint count. + */ + ReturnCode validate_data_index(int data_index) const; + + /*! + * @brief Stall watchdog loop: idles the arm when the host control loop + * stops interacting with the driver while commands may still stream. + */ + void watchdog_loop(); + + /*! + * @brief Records a successful driver interaction for the stall watchdog. + */ + void feed_watchdog(); + + /*! + * @brief Stops the watchdog thread (idempotent). + */ + void stop_watchdog(); + + int joint_count_ = 0; ///< Joint count reported by the vendor controller. + bool opened_ = false; ///< True between successful open() and close(). + std::atomic read_success_count_{0}; ///< Successful whole-arm reads since open(). + + std::mutex command_mutex_; ///< Guards commands_, desired_modes_, modes_dirty_. + std::vector commands_; ///< Per-joint command buffer (indexed by data_index). + std::vector desired_modes_; ///< Per-joint requested modes. + bool modes_dirty_ = false; ///< True when desired_modes_ changed since the last flush. + + std::mutex state_mutex_; ///< Guards states_. + std::vector states_; ///< Per-joint state cache (indexed by data_index). + + std::mutex safe_mutex_; ///< Serializes make_safe() / close(). + bool made_safe_ = false; ///< True once vendor_make_safe() ran. + + std::thread watchdog_thread_; ///< Stall watchdog thread. + std::atomic watchdog_running_{false}; ///< Watchdog thread lifecycle flag. + std::atomic watchdog_tripped_{false}; ///< Latched when the watchdog idled the arm. + std::atomic last_interaction_ms_{0}; ///< Monotonic ms of the last successful read/write. +}; diff --git a/native/pi_control/include/pi_driver_ft.hpp b/native/pi_control/include/pi_driver_ft.hpp new file mode 100644 index 0000000..65a970a --- /dev/null +++ b/native/pi_control/include/pi_driver_ft.hpp @@ -0,0 +1,312 @@ +/*! + * @file pi_driver_ft.hpp + * @brief DriverFt -- FeeTech SMS/STS serial bus servo driver (STS3215 class). + * + * Speaks the FeeTech UART protocol (0xFF 0xFF header, complement checksum, + * INST_READ/WRITE plus SYNC READ 0x82 / SYNC WRITE 0x83 bulk transfers) on a + * plain USB serial port. Mirrors the DriverDxl structure: cached bulk feedback + * per control cycle, queued group writes flushed once per cycle, and sticky + * dead-servo tracking for daisy-chain cable breaks. + * + * Covers the SO-ARM100/101 arms (FeeTech STS3215; Hiwonder HX-30HM / HX-10HM + * are register-compatible and use the same model string). SYNC READ requires + * STS3215 firmware >= 3.9 class; open() verifies it once and fast-fails when + * the bus does not answer a bulk read (no sequential-read fallback). + * + * The Python mirror of this protocol lives in pi_control/servos/ft_serial.py. + */ + +#pragma once +#include +#include +#include + +#include "pi_driver_serial.hpp" + +class ServoFt; + +/*! + * @brief FeeTech SMS/STS serial bus servo driver. + */ +class DriverFt : public DriverSerial { + public: + /*! + * @brief Constructor. + * @param p_device Pointer to the Device instance. + * @param cla Command-line arguments. + */ + DriverFt(Device* p_device, const CommandLineArgs& cla); + + /*! + * @brief Destructor. + */ + ~DriverFt() override; + + /*! + * @brief Opens the serial port, sizes the feedback caches, and verifies + * SYNC READ support with one bulk read (fast-fail when unsupported). + * @param baud_rate Baud rate for serial communication (SO-ARM default 1000000). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode open(int baud_rate) override; + + /*! + * @brief Closes the serial port. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode close() override; + + /*! + * @brief Copies the cached bulk-read feedback of one servo into its Servo object. + * @param p_servo Pointer to the Servo instance (must be a ServoFt). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode read_hardware_values(Servo* p_servo) override; + + /*! + * @brief Reads feedback from all servos with one SYNC READ transaction. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode group_read_hardware_values() override; + + /*! + * @brief Flushes the queued goal position / torque limit SYNC WRITE packets. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode group_write_hardware_values() override; + + /*! + * @brief Returns the lowest dead servo id (daisy-chain break point), or -1. + */ + int last_failed_servo_id() const override { return last_failed_servo_id_; } + + /*! + * @brief Returns the sticky set of servo ids classified dead on the bus. + */ + std::set dead_servo_ids() const override { return dead_servo_ids_; } + + /*! + * @brief Pings a servo (INST_PING) and validates the status packet. + * @param id Servo ID. + * @return ReturnCode::SUCCESS when the servo answered, otherwise an error code. + */ + ReturnCode ping(int id); + + /*! + * @brief Reads a 1/2-byte register from a servo (INST_READ, little-endian). + * @param id Servo ID. + * @param address Register address. + * @param length Register length in bytes (1 or 2). + * @param value Output register value (unsigned raw). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode read_register(int id, uint8_t address, uint8_t length, int32_t& value); + + /*! + * @brief Writes a 1/2-byte register on a servo (INST_WRITE, little-endian). + * + * EEPROM registers (address below the SRAM boundary) are automatically + * wrapped with a Lock Flag unlock/re-lock pair, which the STS3215 firmware + * requires for the write to take effect. + * @param id Servo ID. + * @param address Register address. + * @param value Register value (unsigned raw; pre-encode sign-magnitude values). + * @param length Register length in bytes (1 or 2). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode write_register(int id, uint8_t address, int32_t value, uint8_t length); + + /*! + * @brief Enables or disables torque output (Torque Enable register). + * @param p_servo Pointer to the ServoFt instance. + * @param enable True to enable, false to disable. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode enable_torque(ServoFt* p_servo, bool enable); + + /*! + * @brief Sets the operating mode (0 position / 1 velocity / 2 PWM / 3 step). + * @param p_servo Pointer to the ServoFt instance. + * @param mode Operating mode value. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_operating_mode(ServoFt* p_servo, uint8_t mode); + + /*! + * @brief Writes the position loop P/I/D gains (one byte each, EEPROM). + * @param p_servo Pointer to the ServoFt instance. + * @param pos_p Position P gain (0-254). + * @param pos_i Position I gain (0-254). + * @param pos_d Position D gain (0-254). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_position_pid(ServoFt* p_servo, float pos_p, float pos_i, float pos_d); + + /*! + * @brief Writes the Acceleration register (unit: 100 steps/s^2, 0 disables the ramp). + * @param p_servo Pointer to the ServoFt instance. + * @param acceleration Acceleration register value (0-254). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_acceleration(ServoFt* p_servo, uint8_t acceleration); + + /*! + * @brief Writes the goal position of one servo directly (single WRITE). + * @param p_servo Pointer to the ServoFt instance. + * @param goal_position Goal position in encoder steps (0-4095). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_goal_position_direct(ServoFt* p_servo, int32_t goal_position); + + /*! + * @brief Queues a goal position for the per-cycle SYNC WRITE flush. + * @param p_servo Pointer to the ServoFt instance. + * @param goal_position Goal position in encoder steps (0-4095). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_goal_position_group(ServoFt* p_servo, int32_t goal_position); + + /*! + * @brief Queues a torque limit (0-1000, 0.1% of max torque) for the SYNC WRITE flush. + * @param p_servo Pointer to the ServoFt instance. + * @param torque_limit Torque Limit register value (0-1000). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode set_torque_limit_group(ServoFt* p_servo, int32_t torque_limit); + + /*! + * @brief Discards one servo's queued goal position / torque limit group-write entries. + * + * Used when switching that servo to the passive leader mode: goal entries queued + * earlier in the same control cycle (the final move-to-ready step) must not be + * flushed after its torque is disabled, because a goal position command can + * re-engage torque (Hiwonder HX firmware on SO-ARM101 kits). + * @param p_servo Pointer to the ServoFt instance whose entries are dropped. + */ + void discard_pending_group_writes(ServoFt* p_servo); + + /*! + * @brief Reads the Torque Enable register of one servo. + * @param p_servo Pointer to the ServoFt instance. + * @param enabled Output raw register value (0 = torque off, 1 = torque on). + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode get_torque_enabled(ServoFt* p_servo, int32_t& enabled); + + /*! + * @brief Reads the present position of one servo (single READ). + * @param p_servo Pointer to the ServoFt instance. + * @param present_position Output present position in encoder steps. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode get_present_position(ServoFt* p_servo, int32_t& present_position); + + /*! + * @brief Reads the present position and writes it as the goal position. + * + * Called before enabling torque so the servo holds its current pose instead + * of jumping to a stale goal register (same rationale as DriverDxl). + * @param p_servo Pointer to the ServoFt instance. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode sync_goal_position_to_present(ServoFt* p_servo); + + /*! + * @brief Decodes a FeeTech sign-magnitude value (dedicated sign bit). + * @param value Raw register value. + * @param sign_bit Index of the sign bit (velocity: 15, load: 10). + * @return Signed value. + */ + static int32_t decode_sign_magnitude(uint32_t value, int sign_bit); + + /*! + * @brief Encodes a signed value into FeeTech sign-magnitude representation. + * @param value Signed value. + * @param sign_bit Index of the sign bit. + * @return Raw register value. + */ + static uint32_t encode_sign_magnitude(int32_t value, int sign_bit); + + private: + /*! + * @brief Builds a FeeTech instruction packet and sends it. + * @param id Servo ID (or the broadcast ID). + * @param instruction Instruction byte. + * @param p_params Instruction parameter bytes (may be nullptr when length is 0). + * @param param_length Number of parameter bytes. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode send_instruction(uint8_t id, uint8_t instruction, const uint8_t* p_params, size_t param_length); + + /*! + * @brief Reads one status packet (FF FF ID LEN ERR PARAMS.. CHK) with a deadline. + * @param expected_id Servo ID the status packet must carry; pass the broadcast ID (0xFE) to accept any servo + * (used for SYNC READ responses, which are matched by the ID they carry). + * @param p_params Output buffer for the status parameters (may be nullptr when param_length is 0). + * @param param_length Expected number of status parameter bytes. + * @param timeout_ms Deadline for the complete packet. + * @param p_packet_id Optional output for the ID the packet actually carried (may be nullptr). + * @return ReturnCode::SUCCESS if a valid packet arrived, otherwise an error code. + */ + ReturnCode receive_status(uint8_t expected_id, uint8_t* p_params, size_t param_length, int timeout_ms, + int* p_packet_id); + + /*! + * @brief Sends an instruction and reads the matching status packet, with retries. + * @param id Servo ID. + * @param instruction Instruction byte. + * @param p_params Instruction parameter bytes. + * @param param_length Number of instruction parameter bytes. + * @param p_rsp_params Output buffer for status parameters. + * @param rsp_param_length Expected number of status parameter bytes. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode txrx_instruction(uint8_t id, uint8_t instruction, const uint8_t* p_params, size_t param_length, + uint8_t* p_rsp_params, size_t rsp_param_length); + + /*! + * @brief Writes a register without the EEPROM unlock wrapper (used by the wrapper itself). + */ + ReturnCode write_register_raw(int id, uint8_t address, int32_t value, uint8_t length); + + /*! + * @brief Reads exactly ``length`` bytes from the port before ``deadline_ms`` elapses. + * @return Number of bytes actually read (may be short on timeout). + */ + size_t read_exact(uint8_t* p_buf, size_t length, int deadline_ms); + + /*! + * @brief Discards any stale bytes pending in the receive buffer. + */ + void flush_input(); + + /*! + * @brief Sends one SYNC WRITE packet for a fixed register window. + * @param address Register start address. + * @param data_length Bytes per servo. + * @param entries (servo id, raw value) pairs; values are little-endian encoded. + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode sync_write(uint8_t address, uint8_t data_length, const std::vector>& entries); + + /*! + * @brief Probes unclassified servos with ping() after a bulk-read failure + * and moves the silent ones into dead_servo_ids_ (sticky). + */ + void classify_dead_servos(); + + int servo_num_total_ = 0; ///< Total number of servos registered on the bus. + std::vector sync_read_ids_; ///< Alive servo ids included in the SYNC READ transaction. + std::vector all_servo_ids_; ///< All servo ids as reported by the device at open(). + std::set dead_servo_ids_; ///< Sticky set of servos classified dead (cable break). + int last_failed_servo_id_ = -1; ///< Lowest dead servo id, or -1 when all alive. + + std::vector pres_pos_; ///< Cached present position (raw steps, unsigned single-turn). + std::vector pres_vel_; ///< Cached present velocity (steps/s, sign-magnitude decoded). + std::vector pres_load_; ///< Cached present load (0.1% units, sign-magnitude decoded). + std::vector pres_temp_; ///< Cached present temperature (degrees Celsius). + std::vector pres_status_; ///< Cached Servo Status error bits. + + std::vector> pending_goal_position_; ///< Queued (id, steps) for the goal position SYNC WRITE. + std::vector> pending_torque_limit_; ///< Queued (id, 0.1% units) for the torque limit SYNC WRITE. +}; diff --git a/native/pi_control/include/pi_driver_serial.hpp b/native/pi_control/include/pi_driver_serial.hpp new file mode 100644 index 0000000..5953142 --- /dev/null +++ b/native/pi_control/include/pi_driver_serial.hpp @@ -0,0 +1,89 @@ +/*! + * @file pi_driver_serial.hpp + * @brief DriverSerial -- minimal raw POSIX serial (termios) wrapper. + * + * Designed for protocols that ride a plain USB-CDC `/dev/ttyACM*` port and do + * their own framing. This driver knows nothing about a specific servo/motor + * wire format -- it only owns the file descriptor, sets 8N1 with the + * requested baud rate, and gives the device class raw byte access plus an + * optional asynchronous reception thread. Protocol drivers (e.g. + * :class:`DriverFt` for FeeTech SMS/STS, a future Dynamixel driver) derive + * from it and implement their packet layer on top. + */ + +#pragma once +#include + +#include +#include +#include +#include +#include + +#include "pi_driver.hpp" + +/*! + * @brief Minimal raw serial driver (no framing, no protocol). + */ +class DriverSerial : public Driver { + public: + /*! + * @brief Callback type for received serial bytes. + * @param p_data_buf Pointer to the receive buffer. + * @param read_bytes Number of bytes actually read. + */ + typedef std::function callback_t; + + DriverSerial(Device* p_device, const CommandLineArgs& cla); + ~DriverSerial() override; + + /*! + * @brief Open the serial port at ``baud_rate`` (8N1, no flow control). + * + * The port path comes from the base class' ``control_port_name_`` (set + * by the ``Driver`` constructor from ``cla.control_port_name``). + */ + ReturnCode open(int baud_rate) override; + ReturnCode close() override; + + /*! + * @brief Start a background thread that drains the serial port and + * invokes ``callback`` for every chunk of bytes read. + * + * Idempotent: stops a previous reception loop first if one is running. + */ + ReturnCode start_reception(const callback_t& callback); + + /*! + * @brief Stop the reception thread (no-op if not running). + */ + ReturnCode stop_reception(); + + /*! + * @brief Write raw bytes to the serial port. Blocking ``::write`` semantics. + */ + ReturnCode write_bytes(const uint8_t* p_data, size_t size); + + /*! + * @brief Convenience overload: write a NUL-terminated text command. + */ + ReturnCode write_text(const char* text); + + /*! + * @brief Whether ``open()`` succeeded and we still own a valid fd. + */ + bool is_open() const { return port_handler_ >= 0; } + + protected: + void receive_loop(callback_t callback); + + /*! + * @brief Map a baud-rate integer to the matching termios ``B*`` constant. + * @return The mapped speed_t, or 0 if ``baud_rate`` is unsupported. + */ + static speed_t baud_constant(int baud_rate); + + int port_handler_ = -1; ///< POSIX file descriptor (-1 when closed). + std::atomic is_running_{false}; + std::thread reception_thread_; +}; diff --git a/native/pi_control/include/pi_driver_trossen.hpp b/native/pi_control/include/pi_driver_trossen.hpp new file mode 100644 index 0000000..8386482 --- /dev/null +++ b/native/pi_control/include/pi_driver_trossen.hpp @@ -0,0 +1,66 @@ +/*! + * @file pi_driver_trossen.hpp + * @brief DriverTrossen -- Trossen iNerve arm controller driver (via TrossenArmShim over Ethernet). + * + * Implements the DriverController vendor hooks against TrossenArmShim, the + * isolation boundary around the prebuilt libtrossen_arm SDK (see + * pi_trossen_shim.hpp for why the SDK cannot be linked into the node + * directly): + * - vendor_connect() -> shim configure() (TCP handshake + UDP stream) + * - vendor_read_state() -> cached whole-arm output from the SDK daemon thread + * - vendor_write_commands() -> set_arm_positions / set_arm_external_efforts / + * set_gripper_* streaming calls (goal_time 0, non-blocking) + * - vendor_apply_command_modes() -> set_joint_modes() (one TCP configuration call) + * - vendor_make_safe() -> set_all_modes_idle(); the controller then holds the + * arm passively with its own gravity/friction compensation + * + * All positions/velocities/efforts are exchanged in the controller's native + * joint space (radians and Nm; the gripper joint uses meters and Newtons). + * Zeroing is owned by the controller (position_offset joint characteristic, + * managed from the Python setup backend), so model configs use zero_pos = 0. + * + * The SDK reports failures as C++ exceptions (including UDP stream loss + * detected by its daemon thread, rethrown on the next SDK call); the shim + * catches them and every hook converts to fast-failing ReturnCodes. + */ + +#pragma once +#include +#include +#include + +#include "pi_driver_controller.hpp" +#include "pi_trossen_shim.hpp" + +/*! + * @brief Trossen iNerve whole-arm controller driver. + */ +class DriverTrossen : public DriverController { + public: + /*! + * @brief Constructor. + * @param p_device Pointer to the Device instance. + * @param cla Command-line arguments (control_port_name carries the controller + * IP address; role selects the leader/follower end-effector properties). + * @param controller_model Trossen model string from the model config (e.g. "wxai_v0"). + */ + DriverTrossen(Device* p_device, const CommandLineArgs& cla, const std::string& controller_model); + + /*! + * @brief Destructor. Idles the arm and disconnects (idempotent). + */ + ~DriverTrossen() override; + + protected: + ReturnCode vendor_connect(const std::string& address, int& joint_count) override; + ReturnCode vendor_disconnect() override; + ReturnCode vendor_read_state(std::vector& states) override; + ReturnCode vendor_write_commands(const std::vector& commands) override; + ReturnCode vendor_apply_command_modes(const std::vector& modes) override; + ReturnCode vendor_make_safe() override; + + private: + std::string controller_model_; ///< Trossen model string from the model config. + bool is_leader_role_ = false; ///< True when this device runs as a leader. + std::unique_ptr p_arm_; ///< Shim around the vendor SDK (see pi_trossen_shim.hpp). +}; diff --git a/native/pi_control/include/pi_servo.hpp b/native/pi_control/include/pi_servo.hpp index 7a6d8ec..baf87bd 100644 --- a/native/pi_control/include/pi_servo.hpp +++ b/native/pi_control/include/pi_servo.hpp @@ -26,7 +26,11 @@ enum class ServoType { DM_4340 = 151, ///< Dinamo DM J4340 servo (CAN protocol). DM_4310 = 152, ///< Dinamo DM J4310 servo (CAN protocol). - CAN_PASSIVE_ENCODER = 701 ///< Passive CAN trigger encoder (YAM teaching handle): request/response poll, read-only, never commanded. + FT_STS3215 = 301, ///< FeeTech STS3215 servo (SMS/STS serial protocol; also covers Hiwonder HX-30HM/HX-10HM). + + CAN_PASSIVE_ENCODER = 701, ///< Passive CAN trigger encoder (YAM teaching handle): request/response poll, read-only, never commanded. + + CONTROLLER_JOINT = 801 ///< One joint of a whole-arm controller (Trossen iNerve etc.; no direct bus access). }; /*! @@ -205,6 +209,22 @@ class Servo { */ virtual ReturnCode apply_torque(float torque) = 0; + /*! + * @brief Enables or disables torque output of the servo. + * + * Virtual so device code (e.g. DeviceEffectorNello) can toggle torque on + * any bus-servo family without casting to a concrete type. The default + * implementation fast-fails for servo families without a torque enable + * register. + * @param enable True to enable torque output, false to disable it. + * @return ReturnCode indicating success or failure. + */ + virtual ReturnCode enable_torque(bool enable) { + (void)enable; + PI_ERROR("enable_torque is not supported for servo ID %d (model %s)", id_, servo_model_.c_str()); + return ReturnCode::NOT_SUPPORTED; + } + /*! * @brief Applies torque while retaining the servo's configured derivative damping. * diff --git a/native/pi_control/include/pi_servo_controller.hpp b/native/pi_control/include/pi_servo_controller.hpp new file mode 100644 index 0000000..01fa849 --- /dev/null +++ b/native/pi_control/include/pi_servo_controller.hpp @@ -0,0 +1,160 @@ +/*! + * @file pi_servo_controller.hpp + * @brief ServoController -- per-joint view onto a whole-arm controller (DriverController). + * + * Joints of controller-managed arms (Trossen iNerve etc.) are not individual + * bus servos; the vendor controller owns the servo loops. This class adapts + * one controller joint to the Servo interface so Joint / DeviceArm / + * DeviceEffector logic (leash, safe mode, ready moves, torque grippers) runs + * unchanged: + * + * - move() queues a position target into the DriverController command buffer + * (flushed once per cycle as a whole-arm transaction). + * - apply_torque() queues an external effort on top of the controller's own + * gravity/friction compensation (leader spring / force feedback / torque + * gripper path). + * - change_control_mode_for_{leader,follower}() request per-joint mode + * switches, applied lazily as one vendor configuration call. + * + * Positions are exchanged in the controller's native joint space (radians; + * gripper joints use meters), so model configs use zero_pos = 0 and the + * controller-side position offset is the single zeroing authority. + */ + +#pragma once +#include "pi_driver_controller.hpp" +#include "pi_servo.hpp" + +/*! + * @brief Parameter container class for controller-managed joint configuration. + */ +class ServoControllerParam : public ServoParam { + public: + /*! + * @brief Constructor. + * @param tolerable_pos_difference_rad Tolerable position difference threshold in radians. + * @param max_pos_difference_rad Maximum position difference threshold in radians. + * @param velocity_threshold_rad_sec Velocity threshold in rad/sec. + */ + ServoControllerParam(float tolerable_pos_difference_rad, float max_pos_difference_rad, + float velocity_threshold_rad_sec) + : ServoParam(tolerable_pos_difference_rad, max_pos_difference_rad, velocity_threshold_rad_sec) {} +}; + +/*! + * @brief Manages one joint of a whole-arm controller through the Servo interface. + */ +class ServoController : public Servo { + public: + /*! + * @brief Constructor. + * @param p_device Pointer to the Device object. + * @param p_joint Pointer to the Joint object. + * @param p_driver Pointer to the Driver object (must be a DriverController). + */ + ServoController(Device* p_device, Joint* p_joint, Driver* p_driver); + + /*! + * @brief Destructor. + */ + ~ServoController(); + + /*! + * @brief Initializes the servo with model configuration. + * @param servo_config JSON object containing the servo model configuration. + * @param p_config Pointer to the device model configuration object. + * @return ReturnCode indicating success or failure. + */ + ReturnCode init_config_model(const json& servo_config, const DeviceConfig* p_config) override; + + /*! + * @brief Starts the joint: requests POSITION mode so the controller holds + * the current pose until ready moves take over. + * @return ReturnCode indicating success or failure. + */ + ReturnCode start_hardware() override; + + /*! + * @brief Parks the joint: funnels into DriverController::make_safe(), which + * idles the whole arm exactly once (idempotent across joints). + * @return ReturnCode indicating success or failure. + */ + ReturnCode park_safely() override; + + /*! + * @brief Fails fast when no whole-arm state read has succeeded yet. + * @return ReturnCode indicating success or failure. + */ + ReturnCode verify_position_fresh() override; + + /*! + * @brief Queues a position target for the next whole-arm command flush. + * @param target_pos Target position in relative radians (gripper: meters). + * @return ReturnCode indicating success or failure. + */ + ReturnCode move(float target_pos) override; + + /*! + * @brief Queues a position target with a feedforward velocity. + * + * The controller runs its own servo loops, so target_tor cannot bound the + * motion per-command and is not consumed (joint limits on the controller + * bound the effort). + * @param target_pos Target position in relative radians (gripper: meters). + * @param target_vel Target velocity in relative rad/sec (feedforward). + * @param target_tor Target torque in Nm (not consumed). + * @return ReturnCode indicating success or failure. + */ + ReturnCode move(float target_pos, float target_vel, float target_tor) override; + + /*! + * @brief Queues an external effort on top of the controller's own + * gravity/friction compensation. + * @param torque Torque in relative Nm (gripper: N). + * @return ReturnCode indicating success or failure. + */ + ReturnCode apply_torque(float torque) override; + + /*! + * @brief Same as apply_torque(); the controller applies its own damping. + * @param torque Torque in relative Nm (gripper: N). + * @return ReturnCode indicating success or failure. + */ + ReturnCode apply_torque_with_damping(float torque) override; + + /*! + * @brief Requests EXTERNAL_EFFORT mode for leader (teleoperation) operation. + * + * The controller keeps its own gravity/friction compensation active in this + * mode; the model's spring / force-feedback torques ride on top as external + * efforts, so the C++ Pinocchio gravity path must stay disabled + * (individual config gravity_compensation = false). + * @return ReturnCode indicating success or failure. + */ + ReturnCode change_control_mode_for_leader() override; + + /*! + * @brief Requests POSITION mode for follower operation. + * @return ReturnCode indicating success or failure. + */ + ReturnCode change_control_mode_for_follower() override; + + /*! + * @brief Gets the joint's current position in the controller's native unit. + * @return Current position (radians; gripper joints use meters). + */ + float get_pos_servo() override { return curr_pos_abs_; } + + /*! + * @brief Copies one whole-arm state slice into this servo's caches. + * Called by DriverController::read_hardware_values(). + * @param position Position (rad; gripper: meters). + * @param velocity Velocity (rad/s). + * @param effort Effort (Nm). + * @param temperature Motor temperature (degrees Celsius). + */ + void update_from_controller_state(float position, float velocity, float effort, float temperature); + + protected: + DriverController* p_driver_controller_ = nullptr; ///< Pointer to the whole-arm controller driver. +}; diff --git a/native/pi_control/include/pi_servo_ft.hpp b/native/pi_control/include/pi_servo_ft.hpp new file mode 100644 index 0000000..72371a6 --- /dev/null +++ b/native/pi_control/include/pi_servo_ft.hpp @@ -0,0 +1,183 @@ +/*! + * @file pi_servo_ft.hpp + * @brief ServoFt class for managing FeeTech SMS/STS serial bus servos (STS3215 class). + * + * Covers the SO-ARM100/101 servos: FeeTech STS3215 and the register-compatible + * Hiwonder HX-30HM / HX-10HM (SO-ARM101 kits), which all share the canonical + * "FeeTech STS3215" servo model string. Single-turn position control with a + * hardware Torque Limit register; there is no current control loop, so + * apply_torque() fast-fails and torque-mode effectors approximate torque + * limiting via move(pos, vel, tor). + */ + +#pragma once +#include + +#include "pi_driver_ft.hpp" +#include "pi_servo.hpp" + +/*! + * @brief Parameter container class for FeeTech servo configuration. + */ +class ServoFtParam : public ServoParam { + public: + /*! + * @brief Constructor. + * @param tolerable_pos_difference_rad Tolerable position difference threshold in radians. + * @param max_pos_difference_rad Maximum position difference threshold in radians. + * @param velocity_threshold_rad_sec Velocity threshold in rad/sec. + */ + ServoFtParam(float tolerable_pos_difference_rad, float max_pos_difference_rad, float velocity_threshold_rad_sec) + : ServoParam(tolerable_pos_difference_rad, max_pos_difference_rad, velocity_threshold_rad_sec) {} +}; + +/*! + * @brief Manages FeeTech SMS/STS-type servo motors controlled via serial interface. + */ +class ServoFt : public Servo { + public: + /*! + * @brief Constructor. + * @param p_device Pointer to the Device object. + * @param p_joint Pointer to the Joint object. + * @param p_driver Pointer to the Driver object (must be a DriverFt). + */ + ServoFt(Device* p_device, Joint* p_joint, Driver* p_driver); + + /*! + * @brief Destructor. + */ + ~ServoFt(); + + /*! + * @brief Safely parks the servo before shutdown (torque off, goal aligned to present). + * @return ReturnCode indicating success or failure. + */ + ReturnCode park_safely() override; + + /*! + * @brief Initializes the servo with model configuration. + * @param servo_config JSON object containing the servo model configuration. + * @param p_config Pointer to the device model configuration object. + * @return ReturnCode indicating success or failure. + */ + ReturnCode init_config_model(const json& servo_config, const DeviceConfig* p_config) override; + + /*! + * @brief Starts the servo hardware: position mode, PID gains, acceleration, + * goal-to-present sync, then torque on. + * @return ReturnCode indicating success or failure. + */ + ReturnCode start_hardware() override; + + /*! + * @brief Gets the servo's current position as raw encoder steps. + * @return Current position in encoder steps. + */ + float get_pos_servo() override { return (float)curr_pos_steps_; } + + /*! + * @brief Moves the servo to the target position. + * @param target_pos Target position in relative radians. + * @return ReturnCode indicating success or failure. + */ + ReturnCode move(float target_pos) override; + + /*! + * @brief Moves the servo to the target position with a torque limit. + * + * The STS3215 has no current loop; target_tor is approximated by writing + * the Torque Limit register (0-1000, 0.1% of stall torque) alongside the + * goal position. target_vel is not consumed (the ready-move stepping and + * the Acceleration register bound the motion). + * @param target_pos Target position in relative radians. + * @param target_vel Target velocity in rad/sec (unused). + * @param target_tor Target torque limit in Nm. + * @return ReturnCode indicating success or failure. + */ + ReturnCode move(float target_pos, float target_vel, float target_tor) override; + + /*! + * @brief Direct torque command -- not supported (no current control loop). + * @param torque Torque in Nm. + * @return ReturnCode::NOT_SUPPORTED always. + */ + ReturnCode apply_torque(float torque) override; + + /*! + * @brief Enables or disables torque output of the servo. + * @param enable True to enable torque output, false to disable it. + * @return ReturnCode indicating success or failure. + */ + ReturnCode enable_torque(bool enable) override; + + /*! + * @brief Changes the servo control mode for leader (teleoperation) operation. + * + * The SO-ARM leader is fully passive (LeRobot convention): torque is simply + * disabled so the operator back-drives the arm. Force feedback requires a + * current loop the STS3215 does not have, so it fast-fails. + * @return ReturnCode indicating success or failure. + */ + ReturnCode change_control_mode_for_leader() override; + + /*! + * @brief Changes the servo control mode for follower operation + * (position mode, goal synced to present, torque on). + * @return ReturnCode indicating success or failure. + */ + ReturnCode change_control_mode_for_follower() override; + + /*! + * @brief Gets the servo's encoder resolution. + * @return Servo resolution (encoder counts per revolution). + */ + int get_servo_resolution() override { return servo_resolution_; } + + /*! + * @brief Converts a raw FeeTech velocity value (steps/s) to rad/sec. + * @param servo_value Raw velocity value (sign-magnitude already decoded). + * @return Velocity in rad/sec. + */ + float get_vel_rad_sec(int32_t servo_value) { return servo_value * kv_; } + + /*! + * @brief Converts a raw FeeTech load value (0.1% units) to Nm. + * @param servo_value Raw load value (sign-magnitude already decoded). + * @return Torque in Nm. + */ + float get_tor_nm(int32_t servo_value) { return servo_value * kt_; } + + /*! + * @brief Converts encoder steps to radians (center step = 0 rad). + * @param steps Position in encoder steps. + * @return Position in radians. + */ + float steps_to_rad(int32_t steps) { + return (steps - center_offset_) * (2.0 * M_PI) / (float)servo_resolution_; + } + + /*! + * @brief Converts radians to encoder steps (0 rad = center step). + * @param rad_value Position in radians. + * @return Position in encoder steps. + */ + int32_t rad_to_steps(float rad_value) { + return (int32_t)(rad_value * ((float)servo_resolution_ / (2.0 * M_PI)) + center_offset_); + } + + /*! + * @brief Converts a torque value in Nm to a Torque Limit register value. + * @param torque Torque in Nm (magnitude is used; the register is unsigned). + * @return Torque Limit register value (0-1000, 0.1% of stall torque). + */ + int32_t torque_to_torque_limit(float torque); + + int32_t curr_pos_steps_ = 0; ///< Current position in encoder steps (unsigned single-turn). + uint32_t servo_resolution_ = 0; ///< Servo encoder resolution (counts per full revolution). + int32_t center_offset_ = 0; ///< Encoder step corresponding to 0 rad (calibrated middle, 2048). + + protected: + DriverFt* p_driver_ft_ = nullptr; ///< Pointer to the FeeTech driver. + int prof_accel_ = 0; ///< Optional Acceleration register value (unit 100 steps/s^2; 0 = ramp off). +}; diff --git a/native/pi_control/include/pi_trossen_shim.hpp b/native/pi_control/include/pi_trossen_shim.hpp new file mode 100644 index 0000000..0e5f2ad --- /dev/null +++ b/native/pi_control/include/pi_trossen_shim.hpp @@ -0,0 +1,119 @@ +/*! + * @file pi_trossen_shim.hpp + * @brief TrossenArmShim -- isolation boundary around the prebuilt libtrossen_arm SDK. + * + * libtrossen_arm.a statically bundles its own copies of pinocchio, urdfdom and + * tinyxml2. Linking that archive directly into pi_control_node makes those + * symbols coexist (and interpose) with the shared pinocchio the node links for + * AlgoPino, which mixes two different pinocchio builds inside one process and + * crashes (SIGSEGV in pinocchio::ModelTpl::addJoint during + * TrossenArmDriver::configure). The shim compiles the vendor archive into a + * separate shared library (libpi_trossen_shim) whose archive symbols are + * hidden (Linux: -Wl,--exclude-libs,ALL), so the vendor's pinocchio copy is + * invisible to the rest of the process. + * + * This header is the ONLY interface the node sees: no vendor types, no vendor + * headers. Every vendor exception is caught inside the shim and surfaced as a + * false return + last_error() message so no vendor typeinfo crosses the + * library boundary. + */ + +#pragma once +#include +#include +#include + +/*! + * @brief Thin client for one Trossen iNerve controller connection. + * + * All methods return true on success. On failure they return false and + * last_error() carries the vendor error message (fast-fail: the caller is + * expected to convert to its own error handling immediately). + */ +class TrossenArmShim { + public: + /*! @brief Joint command mode understood by the controller. */ + enum class Mode { + IDLE = 0, ///< Passive hold (controller-side gravity/friction compensation). + POSITION = 1, ///< Streamed position control. + EXTERNAL_EFFORT = 2, ///< Streamed external-effort (torque) control. + }; + + TrossenArmShim(); + ~TrossenArmShim(); + + TrossenArmShim(const TrossenArmShim&) = delete; + TrossenArmShim& operator=(const TrossenArmShim&) = delete; + + /*! + * @brief Connect and configure the controller (TCP handshake + UDP stream). + * @param model Controller model string from the model config (e.g. "wxai_v0"). + * @param leader_end_effector True selects the vendor leader end-effector mass + * properties (teleop handle); false selects the follower set. + * @param address Controller IPv4 address. + * @param clear_error True clears a stale latched controller error before configuring. + * @param timeout_s TCP handshake timeout in seconds. + * @return True on success. + */ + bool configure(const std::string& model, bool leader_end_effector, const std::string& address, bool clear_error, + double timeout_s); + + /*! @brief Release the controller connection (idempotent). @return True on success. */ + bool cleanup(); + + /*! @brief Number of joints reported by the controller (arm + gripper). */ + int num_joints() const; + + /*! @brief Vendor driver (SDK) version string. */ + std::string driver_version() const; + + /*! @brief Controller firmware version string. */ + std::string controller_version() const; + + /*! + * @brief Read the cached whole-arm state (positions, velocities, efforts, rotor temperatures). + * @return True on success; all vectors are resized to num_joints(). + */ + bool read_state(std::vector& positions, std::vector& velocities, std::vector& efforts, + std::vector& temperatures); + + /*! + * @brief Stream position targets for all arm joints (gripper excluded). + */ + bool set_arm_positions(const std::vector& positions, double goal_time, bool blocking, + const std::vector& velocity_ffs); + + /*! + * @brief Stream external-effort targets for all arm joints (gripper excluded). + */ + bool set_arm_external_efforts(const std::vector& efforts, double goal_time, bool blocking); + + /*! @brief Stream a position target for one joint. */ + bool set_joint_position(int joint_index, double position, double goal_time, bool blocking, double velocity_ff); + + /*! @brief Stream an external-effort target for one joint. */ + bool set_joint_external_effort(int joint_index, double effort, double goal_time, bool blocking); + + /*! @brief Stream a position target for the gripper joint. */ + bool set_gripper_position(double position, double goal_time, bool blocking, double velocity_ff); + + /*! @brief Stream an external-effort target for the gripper joint. */ + bool set_gripper_external_effort(double effort, double goal_time, bool blocking); + + /*! + * @brief Apply per-joint command modes (one TCP configuration call). + * @param modes One mode per controller joint. + */ + bool set_joint_modes(const std::vector& modes); + + /*! @brief Idle every joint (controller-side passive hold). */ + bool set_all_modes_idle(); + + /*! @brief Message of the most recent failure (empty when the last call succeeded). */ + const std::string& last_error() const; + + private: + struct Impl; ///< Hides every vendor type behind the shim boundary. + std::unique_ptr impl_; ///< Vendor driver instance (pimpl). + std::string last_error_; ///< Last failure message (vendor exception text). +}; diff --git a/native/pi_control/src/pi_command_line_args.cpp b/native/pi_control/src/pi_command_line_args.cpp index cfc1627..e6c8508 100644 --- a/native/pi_control/src/pi_command_line_args.cpp +++ b/native/pi_control/src/pi_command_line_args.cpp @@ -45,6 +45,8 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { OPT_INFO_LEVEL, po::value()->default_value(0), "The level of information message to see: lower value shows less " "frequent messages")( + OPT_BAUD_RATE, po::value()->default_value(DEFAULT_BAUD_RATE), + "To set the baud rate of the serial port (bps)")( OPT_DOF_ARM, po::value()->default_value(DEFAULT_DOF_ARM), "To set the DOF of the arm")( OPT_SERVO_NUM_ARM, @@ -308,6 +310,15 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { exit(2); } + if (vm.count(OPT_BAUD_RATE)) { + baud_rate = vm[OPT_BAUD_RATE].as(); + PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Baud rate: %d bps", + baud_rate); + } else { + PI_ERROR("--%s is not set", OPT_BAUD_RATE); + exit(2); + } + if (vm.count(OPT_DOF_ARM)) { dof_arm = vm[OPT_DOF_ARM].as(); PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Arm degrees of freedom: %d", diff --git a/native/pi_control/src/pi_control_node.cpp b/native/pi_control/src/pi_control_node.cpp index 9fee8a3..b876cbb 100644 --- a/native/pi_control/src/pi_control_node.cpp +++ b/native/pi_control/src/pi_control_node.cpp @@ -166,7 +166,7 @@ int main(int argc, char** argv) { PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Starting devices..."); - return_code = p_device->start(0); + return_code = p_device->start(cla.baud_rate); if (return_code != ReturnCode::SUCCESS) { // Communication failures should not keep the process running for // long. diff --git a/native/pi_control/src/pi_device.cpp b/native/pi_control/src/pi_device.cpp index 59a0d4a..44a60ec 100644 --- a/native/pi_control/src/pi_device.cpp +++ b/native/pi_control/src/pi_device.cpp @@ -9,8 +9,11 @@ #include "pi_device.hpp" #include "pi_device_arm_arx.hpp" +#include "pi_device_arm_nello.hpp" #include "pi_device_config.hpp" #include "pi_device_effector_arx.hpp" +#include "pi_device_effector_controller.hpp" +#include "pi_device_effector_nello.hpp" #include "pi_info.hpp" Device::Device(const CommandLineArgs& cla) @@ -577,6 +580,17 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf p_device = new DeviceArmArx(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmArx for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); + } else if (arm_type == cfg_model.val_arm_type_controller) { + // Whole-arm controller arms use the DeviceArm base directly: + // its set_control_mode() delegates leader/follower switching + // to the joints, which is exactly what ServoController needs. + p_device = new DeviceArm(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArm (controller) for %s_%s", + cla.device_model.c_str(), cla.device_id.c_str()); + } else if (arm_type == cfg_model.val_arm_type_nello) { + p_device = new DeviceArmNello(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmNello 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; @@ -593,6 +607,14 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf p_device = new DeviceEffectorArx(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorArx for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); + } else if (effector_type == cfg_model.val_effector_type_controller) { + p_device = new DeviceEffectorController(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorController for %s_%s", + cla.device_model.c_str(), cla.device_id.c_str()); + } else if (effector_type == cfg_model.val_effector_type_nello) { + p_device = new DeviceEffectorNello(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorNello for %s_%s", + cla.device_model.c_str(), cla.device_id.c_str()); } else { PI_ERROR("Invalid effector type: %s", effector_type.c_str()); return nullptr; diff --git a/native/pi_control/src/pi_device_arm_nello.cpp b/native/pi_control/src/pi_device_arm_nello.cpp new file mode 100644 index 0000000..797a11d --- /dev/null +++ b/native/pi_control/src/pi_device_arm_nello.cpp @@ -0,0 +1,74 @@ +/*! + * @file pi_device_arm_nello.cpp + * @brief Implementation of the DeviceArmNello class for Nello robotic arm device control. + */ + +#include + +#include "pi_device_arm_nello.hpp" +#include "pi_joint.hpp" +#include "pi_profile.hpp" + +DeviceArmNello::DeviceArmNello(const CommandLineArgs& cla) : DeviceArm(cla) {} + +DeviceArmNello::~DeviceArmNello() {} + +ReturnCode DeviceArmNello::set_control_mode(Role target_role, ControlModeIntent intent) { + // Implementation note: + // - Nello leader/follower switching is already expressed via Joint::change_control_mode_for_{leader,follower}(), + // which maps to servo/driver operation modes. + // - For READY_MOVE_OVERRIDE we still treat it as follower-like (position-based) so that move_to_ready_position() + // can safely send position targets from the current pose. + // - After ready completion, normal operation will call this again with intent NORMAL_OPERATION and the actual role. + ReturnCode rc = ReturnCode::SUCCESS; + + const bool use_follower_like = (intent == ControlModeIntent::READY_MOVE_OVERRIDE) || (target_role == Role::FOLLOWER); + if (use_follower_like) { + for (auto& p_joint : joints_) { + rc = p_joint->change_control_mode_for_follower(); + if (rc != ReturnCode::SUCCESS) return rc; + } + return ReturnCode::SUCCESS; + } + + // Leader + prof_time_t current_time = Profile::get_time_now(); + for (auto& p_joint : joints_) { + rc = p_joint->change_control_mode_for_leader(current_time); + if (rc != ReturnCode::SUCCESS) return rc; + } + + // Leader only: chain to the attached effector (same as DeviceArm::set_control_mode). + // The effector's own step switches its mode once at its ready transition, but that single + // attempt can fail -- the passive-leader torque disable races the final ready-move writes + // on SO-ARM101 -- so re-applying here at arm-ready gives it a second, later chance. The + // call is idempotent. The follower-like branch above intentionally does not chain: Nello + // follower effector modes depend on the configured effector control type and are handled + // by the effector's own ready transition, matching the long-standing Nello behavior. + // Skipped during emergency recovery for the same bus-timeout reason as the base class. + if (p_effector_ && !is_in_emergency_recovery()) { + rc = p_effector_->set_control_mode(p_effector_->get_device_role(), intent); + if (rc != ReturnCode::SUCCESS) return rc; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceArmNello::move_to_ready_position() { + ReturnCode return_code = ReturnCode::SUCCESS; + + return_code = DeviceArm::move_to_ready_position(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to move Nello arm to ready position"); + return return_code; + } + + if (is_ready_ == true) { + // Control-mode restoration is handled by the generic move-to-ready command state machine + // (Device::step()) via set_control_mode(...). Keep move_to_ready_position() focused on motion. + + PI_INFO("DeviceArm", InfoLevel::DETAIL_2, "Ready flag set: %s_%s", model_.c_str(), id_.c_str()); + } + + return return_code; +} diff --git a/native/pi_control/src/pi_device_effector_controller.cpp b/native/pi_control/src/pi_device_effector_controller.cpp new file mode 100644 index 0000000..da1a2dc --- /dev/null +++ b/native/pi_control/src/pi_device_effector_controller.cpp @@ -0,0 +1,18 @@ +/*! + * @file pi_device_effector_controller.cpp + * @brief Implementation of the DeviceEffectorController class. + */ + +#include "pi_device_effector_controller.hpp" + +DeviceEffectorController::DeviceEffectorController(const CommandLineArgs& cla) : DeviceEffectorArx(cla) {} + +DeviceEffectorController::~DeviceEffectorController() {} + +ReturnCode DeviceEffectorController::set_control_mode(Role target_role, ControlModeIntent intent) { + // Controller-managed grippers switch between position (follower / ready + // move) and external effort (leader / torque gripper) on the vendor + // controller; the DeviceEffector base implementation delegates exactly + // that to the joints. + return DeviceEffector::set_control_mode(target_role, intent); +} diff --git a/native/pi_control/src/pi_device_effector_nello.cpp b/native/pi_control/src/pi_device_effector_nello.cpp new file mode 100644 index 0000000..6e34eb5 --- /dev/null +++ b/native/pi_control/src/pi_device_effector_nello.cpp @@ -0,0 +1,160 @@ +/*! + * @file pi_device_effector_nello.cpp + * @brief Implementation of the DeviceEffectorNello class for Nello effector device control. + */ + + #include + +#include "pi_device_effector_nello.hpp" +#include "pi_joint.hpp" +#include "pi_servo.hpp" + +DeviceEffectorNello::DeviceEffectorNello(const CommandLineArgs& cla) : DeviceEffector(cla) {} + +DeviceEffectorNello::~DeviceEffectorNello() {} + +ReturnCode DeviceEffectorNello::set_control_mode(Role target_role, ControlModeIntent intent) { + ReturnCode rc = ReturnCode::SUCCESS; + + // READY_MOVE_OVERRIDE forces a safe position-based behavior regardless of configured effector control type. + // See DeviceEffector::get_effective_control_mode() docs for why we use an override flag instead of mutating + // the persistent control_mode_ (avoid save/restore and make restoration deterministic). + if (intent == ControlModeIntent::READY_MOVE_OVERRIDE) { + set_ready_move_force_position_mode(true); + } else { + set_ready_move_force_position_mode(false); + } + + const bool use_follower_like = (intent == ControlModeIntent::READY_MOVE_OVERRIDE) || (target_role == Role::FOLLOWER); + + if (use_follower_like) { + for (auto& p_joint : joints_) { + rc = p_joint->change_control_mode_for_follower(); + if (rc != ReturnCode::SUCCESS) return rc; + } + return ReturnCode::SUCCESS; + } + + // Leader + prof_time_t current_time = Profile::get_time_now(); + for (auto& p_joint : joints_) { + rc = p_joint->change_control_mode_for_leader(current_time); + if (rc != ReturnCode::SUCCESS) return rc; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceEffectorNello::init(const CommandLineArgs& cla, int argc, char** argv, std::shared_ptr p_topic, + std::shared_ptr p_driver) { + ReturnCode return_code = DeviceEffector::init(cla, argc, argv, p_topic, p_driver); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Base effector initialization failed"); + return return_code; + } + + ///< @note JSON configuration assumes effector servos follow arm servos in the data array. When no arm is + ///< attached, effector servo data indices are adjusted to start from 0. + if (p_arm_ == nullptr) { + int servo_data_index = 0; + for (auto& p_joint : joints_) { + for (auto& p_servo : p_joint->get_servos()) { + // data_index_ is a base Servo member; no family-specific cast + // is needed (works for Dynamixel and FeeTech servos alike). + p_servo->data_index_ = servo_data_index; + servo_data_index++; + } + } + } + + return return_code; +} + +ReturnCode DeviceEffectorNello::move_to_ready_position() { + ReturnCode return_code = ReturnCode::SUCCESS; + + if (is_read_only() == true) { + // Read-only Nello effector (e.g. T6_2.0A leader-side or T6_2.0A as a follower + // gripper that reports position but isn't actuated): mirror the base-class + // short-circuit so the device's is_ready_ flag actually flips. Without this, + // pi_control_node never publishes DEVICE_INFO_READY_NOW and the UI hangs in + // "waiting for ready" forever. + is_ready_ = true; + PI_INFO("DeviceEffector", InfoLevel::ESSENTIAL_0, + "%s_%s is read only, skipping move to ready position", + model_.c_str(), id_.c_str()); + return ReturnCode::SUCCESS; + } + + int i = 0; + for (auto& p_joint : joints_) { + tele_pos_[i] = p_joint->get_pos_rad_relative(); + + // Engage torque once per ready move, not on every control cycle. Re-sending + // enable(true) on the cycle where the effector reaches ready races the + // leader-passive torque disable that follows in the same cycle: SO-ARM101 + // servos (Hiwonder HX firmware) ignore a torque-off arriving right after a + // torque-on, leaving the leader gripper locked. + if (ready_move_torque_engaged_ == false) { + for (auto& p_servo : p_joint->get_servos()) { + // enable_torque() is virtual on the Servo base class, so this path + // works for every bus-servo family (Dynamixel, FeeTech). + p_servo->enable_torque(true); + usleep(100); + } + } + + i++; + } + ready_move_torque_engaged_ = true; + + return_code = DeviceEffector::move_to_ready_position(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to move Nello effector to ready position"); + return return_code; + } + + if (is_ready_ == true) { + // Control-mode restoration is handled by the generic move-to-ready command state machine + // (Device::step()) via set_control_mode(...). Keep move_to_ready_position() focused on motion. + + PI_INFO("DeviceEffector", InfoLevel::DETAIL_2, "Ready flag set: %s_%s", model_.c_str(), id_.c_str()); + } + + return return_code; +} + +ReturnCode DeviceEffectorNello::move_joint_with_torque(Joint* p_joint, float target_pos) { + ReturnCode return_code = ReturnCode::SUCCESS; + + if (is_read_only() == true) { + // If the device is read only, skip the move joint with torque + return ReturnCode::SUCCESS; + } + + ///< @note Torque-based position control requires fast position monitoring (more than 100Hz) for stability + + float clipped_target_pos = + p_joint->clipping(target_pos, p_joint->get_pos_min_relative(), p_joint->get_pos_max_relative()); + float clipped_curr_pos = p_joint->clipping(p_joint->get_pos_rad_relative(), p_joint->get_pos_min_relative(), + p_joint->get_pos_max_relative()); + + float distance = clipped_target_pos - clipped_curr_pos; + + float torq_to_apply = distance_to_torque_ * distance; + + ///< @note The torque is calculated from the parameter joint but applied to all joints + for (auto& p_joint_iter : joints_) { + return_code = p_joint_iter->move(clipped_target_pos, 0, torq_to_apply); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to apply torque to joint %d in %s_%s", p_joint_iter->id_, model_.c_str(), id_.c_str()); + return return_code; + } + } + + PI_INFO("DeviceEffector", InfoLevel::FREQUENT_3, + "Nello effector torque control: target_pos_rel=%.3f, clipped_target_pos=%.3f, curr_pos_rel=%.3f, " + "clipped_curr_pos=%.3f, distance=%.3f, torq_to_apply=%.3f, distance_to_torque_=%.3f", + target_pos, clipped_target_pos, p_joint->get_pos_rad_relative(), clipped_curr_pos, distance, torq_to_apply, + distance_to_torque_); + return return_code; +} diff --git a/native/pi_control/src/pi_driver.cpp b/native/pi_control/src/pi_driver.cpp index 56679c0..0cdad71 100644 --- a/native/pi_control/src/pi_driver.cpp +++ b/native/pi_control/src/pi_driver.cpp @@ -8,6 +8,8 @@ #include "pi_device_config.hpp" #include "pi_driver_arx.hpp" #include "pi_driver_arx_encoder.hpp" +#include "pi_driver_ft.hpp" +#include "pi_driver_trossen.hpp" #include "pi_servo.hpp" Driver::Driver(Device* p_device, const CommandLineArgs& cla) { @@ -39,8 +41,24 @@ std::shared_ptr Driver::new_driver(Device* p_device, const DeviceConfig* p_driver = p_driver_encoder; PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created CAN read-only encoder driver (DriverArxEncoder)"); + } else if (driver_type == p_config->val_driver_type_trossen) { + std::string controller_model; + return_code = p_config->get_field_value(p_config->values_, p_config->fn_controller_model, controller_model); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("controller_model is not defined in the model config (required for TROSSEN_ETHERNET)"); + return nullptr; + } + p_driver = std::make_shared(p_device, cla, controller_model); + PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created Trossen controller driver (DriverTrossen, model=%s)", + controller_model.c_str()); + + } else if (driver_type == p_config->val_driver_type_ft) { + p_driver = std::make_shared(p_device, cla); + PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created FeeTech driver (DriverFt)"); + } else { - PI_ERROR("Unsupported driver type: '%s' (supported types: CAN, CAN_ENCODER)", driver_type.c_str()); + PI_ERROR("Unsupported driver type: '%s' (supported types: CAN, CAN_ENCODER, TROSSEN_ETHERNET, FEETECH)", + driver_type.c_str()); return nullptr; } diff --git a/native/pi_control/src/pi_driver_controller.cpp b/native/pi_control/src/pi_driver_controller.cpp new file mode 100644 index 0000000..e644d29 --- /dev/null +++ b/native/pi_control/src/pi_driver_controller.cpp @@ -0,0 +1,385 @@ +/*! + * @file pi_driver_controller.cpp + * @brief Implementation of the DriverController vendor-neutral whole-arm controller base class. + */ + +#include "pi_driver_controller.hpp" + +#include + +#include "pi_servo_controller.hpp" + +namespace { + +/*! + * @brief Monotonic milliseconds for the stall watchdog. + */ +int64_t steady_now_ms() { + return std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +} // namespace + +DriverController::DriverController(Device* p_device, const CommandLineArgs& cla) : Driver(p_device, cla) {} + +DriverController::~DriverController() { + // Vendor hooks are gone once the derived destructor ran, so the derived + // class must call close() in its own destructor. This base destructor only + // stops the watchdog thread as a last line of defense. + stop_watchdog(); +} + +ReturnCode DriverController::open(int baud_rate) { + (void)baud_rate; // Ethernet controllers have no baud rate. + + if (opened_) { + PI_INFO("DriverController", InfoLevel::HELPFUL_1, "open() called on an already-open controller connection"); + return ReturnCode::SUCCESS; + } + + if (control_port_name_.empty()) { + PI_ERROR("Controller address is empty (--control_port_name must carry the controller IP address)"); + return ReturnCode::INVALID_PARAM; + } + + int joint_count = 0; + ReturnCode return_code = vendor_connect(control_port_name_, joint_count); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to connect to the arm controller at '%s'", control_port_name_.c_str()); + return return_code; + } + if (joint_count <= 0) { + PI_ERROR("Arm controller at '%s' reported an invalid joint count: %d", control_port_name_.c_str(), + joint_count); + vendor_disconnect(); + return ReturnCode::HARDWARE_FAULT; + } + joint_count_ = joint_count; + + { + std::lock_guard lock(command_mutex_); + commands_.assign(joint_count_, JointCommand{}); + desired_modes_.assign(joint_count_, JointCommandMode::UNSET); + modes_dirty_ = false; + } + { + std::lock_guard lock(state_mutex_); + states_.assign(joint_count_, JointState{}); + } + read_success_count_ = 0; + made_safe_ = false; + watchdog_tripped_ = false; + + opened_ = true; + + // One initial whole-arm read so every ServoController position cache is + // valid before start_hardware() / hold-at-current seeding runs. + return_code = group_read_hardware_values(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Initial whole-arm state read failed for controller at '%s'", control_port_name_.c_str()); + close(); + return return_code; + } + + PI_INFO("DriverController", InfoLevel::ESSENTIAL_0, "Connected to arm controller at '%s' (%d joints)", + control_port_name_.c_str(), joint_count_); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::close() { + stop_watchdog(); + + if (!opened_) { + return ReturnCode::SUCCESS; + } + + // Idle the arm before dropping the connection so the controller never + // keeps executing the last streamed command. + make_safe(); + + ReturnCode return_code = vendor_disconnect(); + opened_ = false; + + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Controller disconnect failed for '%s'", control_port_name_.c_str()); + return return_code; + } + + PI_INFO("DriverController", InfoLevel::ESSENTIAL_0, "Disconnected from arm controller at '%s'", + control_port_name_.c_str()); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::read_hardware_values(Servo* p_servo) { + ServoController* p_servo_controller = dynamic_cast(p_servo); + if (p_servo_controller == nullptr) { + PI_ERROR("read_hardware_values() called with a non-ServoController servo"); + return ReturnCode::INVALID_PARAM; + } + + const int data_index = p_servo_controller->data_index_; + ReturnCode return_code = validate_data_index(data_index); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + JointState state; + { + std::lock_guard lock(state_mutex_); + state = states_[data_index]; + } + if (!state.valid) { + PI_ERROR("Joint slot %d has no valid controller state yet (servo ID %d)", data_index, + p_servo_controller->id_); + return ReturnCode::NO_RESPONSE; + } + + p_servo_controller->update_from_controller_state(state.position, state.velocity, state.effort, state.temperature); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::group_read_hardware_values() { + if (!opened_) { + PI_ERROR("group_read_hardware_values() called before open()"); + return ReturnCode::NOT_INITIALIZED; + } + + std::vector states(joint_count_); + ReturnCode return_code = vendor_read_state(states); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Whole-arm state read failed for controller at '%s'", control_port_name_.c_str()); + return return_code; + } + + { + std::lock_guard lock(state_mutex_); + states_ = std::move(states); + } + read_success_count_++; + feed_watchdog(); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::group_write_hardware_values() { + if (!opened_) { + PI_ERROR("group_write_hardware_values() called before open()"); + return ReturnCode::NOT_INITIALIZED; + } + if (watchdog_tripped_) { + PI_ERROR("Stall watchdog idled the arm; refusing further commands (restart required)"); + return ReturnCode::SAFE_MODE_SIG; + } + + std::vector commands; + std::vector modes; + bool apply_modes = false; + { + std::lock_guard lock(command_mutex_); + if (modes_dirty_) { + modes = desired_modes_; + apply_modes = true; + modes_dirty_ = false; + } + commands = commands_; + for (auto& command : commands_) { + command.pending = false; + } + } + + if (apply_modes) { + ReturnCode return_code = vendor_apply_command_modes(modes); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to apply joint control modes on controller at '%s'", control_port_name_.c_str()); + { + // Restore the dirty flag so the next flush retries the mode change + // before any command is interpreted under a stale mode. + std::lock_guard lock(command_mutex_); + modes_dirty_ = true; + } + return return_code; + } + { + // The arm is active again; a later shutdown must run vendor_make_safe() + // even when an earlier make_safe() already latched. + std::lock_guard lock(safe_mutex_); + made_safe_ = false; + } + } + + bool any_pending = false; + for (const auto& command : commands) { + if (command.pending) { + any_pending = true; + break; + } + } + if (any_pending) { + ReturnCode return_code = vendor_write_commands(commands); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Whole-arm command write failed for controller at '%s'", control_port_name_.c_str()); + return return_code; + } + } + + feed_watchdog(); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::arm_comm_loss_protection() { + if (!opened_) { + PI_ERROR("arm_comm_loss_protection() called before open()"); + return ReturnCode::NOT_INITIALIZED; + } + if (watchdog_running_) { + return ReturnCode::SUCCESS; + } + + last_interaction_ms_ = steady_now_ms(); + watchdog_running_ = true; + watchdog_thread_ = std::thread(&DriverController::watchdog_loop, this); + + PI_INFO("DriverController", InfoLevel::ESSENTIAL_0, + "Stall watchdog armed (timeout %d ms): the arm is idled when the host control loop stops", + CONTROLLER_STALL_WATCHDOG_TIMEOUT_MS); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::set_joint_command_mode(int data_index, JointCommandMode mode) { + ReturnCode return_code = validate_data_index(data_index); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + if (mode == JointCommandMode::UNSET) { + PI_ERROR("Joint slot %d: UNSET is not a requestable control mode", data_index); + return ReturnCode::INVALID_PARAM; + } + + std::lock_guard lock(command_mutex_); + if (desired_modes_[data_index] == mode) { + return ReturnCode::SUCCESS; + } + desired_modes_[data_index] = mode; + modes_dirty_ = true; + // Drop a command queued for the previous mode; it must not be sent under + // the new mode. + commands_[data_index] = JointCommand{}; + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::queue_position(int data_index, float position, float velocity_ff) { + ReturnCode return_code = validate_data_index(data_index); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + std::lock_guard lock(command_mutex_); + if (desired_modes_[data_index] != JointCommandMode::POSITION) { + PI_ERROR("Joint slot %d: position command queued while the joint is not in POSITION mode", data_index); + return ReturnCode::INVALID_PARAM; + } + JointCommand& command = commands_[data_index]; + command.mode = JointCommandMode::POSITION; + command.position = position; + command.velocity_ff = velocity_ff; + command.external_effort = 0; + command.pending = true; + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::queue_external_effort(int data_index, float external_effort) { + ReturnCode return_code = validate_data_index(data_index); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + std::lock_guard lock(command_mutex_); + if (desired_modes_[data_index] != JointCommandMode::EXTERNAL_EFFORT) { + PI_ERROR("Joint slot %d: external effort queued while the joint is not in EXTERNAL_EFFORT mode", data_index); + return ReturnCode::INVALID_PARAM; + } + JointCommand& command = commands_[data_index]; + command.mode = JointCommandMode::EXTERNAL_EFFORT; + command.external_effort = external_effort; + command.position = 0; + command.velocity_ff = 0; + command.pending = true; + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::make_safe() { + std::lock_guard lock(safe_mutex_); + if (made_safe_ || !opened_) { + return ReturnCode::SUCCESS; + } + + ReturnCode return_code = vendor_make_safe(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to put the arm controller at '%s' into its safe passive state", control_port_name_.c_str()); + return return_code; + } + + made_safe_ = true; + { + std::lock_guard command_lock(command_mutex_); + for (auto& mode : desired_modes_) { + mode = JointCommandMode::UNSET; + } + for (auto& command : commands_) { + command = JointCommand{}; + } + modes_dirty_ = false; + } + + PI_INFO("DriverController", InfoLevel::ESSENTIAL_0, "Arm controller at '%s' put into safe passive state", + control_port_name_.c_str()); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverController::validate_data_index(int data_index) const { + if (data_index < 0 || data_index >= joint_count_) { + PI_ERROR("Joint slot %d is out of range (controller manages %d joints)", data_index, joint_count_); + return ReturnCode::INVALID_PARAM; + } + return ReturnCode::SUCCESS; +} + +void DriverController::watchdog_loop() { + while (watchdog_running_) { + std::this_thread::sleep_for(std::chrono::milliseconds(CONTROLLER_STALL_WATCHDOG_PERIOD_MS)); + if (!watchdog_running_) { + break; + } + + const int64_t idle_ms = steady_now_ms() - last_interaction_ms_; + if (idle_ms <= CONTROLLER_STALL_WATCHDOG_TIMEOUT_MS) { + continue; + } + if (watchdog_tripped_) { + continue; + } + + PI_ERROR( + "Stall watchdog: no driver interaction for %lld ms (host control loop hang?); " + "idling the arm controller at '%s'", + (long long)idle_ms, control_port_name_.c_str()); + watchdog_tripped_ = true; + make_safe(); + } +} + +void DriverController::feed_watchdog() { + last_interaction_ms_ = steady_now_ms(); +} + +void DriverController::stop_watchdog() { + watchdog_running_ = false; + if (watchdog_thread_.joinable()) { + watchdog_thread_.join(); + } +} diff --git a/native/pi_control/src/pi_driver_ft.cpp b/native/pi_control/src/pi_driver_ft.cpp new file mode 100644 index 0000000..1ef0cdf --- /dev/null +++ b/native/pi_control/src/pi_driver_ft.cpp @@ -0,0 +1,768 @@ +/*! + * @file pi_driver_ft.cpp + * @brief Implementation of the DriverFt class for FeeTech SMS/STS serial bus servos. + */ + +#include "pi_driver_ft.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include "pi_info.hpp" +#include "pi_servo_ft.hpp" + +namespace { + +// FeeTech SMS/STS wire protocol (mirrors pi_control/servos/ft_serial.py). +constexpr uint8_t kFtHeaderByte = 0xFF; +constexpr uint8_t kFtBroadcastId = 0xFE; + +constexpr uint8_t kFtInstPing = 0x01; +constexpr uint8_t kFtInstRead = 0x02; +constexpr uint8_t kFtInstWrite = 0x03; +constexpr uint8_t kFtInstSyncRead = 0x82; +constexpr uint8_t kFtInstSyncWrite = 0x83; + +// SMS/STS control table (subset used by the C++ control loop). +constexpr uint8_t kFtAddrPositionPGain = 21; +constexpr uint8_t kFtAddrPositionDGain = 22; +constexpr uint8_t kFtAddrPositionIGain = 23; +constexpr uint8_t kFtAddrOperatingMode = 33; +constexpr uint8_t kFtAddrTorqueEnable = 40; +constexpr uint8_t kFtAddrAcceleration = 41; +constexpr uint8_t kFtAddrGoalPosition = 42; +constexpr uint8_t kFtAddrTorqueLimit = 48; +constexpr uint8_t kFtAddrLockFlag = 55; +constexpr uint8_t kFtAddrPresentPosition = 56; + +// Bulk feedback window: Present Position(2) + Velocity(2) + Load(2) + +// Voltage(1) + Temperature(1) + Async Write Flag(1) + Servo Status(1), +// contiguous at addresses 56..65. +constexpr uint8_t kFtFeedbackLength = 10; + +// Registers below this address live in EEPROM on SMS/STS servos and require +// the Lock Flag (addr 55) to be cleared before a write takes effect. +constexpr uint8_t kFtSramStartAddr = 40; + +constexpr uint8_t kFtLockFlagLocked = 1; +constexpr uint8_t kFtLockFlagUnlocked = 0; + +// Sign-magnitude bit positions (STS3215 feedback encoding). +constexpr int kFtVelocitySignBit = 15; +constexpr int kFtLoadSignBit = 10; + +// Per-packet response deadline. At 1 Mbps a 16-byte status packet takes +// ~0.16 ms on the wire; the dominant term is USB latency (up to one 16 ms +// FTDI/CDC latency window), so allow two windows plus margin. +constexpr int kFtResponseTimeoutMs = 40; + +// Retry policy for single-register transactions; mirrors DriverDxl's +// kSingleRegMaxAttempts rationale (transient bus glitches on half-duplex +// serial should not abort a control-mode change). +constexpr int kFtSingleRegMaxAttempts = 3; + +// Instruction packet: FF FF ID LEN INST [params] CHK. +constexpr size_t kFtInstructionOverhead = 6; +constexpr size_t kFtTxBufCapacity = 64; +constexpr size_t kFtMaxInstructionParams = kFtTxBufCapacity - kFtInstructionOverhead; + +// Status packet parameter capacity (largest expected: the feedback window). +constexpr size_t kFtMaxStatusParams = 32; + +constexpr int kFtGoalPositionMin = 0; +constexpr int kFtGoalPositionMax = 4095; +constexpr int kFtTorqueLimitMin = 0; +constexpr int kFtTorqueLimitMax = 1000; + +uint8_t ft_checksum(const uint8_t* p_data, size_t length) { + unsigned int sum = 0; + for (size_t i = 0; i < length; i++) { + sum += p_data[i]; + } + return static_cast(~sum & 0xFF); +} + +} // namespace + +DriverFt::DriverFt(Device* p_device, const CommandLineArgs& cla) : DriverSerial(p_device, cla) { + if (p_device == nullptr) { + PI_ERROR("Device pointer is null in DriverFt constructor"); + } +} + +DriverFt::~DriverFt() {} + +int32_t DriverFt::decode_sign_magnitude(uint32_t value, int sign_bit) { + const uint32_t sign_mask = 1u << sign_bit; + if (value & sign_mask) { + return -static_cast(value & (sign_mask - 1)); + } + return static_cast(value); +} + +uint32_t DriverFt::encode_sign_magnitude(int32_t value, int sign_bit) { + if (value < 0) { + return static_cast(-value) | (1u << sign_bit); + } + return static_cast(value); +} + +ReturnCode DriverFt::open(int baud_rate) { + ReturnCode return_code = DriverSerial::open(baud_rate); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + if (p_device_ == nullptr) { + PI_ERROR("Device pointer is null in DriverFt::open()"); + return ReturnCode::NOT_INITIALIZED; + } + + all_servo_ids_.clear(); + p_device_->get_servo_ids(all_servo_ids_); + sync_read_ids_ = all_servo_ids_; + dead_servo_ids_.clear(); + last_failed_servo_id_ = -1; + + servo_num_total_ = static_cast(all_servo_ids_.size()); + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, "Total number of FeeTech servos: %d", servo_num_total_); + + pres_pos_.assign(servo_num_total_, 0); + pres_vel_.assign(servo_num_total_, 0); + pres_load_.assign(servo_num_total_, 0); + pres_temp_.assign(servo_num_total_, 0); + pres_status_.assign(servo_num_total_, 0); + + pending_goal_position_.clear(); + pending_torque_limit_.clear(); + pending_goal_position_.reserve(servo_num_total_); + pending_torque_limit_.reserve(servo_num_total_); + + if (servo_num_total_ == 0) { + // Devices with no bus servos of their own (mirrors the DriverDxl empty + // id-list tolerance); nothing to verify. + return ReturnCode::SUCCESS; + } + + // Verify SYNC READ support once (requires STS3215 firmware with the 0x82 + // instruction). Older firmware silently ignores it, which would otherwise + // surface as a timeout on every control cycle -- fast-fail here instead. + return_code = group_read_hardware_values(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR( + "FeeTech SYNC READ verification failed on '%s'. Check servo power/cabling and that the " + "servo firmware supports SYNC READ (0x82); there is no sequential-read fallback.", + control_port_name_.c_str()); + return ReturnCode::FAIL; + } + + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, "DriverFt initialization complete: %d servos ready to operate", + servo_num_total_); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::close() { return DriverSerial::close(); } + +void DriverFt::flush_input() { + if (port_handler_ >= 0) { + ::tcflush(port_handler_, TCIFLUSH); + } +} + +size_t DriverFt::read_exact(uint8_t* p_buf, size_t length, int deadline_ms) { + if (port_handler_ < 0 || p_buf == nullptr || length == 0) { + return 0; + } + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(deadline_ms); + size_t received = 0; + while (received < length) { + const ssize_t n = ::read(port_handler_, p_buf + received, length - received); + if (n > 0) { + received += static_cast(n); + continue; + } + if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR) { + PI_ERROR("DriverFt::read_exact: read failed on '%s': %s", control_port_name_.c_str(), + std::strerror(errno)); + return received; + } + + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) { + return received; + } + const int remain_ms = + static_cast(std::chrono::duration_cast(deadline - now).count()); + struct pollfd pfd {}; + pfd.fd = port_handler_; + pfd.events = POLLIN; + ::poll(&pfd, 1, remain_ms > 0 ? remain_ms : 1); + } + return received; +} + +ReturnCode DriverFt::send_instruction(uint8_t id, uint8_t instruction, const uint8_t* p_params, size_t param_length) { + if (param_length > kFtMaxInstructionParams) { + PI_ERROR("DriverFt::send_instruction: param_length=%zu exceeds limit %zu (servo ID %d, instruction 0x%02X)", + param_length, kFtMaxInstructionParams, id, instruction); + return ReturnCode::INVALID_PARAM; + } + + uint8_t packet[kFtTxBufCapacity]; + packet[0] = kFtHeaderByte; + packet[1] = kFtHeaderByte; + packet[2] = id; + packet[3] = static_cast(param_length + 2); + packet[4] = instruction; + if (param_length > 0) { + std::memcpy(&packet[5], p_params, param_length); + } + // Checksum covers ID..params (bytes 2 .. 4+param_length). + packet[5 + param_length] = ft_checksum(&packet[2], param_length + 3); + + return write_bytes(packet, kFtInstructionOverhead + param_length); +} + +ReturnCode DriverFt::receive_status(uint8_t expected_id, uint8_t* p_params, size_t param_length, int timeout_ms, + int* p_packet_id) { + if (param_length > kFtMaxStatusParams) { + PI_ERROR("DriverFt::receive_status: param_length=%zu exceeds capacity %zu", param_length, kFtMaxStatusParams); + return ReturnCode::INVALID_PARAM; + } + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + auto remaining_ms = [&deadline]() { + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) { + return 0; + } + return static_cast(std::chrono::duration_cast(deadline - now).count()); + }; + + // Resynchronize on the FF FF header, byte by byte. A servo ID is never + // 0xFF (max 253), so the first non-FF byte after two or more FF bytes is + // the packet ID. + uint8_t packet_id = 0; + int header_ff_count = 0; + while (true) { + uint8_t byte = 0; + const int remain = remaining_ms(); + if (remain <= 0 || read_exact(&byte, 1, remain) != 1) { + PI_ERROR("DriverFt: status packet timeout waiting for servo ID %d", expected_id); + return ReturnCode::NO_RESPONSE; + } + if (byte == kFtHeaderByte) { + header_ff_count++; + continue; + } + if (header_ff_count >= 2) { + packet_id = byte; + break; + } + header_ff_count = 0; + } + + uint8_t length_error[2] = {0, 0}; + if (read_exact(length_error, 2, remaining_ms()) != 2) { + PI_ERROR("DriverFt: status packet truncated after header (servo ID %d)", packet_id); + return ReturnCode::NO_RESPONSE; + } + const uint8_t packet_length = length_error[0]; + const uint8_t error_byte = length_error[1]; + + if (packet_length != param_length + 2) { + PI_ERROR("DriverFt: unexpected status length %d (expected %zu) from servo ID %d", packet_length, + param_length + 2, packet_id); + flush_input(); + return ReturnCode::FAIL; + } + + uint8_t body[kFtMaxStatusParams + 1]; + const size_t body_length = param_length + 1; // params + checksum + if (read_exact(body, body_length, remaining_ms()) != body_length) { + PI_ERROR("DriverFt: status packet truncated in body (servo ID %d)", packet_id); + return ReturnCode::NO_RESPONSE; + } + + // Checksum covers ID, LEN, ERR and params. + unsigned int sum = static_cast(packet_id) + packet_length + error_byte; + for (size_t i = 0; i < param_length; i++) { + sum += body[i]; + } + const uint8_t expected_checksum = static_cast(~sum & 0xFF); + if (body[param_length] != expected_checksum) { + PI_ERROR("DriverFt: status checksum mismatch from servo ID %d (received 0x%02X, expected 0x%02X)", packet_id, + body[param_length], expected_checksum); + flush_input(); + return ReturnCode::FAIL; + } + + if (expected_id != kFtBroadcastId && packet_id != expected_id) { + PI_ERROR("DriverFt: status packet from unexpected servo ID %d (expected %d)", packet_id, expected_id); + flush_input(); + return ReturnCode::FAIL; + } + + if (error_byte != 0) { + // Servo answered but reports fault bits (voltage/angle/overheat/ + // overcurrent/overload). Surface them but treat the transaction as + // completed; latched faults also show up in the Servo Status register. + PI_WARN("DriverFt: servo ID %d status error bits 0x%02X", packet_id, error_byte); + } + + if (param_length > 0 && p_params != nullptr) { + std::memcpy(p_params, body, param_length); + } + if (p_packet_id != nullptr) { + *p_packet_id = packet_id; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::txrx_instruction(uint8_t id, uint8_t instruction, const uint8_t* p_params, size_t param_length, + uint8_t* p_rsp_params, size_t rsp_param_length) { + ReturnCode return_code = ReturnCode::FAIL; + for (int attempt = 1; attempt <= kFtSingleRegMaxAttempts; attempt++) { + // Discard any stale bytes (late response of a previously timed-out + // transaction) so they cannot be misread as this response. + flush_input(); + + return_code = send_instruction(id, instruction, p_params, param_length); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to send instruction 0x%02X to servo ID %d", instruction, id); + return return_code; + } + + return_code = receive_status(id, p_rsp_params, rsp_param_length, kFtResponseTimeoutMs, nullptr); + if (return_code == ReturnCode::SUCCESS) { + return ReturnCode::SUCCESS; + } + + if (attempt < kFtSingleRegMaxAttempts) { + PI_WARN("DriverFt: transaction 0x%02X with servo ID %d failed (attempt %d/%d), retrying", instruction, id, + attempt, kFtSingleRegMaxAttempts); + } + } + return return_code; +} + +ReturnCode DriverFt::ping(int id) { + return txrx_instruction(static_cast(id), kFtInstPing, nullptr, 0, nullptr, 0); +} + +ReturnCode DriverFt::read_register(int id, uint8_t address, uint8_t length, int32_t& value) { + if (length != 1 && length != 2) { + PI_ERROR("DriverFt::read_register: unsupported length %d (servo ID %d, address %d)", length, id, address); + return ReturnCode::INVALID_PARAM; + } + + const uint8_t params[2] = {address, length}; + uint8_t response[2] = {0, 0}; + ReturnCode return_code = txrx_instruction(static_cast(id), kFtInstRead, params, 2, response, length); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to read register %d (servo ID %d)", address, id); + return return_code; + } + + value = (length == 1) ? response[0] : static_cast(response[0] | (response[1] << 8)); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::write_register_raw(int id, uint8_t address, int32_t value, uint8_t length) { + if (length != 1 && length != 2) { + PI_ERROR("DriverFt::write_register: unsupported length %d (servo ID %d, address %d)", length, id, address); + return ReturnCode::INVALID_PARAM; + } + + uint8_t params[3]; + params[0] = address; + params[1] = static_cast(value & 0xFF); + size_t param_length = 2; + if (length == 2) { + params[2] = static_cast((value >> 8) & 0xFF); + param_length = 3; + } + + ReturnCode return_code = txrx_instruction(static_cast(id), kFtInstWrite, params, param_length, nullptr, 0); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to write register %d = %d (servo ID %d)", address, value, id); + } + return return_code; +} + +ReturnCode DriverFt::write_register(int id, uint8_t address, int32_t value, uint8_t length) { + if (address >= kFtSramStartAddr) { + return write_register_raw(id, address, value, length); + } + + // EEPROM register: the STS3215 ships write-locked (Lock Flag = 1) and + // silently ignores EEPROM writes in that state, so wrap the write with an + // unlock/re-lock pair. + ReturnCode return_code = write_register_raw(id, kFtAddrLockFlag, kFtLockFlagUnlocked, 1); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to unlock EEPROM before writing register %d (servo ID %d)", address, id); + return return_code; + } + + const ReturnCode write_result = write_register_raw(id, address, value, length); + + return_code = write_register_raw(id, kFtAddrLockFlag, kFtLockFlagLocked, 1); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to re-lock EEPROM after writing register %d (servo ID %d)", address, id); + return return_code; + } + + return write_result; +} + +ReturnCode DriverFt::sync_write(uint8_t address, uint8_t data_length, + const std::vector>& entries) { + if (entries.empty()) { + return ReturnCode::SUCCESS; + } + + // SYNC WRITE params: ADDR, DATALEN, then (ID, DATA...) per servo. + const size_t param_length = 2 + entries.size() * (1 + data_length); + if (param_length > kFtMaxInstructionParams) { + PI_ERROR("DriverFt::sync_write: packet too large for %zu servos (address %d)", entries.size(), address); + return ReturnCode::INVALID_PARAM; + } + + uint8_t params[kFtMaxInstructionParams]; + params[0] = address; + params[1] = data_length; + size_t offset = 2; + for (const auto& [id, value] : entries) { + params[offset++] = static_cast(id); + params[offset++] = static_cast(value & 0xFF); + if (data_length == 2) { + params[offset++] = static_cast((value >> 8) & 0xFF); + } + } + + // SYNC WRITE is broadcast; servos do not answer. + return send_instruction(kFtBroadcastId, kFtInstSyncWrite, params, param_length); +} + +void DriverFt::classify_dead_servos() { + bool need_probe = false; + for (int id : all_servo_ids_) { + if (dead_servo_ids_.count(id) == 0) { + need_probe = true; + break; + } + } + if (need_probe) { + for (int id : all_servo_ids_) { + if (dead_servo_ids_.count(id) != 0) continue; + if (ping(id) != ReturnCode::SUCCESS) { + flush_input(); + dead_servo_ids_.insert(id); + PI_ERROR("Identified dead FeeTech servo via ping: ID %d", id); + } + } + } + + // Rebuild the SYNC READ id list so subsequent bulk reads only talk to the + // alive servos (keeps their cached pres_* arrays fresh during recovery). + sync_read_ids_.clear(); + for (int id : all_servo_ids_) { + if (dead_servo_ids_.count(id) == 0) { + sync_read_ids_.push_back(id); + } + } + last_failed_servo_id_ = dead_servo_ids_.empty() ? -1 : *dead_servo_ids_.begin(); +} + +ReturnCode DriverFt::group_read_hardware_values() { + // last_failed_servo_id_ / dead_servo_ids_ are sticky across cycles; see + // DriverDxl::group_read_hardware_values() for the rationale. + + if (port_handler_ < 0) { + PI_ERROR("Serial port is not open in group_read_hardware_values()"); + return ReturnCode::NOT_INITIALIZED; + } + + if (all_servo_ids_.empty()) { + return ReturnCode::SUCCESS; + } + + if (sync_read_ids_.empty()) { + // Every servo is classified dead; nothing to read. + return ReturnCode::FAIL; + } + + // Build one SYNC READ over the feedback window for all alive servos. + const size_t param_length = 2 + sync_read_ids_.size(); + if (param_length > kFtMaxInstructionParams) { + PI_ERROR("DriverFt: too many servos (%zu) for one SYNC READ packet", sync_read_ids_.size()); + return ReturnCode::INVALID_PARAM; + } + + uint8_t params[kFtMaxInstructionParams]; + params[0] = kFtAddrPresentPosition; + params[1] = kFtFeedbackLength; + for (size_t i = 0; i < sync_read_ids_.size(); i++) { + params[2 + i] = static_cast(sync_read_ids_[i]); + } + + flush_input(); + ReturnCode return_code = send_instruction(kFtBroadcastId, kFtInstSyncRead, params, param_length); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("DriverFt: failed to send SYNC READ packet"); + return return_code; + } + + // Collect one status packet per servo. Responses arrive in the id-list + // order, but a dropped response must not desynchronize the parse, so each + // packet is matched by the ID it carries rather than by arrival order. + std::set pending_ids(sync_read_ids_.begin(), sync_read_ids_.end()); + uint8_t feedback[kFtFeedbackLength]; + for (size_t i = 0; i < sync_read_ids_.size(); i++) { + int packet_id = -1; + return_code = receive_status(kFtBroadcastId, feedback, kFtFeedbackLength, kFtResponseTimeoutMs, &packet_id); + if (return_code != ReturnCode::SUCCESS) { + break; + } + + if (pending_ids.count(packet_id) == 0) { + PI_ERROR("DriverFt: SYNC READ response from unexpected servo ID %d", packet_id); + return_code = ReturnCode::FAIL; + break; + } + pending_ids.erase(packet_id); + + const int data_index = find_data_index(packet_id); + if (data_index < 0 || data_index >= servo_num_total_) { + PI_ERROR("DriverFt: no data index registered for servo ID %d", packet_id); + return_code = ReturnCode::FAIL; + break; + } + + pres_pos_[data_index] = static_cast(feedback[0] | (feedback[1] << 8)); + pres_vel_[data_index] = + decode_sign_magnitude(static_cast(feedback[2] | (feedback[3] << 8)), kFtVelocitySignBit); + pres_load_[data_index] = + decode_sign_magnitude(static_cast(feedback[4] | (feedback[5] << 8)), kFtLoadSignBit); + // feedback[6] is Present Input Voltage (0.1 V); openpi has no consumer for it. + pres_temp_[data_index] = feedback[7]; + pres_status_[data_index] = feedback[9]; + } + + if (return_code != ReturnCode::SUCCESS || !pending_ids.empty()) { + for (int id : pending_ids) { + PI_ERROR("DriverFt: no SYNC READ response from servo ID %d", id); + } + flush_input(); + classify_dead_servos(); + return ReturnCode::FAIL; + } + + // Bulk read succeeded for every still-registered servo; refresh the + // reported break point (dead_servo_ids_ stays sticky, see DriverDxl). + last_failed_servo_id_ = dead_servo_ids_.empty() ? -1 : *dead_servo_ids_.begin(); + + std::string info_pos, info_vel, info_load; + for (int i = 0; i < servo_num_total_; i++) { + info_pos += std::to_string(pres_pos_[i]) + ", "; + info_vel += std::to_string(pres_vel_[i]) + ", "; + info_load += std::to_string(pres_load_[i]) + ", "; + } + PI_INFO("Driver", InfoLevel::FREQUENT_3, "\nPositions (steps): %s\nVelocities (steps/s): %s\nLoads (0.1%%): %s", + info_pos.c_str(), info_vel.c_str(), info_load.c_str()); + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::read_hardware_values(Servo* p_servo) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in read_hardware_values()"); + return ReturnCode::INVALID_PARAM; + } + + if (servo_num_total_ <= 0) { + PI_ERROR("Servo count is not properly initialized: %d", servo_num_total_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_servo->data_index_ < 0 || p_servo->data_index_ >= servo_num_total_) { + PI_ERROR("Servo data index %d (servo ID %d) exceeds maximum index %d", p_servo->data_index_, p_servo->id_, + servo_num_total_ - 1); + return ReturnCode::INVALID_PARAM; + } + + ServoFt* p_servo_ft = (ServoFt*)p_servo; + const int data_index = p_servo->data_index_; + + p_servo_ft->curr_pos_steps_ = pres_pos_[data_index]; + p_servo_ft->curr_pos_abs_ = p_servo_ft->steps_to_rad(pres_pos_[data_index]); + p_servo_ft->curr_vel_ = p_servo_ft->get_vel_rad_sec(pres_vel_[data_index]); + p_servo_ft->curr_tor_ = p_servo_ft->get_tor_nm(pres_load_[data_index]); + p_servo_ft->temperature_ = static_cast(pres_temp_[data_index]); + // Servo Status error bits: bit0 voltage, bit1 angle/sensor, bit2 overheat, + // bit3 overcurrent, bit5 overload (SMS/STS memory table). Raw value, no + // decoding -- same convention as the other servo families. + p_servo_ft->motor_error_code_ = pres_status_[data_index]; + + return Driver::read_hardware_values(p_servo); +} + +ReturnCode DriverFt::group_write_hardware_values() { + if (port_handler_ < 0) { + PI_ERROR("Serial port is not open in group_write_hardware_values()"); + return ReturnCode::NOT_INITIALIZED; + } + + // Torque limit first, then goal position (mirrors DriverDxl's goal-current- + // before-goal-position ordering so the limit is in place when motion starts). + ReturnCode return_code = sync_write(kFtAddrTorqueLimit, 2, pending_torque_limit_); + pending_torque_limit_.clear(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Group write for torque limit failed"); + pending_goal_position_.clear(); + return return_code; + } + + return_code = sync_write(kFtAddrGoalPosition, 2, pending_goal_position_); + pending_goal_position_.clear(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Group write for goal position failed"); + return return_code; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::enable_torque(ServoFt* p_servo, bool enable) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in enable_torque()"); + return ReturnCode::INVALID_PARAM; + } + return write_register(p_servo->id_, kFtAddrTorqueEnable, enable ? 1 : 0, 1); +} + +ReturnCode DriverFt::set_operating_mode(ServoFt* p_servo, uint8_t mode) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_operating_mode()"); + return ReturnCode::INVALID_PARAM; + } + return write_register(p_servo->id_, kFtAddrOperatingMode, mode, 1); +} + +ReturnCode DriverFt::set_position_pid(ServoFt* p_servo, float pos_p, float pos_i, float pos_d) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_position_pid()"); + return ReturnCode::INVALID_PARAM; + } + + auto to_gain_byte = [](float gain) { + if (gain < 0.0f) gain = 0.0f; + if (gain > 254.0f) gain = 254.0f; + return static_cast(gain); + }; + + ReturnCode return_code = write_register(p_servo->id_, kFtAddrPositionPGain, to_gain_byte(pos_p), 1); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + return_code = write_register(p_servo->id_, kFtAddrPositionDGain, to_gain_byte(pos_d), 1); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + return write_register(p_servo->id_, kFtAddrPositionIGain, to_gain_byte(pos_i), 1); +} + +ReturnCode DriverFt::set_acceleration(ServoFt* p_servo, uint8_t acceleration) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_acceleration()"); + return ReturnCode::INVALID_PARAM; + } + return write_register(p_servo->id_, kFtAddrAcceleration, acceleration, 1); +} + +ReturnCode DriverFt::set_goal_position_direct(ServoFt* p_servo, int32_t goal_position) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_goal_position_direct()"); + return ReturnCode::INVALID_PARAM; + } + if (goal_position < kFtGoalPositionMin) goal_position = kFtGoalPositionMin; + if (goal_position > kFtGoalPositionMax) goal_position = kFtGoalPositionMax; + return write_register(p_servo->id_, kFtAddrGoalPosition, goal_position, 2); +} + +ReturnCode DriverFt::set_goal_position_group(ServoFt* p_servo, int32_t goal_position) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_goal_position_group()"); + return ReturnCode::INVALID_PARAM; + } + if (dead_servo_ids_.count(p_servo->id_) != 0) { + // Same policy as DriverDxl group writes: skip dead servos so the + // control loop does not stall on their bus timeouts. + return ReturnCode::SUCCESS; + } + if (goal_position < kFtGoalPositionMin) goal_position = kFtGoalPositionMin; + if (goal_position > kFtGoalPositionMax) goal_position = kFtGoalPositionMax; + pending_goal_position_.emplace_back(p_servo->id_, static_cast(goal_position)); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverFt::set_torque_limit_group(ServoFt* p_servo, int32_t torque_limit) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in set_torque_limit_group()"); + return ReturnCode::INVALID_PARAM; + } + if (dead_servo_ids_.count(p_servo->id_) != 0) { + return ReturnCode::SUCCESS; + } + if (torque_limit < kFtTorqueLimitMin) torque_limit = kFtTorqueLimitMin; + if (torque_limit > kFtTorqueLimitMax) torque_limit = kFtTorqueLimitMax; + pending_torque_limit_.emplace_back(p_servo->id_, static_cast(torque_limit)); + return ReturnCode::SUCCESS; +} + +void DriverFt::discard_pending_group_writes(ServoFt* p_servo) { + if (p_servo == nullptr) { + return; + } + const int id = p_servo->id_; + auto drop_id = [id](std::vector>& entries) { + entries.erase(std::remove_if(entries.begin(), entries.end(), + [id](const std::pair& entry) { return entry.first == id; }), + entries.end()); + }; + drop_id(pending_goal_position_); + drop_id(pending_torque_limit_); +} + +ReturnCode DriverFt::get_torque_enabled(ServoFt* p_servo, int32_t& enabled) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in get_torque_enabled()"); + return ReturnCode::INVALID_PARAM; + } + return read_register(p_servo->id_, kFtAddrTorqueEnable, 1, enabled); +} + +ReturnCode DriverFt::get_present_position(ServoFt* p_servo, int32_t& present_position) { + if (p_servo == nullptr) { + PI_ERROR("Servo pointer is null in get_present_position()"); + return ReturnCode::INVALID_PARAM; + } + return read_register(p_servo->id_, kFtAddrPresentPosition, 2, present_position); +} + +ReturnCode DriverFt::sync_goal_position_to_present(ServoFt* p_servo) { + int32_t present_position = 0; + ReturnCode return_code = get_present_position(p_servo, present_position); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to read present position for goal sync (servo ID %d)", p_servo ? p_servo->id_ : -1); + return return_code; + } + return set_goal_position_direct(p_servo, present_position); +} diff --git a/native/pi_control/src/pi_driver_serial.cpp b/native/pi_control/src/pi_driver_serial.cpp new file mode 100644 index 0000000..c639565 --- /dev/null +++ b/native/pi_control/src/pi_driver_serial.cpp @@ -0,0 +1,409 @@ +/*! + * @file pi_driver_serial.cpp + * @brief Implementation of the DriverSerial raw-tty wrapper. + */ + +#include "pi_driver_serial.hpp" + +#include +#include +#include +#include + +#if defined(__linux__) +#include +#elif defined(__APPLE__) +#include +#endif + +#include +#include +#include +#include +#include +#include +#include + +#include "pi_info.hpp" + +namespace { + +std::string tty_name_from_path(const std::string& path) { + char resolved[PATH_MAX] = {0}; + const char* p = ::realpath(path.c_str(), resolved); + const std::string full = (p != nullptr) ? std::string(resolved) : path; + const size_t slash = full.find_last_of('/'); + return slash == std::string::npos ? full : full.substr(slash + 1); +} + +bool file_exists(const std::string& path) { + struct stat st {}; + return ::stat(path.c_str(), &st) == 0; +} + +bool write_text_file(const std::string& path, const std::string& value) { + std::ofstream f(path); + if (!f.is_open()) { + return false; + } + f << value; + return f.good(); +} + +void apply_usb_serial_latency_timer(const std::string& port_path) { +#if defined(__linux__) + const std::string tty = tty_name_from_path(port_path); + if (tty.empty()) { + return; + } + + const std::string candidates[] = { + "/sys/class/tty/" + tty + "/device/latency_timer", + "/sys/bus/usb-serial/devices/" + tty + "/latency_timer", + }; + bool saw_latency_timer = false; + for (const std::string& latency_path : candidates) { + if (!file_exists(latency_path)) { + continue; + } + saw_latency_timer = true; + // FTDI latency_timer accepts 1..255 ms; 0 is not a valid sysfs value on + // normal Linux ftdi_sio, so 1 ms is the practical minimum here. + if (write_text_file(latency_path, "1\n")) { + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "DriverSerial: set USB serial latency_timer=1 at %s", + latency_path.c_str()); + } else { + PI_WARN("DriverSerial: failed to set USB serial latency_timer at %s: %s", + latency_path.c_str(), std::strerror(errno)); + } + } + if (!saw_latency_timer) { + PI_INFO("Driver", InfoLevel::FREQUENT_3, + "DriverSerial: no USB serial latency_timer sysfs entry for %s (%s)", + port_path.c_str(), tty.c_str()); + } +#else + // macOS uses IOSSDATALAT (per-fd ioctl) applied below in apply_low_latency_flag(). + (void)port_path; +#endif +} + +void apply_low_latency_flag(int fd, const std::string& port_path) { +#if defined(__linux__) + serial_struct serial {}; + if (::ioctl(fd, TIOCGSERIAL, &serial) != 0) { + PI_INFO("Driver", InfoLevel::FREQUENT_3, + "DriverSerial: TIOCGSERIAL low-latency query unsupported for '%s': %s", + port_path.c_str(), std::strerror(errno)); + return; + } + if ((serial.flags & ASYNC_LOW_LATENCY) != 0) { + return; + } + serial.flags |= ASYNC_LOW_LATENCY; + if (::ioctl(fd, TIOCSSERIAL, &serial) != 0) { + PI_WARN("DriverSerial: failed to set ASYNC_LOW_LATENCY for '%s': %s", + port_path.c_str(), std::strerror(errno)); + return; + } + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "DriverSerial: enabled ASYNC_LOW_LATENCY for '%s'", port_path.c_str()); +#elif defined(__APPLE__) + // IOSSDATALAT: per-fd minimum latency in microseconds. 1 us is the + // documented minimum and the FTDI driver clamps low values internally. + // This is the macOS counterpart of Linux's ASYNC_LOW_LATENCY. + unsigned long latency_us = 1; + if (::ioctl(fd, IOSSDATALAT, &latency_us) != 0) { + PI_INFO("Driver", InfoLevel::FREQUENT_3, + "DriverSerial: IOSSDATALAT low-latency query unsupported for '%s': %s", + port_path.c_str(), std::strerror(errno)); + return; + } + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "DriverSerial: enabled IOSSDATALAT low latency for '%s'", port_path.c_str()); +#else + (void)fd; + (void)port_path; +#endif +} + +#if defined(__APPLE__) +// Apply an arbitrary baud rate on macOS via IOSSIOSPEED. This is the same +// mechanism pyserial uses. Required because Apple termios B macros +// stop at B230400; our Dynamixel servos need 1 Mbps / 3 Mbps. +bool apply_mac_custom_baud(int fd, int baud_rate) { + speed_t speed = static_cast(baud_rate); + return ::ioctl(fd, IOSSIOSPEED, &speed) == 0; +} +#endif + +} // namespace + +DriverSerial::DriverSerial(Device* p_device, const CommandLineArgs& cla) + : Driver(p_device, cla) {} + +DriverSerial::~DriverSerial() { + stop_reception(); + DriverSerial::close(); +} + +speed_t DriverSerial::baud_constant(int baud_rate) { + // Apple only defines B-prefixed macros up to B230400. Higher + // rates (460800 / 500000 / 921600 / 1 Mbps) ship as Linux extensions in + // and are absent on macOS. Each high-rate entry is + // therefore guarded by its own #if defined(B...) so the same source builds + // on both platforms; macOS falls back to IOSSIOSPEED for those values + // (see DriverSerial::open() — the speed==0 branch on Apple). + static const std::map kMap = { + {9600, B9600}, {19200, B19200}, {38400, B38400}, {57600, B57600}, + {115200, B115200}, {230400, B230400}, +#if defined(B460800) + {460800, B460800}, +#endif +#if defined(B500000) + {500000, B500000}, +#endif +#if defined(B576000) + {576000, B576000}, +#endif +#if defined(B921600) + {921600, B921600}, +#endif +#if defined(B1000000) + {1000000, B1000000}, +#endif + }; + auto it = kMap.find(baud_rate); + return (it == kMap.end()) ? static_cast(0) : it->second; +} + +ReturnCode DriverSerial::open(int baud_rate) { + if (port_handler_ >= 0) { + // Already open -- treat as success so callers can safely retry. + return ReturnCode::SUCCESS; + } + + if (control_port_name_.empty()) { + PI_ERROR("DriverSerial::open: empty control_port_name_ (set --control_port)"); + return ReturnCode::INVALID_PARAM; + } + +#if defined(__APPLE__) + // macOS exposes every USB-serial under two synonyms: ``/dev/tty.`` + // (POSIX dial-in — blocks on DCD until the modem asserts carrier) and + // ``/dev/cu.`` (call-up — opens immediately). USB-serial adapters + // never assert DCD, so opening ``tty.usbserial-*`` or ``tty.usbmodem*`` + // hangs ``open()`` forever. Fail fast with a hint instead of letting + // the next O_NONBLOCK ``::open()`` call block the entire process. + if (control_port_name_.rfind("/dev/tty.usbserial", 0) == 0 || + control_port_name_.rfind("/dev/tty.usbmodem", 0) == 0 || + control_port_name_.rfind("/dev/tty.SLAB_USBtoUART", 0) == 0) { + std::string cu_hint = control_port_name_; + cu_hint.replace(0, 9, "/dev/cu."); // "/dev/tty." -> "/dev/cu." + PI_ERROR("DriverSerial::open: refusing macOS DCD-blocking path '%s'. " + "Use the call-up variant '%s' instead.", + control_port_name_.c_str(), cu_hint.c_str()); + return ReturnCode::INVALID_PARAM; + } +#endif + + const speed_t speed = baud_constant(baud_rate); +#if defined(__APPLE__) + // macOS termios maxes out at B230400 (B460800 / B921600 / B1000000 are + // absent from ). For anything above that we run cfset*speed() + // with B9600 as a placeholder and then apply the real speed via the + // IOSSIOSPEED ioctl. baud_constant() returning 0 is therefore expected + // for 1 Mbps / 3 Mbps / etc. on macOS and is handled below. + const bool use_custom_baud_macos = (speed == 0); + const speed_t termios_speed = use_custom_baud_macos ? B9600 : speed; +#else + if (speed == 0) { + PI_ERROR("DriverSerial::open: unsupported baud rate %d for port '%s'", + baud_rate, control_port_name_.c_str()); + return ReturnCode::NOT_SUPPORTED; + } + const speed_t termios_speed = speed; +#endif + + apply_usb_serial_latency_timer(control_port_name_); + + const int fd = ::open(control_port_name_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd < 0) { + PI_ERROR("DriverSerial::open: failed to open '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + return ReturnCode::FAIL; + } + apply_low_latency_flag(fd, control_port_name_); + + struct termios tty; + std::memset(&tty, 0, sizeof(tty)); + if (::tcgetattr(fd, &tty) != 0) { + PI_ERROR("DriverSerial::open: tcgetattr failed for '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + ::close(fd); + return ReturnCode::FAIL; + } + + ::cfsetispeed(&tty, termios_speed); + ::cfsetospeed(&tty, termios_speed); + + // 8N1, no flow control, raw I/O. + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; + tty.c_cflag |= (CLOCAL | CREAD); + tty.c_cflag &= ~(PARENB | PARODD); // no parity + tty.c_cflag &= ~CSTOPB; // 1 stop bit + tty.c_cflag &= ~CRTSCTS; // no HW flow control + + tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXOFF | IXANY); + tty.c_lflag = 0; // raw, no echo, no canonical processing + tty.c_oflag = 0; // raw output + + // Non-blocking semantics on read -- the reception thread polls. + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 0; + + if (::tcsetattr(fd, TCSANOW, &tty) != 0) { + PI_ERROR("DriverSerial::open: tcsetattr failed for '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + ::close(fd); + return ReturnCode::FAIL; + } + +#if defined(__APPLE__) + if (use_custom_baud_macos) { + if (!apply_mac_custom_baud(fd, baud_rate)) { + PI_ERROR("DriverSerial::open: IOSSIOSPEED failed for '%s' @ %d bps: %s", + control_port_name_.c_str(), baud_rate, std::strerror(errno)); + ::close(fd); + return ReturnCode::FAIL; + } + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "DriverSerial: applied macOS custom baud %d via IOSSIOSPEED on '%s'", + baud_rate, control_port_name_.c_str()); + } +#endif + + // Some USB CDC devices do not start streaming replies until the host + // asserts DTR/RTS. Harmless for plain UARTs. + int modem_bits = 0; + if (::ioctl(fd, TIOCMGET, &modem_bits) == 0) { + modem_bits |= (TIOCM_DTR | TIOCM_RTS); + if (::ioctl(fd, TIOCMSET, &modem_bits) != 0) { + PI_WARN("DriverSerial::open: failed to set DTR/RTS for '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } + } else { + PI_WARN("DriverSerial::open: failed to read modem bits for '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + } + + // Flush any boot-time noise the device may have queued. + ::tcflush(fd, TCIOFLUSH); + + port_handler_ = fd; + PI_INFO("Driver", InfoLevel::ESSENTIAL_0, + "Serial port opened: port=%s baud=%d", control_port_name_.c_str(), baud_rate); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverSerial::close() { + stop_reception(); + if (port_handler_ >= 0) { + ::close(port_handler_); + port_handler_ = -1; + } + return ReturnCode::SUCCESS; +} + +ReturnCode DriverSerial::write_bytes(const uint8_t* p_data, size_t size) { + if (port_handler_ < 0) { + return ReturnCode::NOT_INITIALIZED; + } + if (p_data == nullptr || size == 0) { + return ReturnCode::INVALID_PARAM; + } + + size_t written_total = 0; + while (written_total < size) { + const ssize_t n = ::write(port_handler_, p_data + written_total, size - written_total); + if (n < 0) { + if (errno == EINTR) { + continue; + } + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // The CDC kernel buffer is full -- yield briefly and retry so + // we do not silently drop bytes mid-frame. + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + continue; + } + PI_ERROR("DriverSerial::write_bytes: write failed on '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + return ReturnCode::FAIL; + } + written_total += static_cast(n); + } + return ReturnCode::SUCCESS; +} + +ReturnCode DriverSerial::write_text(const char* text) { + if (text == nullptr) { + return ReturnCode::INVALID_PARAM; + } + return write_bytes(reinterpret_cast(text), std::strlen(text)); +} + +ReturnCode DriverSerial::start_reception(const callback_t& callback) { + stop_reception(); + + if (port_handler_ < 0) { + PI_ERROR("DriverSerial::start_reception: port not open"); + return ReturnCode::NOT_INITIALIZED; + } + if (!callback) { + PI_ERROR("DriverSerial::start_reception: empty callback"); + return ReturnCode::INVALID_PARAM; + } + + is_running_.store(true, std::memory_order_release); + reception_thread_ = std::thread(&DriverSerial::receive_loop, this, callback); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverSerial::stop_reception() { + if (!is_running_.exchange(false, std::memory_order_acq_rel)) { + if (reception_thread_.joinable()) { + reception_thread_.join(); + } + return ReturnCode::SUCCESS; + } + if (reception_thread_.joinable()) { + reception_thread_.join(); + } + return ReturnCode::SUCCESS; +} + +void DriverSerial::receive_loop(callback_t callback) { + uint8_t buffer[256]; + while (is_running_.load(std::memory_order_acquire)) { + const int fd = port_handler_; + if (fd < 0) { + break; + } + const ssize_t n = ::read(fd, buffer, sizeof(buffer)); + if (n > 0) { + callback(buffer, static_cast(n)); + continue; + } + if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR) { + PI_WARN("DriverSerial::receive_loop: read error on '%s': %s", + control_port_name_.c_str(), std::strerror(errno)); + // Stop on hard errors so the device class can react (e.g. mark not-ready). + break; + } + // Cooperative idle so we do not pin a CPU on an empty CDC pipe. + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } +} diff --git a/native/pi_control/src/pi_driver_trossen.cpp b/native/pi_control/src/pi_driver_trossen.cpp new file mode 100644 index 0000000..d21b285 --- /dev/null +++ b/native/pi_control/src/pi_driver_trossen.cpp @@ -0,0 +1,211 @@ +/*! + * @file pi_driver_trossen.cpp + * @brief Implementation of the DriverTrossen Trossen iNerve controller driver. + */ + +#include "pi_driver_trossen.hpp" + +// Streaming command interpolation horizon. 0 executes the target immediately +// (the host-side trajectory planning / leash already shapes the motion); the +// SDK treats sub-millisecond horizons as direct linear stepping. +#define TROSSEN_STREAM_GOAL_TIME_S 0.0 + +// TCP handshake timeout for configure(). Generous enough for a controller +// that is still booting, small enough to fail fast on a wrong IP. +#define TROSSEN_CONFIGURE_TIMEOUT_S 10.0 + +DriverTrossen::DriverTrossen(Device* p_device, const CommandLineArgs& cla, const std::string& controller_model) + : DriverController(p_device, cla), controller_model_(controller_model) { + is_leader_role_ = (cla.role == Role::LEADER); +} + +DriverTrossen::~DriverTrossen() { + // Must run here (not only in the base destructor): close() calls the + // vendor hooks, which are gone once this destructor finished. + close(); +} + +ReturnCode DriverTrossen::vendor_connect(const std::string& address, int& joint_count) { + p_arm_ = std::make_unique(); + // clear_error = true: recover from a stale error latched by a previous + // session (e.g. a hard-killed host) instead of refusing to configure. + // Leader and follower differ in end-effector mass properties (the leader + // carries the teleop handle); using the wrong set skews the controller's + // own gravity compensation. + if (!p_arm_->configure(controller_model_, is_leader_role_, address, true, TROSSEN_CONFIGURE_TIMEOUT_S)) { + PI_ERROR("libtrossen_arm configure failed for '%s': %s", address.c_str(), p_arm_->last_error().c_str()); + p_arm_.reset(); + return ReturnCode::NO_RESPONSE; + } + + joint_count = p_arm_->num_joints(); + PI_INFO("DriverTrossen", InfoLevel::ESSENTIAL_0, + "Configured Trossen controller at '%s': model=%s, driver=%s, controller_firmware=%s, joints=%d", + address.c_str(), controller_model_.c_str(), p_arm_->driver_version().c_str(), + p_arm_->controller_version().c_str(), joint_count); + return ReturnCode::SUCCESS; +} + +ReturnCode DriverTrossen::vendor_disconnect() { + if (p_arm_ == nullptr) { + return ReturnCode::SUCCESS; + } + + const bool clean = p_arm_->cleanup(); + if (!clean) { + PI_ERROR("libtrossen_arm cleanup failed: %s", p_arm_->last_error().c_str()); + } + p_arm_.reset(); + return clean ? ReturnCode::SUCCESS : ReturnCode::FAIL; +} + +ReturnCode DriverTrossen::vendor_read_state(std::vector& states) { + if (p_arm_ == nullptr) { + PI_ERROR("vendor_read_state() called without a configured Trossen driver"); + return ReturnCode::NOT_INITIALIZED; + } + + std::vector positions; + std::vector velocities; + std::vector efforts; + std::vector temperatures; + if (!p_arm_->read_state(positions, velocities, efforts, temperatures)) { + PI_ERROR("libtrossen_arm state read failed: %s", p_arm_->last_error().c_str()); + return ReturnCode::NO_RESPONSE; + } + + if (positions.size() != states.size() || velocities.size() != states.size() || efforts.size() != states.size() || + temperatures.size() != states.size()) { + PI_ERROR("Trossen state size mismatch: expected %zu joints, got pos=%zu vel=%zu eff=%zu temp=%zu", + states.size(), positions.size(), velocities.size(), efforts.size(), temperatures.size()); + return ReturnCode::HARDWARE_FAULT; + } + + for (size_t i = 0; i < states.size(); i++) { + states[i].position = (float)positions[i]; + states[i].velocity = (float)velocities[i]; + states[i].effort = (float)efforts[i]; + states[i].temperature = (float)temperatures[i]; + states[i].valid = true; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverTrossen::vendor_write_commands(const std::vector& commands) { + if (p_arm_ == nullptr) { + PI_ERROR("vendor_write_commands() called without a configured Trossen driver"); + return ReturnCode::NOT_INITIALIZED; + } + + const int joint_count = (int)commands.size(); + const int arm_joint_count = joint_count - 1; // Trossen layout: N-1 arm joints + 1 gripper (last index). + const int gripper_index = joint_count - 1; + + // Whole-group transactions when every arm joint carries the same pending + // command type; per-joint fallback otherwise (e.g. during staggered + // start-up moves). + int arm_pending_positions = 0; + int arm_pending_efforts = 0; + for (int i = 0; i < arm_joint_count; i++) { + if (!commands[i].pending) { + continue; + } + if (commands[i].mode == JointCommandMode::POSITION) { + arm_pending_positions++; + } else if (commands[i].mode == JointCommandMode::EXTERNAL_EFFORT) { + arm_pending_efforts++; + } + } + + bool write_ok = true; + if (arm_pending_positions == arm_joint_count) { + std::vector positions(arm_joint_count); + std::vector velocity_ffs(arm_joint_count); + for (int i = 0; i < arm_joint_count; i++) { + positions[i] = commands[i].position; + velocity_ffs[i] = commands[i].velocity_ff; + } + write_ok = p_arm_->set_arm_positions(positions, TROSSEN_STREAM_GOAL_TIME_S, false, velocity_ffs); + } else if (arm_pending_efforts == arm_joint_count) { + std::vector efforts(arm_joint_count); + for (int i = 0; i < arm_joint_count; i++) { + efforts[i] = commands[i].external_effort; + } + write_ok = p_arm_->set_arm_external_efforts(efforts, TROSSEN_STREAM_GOAL_TIME_S, false); + } else { + for (int i = 0; i < arm_joint_count && write_ok; i++) { + if (!commands[i].pending) { + continue; + } + if (commands[i].mode == JointCommandMode::POSITION) { + write_ok = p_arm_->set_joint_position(i, commands[i].position, TROSSEN_STREAM_GOAL_TIME_S, false, + commands[i].velocity_ff); + } else if (commands[i].mode == JointCommandMode::EXTERNAL_EFFORT) { + write_ok = p_arm_->set_joint_external_effort(i, commands[i].external_effort, + TROSSEN_STREAM_GOAL_TIME_S, false); + } + } + } + + if (write_ok && commands[gripper_index].pending) { + const JointCommand& gripper = commands[gripper_index]; + if (gripper.mode == JointCommandMode::POSITION) { + write_ok = p_arm_->set_gripper_position(gripper.position, TROSSEN_STREAM_GOAL_TIME_S, false, + gripper.velocity_ff); + } else if (gripper.mode == JointCommandMode::EXTERNAL_EFFORT) { + write_ok = p_arm_->set_gripper_external_effort(gripper.external_effort, TROSSEN_STREAM_GOAL_TIME_S, false); + } + } + + if (!write_ok) { + PI_ERROR("libtrossen_arm command write failed: %s", p_arm_->last_error().c_str()); + return ReturnCode::FAIL; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverTrossen::vendor_apply_command_modes(const std::vector& modes) { + if (p_arm_ == nullptr) { + PI_ERROR("vendor_apply_command_modes() called without a configured Trossen driver"); + return ReturnCode::NOT_INITIALIZED; + } + + std::vector shim_modes(modes.size(), TrossenArmShim::Mode::IDLE); + for (size_t i = 0; i < modes.size(); i++) { + switch (modes[i]) { + case JointCommandMode::POSITION: + shim_modes[i] = TrossenArmShim::Mode::POSITION; + break; + case JointCommandMode::EXTERNAL_EFFORT: + shim_modes[i] = TrossenArmShim::Mode::EXTERNAL_EFFORT; + break; + case JointCommandMode::UNSET: + shim_modes[i] = TrossenArmShim::Mode::IDLE; + break; + } + } + + if (!p_arm_->set_joint_modes(shim_modes)) { + PI_ERROR("libtrossen_arm set_joint_modes failed: %s", p_arm_->last_error().c_str()); + return ReturnCode::FAIL; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode DriverTrossen::vendor_make_safe() { + if (p_arm_ == nullptr) { + return ReturnCode::SUCCESS; + } + + // idle: the controller stops executing streamed commands and holds the + // arm passively with its own gravity/friction compensation. + if (!p_arm_->set_all_modes_idle()) { + PI_ERROR("libtrossen_arm set_all_modes(idle) failed: %s", p_arm_->last_error().c_str()); + return ReturnCode::FAIL; + } + + return ReturnCode::SUCCESS; +} diff --git a/native/pi_control/src/pi_servo.cpp b/native/pi_control/src/pi_servo.cpp index 110f193..11ab556 100644 --- a/native/pi_control/src/pi_servo.cpp +++ b/native/pi_control/src/pi_servo.cpp @@ -10,7 +10,9 @@ #include "pi_joint.hpp" #include "pi_servo_can_encoder.hpp" +#include "pi_servo_controller.hpp" #include "pi_servo_dm.hpp" +#include "pi_servo_ft.hpp" #define MAX_CNT_POS_EXCEED \ 100 ///< Threshold count for position limit exceed detection @@ -370,6 +372,13 @@ ReturnCode Servo::new_servos(const json& joint_config, p_driver); PI_INFO("Servo", InfoLevel::HELPFUL_1, "Created ServoCanPassiveEncoder instance"); + } else if (servo_model == p_config_model->val_servo_model_trossen_wxai) { + p_servo = std::make_unique(p_device, p_joint, p_driver); + PI_INFO("Servo", InfoLevel::HELPFUL_1, + "Created ServoController instance"); + } else if (servo_model == p_config_model->val_servo_model_ft_sts3215) { + p_servo = std::make_unique(p_device, p_joint, p_driver); + PI_INFO("Servo", InfoLevel::HELPFUL_1, "Created ServoFt instance"); } else { PI_ERROR("Unsupported servo model: %s", servo_model.c_str()); return ReturnCode::NOT_SUPPORTED; diff --git a/native/pi_control/src/pi_servo_controller.cpp b/native/pi_control/src/pi_servo_controller.cpp new file mode 100644 index 0000000..1f0a7c7 --- /dev/null +++ b/native/pi_control/src/pi_servo_controller.cpp @@ -0,0 +1,201 @@ +/*! + * @file pi_servo_controller.cpp + * @brief Implementation of the ServoController per-joint view onto a whole-arm controller. + */ + +#include "pi_servo_controller.hpp" + +#include "pi_joint.hpp" + +const ServoControllerParam g_servo_controller_param(DEFAULT_TOLERABLE_POS_DIFFERENCE_RAD, MAX_POS_DIFFERENCE_RAD, + DEFAULT_VELOCITY_THRESHOLD_RAD_SEC); + +ServoController::ServoController(Device* p_device, Joint* p_joint, Driver* p_driver) + : Servo(p_device, p_joint, p_driver) { + p_driver_controller_ = dynamic_cast(p_driver); +} + +ServoController::~ServoController() {} + +ReturnCode ServoController::init_config_model(const json& servo_config, const DeviceConfig* p_config) { + p_servo_param_ = &g_servo_controller_param; + + ReturnCode return_code = Servo::init_config_model(servo_config, p_config); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + if (servo_model_ == p_config->val_servo_model_trossen_wxai) { + type_ = ServoType::CONTROLLER_JOINT; + } else { + PI_ERROR("Unsupported servo model '%s' (servo ID %d)", servo_model_.c_str(), id_); + return ReturnCode::NOT_SUPPORTED; + } + + if (p_driver_controller_ == nullptr) { + PI_ERROR("Servo ID %d requires a DriverController-based driver (check driver_type in the model config)", id_); + return ReturnCode::INVALID_PARAM; + } + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::start_hardware() { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_ == nullptr) { + PI_ERROR("Device pointer is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping start hardware", id_); + return ReturnCode::SUCCESS; + } + + // POSITION mode makes the controller hold the current pose; the mode is + // applied together with the hold-at-current seed in the first command flush, + // so there is no window where a stale target could be executed. + ReturnCode return_code = + p_driver_controller_->set_joint_command_mode(data_index_, DriverController::JointCommandMode::POSITION); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to request POSITION mode (servo ID %d)", id_); + return return_code; + } + + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo %d started hardware (controller joint slot %d)", id_, + data_index_); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::park_safely() { + if (parked_ == true) { + return ReturnCode::SUCCESS; + } + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping park safely", id_); + return ReturnCode::SUCCESS; + } + if (p_driver_controller_ == nullptr) { + PI_ERROR("Controller driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + // Whole-arm controllers park all joints at once; make_safe() is idempotent + // so the first parked joint idles the arm and the rest are no-ops. + ReturnCode return_code = p_driver_controller_->make_safe(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to park controller joint (servo ID %d)", id_); + return return_code; + } + + parked_ = true; + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d parked safely (controller idled)", id_); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::verify_position_fresh() { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Controller driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (!p_driver_controller_->has_valid_state()) { + PI_ERROR("Servo ID %d: no whole-arm state has been received from the controller yet", id_); + return ReturnCode::NO_RESPONSE; + } + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::move(float target_pos) { + return move(target_pos, 0, 0); +} + +ReturnCode ServoController::move(float target_pos, float target_vel, float target_tor) { + // The controller runs its own servo loops; per-command torque bounds are + // not part of the whole-arm command set (joint limits bound the effort). + (void)target_tor; + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + float clipped_pos = clipping(target_pos, pos_min_rel_, pos_max_rel_); + float target_pos_absolute = get_pos_rad_absolute(clipped_pos); + float velocity_ff = target_vel * dir_invert_; + + ReturnCode return_code = p_driver_controller_->queue_position(data_index_, target_pos_absolute, velocity_ff); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to queue position command (servo ID %d)", id_); + return return_code; + } + + PI_INFO("Servo", InfoLevel::FREQUENT_3, "Joint ID %d, Servo ID %d: move target_pos=%.3f, absolute=%.3f", + p_joint_->id_, id_, target_pos, target_pos_absolute); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::apply_torque(float torque) { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + ReturnCode return_code = p_driver_controller_->queue_external_effort(data_index_, torque * dir_invert_); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to queue external effort command (servo ID %d)", id_); + return return_code; + } + + PI_INFO("Servo", InfoLevel::FREQUENT_3, "Joint ID %d, Servo ID %d: external effort %.3f", p_joint_->id_, id_, + torque); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoController::apply_torque_with_damping(float torque) { + // Damping runs inside the vendor controller's servo loops. + return apply_torque(torque); +} + +ReturnCode ServoController::change_control_mode_for_leader() { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: device is read only, skipping change_control_mode_for_leader", id_); + return ReturnCode::SUCCESS; + } + + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: leader mode, requesting EXTERNAL_EFFORT (controller gravity compensation stays active)", + id_); + return p_driver_controller_->set_joint_command_mode(data_index_, + DriverController::JointCommandMode::EXTERNAL_EFFORT); +} + +ReturnCode ServoController::change_control_mode_for_follower() { + if (p_driver_controller_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: device is read only, skipping change_control_mode_for_follower", id_); + return ReturnCode::SUCCESS; + } + + return p_driver_controller_->set_joint_command_mode(data_index_, DriverController::JointCommandMode::POSITION); +} + +void ServoController::update_from_controller_state(float position, float velocity, float effort, float temperature) { + curr_pos_abs_ = position; + curr_vel_ = velocity; + curr_tor_ = effort; + temperature_ = temperature; +} diff --git a/native/pi_control/src/pi_servo_ft.cpp b/native/pi_control/src/pi_servo_ft.cpp new file mode 100644 index 0000000..d3c00c9 --- /dev/null +++ b/native/pi_control/src/pi_servo_ft.cpp @@ -0,0 +1,395 @@ +/*! + * @file pi_servo_ft.cpp + * @brief Implementation of the ServoFt class for FeeTech SMS/STS serial bus servos. + */ + +#include +#include + +#include "pi_driver_ft.hpp" +#include "pi_joint.hpp" +#include "pi_servo_ft.hpp" + +// SMS/STS Operating Mode register values (addr 33). +#define FT_MODE_POSITION 0 + +// Torque Limit register full scale (0.1% of stall torque). +#define FT_TORQUE_LIMIT_FULL_SCALE 1000 + +// Acceleration register bounds (addr 41, unit 100 steps/s^2). +#define FT_ACCELERATION_MAX 254 + +// Verified torque disable: attempts and delay between attempts. The HX +// firmware on SO-ARM101 kits ACKs but ignores a torque-off that arrives right +// after a torque-on / goal write, so the disable is verified via readback and +// retried until the register actually reads 0. +#define FT_TORQUE_DISABLE_MAX_ATTEMPTS 5 +#define FT_TORQUE_DISABLE_RETRY_DELAY_US 10000 + +const ServoFtParam g_servo_ft_param(DEFAULT_TOLERABLE_POS_DIFFERENCE_RAD, MAX_POS_DIFFERENCE_RAD, + DEFAULT_VELOCITY_THRESHOLD_RAD_SEC); + +ServoFt::ServoFt(Device* p_device, Joint* p_joint, Driver* p_driver) : Servo(p_device, p_joint, p_driver) { + p_driver_ft_ = (DriverFt*)p_driver; +} + +ServoFt::~ServoFt() {} + +ReturnCode ServoFt::init_config_model(const json& servo_config, const DeviceConfig* p_config) { + p_servo_param_ = &g_servo_ft_param; + + ReturnCode return_code = Servo::init_config_model(servo_config, p_config); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + + if (servo_model_ == p_config->val_servo_model_ft_sts3215) { + type_ = ServoType::FT_STS3215; + } else { + PI_ERROR("Unsupported servo model '%s' (servo ID %d)", servo_model_.c_str(), id_); + return ReturnCode::NOT_SUPPORTED; + } + + return_code = p_config->get_field_value(servo_config, p_config->fn_servo_resolution, servo_resolution_); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + PI_INFO("ServoFt", InfoLevel::HELPFUL_1, "Servo ID %d: servo_resolution=%d", id_, servo_resolution_); + // STS3215 operates single-turn with the calibrated middle at + // resolution / 2 (step 2048), which maps to 0 rad. + center_offset_ = servo_resolution_ / 2; + + // Optional: Acceleration register value (unit 100 steps/s^2). Reuses the + // generic prof_accel field; 0 (default) leaves the servo's own ramp off. + return_code = p_config->get_field_value_optional(servo_config, p_config->fn_servo_prof_accel, prof_accel_); + if (return_code == ReturnCode::SUCCESS) { + PI_INFO("ServoFt", InfoLevel::HELPFUL_1, "Servo ID %d: prof_accel=%d", id_, prof_accel_); + if (prof_accel_ < 0 || prof_accel_ > FT_ACCELERATION_MAX) { + PI_ERROR("Servo ID %d: prof_accel must be within 0..%d for FeeTech servos, but found %d", id_, + FT_ACCELERATION_MAX, prof_accel_); + return ReturnCode::INVALID_PARAM; + } + } + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoFt::start_hardware() { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_device_ == nullptr) { + PI_ERROR("Device pointer is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping start hardware", id_); + return ReturnCode::SUCCESS; + } + + ReturnCode return_code = p_driver_ft_->enable_torque(this, false); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to disable torque (servo ID %d)", id_); + return return_code; + } + usleep(200); + + return_code = p_driver_ft_->set_operating_mode(this, FT_MODE_POSITION); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set position operating mode (servo ID %d)", id_); + return return_code; + } + usleep(200); + + // Position PID gains are written only when a P gain is configured; the + // factory defaults (P=32, D=32, I=0) are otherwise kept, matching the + // LeRobot SO-ARM bring-up which does not touch these registers. + if (pos_kp_ > 0) { + return_code = p_driver_ft_->set_position_pid(this, pos_kp_, pos_ki_, pos_kd_); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set position PID gain (servo ID %d)", id_); + return return_code; + } + usleep(200); + } + + if (prof_accel_ > 0) { + return_code = p_driver_ft_->set_acceleration(this, static_cast(prof_accel_)); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set acceleration (servo ID %d)", id_); + return return_code; + } + usleep(200); + } + + // Align the goal with the present position BEFORE engaging torque so the + // servo holds its pose instead of jumping to a stale goal register (same + // rationale as ServoDxl::start_hardware()). + return_code = p_driver_ft_->sync_goal_position_to_present(this); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to sync goal position to present position before enabling torque (servo ID %d)", id_); + return return_code; + } + usleep(200); + + return_code = p_driver_ft_->enable_torque(this, true); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to enable torque (servo ID %d)", id_); + return return_code; + } + usleep(200); + + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo %d started hardware", id_); + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoFt::park_safely() { + if (parked_ == true) { + return ReturnCode::SUCCESS; + } + + if (p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping park safely", id_); + return ReturnCode::SUCCESS; + } + + if (p_driver_ft_ == nullptr) { + PI_ERROR("FeeTech driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + ReturnCode return_code = read_hardware_values(); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to read current hardware values (servo ID %d)", id_); + return return_code; + } + usleep(200); + + // Keep the goal register aligned with the resting pose so the next + // torque-on (e.g. a restart without a full re-init) does not jump. This + // must happen BEFORE the torque disable: a goal position write arriving + // after the disable re-engages torque on SO-ARM101 kits (Hiwonder HX + // firmware) and the arm stays locked after shutdown. + return_code = p_driver_ft_->set_goal_position_direct(this, curr_pos_steps_); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set goal position to current position (servo ID %d)", id_); + return return_code; + } + usleep(200); + + return_code = p_driver_ft_->enable_torque(this, false); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to disable torque (servo ID %d)", id_); + return return_code; + } + usleep(200); + + parked_ = true; + + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d parked safely", id_); + + return return_code; +} + +ReturnCode ServoFt::move(float target_pos) { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + float clipped_pos = clipping(target_pos, pos_min_rel_, pos_max_rel_); + float target_pos_absolute = get_pos_rad_absolute(clipped_pos); + int32_t target_pos_steps = rad_to_steps(target_pos_absolute); + + ReturnCode return_code = p_driver_ft_->set_goal_position_group(this, target_pos_steps); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set goal position (servo ID %d)", id_); + return return_code; + } + + PI_INFO("Servo", InfoLevel::FREQUENT_3, "Joint ID %d, Servo ID %d: move target_pos=%.3f rad, target_steps=%d", + p_joint_->id_, id_, target_pos, target_pos_steps); + + return return_code; +} + +ReturnCode ServoFt::move(float target_pos, float target_vel, float target_tor) { + // target_vel is part of the Servo interface but the FeeTech position mode + // does not consume an explicit goal velocity here. + (void)target_vel; + if (p_driver_ft_ == nullptr || p_joint_ == nullptr || p_device_ == nullptr) { + PI_ERROR("Required pointers are not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + float clipped_pos = clipping(target_pos, pos_min_rel_, pos_max_rel_); + float target_pos_absolute = get_pos_rad_absolute(clipped_pos); + int32_t target_pos_steps = rad_to_steps(target_pos_absolute); + + float rescaled_torque = target_tor * p_joint_->torq_rescale_; + float clipped_torque = clipping(rescaled_torque, p_joint_->torq_min_, p_joint_->torq_max_); + int32_t torque_limit = torque_to_torque_limit(clipped_torque); + + PI_INFO("ServoFt", InfoLevel::FREQUENT_3, + "Joint ID %d, Servo ID %d: move target_pos=%.3f rad, target_steps=%d, requested_torque=%.3f Nm, " + "torque_limit=%d", + p_joint_->id_, id_, target_pos, target_pos_steps, target_tor, torque_limit); + + ReturnCode return_code = p_driver_ft_->set_torque_limit_group(this, torque_limit); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set torque limit (servo ID %d)", id_); + return return_code; + } + + return_code = p_driver_ft_->set_goal_position_group(this, target_pos_steps); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set goal position (servo ID %d)", id_); + return return_code; + } + + return return_code; +} + +ReturnCode ServoFt::apply_torque(float torque) { + (void)torque; + PI_ERROR( + "Servo ID %d: apply_torque is not supported for %s (no current control loop); " + "use position control with a torque limit instead", + id_, servo_model_.c_str()); + return ReturnCode::NOT_SUPPORTED; +} + +ReturnCode ServoFt::enable_torque(bool enable) { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: device is read only, skipping enable_torque", id_); + return ReturnCode::SUCCESS; + } + return p_driver_ft_->enable_torque(this, enable); +} + +int32_t ServoFt::torque_to_torque_limit(float torque) { + if (kt_ <= 0) { + PI_ERROR("Servo ID %d: kt is not configured; cannot convert torque to a torque limit", id_); + return 0; + } + + // The Torque Limit register is an unsigned magnitude (the servo applies it + // in both directions), so only the requested torque magnitude matters. + int32_t torque_limit = (int32_t)(fabs(torque) / kt_); + if (torque_limit > FT_TORQUE_LIMIT_FULL_SCALE) { + torque_limit = FT_TORQUE_LIMIT_FULL_SCALE; + } + return torque_limit; +} + +ReturnCode ServoFt::change_control_mode_for_leader() { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: device is read only, skipping change_control_mode_for_leader", id_); + return ReturnCode::SUCCESS; + } + + if (p_device_ != nullptr && p_device_->is_force_feedback_enabled()) { + PI_ERROR( + "Servo ID %d: force feedback is not supported for %s (no current control loop); " + "run the leader without --force_feedback", + id_, servo_model_.c_str()); + return ReturnCode::NOT_SUPPORTED; + } + + // SO-ARM leader convention (same as LeRobot): fully passive, torque off so + // the operator back-drives the arm. The lightweight leader hardware does + // not need gravity compensation. + // + // Same-cycle goal entries queued by the final move-to-ready step must be + // dropped BEFORE disabling torque: flushing a goal position after the + // disable re-engages torque on SO-ARM101 kits (Hiwonder HX firmware) and + // the leader ends up locked at home. + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, "Servo ID %d: leader mode, disabling torque (passive)", id_); + p_driver_ft_->discard_pending_group_writes(this); + + // Disable-and-verify loop: the write is ACKed even when the firmware ignores + // it (observed when the disable lands right after a torque-on / goal write on + // the leader gripper), so read Torque Enable back and retry until it is 0. + int32_t torque_enabled = -1; + for (int attempt = 1; attempt <= FT_TORQUE_DISABLE_MAX_ATTEMPTS; attempt++) { + ReturnCode return_code = p_driver_ft_->enable_torque(this, false); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to disable torque for passive leader (servo ID %d)", id_); + return return_code; + } + usleep(200); + + return_code = p_driver_ft_->get_torque_enabled(this, torque_enabled); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to read back Torque Enable after passive-leader disable (servo ID %d)", id_); + return return_code; + } + if (torque_enabled == 0) { + return ReturnCode::SUCCESS; + } + + PI_WARN("Servo ID %d: Torque Enable reads %d after passive-leader disable (attempt %d/%d), retrying", id_, + torque_enabled, attempt, FT_TORQUE_DISABLE_MAX_ATTEMPTS); + usleep(FT_TORQUE_DISABLE_RETRY_DELAY_US); + } + + PI_ERROR("Servo ID %d: Torque Enable still reads %d after %d passive-leader disable attempts (expected 0)", id_, + torque_enabled, FT_TORQUE_DISABLE_MAX_ATTEMPTS); + return ReturnCode::FAIL; +} + +ReturnCode ServoFt::change_control_mode_for_follower() { + if (p_driver_ft_ == nullptr) { + PI_ERROR("Driver is not initialized (servo ID %d)", id_); + return ReturnCode::NOT_INITIALIZED; + } + + if (p_device_ != nullptr && p_device_->is_read_only() == true) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: device is read only, skipping change_control_mode_for_follower", id_); + return ReturnCode::SUCCESS; + } + + ReturnCode return_code = p_driver_ft_->enable_torque(this, false); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to disable torque (servo ID %d)", id_); + return return_code; + } + usleep(10000); + + return_code = p_driver_ft_->set_operating_mode(this, FT_MODE_POSITION); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to set position operating mode (servo ID %d)", id_); + return return_code; + } + + // Align the goal with the present position before re-enabling torque to + // avoid a stale-goal jump after the mode switch. + return_code = p_driver_ft_->sync_goal_position_to_present(this); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to sync goal position to present (servo ID %d)", id_); + return return_code; + } + + return_code = p_driver_ft_->enable_torque(this, true); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to enable torque (servo ID %d)", id_); + return return_code; + } + + return return_code; +} diff --git a/native/pi_control/src/pi_trossen_shim.cpp b/native/pi_control/src/pi_trossen_shim.cpp new file mode 100644 index 0000000..04eac4a --- /dev/null +++ b/native/pi_control/src/pi_trossen_shim.cpp @@ -0,0 +1,283 @@ +/*! + * @file pi_trossen_shim.cpp + * @brief Implementation of TrossenArmShim (compiled into libpi_trossen_shim). + * + * This is the only translation unit that includes vendor headers. It is built + * into a shared library together with the prebuilt libtrossen_arm.a; the + * archive's symbols (including its bundled pinocchio/urdfdom/tinyxml2 copies) + * are hidden from the rest of the process (see CMakeLists.txt), which is what + * prevents the ODR clash with the node's own shared pinocchio. + */ + +#include "pi_trossen_shim.hpp" + +#include + +#include "libtrossen_arm/trossen_arm.hpp" + +// Trossen model strings accepted in model configs (fn_controller_model). +#define TROSSEN_MODEL_WXAI_V0 "wxai_v0" + +struct TrossenArmShim::Impl { + trossen_arm::TrossenArmDriver driver; +}; + +namespace { + +trossen_arm::Mode to_vendor_mode(TrossenArmShim::Mode mode) { + switch (mode) { + case TrossenArmShim::Mode::POSITION: + return trossen_arm::Mode::position; + case TrossenArmShim::Mode::EXTERNAL_EFFORT: + return trossen_arm::Mode::external_effort; + case TrossenArmShim::Mode::IDLE: + break; + } + return trossen_arm::Mode::idle; +} + +} // namespace + +TrossenArmShim::TrossenArmShim() = default; + +TrossenArmShim::~TrossenArmShim() { + if (impl_ != nullptr) { + try { + impl_->driver.cleanup(); + } catch (const std::exception&) { + // Destructor: nothing actionable; the connection dies with the process. + } + } +} + +bool TrossenArmShim::configure(const std::string& model, bool leader_end_effector, const std::string& address, + bool clear_error, double timeout_s) { + trossen_arm::Model vendor_model; + if (model == TROSSEN_MODEL_WXAI_V0) { + vendor_model = trossen_arm::Model::wxai_v0; + } else { + last_error_ = "unsupported Trossen controller model '" + model + "' (supported: " TROSSEN_MODEL_WXAI_V0 ")"; + return false; + } + + const trossen_arm::EndEffector end_effector = leader_end_effector + ? trossen_arm::StandardEndEffector::wxai_v0_leader + : trossen_arm::StandardEndEffector::wxai_v0_follower; + + try { + impl_ = std::make_unique(); + impl_->driver.configure(vendor_model, end_effector, address, clear_error, timeout_s); + } catch (const std::exception& e) { + last_error_ = e.what(); + impl_.reset(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::cleanup() { + if (impl_ == nullptr) { + return true; + } + + try { + impl_->driver.cleanup(); + } catch (const std::exception& e) { + last_error_ = e.what(); + impl_.reset(); + return false; + } + + impl_.reset(); + last_error_.clear(); + return true; +} + +int TrossenArmShim::num_joints() const { + if (impl_ == nullptr) { + return 0; + } + return (int)impl_->driver.get_num_joints(); +} + +std::string TrossenArmShim::driver_version() const { + if (impl_ == nullptr) { + return ""; + } + return impl_->driver.get_driver_version(); +} + +std::string TrossenArmShim::controller_version() const { + if (impl_ == nullptr) { + return ""; + } + return impl_->driver.get_controller_version(); +} + +bool TrossenArmShim::read_state(std::vector& positions, std::vector& velocities, + std::vector& efforts, std::vector& temperatures) { + if (impl_ == nullptr) { + last_error_ = "read_state() called without a configured driver"; + return false; + } + + try { + positions = impl_->driver.get_all_positions(); + velocities = impl_->driver.get_all_velocities(); + efforts = impl_->driver.get_all_efforts(); + temperatures = impl_->driver.get_all_rotor_temperatures(); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_arm_positions(const std::vector& positions, double goal_time, bool blocking, + const std::vector& velocity_ffs) { + if (impl_ == nullptr) { + last_error_ = "set_arm_positions() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_arm_positions(positions, goal_time, blocking, velocity_ffs); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_arm_external_efforts(const std::vector& efforts, double goal_time, bool blocking) { + if (impl_ == nullptr) { + last_error_ = "set_arm_external_efforts() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_arm_external_efforts(efforts, goal_time, blocking); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_joint_position(int joint_index, double position, double goal_time, bool blocking, + double velocity_ff) { + if (impl_ == nullptr) { + last_error_ = "set_joint_position() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_joint_position((uint8_t)joint_index, position, goal_time, blocking, velocity_ff); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_joint_external_effort(int joint_index, double effort, double goal_time, bool blocking) { + if (impl_ == nullptr) { + last_error_ = "set_joint_external_effort() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_joint_external_effort((uint8_t)joint_index, effort, goal_time, blocking); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_gripper_position(double position, double goal_time, bool blocking, double velocity_ff) { + if (impl_ == nullptr) { + last_error_ = "set_gripper_position() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_gripper_position(position, goal_time, blocking, velocity_ff); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_gripper_external_effort(double effort, double goal_time, bool blocking) { + if (impl_ == nullptr) { + last_error_ = "set_gripper_external_effort() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_gripper_external_effort(effort, goal_time, blocking); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_joint_modes(const std::vector& modes) { + if (impl_ == nullptr) { + last_error_ = "set_joint_modes() called without a configured driver"; + return false; + } + + std::vector vendor_modes(modes.size(), trossen_arm::Mode::idle); + for (size_t i = 0; i < modes.size(); i++) { + vendor_modes[i] = to_vendor_mode(modes[i]); + } + + try { + impl_->driver.set_joint_modes(vendor_modes); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +bool TrossenArmShim::set_all_modes_idle() { + if (impl_ == nullptr) { + last_error_ = "set_all_modes_idle() called without a configured driver"; + return false; + } + + try { + impl_->driver.set_all_modes(trossen_arm::Mode::idle); + } catch (const std::exception& e) { + last_error_ = e.what(); + return false; + } + + last_error_.clear(); + return true; +} + +const std::string& TrossenArmShim::last_error() const { return last_error_; } diff --git a/pyproject.toml b/pyproject.toml index 8be847b..7cf7cef 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -14,6 +14,14 @@ dependencies = [ # Direct CAN access for the servo zeroing registry (openpi_control.servos). "python-can>=4.3", "pyzmq>=25", + # Trossen iNerve Ethernet controllers: discovery, EEPROM zeroing, and IP + # provisioning (openpi_control.servos.trossen_eth). Pinned to the same + # release as the C++ libtrossen_arm in scripts/build_deps.sh so the driver + # <-> controller firmware compatibility stays aligned. + "trossen-arm==1.10.0", + # USB-serial bus-servo maintenance sessions (openpi_control.servos.buses) + # for the FeeTech/Dynamixel serial families (e.g. SO-ARM101). + "pyserial>=3.5", ] [project.optional-dependencies] diff --git a/scripts/build_deps.sh b/scripts/build_deps.sh index d3d3efb..048a5bf 100755 --- a/scripts/build_deps.sh +++ b/scripts/build_deps.sh @@ -26,6 +26,12 @@ pinocchio_ref="${PINOCCHIO_GIT_REF:-v3.4.0}" cppzmq_source="${deps_root}/cppzmq" cppzmq_prefix="${cppzmq_source}/install" cppzmq_ref="${CPPZMQ_GIT_REF:-v4.9.0}" +# Trossen iNerve controller SDK (prebuilt static lib + headers). Pinned so the +# driver <-> controller firmware compatibility stays reproducible: the +# controller rejects a driver whose major.minor doesn't match its firmware. +trossen_arm_source="${deps_root}/trossen_arm" +trossen_arm_prefix="${trossen_arm_source}/install" +trossen_arm_ref="${TROSSEN_ARM_GIT_REF:-v1.10.0}" build_jobs="${BUILD_JOBS:-2}" # A cached clone built from a different pinned ref must not be reused @@ -45,6 +51,7 @@ 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}" if ! [[ "${build_jobs}" =~ ^[1-9][0-9]*$ ]]; then echo "BUILD_JOBS must be a positive integer; got ${build_jobs@Q}" >&2 @@ -62,26 +69,50 @@ install -d "${cppzmq_prefix}/include" install -m 0644 "${cppzmq_source}/zmq.hpp" "${cppzmq_prefix}/include/zmq.hpp" install -m 0644 "${cppzmq_source}/zmq_addon.hpp" "${cppzmq_prefix}/include/zmq_addon.hpp" -if [[ ! -d "${pinocchio_source}/.git" ]]; then - git clone --branch "${pinocchio_ref}" --depth 1 \ - https://github.com/stack-of-tasks/pinocchio.git "${pinocchio_source}" -fi +# Per-dep skip: verify_cached_ref above already guarantees that any cached +# clone is at the pinned ref (a ref bump against an old cache fails fast), so +# an existing install artifact means this exact pinned version is installed. +if [[ -f "${pinocchio_prefix}/lib/pkgconfig/pinocchio.pc" ]]; then + echo "Pinocchio ${pinocchio_ref} already installed at ${pinocchio_prefix}; skipping." +else + if [[ ! -d "${pinocchio_source}/.git" ]]; then + git clone --branch "${pinocchio_ref}" --depth 1 \ + https://github.com/stack-of-tasks/pinocchio.git "${pinocchio_source}" + fi -if [[ ! -f "${pinocchio_source}/cmake/base.cmake" ]]; then - git -C "${pinocchio_source}" submodule update --init --depth 1 cmake \ - || git -C "${pinocchio_source}" submodule update --init cmake + if [[ ! -f "${pinocchio_source}/cmake/base.cmake" ]]; then + git -C "${pinocchio_source}" submodule update --init --depth 1 cmake \ + || git -C "${pinocchio_source}" submodule update --init cmake + fi + + cmake -S "${pinocchio_source}" -B "${pinocchio_source}/build" \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX="${pinocchio_prefix}" \ + -DBUILD_BENCHMARK=OFF \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_PYTHON_INTERFACE=OFF \ + -DBUILD_TESTING=OFF \ + -DENABLE_TEMPLATE_INSTANTIATION=OFF + cmake --build "${pinocchio_source}/build" --parallel "${build_jobs}" + cmake --install "${pinocchio_source}/build" fi -cmake -S "${pinocchio_source}" -B "${pinocchio_source}/build" \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX="${pinocchio_prefix}" \ - -DBUILD_BENCHMARK=OFF \ - -DBUILD_EXAMPLES=OFF \ - -DBUILD_PYTHON_INTERFACE=OFF \ - -DBUILD_TESTING=OFF \ - -DENABLE_TEMPLATE_INSTANTIATION=OFF -cmake --build "${pinocchio_source}/build" --parallel "${build_jobs}" -cmake --install "${pinocchio_source}/build" +if [[ -f "${trossen_arm_prefix}/lib/libtrossen_arm.a" ]]; then + echo "libtrossen_arm ${trossen_arm_ref} already installed at ${trossen_arm_prefix}; skipping." +else + if [[ ! -d "${trossen_arm_source}/.git" ]]; then + git clone --branch "${trossen_arm_ref}" --depth 1 \ + https://github.com/TrossenRobotics/trossen_arm.git "${trossen_arm_source}" + fi + + # The repo ships a prebuilt static library + headers per OS/arch; "building" + # is just a CMake install into the prefix. + cmake -S "${trossen_arm_source}" -B "${trossen_arm_source}/build" \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX="${trossen_arm_prefix}" + cmake --build "${trossen_arm_source}/build" --parallel "${build_jobs}" + cmake --install "${trossen_arm_source}/build" +fi if [[ ! -f "${pinocchio_prefix}/lib/pkgconfig/pinocchio.pc" ]]; then echo "Pinocchio installation is missing pinocchio.pc: ${pinocchio_prefix}" >&2 @@ -91,6 +122,10 @@ if [[ ! -f "${cppzmq_prefix}/include/zmq.hpp" ]]; then echo "cppzmq installation is missing zmq.hpp: ${cppzmq_prefix}" >&2 exit 1 fi +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 cat < None: raise ConfigurationError("SocketCAN interface must be a simple interface name") +@dataclass(frozen=True, slots=True) +class EthernetConnection: + """A whole-arm Ethernet controller (e.g. Trossen iNerve) at a fixed IPv4 address.""" + + ip: str + + def __post_init__(self) -> None: + try: + ipaddress.IPv4Address(self.ip) + except ValueError as err: + raise ConfigurationError( + f"invalid controller IPv4 address {self.ip!r} for Ethernet connection" + ) from err + + +@dataclass(frozen=True, slots=True) +class SerialConnection: + """A USB-serial bus-servo chain (e.g. SO-ARM101 FeeTech) at a tty device path.""" + + device: str + + def __post_init__(self) -> None: + if not self.device.startswith("/dev/"): + raise ConfigurationError( + f"serial connection device must be a /dev path, got {self.device!r}" + ) + + +ArmConnection = SocketCanConnection | EthernetConnection | SerialConnection + + +def connection_for_interface(interface: str) -> ArmConnection: + """Build the arm connection for one bus interface string from a runtime config. + + The runtime config stores the interface part of a bus spec ("can:", + "eth:", or "serial:"). The three forms never overlap: an IPv4 + literal selects a whole-arm Ethernet controller, a /dev path selects a + USB-serial bus, anything else is a SocketCAN interface name. + """ + if interface.startswith("/dev/"): + return SerialConnection(interface) + try: + ipaddress.IPv4Address(interface) + except ValueError: + return SocketCanConnection(interface) + return EthernetConnection(interface) + + @dataclass(frozen=True, slots=True) class SafetyLimits: max_bilateral_gain: float = 0.3 @@ -137,7 +196,7 @@ class ArmConfig: name: str model: str - connection: SocketCanConnection + connection: ArmConnection instance_config: Path | None = None effector_model: str | None = None effector_instance_config: Path | None = None @@ -236,3 +295,13 @@ def is_read_only(self) -> bool: """True when the model declares read_only (leader-only, no actuation, e.g. ARX_ENC).""" data = json.loads(self.resolve_assets().model_config.read_text()) return bool(data.get("read_only", False)) + + def catalog_baudrate(self) -> int: + """Bus baud rate declared by the model catalog (required for serial buses).""" + data = json.loads(self.resolve_assets().model_config.read_text()) + baudrate = data.get("catalog", {}).get("baudrate") + if not isinstance(baudrate, int) or baudrate <= 0: + raise ConfigurationError( + f"model {self.model!r} catalog does not declare a positive integer baudrate" + ) + return baudrate diff --git a/src/openpi_control/models/arms/SO101/SO101.json b/src/openpi_control/models/arms/SO101/SO101.json new file mode 100644 index 0000000..4f8e3e3 --- /dev/null +++ b/src/openpi_control/models/arms/SO101/SO101.json @@ -0,0 +1,221 @@ +{ + "config_version": "1.1.1", + "device_model": "SO101", + "device_type": "arm", + "arm_type": "nello", + "topic_type": "ROS", + "driver_type": "FEETECH", + "algo_type": "Pinocchio", + "planning_type": "None", + "catalog": { + "display_label": "SO101: SO-ARM100/101 5 DOF Arm", + "order": 11, + "baudrate": 1000000, + "default_effector_model": "E_SO101", + "port_type": "Serial" + }, + "joint_init_sequence": [ + 2, + 1, + 3, + 4, + 0 + ], + "joints": [ + { + "joint_id": 0, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 1, + "data_index": 0, + "dir_invert": 1, + "pos_min": -1.91986, + "pos_max": 1.91986, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + }, + { + "joint_id": 1, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 2, + "data_index": 1, + "dir_invert": 1, + "pos_min": -1.74533, + "pos_max": 1.74533, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + }, + { + "joint_id": 2, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 3, + "data_index": 2, + "dir_invert": 1, + "pos_min": -1.69, + "pos_max": 1.69, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + }, + { + "joint_id": 3, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 4, + "data_index": 3, + "dir_invert": 1, + "pos_min": -1.65806, + "pos_max": 1.65806, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + }, + { + "joint_id": 4, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 5, + "data_index": 4, + "dir_invert": 1, + "pos_min": -2.74385, + "pos_max": 2.84121, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + } + ] +} diff --git a/src/openpi_control/models/arms/SO101/SO101.urdf b/src/openpi_control/models/arms/SO101/SO101.urdf new file mode 100644 index 0000000..3b7ec05 --- /dev/null +++ b/src/openpi_control/models/arms/SO101/SO101.urdf @@ -0,0 +1,414 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 1 + + + + \ No newline at end of file diff --git a/src/openpi_control/models/arms/SO101/SO101_01.json b/src/openpi_control/models/arms/SO101/SO101_01.json new file mode 100644 index 0000000..d236cc7 --- /dev/null +++ b/src/openpi_control/models/arms/SO101/SO101_01.json @@ -0,0 +1,75 @@ +{ + "config_version": "1.1.1", + "device_model": "SO101", + "device_type": "arm", + "arm_type": "nello", + "spring_effect": false, + "gravity_compensation": false, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 0, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 1, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 2, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 2, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 3, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 3, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 4, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 4, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 5, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json new file mode 100644 index 0000000..2ce0345 --- /dev/null +++ b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json @@ -0,0 +1,268 @@ +{ + "config_version": "1.1.1", + "device_model": "Trossen_wai_ctrl", + "device_type": "arm", + "arm_type": "controller", + "topic_type": "ROS", + "driver_type": "TROSSEN_ETHERNET", + "controller_model": "wxai_v0", + "algo_type": "KDL", + "planning_type": "None", + "catalog": { + "display_label": "Trossen_wai_ctrl: Trossen WidowX AI (Ethernet controller)", + "order": 8, + "baudrate": 1000000, + "default_effector_model": "E_Trossen_ctrl", + "port_type": "Ethernet" + }, + "joint_init_sequence": [ + 3, + 2, + 5, + 4, + 1, + 6 + ], + "joints": [ + { + "joint_id": 0, + "nominal_stall_ok": false, + "vel_max": 0.5, + "follow_vel_max": 2.5, + "torq_min": -15, + "torq_max": 15, + "safe_torq_min": -7, + "safe_torq_max": 7, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.1, + "spring_preload": 0.0, + "spring_force_config": true, + "spring_type": 2, + "threshold_angle_change": 0.00872664625997165, + "threshold_time_sec": 0.3, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 1, + "data_index": 0, + "dir_invert": 1, + "pos_min": -3.1416, + "pos_max": 3.1416, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 1, + "nominal_stall_ok": true, + "vel_max": 0.5, + "follow_vel_max": 2.6, + "torq_min": -15, + "torq_max": 15, + "safe_torq_min": -7, + "safe_torq_max": 7, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 2, + "data_index": 1, + "dir_invert": 1, + "pos_min": 0.0, + "pos_max": 3.1416, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 2, + "nominal_stall_ok": true, + "vel_max": 0.5, + "follow_vel_max": 2.8, + "torq_min": -15, + "torq_max": 15, + "safe_torq_min": -7, + "safe_torq_max": 7, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.4, + "spring_preload": 0, + "spring_force_config": true, + "spring_type": -1, + "threshold_angle_change": 0.00872664625997165, + "threshold_time_sec": 0.3, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 3, + "data_index": 2, + "dir_invert": 1, + "pos_min": 0.0, + "pos_max": 2.3562, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 3, + "nominal_stall_ok": true, + "vel_max": 0.5, + "follow_vel_max": 6.0, + "torq_min": -7, + "torq_max": 7, + "safe_torq_min": -5, + "safe_torq_max": 5, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.0, + "spring_preload": 0.0, + "spring_force_config": false, + "spring_type": 1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 4, + "data_index": 3, + "dir_invert": 1, + "pos_min": -1.5708, + "pos_max": 1.5708, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 4, + "nominal_stall_ok": false, + "vel_max": 0.5, + "follow_vel_max": 6.0, + "torq_min": -7, + "torq_max": 7, + "safe_torq_min": -5, + "safe_torq_max": 5, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.0, + "spring_preload": 0.0, + "spring_force_config": false, + "spring_type": 1, + "threshold_angle_change": 0, + "threshold_time_sec": 10, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 5, + "data_index": 4, + "dir_invert": 1, + "pos_min": -1.5708, + "pos_max": 1.5708, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + }, + { + "joint_id": 5, + "nominal_stall_ok": false, + "vel_max": 0.5, + "follow_vel_max": 6.0, + "torq_min": -7, + "torq_max": 7, + "safe_torq_min": -5, + "safe_torq_max": 5, + "torq_rescale": 1.1, + "pos_rescale": 1, + "pos_error_margin": 0.05, + "safe_mode_derating": 0.05, + "accel_max": 1.0, + "spring_constant": 0.0, + "spring_preload": 0.0, + "spring_force_config": false, + "spring_type": 1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 6, + "data_index": 5, + "dir_invert": 1, + "pos_min": -3.1416, + "pos_max": 3.1416, + "reverse_flag": false, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.urdf b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.urdf new file mode 100644 index 0000000..bc943e0 --- /dev/null +++ b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.urdf @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json new file mode 100644 index 0000000..1daf2fa --- /dev/null +++ b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json @@ -0,0 +1,87 @@ +{ + "config_version": "1.1.1", + "device_model": "Trossen_wai_ctrl", + "device_type": "arm", + "arm_type": "controller", + "spring_effect": false, + "gravity_compensation": false, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 0, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 1, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 2, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 2, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 3, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 3, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 4, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 4, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 5, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + }, + { + "joint_id": 5, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 6, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_SO101/E_SO101.json b/src/openpi_control/models/effectors/E_SO101/E_SO101.json new file mode 100644 index 0000000..9810016 --- /dev/null +++ b/src/openpi_control/models/effectors/E_SO101/E_SO101.json @@ -0,0 +1,58 @@ +{ + "config_version": "1.1.1", + "device_model": "E_SO101", + "device_type": "effector", + "effector_type": "nello", + "topic_type": "ROS", + "driver_type": "FEETECH", + "algo_type": "Algo", + "planning_type": "None", + "catalog": { + "display_label": "E_SO101: SO-ARM100/101 Gripper", + "order": 10, + "baudrate": 1000000, + "need_repeated_command": false + }, + "joints": [ + { + "joint_id": 5, + "vel_max": 3.0, + "torq_min": -1.9, + "torq_max": 1.9, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1.0, + "pos_rescale": 1, + "pos_error_margin": 0.1, + "safe_mode_derating": 0.25, + "accel_max": 1.0, + "spring_constant": 0, + "spring_preload": 0, + "spring_force_config": false, + "spring_type": -1, + "threshold_angle_change": 0, + "threshold_time_sec": 0, + "max_length": 0.06, + "spring_invert": false, + "servos": [ + { + "servo_model": "FeeTech STS3215", + "servo_id": 6, + "data_index": 5, + "dir_invert": 1, + "pos_min": -0.174533, + "pos_max": 1.74533, + "reverse_flag": false, + "response_delay": 0.025, + "kT": 0.002, + "kA": 0.0, + "kV": 0.001534, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0, + "servo_resolution": 4096 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json b/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json new file mode 100644 index 0000000..9fa6d12 --- /dev/null +++ b/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json @@ -0,0 +1,32 @@ +{ + "config_version": "1.1.1", + "device_model": "E_SO101", + "device_type": "effector", + "effector_type": "nello", + "spring_effect": false, + "control_mode": "position", + "base_rpy": [ + 0, + 0, + 0 + ], + "rpy_invert": [ + 1, + 1, + 1 + ], + "joints": [ + { + "joint_id": 5, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 6, + "zero_pos": 0.0, + "home_pos": 0.0, + "spring_home_pos": 0.0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_SO101/E_SO101_mass.json b/src/openpi_control/models/effectors/E_SO101/E_SO101_mass.json new file mode 100644 index 0000000..d70ec4b --- /dev/null +++ b/src/openpi_control/models/effectors/E_SO101/E_SO101_mass.json @@ -0,0 +1,16 @@ +{ + "mass": 0.012000, + "center_of_mass": [ + -0.00157495, + -0.0300244, + 0.0192755 + ], + "inertia": { + "ixx": 0.00001, + "iyy": 0.00000, + "izz": 0.00001, + "ixy": 0.00000, + "ixz": 0.00000, + "iyz": 0.00000 + } +} diff --git a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json new file mode 100644 index 0000000..bb0ff0c --- /dev/null +++ b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json @@ -0,0 +1,66 @@ +{ + "config_version": "1.1.1", + "device_model": "E_Trossen_ctrl", + "device_type": "effector", + "effector_type": "controller", + "topic_type": "ROS", + "driver_type": "TROSSEN_ETHERNET", + "algo_type": "Algo", + "planning_type": "None", + "catalog": { + "display_label": "E_Trossen_ctrl: Trossen Gripper (Ethernet controller)", + "order": 8, + "baudrate": 1000000, + "need_repeated_command": true + }, + "joint_init_sequence": [ + 3, + 2, + 5, + 4, + 1, + 6 + ], + "joints": [ + { + "joint_id": 6, + "vel_max": 0.1, + "torq_min": -120, + "torq_max": 120, + "safe_torq_min": 0, + "safe_torq_max": 0, + "torq_rescale": 1, + "pos_rescale": 1, + "pos_error_margin": 0.01, + "pos_max_safety_margin": 0.002, + "safe_mode_derating": 0.2, + "accel_max": 1.0, + "spring_constant": 25, + "spring_preload": 0, + "spring_force_config": true, + "spring_type": 1, + "threshold_angle_change": 0.0005, + "threshold_time_sec": 0.1, + "max_length": 0.04, + "spring_invert": false, + "servos": [ + { + "servo_model": "Trossen WXAI Joint", + "servo_id": 7, + "data_index": 6, + "dir_invert": 1, + "reverse_flag": false, + "pos_max": 0.04, + "pos_min": 0, + "response_delay": 0.0, + "kT": 1, + "kA": 1, + "kV": 1, + "pos_kp": 0, + "pos_ki": 0, + "pos_kd": 0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json new file mode 100644 index 0000000..ba7c9c3 --- /dev/null +++ b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json @@ -0,0 +1,28 @@ +{ + "config_version": "1.1.1", + "device_model": "E_Trossen_ctrl", + "device_type": "effector", + "effector_type": "controller", + "spring_effect": false, + "control_mode": "position", + "dist_to_torque_const": 2.0, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 6, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 7, + "zero_pos": 0.0, + "home_pos": 0, + "spring_home_pos": 0.03 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_mass.json b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_mass.json new file mode 100644 index 0000000..e77917e --- /dev/null +++ b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_mass.json @@ -0,0 +1,16 @@ +{ + "mass": 0.47779, + "center_of_mass": [ + 0.03767, + 0.00074, + 0.01295 + ], + "inertia": { + "ixx": 0.00044008483, + "iyy": 0.00058988009, + "izz": 0.00037294564, + "ixy": 0.00000154213, + "ixz": 0.00009455425, + "iyz": 0.00001636757 + } +} \ No newline at end of file diff --git a/src/openpi_control/native.py b/src/openpi_control/native.py index 51d5192..9ac9b70 100644 --- a/src/openpi_control/native.py +++ b/src/openpi_control/native.py @@ -21,7 +21,14 @@ from . import log_paths from .backend import ArmBackend -from .config import ArmConfig, InputLayout, ResolvedArmAssets, SocketCanConnection +from .config import ( + ArmConfig, + ArmConnection, + EthernetConnection, + InputLayout, + ResolvedArmAssets, + SerialConnection, +) from .exceptions import ( CommandRejectedError, ConfigurationError, @@ -47,6 +54,7 @@ encode_command, port_candidates, ) +from .servos import trossen_eth from .types import ( ArmCapabilities, ArmMode, @@ -107,7 +115,22 @@ def native_executable() -> Path: return Path(found) if found else packaged -def validate_connection(connection: SocketCanConnection) -> None: +def validate_connection(connection: ArmConnection) -> None: + if isinstance(connection, EthernetConnection): + if not trossen_eth.reachable(connection.ip): + raise ConnectionUnavailableError( + f"Ethernet controller at {connection.ip} is not reachable; check the cable, " + "power, and that the host has an address on the controller's subnet" + ) + return + if isinstance(connection, SerialConnection): + device = Path(connection.device) + if not device.exists(): + raise ConnectionUnavailableError( + f"serial device {connection.device!r} does not exist; " + "check the USB cable and adapter" + ) + return path = Path("/sys/class/net") / connection.interface if not path.exists(): raise ConnectionUnavailableError( @@ -360,7 +383,20 @@ def _connect_prepared( self._inputs_sub = _Subscriber(self._context, topics.inputs) if role is ArmRole.FOLLOWER: self._direct_pub = _Publisher(self._context, topics.direct_command) - connection_args = ["--control_port", config.connection.interface] + # 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): + connection_args = ["--control_port", config.connection.ip] + elif isinstance(config.connection, SerialConnection): + connection_args = [ + "--control_port", + config.connection.device, + "--baud_rate", + str(config.catalog_baudrate()), + ] + else: + connection_args = ["--control_port", config.connection.interface] args = [ str(executable), "--role", diff --git a/src/openpi_control/servos/__init__.py b/src/openpi_control/servos/__init__.py index 98c3e0d..15ee8e8 100644 --- a/src/openpi_control/servos/__init__.py +++ b/src/openpi_control/servos/__init__.py @@ -12,10 +12,15 @@ string in the model JSON and in this table. Driver module API (uniform across families): - PORT_TYPE: str "can" or "serial" — which bus session the - driver needs (see ``buses.open_bus``) - set_zero(bus, servo_id) set the current position as the firmware zero; - returns None on success or an error string + PORT_TYPE: str "can", "serial", or "ethernet" — which bus + session the driver needs (see ``buses.open_bus``) + WHOLE_ARM_ZERO: bool False: the family zeroes one servo at a time and + defines ``set_zero(bus, servo_id)``. True: the + family (whole-arm Ethernet controllers, e.g. + Trossen iNerve) zeroes every joint in one + controller transaction and defines + ``set_zero_whole_arm(bus)`` instead. + set_zero* functions return None on success or an error string Adding a servo family: create ``_.py`` with that API (port the routine from robot-test ``pi_control/servos``), then register every canonical @@ -27,7 +32,7 @@ import types -from openpi_control.servos import dm_can, dxl_serial, encos_can +from openpi_control.servos import dm_can, dxl_serial, encos_can, ft_serial, trossen_eth # Servo model string -> driver module, or None for read-only encoders whose # zero is fixed in hardware (reported as skipped by the zeroing tool, never @@ -46,6 +51,10 @@ "Dynamixel XH430-W210": dxl_serial, "Dynamixel XC330-T288": dxl_serial, "Dynamixel XH430-W350": dxl_serial, + # FeeTech SMS/STS serial (SO-ARM100/101; also covers Hiwonder HX-30HM/HX-10HM). + "FeeTech STS3215": ft_serial, + # Whole-arm Ethernet controller joints (zeroed via one EEPROM write). + "Trossen WXAI Joint": trossen_eth, # Read-only encoders: no motor, zero reference fixed in hardware. "ARX Remote Encoder": None, "CAN Passive Encoder": None, diff --git a/src/openpi_control/servos/buses.py b/src/openpi_control/servos/buses.py index aecc5ce..c141367 100644 --- a/src/openpi_control/servos/buses.py +++ b/src/openpi_control/servos/buses.py @@ -1,31 +1,43 @@ """Bus sessions for servo maintenance tools, keyed by the driver's PORT_TYPE. -Servo drivers declare which transport they need ("can" or "serial"); this -module owns opening/validating that transport so tools never hard-code a bus -type. CAN is implemented (SocketCAN via python-can, with the settle timing -verified in robot-test). Serial is a declared seam: the checks work, but -opening a session raises until the first serial servo family (Dynamixel / -FeeTech) is actually integrated — port the session handling from robot-test -``pi_control/servos/dxl_serial.py`` (plain pyserial at the family's baud rate) -at that point. +Servo drivers declare which transport they need ("can", "serial", or +"ethernet"); this module owns opening/validating that transport so tools +never hard-code a bus type. CAN is implemented (SocketCAN via python-can, +with the settle timing verified in robot-test). Ethernet opens a Trossen +controller session (the "bus" is the vendor driver object). Serial opens a +plain pyserial session at the caller-provided baud rate (the model catalog's +``baudrate``); the protocol on top belongs to the servo driver module +(``ft_serial`` for FeeTech, ``dxl_serial`` once Dynamixel is integrated). """ from __future__ import annotations import contextlib +import ipaddress import pathlib import time from collections.abc import Iterator import can +import serial as pyserial +import trossen_arm + +from openpi_control.servos import trossen_eth PORT_TYPE_CAN = "can" PORT_TYPE_SERIAL = "serial" +PORT_TYPE_ETHERNET = trossen_eth.PORT_TYPE + +_SUPPORTED_PORT_TYPES = f"{PORT_TYPE_CAN}, {PORT_TYPE_SERIAL}, {PORT_TYPE_ETHERNET}" # robot-test waits 1 s after opening a CAN bus before the first frame; # adapters (especially SLCAN) drop the first frames when hit too early. _CAN_OPEN_SETTLE_S = 1.0 +# Serial per-read deadline: dominated by one USB latency window (up to 16 ms +# FTDI/CDC), so 100 ms leaves ample margin (robot-test ft_serial timing). +_SERIAL_READ_TIMEOUT_S = 0.1 + def check_interface(port_type: str, interface: str) -> str | None: """Return None when ``interface`` exists for ``port_type``, an error message otherwise.""" @@ -43,28 +55,56 @@ def check_interface(port_type: str, interface: str) -> str | None: f"serial device {interface!r} does not exist. Plug in the adapter and " "check the names with run/devices.sh (ls /dev/serial/by-id)." ) - raise SystemExit( - f"unknown port type {port_type!r}; supported: {PORT_TYPE_CAN}, {PORT_TYPE_SERIAL}" - ) + if port_type == PORT_TYPE_ETHERNET: + try: + ipaddress.IPv4Address(interface) + except ValueError: + return ( + f"Ethernet controller address {interface!r} is not a valid IPv4 address; " + "check the bus entry in the config TOML (eth:)." + ) + if trossen_eth.reachable(interface): + return None + return ( + f"Ethernet controller at {interface} did not answer a discovery probe. Check the " + "cable and power, and that the host has an address on the controller's subnet." + ) + raise SystemExit(f"unknown port type {port_type!r}; supported: {_SUPPORTED_PORT_TYPES}") @contextlib.contextmanager -def open_bus(port_type: str, interface: str) -> Iterator[can.BusABC]: - """Open a settled bus session on ``interface`` for drivers of ``port_type``.""" +def open_bus( + port_type: str, interface: str, *, baudrate: int | None = None +) -> Iterator[can.BusABC | trossen_arm.TrossenArmDriver | pyserial.Serial]: + """Open a settled bus session on ``interface`` for drivers of ``port_type``. + + Serial sessions require ``baudrate`` (the arm model catalog's ``baudrate`` + field) — the transport is protocol-neutral, so the caller must state the + bus speed explicitly. + """ if port_type == PORT_TYPE_CAN: with can.interface.Bus(channel=interface, interface="socketcan") as bus: time.sleep(_CAN_OPEN_SETTLE_S) yield bus return + if port_type == PORT_TYPE_ETHERNET: + # The "bus" is the vendor driver session; ``interface`` is the + # controller's IPv4 address. + with trossen_eth.open_session(interface) as driver: + yield driver + return if port_type == PORT_TYPE_SERIAL: - raise NotImplementedError( - "serial bus sessions are not integrated yet: port the pyserial session " - "handling from robot-test pi_control/servos/dxl_serial.py when the first " - "serial servo family is brought up" - ) - raise SystemExit( - f"unknown port type {port_type!r}; supported: {PORT_TYPE_CAN}, {PORT_TYPE_SERIAL}" - ) + if baudrate is None or baudrate <= 0: + raise SystemExit( + "serial bus sessions need an explicit positive baudrate " + "(the arm model catalog's 'baudrate' field)" + ) + with pyserial.Serial( + port=interface, baudrate=baudrate, timeout=_SERIAL_READ_TIMEOUT_S + ) as bus: + yield bus + return + raise SystemExit(f"unknown port type {port_type!r}; supported: {_SUPPORTED_PORT_TYPES}") def recv_from(bus: can.BusABC, expected_ids: tuple[int, ...], timeout_s: float) -> bool: diff --git a/src/openpi_control/servos/dm_can.py b/src/openpi_control/servos/dm_can.py index 703adbe..584ccf0 100644 --- a/src/openpi_control/servos/dm_can.py +++ b/src/openpi_control/servos/dm_can.py @@ -15,6 +15,7 @@ from openpi_control.servos import buses PORT_TYPE = buses.PORT_TYPE_CAN +WHOLE_ARM_ZERO = False _ZERO_PAYLOAD = bytes([0xFF] * 7 + [0xFE]) _RESPONSE_TIMEOUT_S = 0.05 diff --git a/src/openpi_control/servos/dxl_serial.py b/src/openpi_control/servos/dxl_serial.py index 0386cfe..0f6edaa 100644 --- a/src/openpi_control/servos/dxl_serial.py +++ b/src/openpi_control/servos/dxl_serial.py @@ -15,6 +15,7 @@ from openpi_control.servos import buses PORT_TYPE = buses.PORT_TYPE_SERIAL +WHOLE_ARM_ZERO = False def set_zero(bus: object, servo_id: int) -> str | None: diff --git a/src/openpi_control/servos/encos_can.py b/src/openpi_control/servos/encos_can.py index 41f1365..0ba89a7 100644 --- a/src/openpi_control/servos/encos_can.py +++ b/src/openpi_control/servos/encos_can.py @@ -17,6 +17,7 @@ from openpi_control.servos import buses PORT_TYPE = buses.PORT_TYPE_CAN +WHOLE_ARM_ZERO = False _CAN_ID_BROADCAST = 0x7FF _CMD_SET_ZERO = 0x03 diff --git a/src/openpi_control/servos/ft_serial.py b/src/openpi_control/servos/ft_serial.py new file mode 100644 index 0000000..d8f02f2 --- /dev/null +++ b/src/openpi_control/servos/ft_serial.py @@ -0,0 +1,231 @@ +"""FeeTech SMS/STS serial servo driver — packet layer, ping/scan, and zeroing. + +Ported from robot-test ``pi_control/servos/ft_serial.py``, reduced to what the +zeroing tool and the device wizard need for the STS3215 class (SO-ARM100/101; +the Hiwonder HX-30HM / HX-10HM in SO-ARM101 kits share the exact register +layout and protocol, so no separate servo model exists). + +The C++ mirror of this protocol lives in native/pi_control/src/pi_driver_ft.cpp. +""" + +from __future__ import annotations + +import logging +import struct + +import serial + +from openpi_control.servos import buses + +PORT_TYPE = buses.PORT_TYPE_SERIAL +WHOLE_ARM_ZERO = False + +# FeeTech instruction set (subset used here). +INST_PING = 0x01 +INST_READ = 0x02 +INST_WRITE = 0x03 + +ID_BROADCAST = 0xFE + +# SMS/STS control table (subset). +ADDR_BAUD_RATE = 6 +ADDR_TORQUE_ENABLE = 40 +ADDR_LOCK_FLAG = 55 +ADDR_PRESENT_POSITION = 56 + +# Writing 128 to Torque Enable calibrates the current position to the encoder +# center (2048) on SMS/STS servos — the standard STS3215 zeroing method (same +# operation as the FeeTech FD software "Calibrate middle" button). +TORQUE_ENABLE_CALIBRATE_MIDDLE = 128 + +# Baud Rate register (addr 6) value mapping for the STS3215 class. +BAUDRATE_REG_VALUES = { + 1000000: 0, + 500000: 1, + 250000: 2, + 128000: 3, + 115200: 4, + 76800: 5, + 57600: 6, + 38400: 7, +} + + +def check_sum(packet: bytes) -> int: + """FeeTech checksum: ~(sum of bytes after the FF FF header) & 0xFF.""" + return (~(sum(packet[2:]) & 0xFF)) & 0xFF + + +def build_packet(servo_id: int, instruction: int, parameters: bytes) -> bytes: + """Build an instruction packet: FF FF ID LEN INST [params] CHK.""" + packet = bytearray(b"\xff\xff") + packet += struct.pack(" bool: + """Write a register (INST_WRITE) and validate the status packet.""" + value_bytes = value.to_bytes(length, byteorder="little", signed=value < 0) + packet = build_packet(servo_id, INST_WRITE, struct.pack(" bytes | None: + """Read a register window (INST_READ); returns the parameter bytes or None.""" + packet = build_packet(servo_id, INST_READ, struct.pack(" bool: + """Ping one servo (INST_PING) and validate the status packet.""" + packet = build_packet(servo_id, INST_PING, b"") + bus.reset_input_buffer() + bus.write(packet) + bus.flush() + + response = bus.read(6) + if len(response) < 6: + return False + if response[:2] != b"\xff\xff" or response[2] != servo_id or response[3] != 0x02: + return False + return response[-1] == check_sum(response[:-1]) + + +def scan(bus: serial.Serial, servo_ids: tuple[int, ...]) -> list[int]: + """Ping each id in turn; returns the ids that answered.""" + return [servo_id for servo_id in servo_ids if ping(bus, servo_id)] + + +def torque_enable(bus: serial.Serial, servo_id: int, enable: bool | int) -> bool: + """Enable/disable torque output (also carries the calibrate-middle value 128).""" + return ft_write(bus, servo_id, ADDR_TORQUE_ENABLE, int(enable), 1) + + +def set_eeprom_lock(bus: serial.Serial, servo_id: int, locked: bool) -> bool: + """Set the EEPROM write Lock Flag (STS3215 ships write-locked).""" + return ft_write(bus, servo_id, ADDR_LOCK_FLAG, 1 if locked else 0, 1) + + +def get_baudrate(bus: serial.Serial, servo_id: int) -> int | None: + """Read the Baud Rate register and return it in bps, or None on failure.""" + response = ft_read(bus, servo_id, ADDR_BAUD_RATE, 1) + if response is None or len(response) < 1: + logging.getLogger(__name__).error("ID %d: failed to read Baud Rate register", servo_id) + return None + for bps, reg_value in BAUDRATE_REG_VALUES.items(): + if reg_value == response[0]: + return bps + logging.getLogger(__name__).error( + "ID %d: unknown Baud Rate register value %d", servo_id, response[0] + ) + return None + + +def set_baudrate(bus: serial.Serial, servo_id: int, new_baudrate: int) -> bool: + """Change the servo's UART baudrate and follow it on the host side. + + Steps: unlock EEPROM, write the Baud Rate register (takes effect + immediately, no reboot instruction exists in the FeeTech protocol), switch + the host serial port to the new baud, re-lock EEPROM, verify with a ping. + On failure the host baud is restored. + """ + if new_baudrate not in BAUDRATE_REG_VALUES: + logging.getLogger(__name__).error( + "ID %d: baudrate %d is not supported (allowed: %s)", + servo_id, + new_baudrate, + sorted(BAUDRATE_REG_VALUES), + ) + return False + + old_baudrate = int(bus.baudrate) + if not set_eeprom_lock(bus, servo_id, locked=False): + logging.getLogger(__name__).error( + "ID %d: failed to unlock EEPROM for baudrate change", servo_id + ) + return False + + if not ft_write(bus, servo_id, ADDR_BAUD_RATE, BAUDRATE_REG_VALUES[new_baudrate], 1): + logging.getLogger(__name__).error("ID %d: failed to write Baud Rate register", servo_id) + set_eeprom_lock(bus, servo_id, locked=True) + return False + + # The servo switches immediately after the write; follow it on the host side. + bus.baudrate = int(new_baudrate) + bus.reset_input_buffer() + + if not set_eeprom_lock(bus, servo_id, locked=True): + logging.getLogger(__name__).error( + "ID %d: failed to re-lock EEPROM after baudrate change", servo_id + ) + + if not ping(bus, servo_id): + logging.getLogger(__name__).error( + "ID %d: no answer at new baudrate %d; restoring host baud %d", + servo_id, + new_baudrate, + old_baudrate, + ) + bus.baudrate = old_baudrate + return False + return True + + +def set_zero(bus: serial.Serial, servo_id: int) -> str | None: + """Calibrate the current position to the encoder center (STS3215 zeroing). + + Torque Enable = 128 sets the present position to step 2048; the write is + wrapped with the EEPROM Lock Flag unlock/re-lock pair the STS3215 firmware + requires. Returns None on success or an error detail string. + """ + if not set_eeprom_lock(bus, servo_id, locked=False): + return "failed to unlock EEPROM for calibrate-middle" + calibrated = torque_enable(bus, servo_id, TORQUE_ENABLE_CALIBRATE_MIDDLE) + if not set_eeprom_lock(bus, servo_id, locked=True): + return "failed to re-lock EEPROM after calibrate-middle" + if not calibrated: + return "calibrate-middle command was not acknowledged" + return None diff --git a/src/openpi_control/servos/trossen_eth.py b/src/openpi_control/servos/trossen_eth.py new file mode 100644 index 0000000..cbd3e98 --- /dev/null +++ b/src/openpi_control/servos/trossen_eth.py @@ -0,0 +1,528 @@ +"""Trossen iNerve Ethernet controller helpers: discovery, zeroing, and IP provisioning. + +Central home for every Python-side interaction with the ``trossen_arm`` SDK +(same pinned release as the C++ ``libtrossen_arm`` linked by the native node, +so driver <-> controller firmware compatibility stays aligned): + +- ``discover()`` read-only /24 TCP sweep for controllers +- ``reachable()`` single-host reachability gate (native connect) +- ``open_session()`` configured driver session (idled + cleaned up) +- ``set_zero_whole_arm()`` persist the current pose as zero (one EEPROM write) +- ``read_ip_settings()`` / ``provision_manual_ip()`` IP provisioning +- ``local_subnet_prefixes()`` / ``candidate_ips()`` / ``ip_in_use()`` + host-side helpers for the provisioning flow +- ``wired_up_interfaces()`` / ``free_host_ip()`` / ``add_persistent_address()`` + / ``add_temporary_address()`` / ``remove_temporary_address()`` + host NIC setup for the factory controller subnet +""" + +from __future__ import annotations + +import contextlib +import ipaddress +import logging +import pathlib +import shutil +import subprocess +import threading +import time +from collections.abc import Iterator +from concurrent.futures import ThreadPoolExecutor +from dataclasses import dataclass + +import trossen_arm + +PORT_TYPE = "ethernet" + +# All joint offsets are persisted in ONE EEPROM write; per-servo zero commands +# would wear the controller flash once per joint (see set_zero_whole_arm). +WHOLE_ARM_ZERO = True + +# Trossen model string accepted in model configs (must match the C++ +# pi_trossen_shim.cpp TROSSEN_MODEL_WXAI_V0 constant). +TROSSEN_MODEL_WXAI_V0 = "wxai_v0" + +# Vendor factory default: every controller ships at this address, so two +# factory-fresh arms on one LAN collide until they are provisioned. +FACTORY_DEFAULT_IP = "192.168.1.2" + +# Default provisioning target per arm slot: the host octet on the controller's +# /24 subnet. When a base octet is already in use on the LAN the walk +# continues at base + k * PROVISION_HOST_OCTET_STEP (see candidate_ips), so +# every slot keeps a disjoint, predictable candidate set (e.g. follower_left: +# .11 / .111 / .211). +PROVISION_SLOT_HOST_OCTETS = { + "follower_left": 11, + "follower_right": 12, + "leader_left": 21, + "leader_right": 22, +} +PROVISION_HOST_OCTET_STEP = 100 + +# Host-side address on the controller subnet (the wizard can add it via +# NetworkManager when missing). The walk starts high to stay clear of typical +# LAN allocations and skips every arm provisioning candidate octet. +HOST_OCTET_START = 200 +_ARM_CANDIDATE_OCTETS = frozenset( + base + step + for base in PROVISION_SLOT_HOST_OCTETS.values() + for step in range(0, 255, PROVISION_HOST_OCTET_STEP) + if base + step < 255 +) + +# Duplicate-address probes for a provisioning candidate. ping catches any live +# IP stack; arping -D (duplicate address detection) additionally catches hosts +# that drop ICMP. One second is enough for a LAN device to defend its address. +_PING_TIMEOUT_S = 1 +_ARPING_TIMEOUT_S = 2 + +# Discovery probes every host of a /24 subnet with a short per-host TCP +# timeout. The vendor discover() probes serially, so the host range is split +# into chunks probed on parallel threads (the SDK releases the GIL during the +# socket wait). Worker count is capped at 16: measured on hardware in +# robot-test, 32 concurrent probes flood the link and the controller's reply +# gets dropped past the per-host timeout (missed discovery). +DISCOVERY_TIMEOUT_S = 0.05 +DISCOVERY_CHUNK_HOSTS = 16 +DISCOVERY_MAX_WORKERS = 16 + +# TCP handshake budget for an actual configure(): generous enough for a +# booting controller, small enough to fail fast on a wrong IP. +CONFIGURE_TIMEOUT_S = 10.0 + +# Single-host reachability gate: a controller that is momentarily busy (the +# config port is single-client) can miss one probe, so a few attempts with a +# longer per-host timeout are used. +REACHABILITY_TIMEOUT_S = 0.5 +REACHABILITY_ATTEMPTS = 3 +REACHABILITY_RETRY_DELAY_S = 1.0 + +# iNerve controllers use a PJRC Ethernet MAC. This lets the wizard distinguish +# a controller whose network stack answers but whose Trossen SDK service does +# not from a controller that is absent from the network entirely. +CONTROLLER_MAC_PREFIXES = ("04:e9:e5",) +_PING_REACHABILITY_TIMEOUT_S = 1 +POWER_CYCLE_OFF_S = 10 +CONTROLLER_BOOT_WAIT_S = 20 + +# After a provisioning reboot the controller needs time to come back before +# the re-discovery verification can succeed. +PROVISION_REBOOT_WAIT_S = 10.0 + +# Guards discover(): concurrent sweeps double the worker count, flood the +# link, and can make the controller's single-client config port stop +# answering real connects for a while. +_DISCOVER_SWEEP_LOCK = threading.Lock() + + +@dataclass(frozen=True) +class DiscoveredController: + """One controller that answered a discovery probe.""" + + address: str + model: str + firmware_version: str + error_state: str + + +@dataclass(frozen=True) +class IpSettings: + """Controller EEPROM network settings (read via a configure session).""" + + manual_ip: str + ip_method: str + firmware_version: str + + +def discover(subnets: list[str]) -> list[DiscoveredController]: + """Scan IPv4 /24 ``subnets`` (prefixes like ``"192.168.1"``) for controllers. + + Read-only: probes TCP connects only, never changes host or controller + network state. + """ + with _DISCOVER_SWEEP_LOCK: + return _discover_locked(subnets) + + +def reachable(ip: str) -> bool: + """True when the controller at ``ip`` answers a discovery probe.""" + subnet, host = _split_host(ip) + for attempt in range(REACHABILITY_ATTEMPTS): + if attempt > 0: + time.sleep(REACHABILITY_RETRY_DELAY_S) + with _DISCOVER_SWEEP_LOCK: + found = trossen_arm.TrossenArmDriver.discover( + subnet=subnet, ip_start=host, ip_end=host, timeout=REACHABILITY_TIMEOUT_S + ) + if found: + return True + return False + + +def ping_reachable_controller_ips(subnets: list[str]) -> list[str]: + """Return controller-like hosts that answer ping on the scanned subnets. + + A discovery sweep populates the local ARP/neighbor table even when the + controller's TCP service is unavailable. The known controller MAC prefix + filters out unrelated LAN hosts before the explicit ping check. + """ + result = subprocess.run( + ["ip", "neigh", "show"], + capture_output=True, + text=True, + check=False, + ) + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + logging.getLogger(__name__).warning("Could not inspect the IP neighbor table: %s", detail) + return [] + + candidates: list[str] = [] + for line in result.stdout.splitlines(): + parts = line.split() + if len(parts) < 5 or "lladdr" not in parts: + continue + ip = parts[0] + try: + address = ipaddress.IPv4Address(ip) + except ipaddress.AddressValueError: + continue + prefix = ".".join(str(address).split(".")[:3]) + mac = parts[parts.index("lladdr") + 1].lower() + if prefix not in subnets or not mac.startswith(CONTROLLER_MAC_PREFIXES): + continue + ping = subprocess.run( + ["ping", "-c", "1", "-W", str(_PING_REACHABILITY_TIMEOUT_S), str(address)], + capture_output=True, + text=True, + check=False, + ) + if ping.returncode == 0: + candidates.append(str(address)) + return sorted(set(candidates), key=ipaddress.IPv4Address) + + +@contextlib.contextmanager +def open_session(ip: str, *, clear_error: bool = False) -> Iterator[trossen_arm.TrossenArmDriver]: + """Configured driver session for maintenance work; idled and cleaned up on exit. + + The follower end-effector mass set only matters for the controller's own + gravity compensation while idle, which is correct for both roles at + standstill (the maintenance path never drives the arm). + """ + driver = trossen_arm.TrossenArmDriver() + driver.configure( + trossen_arm.Model.wxai_v0, + trossen_arm.StandardEndEffector.wxai_v0_follower, + str(ip), + clear_error, + CONFIGURE_TIMEOUT_S, + ) + try: + yield driver + finally: + try: + driver.set_all_modes(trossen_arm.Mode.idle) + finally: + driver.cleanup() + + +def set_zero_whole_arm(driver: trossen_arm.TrossenArmDriver) -> str | None: + """Persist the current pose as zero via position_offset (one EEPROM write). + + Trossen semantics (docs "position_offset"): position_motor = position + + position_offset, i.e. the offset is subtracted from motor feedback. To + make the current pose read 0: new_offset = old_offset + current_position. + + Returns None on success or an error message (arm_zero driver contract). + """ + positions = driver.get_all_positions() + characteristics = driver.get_joint_characteristics() + if len(characteristics) != len(positions): + return ( + f"joint characteristics size mismatch: {len(characteristics)} characteristics " + f"vs {len(positions)} positions" + ) + for characteristic, position in zip(characteristics, positions, strict=True): + characteristic.position_offset = float(characteristic.position_offset) + float(position) + driver.set_joint_characteristics(characteristics) + logging.getLogger(__name__).info( + "Trossen zero set at current pose (new position_offsets: %s)", + [round(float(c.position_offset), 4) for c in characteristics], + ) + return None + + +def read_ip_settings(ip: str) -> IpSettings: + """Read the controller's EEPROM network settings (no changes).""" + with open_session(ip) as driver: + return IpSettings( + manual_ip=str(driver.get_manual_ip()), + ip_method=str(driver.get_ip_method()), + firmware_version=str(driver.get_controller_version()), + ) + + +def provision_manual_ip(current_ip: str, new_ip: str) -> None: + """Persist ``new_ip`` as the controller's manual IP and reboot it. + + The controller writes the address to EEPROM and only applies it after the + reboot; the caller must re-discover the controller at ``new_ip`` to verify + (see PROVISION_REBOOT_WAIT_S). Raises on any SDK failure. + """ + ipaddress.IPv4Address(new_ip) # Fast-fail before touching the controller. + driver = trossen_arm.TrossenArmDriver() + driver.configure( + trossen_arm.Model.wxai_v0, + trossen_arm.StandardEndEffector.wxai_v0_follower, + str(current_ip), + False, + CONFIGURE_TIMEOUT_S, + ) + driver.set_manual_ip(str(new_ip)) + driver.set_ip_method(trossen_arm.IPMethod.manual) + # cleanup(True) reboots the controller so the EEPROM settings take effect. + driver.cleanup(True) + logging.getLogger(__name__).info( + "Trossen controller IP provisioned: %s -> %s (EEPROM, controller rebooting)", + current_ip, + new_ip, + ) + + +def local_subnet_prefixes() -> list[str]: + """The /24 prefixes (``"a.b.c"``) of all global IPv4 addresses of this host. + + Read-only (``ip -4 addr show``); loopback and link-local are excluded. + These are the subnets a discovery sweep can actually reach. + """ + result = subprocess.run( + ["ip", "-4", "-o", "addr", "show", "scope", "global"], + capture_output=True, + text=True, + check=True, + ) + prefixes: list[str] = [] + for line in result.stdout.splitlines(): + parts = line.split() + if "inet" not in parts: + continue + address = parts[parts.index("inet") + 1].split("/")[0] + prefix = ".".join(address.split(".")[:3]) + if prefix.startswith(("127.", "169.254.")) or prefix in prefixes: + continue + prefixes.append(prefix) + return prefixes + + +def candidate_ips(subnet: str, slot: str) -> list[str]: + """Provisioning candidate addresses for ``slot`` on the ``"a.b.c"`` subnet. + + The walk starts at the slot's reserved host octet and steps by + PROVISION_HOST_OCTET_STEP, keeping each slot's candidates disjoint from + every other slot's. When all candidates are in use the caller must ask + for a manual address. + """ + base = PROVISION_SLOT_HOST_OCTETS[slot] + return [f"{subnet}.{octet}" for octet in range(base, 255, PROVISION_HOST_OCTET_STEP)] + + +def ip_in_use(ip: str, interface: str | None = None) -> bool: + """Best-effort duplicate-address check for a provisioning candidate. + + True when anything on the LAN answers a ping or an ``arping -D`` + duplicate-address probe. arping needs the outgoing ``interface`` and is + skipped when unavailable (not installed, or no raw-socket privilege) — + ping alone still catches every normal IP stack. + """ + ping = subprocess.run( + ["ping", "-c", "1", "-W", str(_PING_TIMEOUT_S), str(ip)], + capture_output=True, + text=True, + check=False, + ) + if ping.returncode == 0: + return True + if interface is None or shutil.which("arping") is None: + return False + # arping -D exits 0 when the address is free, non-zero when a reply came + # back (duplicate) — but also on errors (e.g. missing raw-socket + # privilege), so only a reply actually printed counts as "in use". + arping = subprocess.run( + ["arping", "-D", "-c", "2", "-w", str(_ARPING_TIMEOUT_S), "-I", str(interface), str(ip)], + capture_output=True, + text=True, + check=False, + ) + return arping.returncode != 0 and "reply from" in arping.stdout.lower() + + +def wired_up_interfaces(sys_class_net: pathlib.Path = pathlib.Path("/sys/class/net")) -> list[str]: + """Wired Ethernet interfaces whose link is up, sorted by name. + + Wireless interfaces are excluded (adding addresses there can disturb the + Wi-Fi LAN), as are loopback and non-Ethernet devices. + """ + interfaces: list[str] = [] + if not sys_class_net.is_dir(): + return interfaces + for iface in sys_class_net.iterdir(): + if (iface / "wireless").exists(): + continue + try: + # ARPHRD_ETHER == 1; excludes loopback (772), CAN (280), ... + if (iface / "type").read_text().strip() != "1": + continue + if (iface / "operstate").read_text().strip() != "up": + continue + except OSError: + continue # interface disappeared mid-scan + interfaces.append(iface.name) + return sorted(interfaces) + + +def free_host_ip(subnet: str, interface: str) -> str | None: + """A conflict-free host address on the ``"a.b.c"`` /24 for this machine. + + Walks from HOST_OCTET_START upward, skipping every arm provisioning + candidate octet and every address something on the LAN already answers + for (ping + arping duplicate detection via ip_in_use). None when the + whole range is in use. + """ + for octet in range(HOST_OCTET_START, 255): + if octet in _ARM_CANDIDATE_OCTETS: + continue + candidate = f"{subnet}.{octet}" + if not ip_in_use(candidate, interface): + return candidate + return None + + +def add_temporary_address(interface: str, cidr: str) -> None: + """Add ``cidr`` to ``interface`` until reboot/removal (``sudo ip addr add``). + + Used to probe which NIC reaches the arm controllers before anything is + persisted; pair with remove_temporary_address. Raises RuntimeError on + failure. + """ + result = subprocess.run( + ["sudo", "ip", "addr", "add", str(cidr), "dev", str(interface)], + capture_output=True, + text=True, + check=False, + ) + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + raise RuntimeError(f"adding temporary {cidr} to {interface} failed: {detail}") + + +def remove_temporary_address(interface: str, cidr: str) -> None: + """Remove an address added by add_temporary_address (best effort, logged).""" + result = subprocess.run( + ["sudo", "ip", "addr", "del", str(cidr), "dev", str(interface)], + capture_output=True, + text=True, + check=False, + ) + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + logging.getLogger(__name__).warning( + "Removing temporary %s from %s failed: %s", cidr, interface, detail + ) + + +def add_persistent_address(interface: str, cidr: str) -> None: + """Add ``cidr`` (e.g. "192.168.1.200/24") to ``interface`` via NetworkManager. + + The address is ADDED next to the connection's existing addresses (DHCP + stays untouched) and persists across reboots. Raises RuntimeError when the + interface has no active NetworkManager connection or nmcli fails — the + caller falls back to printing manual setup instructions. + """ + if shutil.which("nmcli") is None: + raise RuntimeError( + "nmcli not found — configure the address manually " + "(see packages/openpi-runtime/docs/trossen/README.md)" + ) + show = subprocess.run( + ["nmcli", "-g", "GENERAL.CONNECTION", "device", "show", str(interface)], + capture_output=True, + text=True, + check=False, + ) + connection = show.stdout.strip() + if show.returncode != 0 or not connection or connection == "--": + raise RuntimeError( + f"interface {interface} has no active NetworkManager connection — " + "configure the address manually (see packages/openpi-runtime/docs/trossen/README.md)" + ) + for command in ( + ["sudo", "nmcli", "connection", "modify", connection, "+ipv4.addresses", str(cidr)], + ["sudo", "nmcli", "connection", "up", connection], + ): + result = subprocess.run(command, capture_output=True, text=True, check=False) + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + raise RuntimeError(f"{' '.join(command)} failed: {detail}") + logging.getLogger(__name__).info( + "Added %s to NetworkManager connection %r (interface %s)", cidr, connection, interface + ) + + +def interface_for_subnet(subnet: str) -> str | None: + """The host interface owning an address on the ``"a.b.c"`` /24, if any.""" + result = subprocess.run( + ["ip", "-4", "-o", "addr", "show", "scope", "global"], + capture_output=True, + text=True, + check=True, + ) + for line in result.stdout.splitlines(): + parts = line.split() + if "inet" not in parts: + continue + address = parts[parts.index("inet") + 1].split("/")[0] + if ".".join(address.split(".")[:3]) == subnet: + return parts[1] + return None + + +def _discover_locked(subnets: list[str]) -> list[DiscoveredController]: + def _probe_chunk(subnet: str, ip_start: int, ip_end: int) -> list[DiscoveredController]: + return [ + DiscoveredController( + address=str(found.ip), + model=str(trossen_arm.MODEL_NAME[found.model]), + firmware_version=str(found.firmware_version), + error_state=str(trossen_arm.ERROR_INFORMATION[found.error_state]), + ) + for found in trossen_arm.TrossenArmDriver.discover( + subnet=subnet, + ip_start=ip_start, + ip_end=ip_end, + timeout=DISCOVERY_TIMEOUT_S, + ) + ] + + chunks = [ + (subnet, start, min(start + DISCOVERY_CHUNK_HOSTS - 1, 254)) + for subnet in subnets + for start in range(1, 255, DISCOVERY_CHUNK_HOSTS) + ] + results: list[DiscoveredController] = [] + if chunks: + with ThreadPoolExecutor(max_workers=DISCOVERY_MAX_WORKERS) as pool: + for chunk_result in pool.map(lambda args: _probe_chunk(*args), chunks): + results.extend(chunk_result) + if results: + logging.getLogger(__name__).info( + "Discovered %d Trossen controller(s): %s", len(results), [r.address for r in results] + ) + return results + + +def _split_host(ip: str) -> tuple[str, int]: + """Split ``"a.b.c.d"`` into the ``"a.b.c"`` prefix and the host octet.""" + ipaddress.IPv4Address(ip) + octets = str(ip).split(".") + return ".".join(octets[:3]), int(octets[3]) diff --git a/tests/test_config.py b/tests/test_config.py index c4425c2..2a358de 100644 --- a/tests/test_config.py +++ b/tests/test_config.py @@ -3,7 +3,15 @@ import pytest -from openpi_control import ArmConfig, ConfigurationError, InputLayout, SocketCanConnection +from openpi_control import ( + ArmConfig, + ConfigurationError, + EthernetConnection, + InputLayout, + SerialConnection, + SocketCanConnection, + connection_for_interface, +) from openpi_control.config import SUPPORTED_MODELS from openpi_control.protocol import topics_for @@ -15,7 +23,8 @@ def test_physical_model_catalog_is_complete(model: str) -> None: assert assets.model_config.is_file() assert assets.instance_config.is_file() assert assets.urdf.is_file() - assert len(config.joint_names()) == 6 + # 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) # 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" @@ -23,6 +32,26 @@ def test_physical_model_catalog_is_complete(model: str) -> None: assert json.loads(assets.model_config.read_text())["algo_type"] in ("Pinocchio", "KDL") +def test_connection_for_interface_dispatches_on_the_interface_form() -> None: + assert connection_for_interface("can_left") == SocketCanConnection("can_left") + assert connection_for_interface("192.168.1.11") == EthernetConnection("192.168.1.11") + assert connection_for_interface("/dev/serial/by-id/usb-test-if00") == SerialConnection( + "/dev/serial/by-id/usb-test-if00" + ) + + +def test_serial_connection_requires_a_dev_path() -> None: + with pytest.raises(ConfigurationError, match="/dev path"): + SerialConnection("ttyUSB0") + + +def test_so101_catalog_declares_serial_port_type_and_baudrate() -> None: + config = ArmConfig("arm", "SO101", SocketCanConnection("test"), effector_model="E_SO101") + catalog = json.loads(config.resolve_assets().model_config.read_text())["catalog"] + assert catalog["port_type"] == "Serial" + assert config.catalog_baudrate() == 1000000 + + def test_logical_identity_is_separate_from_instance_config(tmp_path: Path) -> None: canonical = ( ArmConfig("source", "Yam", SocketCanConnection("test")).resolve_assets().instance_config diff --git a/tests/test_servos.py b/tests/test_servos.py index e140d83..864ce1c 100644 --- a/tests/test_servos.py +++ b/tests/test_servos.py @@ -8,7 +8,7 @@ import pytest from openpi_control import servos -from openpi_control.servos import buses, dm_can, dxl_serial, encos_can +from openpi_control.servos import buses, dm_can, dxl_serial, encos_can, ft_serial, trossen_eth _ENCOS_BROADCAST_ID = 0x7FF @@ -42,6 +42,8 @@ def test_registry_resolves_every_model_json_servo() -> None: assert servos.zero_driver("DM J4340") is dm_can assert servos.zero_driver("Encos EC-A4310-P2-36") is encos_can assert servos.zero_driver("Dynamixel XM430-W210") is dxl_serial + assert servos.zero_driver("FeeTech STS3215") is ft_serial + assert servos.zero_driver("Trossen WXAI Joint") is trossen_eth assert servos.zero_driver("ARX Remote Encoder") is None assert servos.zero_driver("CAN Passive Encoder") is None @@ -51,10 +53,19 @@ def test_registry_rejects_unknown_model() -> None: servos.zero_driver("Unknown Servo 9000") -def test_every_driver_declares_a_known_port_type() -> None: +def test_every_driver_declares_a_known_port_type_and_zero_entrypoint() -> None: for driver in servos.SERVO_ZERO_DRIVERS.values(): - if driver is not None: - assert driver.PORT_TYPE in (buses.PORT_TYPE_CAN, buses.PORT_TYPE_SERIAL) + if driver is None: + continue + assert driver.PORT_TYPE in ( + buses.PORT_TYPE_CAN, + buses.PORT_TYPE_SERIAL, + buses.PORT_TYPE_ETHERNET, + ) + if driver.WHOLE_ARM_ZERO: + assert callable(driver.set_zero_whole_arm) + else: + assert callable(driver.set_zero) def test_dm_zero_acknowledged() -> None: @@ -106,12 +117,153 @@ def test_dxl_zero_fails_fast_until_integrated() -> None: dxl_serial.set_zero(object(), 1) -def test_serial_bus_session_fails_fast_until_integrated() -> None: - with pytest.raises(NotImplementedError, match="serial bus sessions"): +def test_serial_bus_session_requires_a_baudrate() -> None: + with pytest.raises(SystemExit, match="explicit positive baudrate"): with buses.open_bus(buses.PORT_TYPE_SERIAL, "/dev/ttyUSB_test"): pass +def test_serial_bus_session_opens_pyserial_with_catalog_baudrate( + monkeypatch: pytest.MonkeyPatch, +) -> None: + opened: dict[str, object] = {} + + class _FakePortSession: + def __init__(self, *, port: str, baudrate: int, timeout: float) -> None: + opened.update(port=port, baudrate=baudrate, timeout=timeout) + + def __enter__(self) -> _FakePortSession: + return self + + def __exit__(self, *_: object) -> None: + pass + + monkeypatch.setattr(buses.pyserial, "Serial", _FakePortSession) + with buses.open_bus(buses.PORT_TYPE_SERIAL, "/dev/ttyUSB_test", baudrate=1000000) as bus: + assert isinstance(bus, _FakePortSession) + assert opened["port"] == "/dev/ttyUSB_test" + assert opened["baudrate"] == 1000000 + assert opened["timeout"] == pytest.approx(0.1) + + +# --- FeeTech SMS/STS (ft_serial) ------------------------------------------ + + +class _FakeSerial: + """Minimal pyserial double: records writes, replays scripted response bytes.""" + + def __init__(self, responses: list[bytes]) -> None: + self.written: list[bytes] = [] + self.baudrate = 1000000 + self._responses = responses + self._buffer = b"" + + def write(self, data: bytes) -> int: + self.written.append(bytes(data)) + return len(data) + + def flush(self) -> None: + pass + + def reset_input_buffer(self) -> None: + self._buffer = b"" + + def read(self, size: int) -> bytes: + if not self._buffer and self._responses: + self._buffer = self._responses.pop(0) + result, self._buffer = self._buffer[:size], self._buffer[size:] + return result + + +def _ft_status(servo_id: int, *, error: int = 0, params: bytes = b"") -> bytes: + """A FeeTech status packet: FF FF ID LEN ERR [params] CHK.""" + packet = bytearray(b"\xff\xff") + bytes([servo_id, 2 + len(params), error]) + params + packet.append(ft_serial.check_sum(packet)) + return bytes(packet) + + +def test_ft_build_packet_checksum_matches_protocol() -> None: + # PING to servo 1: FF FF 01 02 01 FB (complement checksum over ID..params). + packet = ft_serial.build_packet(1, ft_serial.INST_PING, b"") + assert packet == bytes([0xFF, 0xFF, 0x01, 0x02, 0x01, 0xFB]) + + +def test_ft_ping_and_scan_report_answering_ids() -> None: + # Servo 2 stays silent (empty response); 1 and 3 answer valid status packets. + bus = _FakeSerial([_ft_status(1), b"", _ft_status(3)]) + assert ft_serial.scan(bus, (1, 2, 3)) == [1, 3] + + +def test_ft_read_returns_register_bytes() -> None: + bus = _FakeSerial([_ft_status(1, params=bytes([0x04]))]) + assert ft_serial.ft_read(bus, 1, ft_serial.ADDR_BAUD_RATE, 1) == bytes([0x04]) + # The read request names the address and length. + assert bus.written[0][5] == ft_serial.ADDR_BAUD_RATE + assert bus.written[0][6] == 1 + + +def test_ft_read_rejects_corrupted_checksum() -> None: + good = _ft_status(1, params=bytes([0x04])) + corrupted = good[:-1] + bytes([good[-1] ^ 0xFF]) + bus = _FakeSerial([corrupted]) + assert ft_serial.ft_read(bus, 1, ft_serial.ADDR_BAUD_RATE, 1) is None + + +def test_ft_write_reports_servo_error_bits() -> None: + bus = _FakeSerial([_ft_status(1, error=0x20)]) + assert ft_serial.ft_write(bus, 1, ft_serial.ADDR_TORQUE_ENABLE, 1, 1) is False + + +def test_ft_set_zero_wraps_calibrate_middle_in_eeprom_unlock_lock() -> None: + bus = _FakeSerial([_ft_status(2), _ft_status(2), _ft_status(2)]) + assert ft_serial.set_zero(bus, 2) is None + # Packet layout: FF FF ID LEN INST ADDR VALUE CHK. + addresses = [packet[5] for packet in bus.written] + values = [packet[6] for packet in bus.written] + assert addresses == [ + ft_serial.ADDR_LOCK_FLAG, + ft_serial.ADDR_TORQUE_ENABLE, + ft_serial.ADDR_LOCK_FLAG, + ] + assert values == [0, ft_serial.TORQUE_ENABLE_CALIBRATE_MIDDLE, 1] + + +def test_ft_set_zero_reports_unlock_failure() -> None: + bus = _FakeSerial([]) # the unlock write times out + error = ft_serial.set_zero(bus, 2) + assert error is not None and "unlock" in error + + +def test_ft_baudrate_register_values_are_reversible() -> None: + mapping = ft_serial.BAUDRATE_REG_VALUES + assert mapping[1000000] == 0 + assert len(set(mapping.values())) == len(mapping) + + +def test_ft_get_baudrate_reads_the_register() -> None: + bus = _FakeSerial([_ft_status(1, params=bytes([ft_serial.BAUDRATE_REG_VALUES[115200]]))]) + assert ft_serial.get_baudrate(bus, 1) == 115200 + + +def test_ft_set_baudrate_switches_the_host_side_too() -> None: + bus = _FakeSerial([_ft_status(1), _ft_status(1), _ft_status(1), _ft_status(1)]) + assert ft_serial.set_baudrate(bus, 1, 500000) is True + assert bus.baudrate == 500000 + + +def test_ft_set_baudrate_restores_host_baud_when_servo_goes_silent() -> None: + # unlock ack + write ack, then silence: no lock ack, no ping answer. + bus = _FakeSerial([_ft_status(1), _ft_status(1)]) + assert ft_serial.set_baudrate(bus, 1, 500000) is False + assert bus.baudrate == 1000000 + + +def test_ft_set_baudrate_rejects_unsupported_rates() -> None: + bus = _FakeSerial([]) + assert ft_serial.set_baudrate(bus, 1, 12345) is False + assert bus.written == [] + + def test_check_interface_serial_uses_device_path(tmp_path: pathlib.Path) -> None: device = tmp_path / "ttyUSB_test" assert buses.check_interface(buses.PORT_TYPE_SERIAL, str(device)) is not None @@ -122,3 +274,207 @@ def test_check_interface_serial_uses_device_path(tmp_path: pathlib.Path) -> None def test_check_interface_can_reports_missing_interface() -> None: error = buses.check_interface(buses.PORT_TYPE_CAN, "can_does_not_exist") assert error is not None and "does not exist" in error + + +class _FakeJointCharacteristic: + def __init__(self, position_offset: float) -> None: + self.position_offset = position_offset + + +class _FakeTrossenDriver: + """trossen_arm.TrossenArmDriver double for the whole-arm zeroing routine.""" + + def __init__(self, positions: list[float], offsets: list[float]) -> None: + self._positions = positions + self._characteristics = [_FakeJointCharacteristic(offset) for offset in offsets] + self.written: list[_FakeJointCharacteristic] | None = None + + def get_all_positions(self) -> list[float]: + return self._positions + + def get_joint_characteristics(self) -> list[_FakeJointCharacteristic]: + return self._characteristics + + def set_joint_characteristics(self, characteristics: list[_FakeJointCharacteristic]) -> None: + self.written = characteristics + + +def test_trossen_whole_arm_zero_accumulates_position_offsets() -> None: + # Trossen semantics: new_offset = old_offset + current_position makes the + # current pose read zero (position_motor = position + position_offset). + driver = _FakeTrossenDriver(positions=[0.1, -0.2, 0.0], offsets=[1.0, 2.0, -0.5]) + assert trossen_eth.set_zero_whole_arm(driver) is None + assert driver.written is not None + assert [c.position_offset for c in driver.written] == pytest.approx([1.1, 1.8, -0.5]) + + +def test_trossen_whole_arm_zero_reports_size_mismatch() -> None: + driver = _FakeTrossenDriver(positions=[0.1, 0.2], offsets=[0.0]) + error = trossen_eth.set_zero_whole_arm(driver) + assert error is not None and "size mismatch" in error + assert driver.written is None + + +def test_trossen_declares_whole_arm_ethernet_family() -> None: + assert trossen_eth.PORT_TYPE == buses.PORT_TYPE_ETHERNET + assert trossen_eth.WHOLE_ARM_ZERO is True + + +def test_check_interface_ethernet_rejects_invalid_ip() -> None: + error = buses.check_interface(buses.PORT_TYPE_ETHERNET, "not-an-ip") + assert error is not None and "IPv4" in error + + +def test_check_interface_ethernet_probes_reachability(monkeypatch: pytest.MonkeyPatch) -> None: + probed: list[str] = [] + monkeypatch.setattr(trossen_eth, "reachable", lambda ip: probed.append(ip) or True) + assert buses.check_interface(buses.PORT_TYPE_ETHERNET, "192.168.1.11") is None + assert probed == ["192.168.1.11"] + monkeypatch.setattr(trossen_eth, "reachable", lambda ip: False) + error = buses.check_interface(buses.PORT_TYPE_ETHERNET, "192.168.1.11") + assert error is not None and "did not answer" in error + + +def test_ping_reachable_controller_ips_filters_by_subnet_mac_and_ping( + monkeypatch: pytest.MonkeyPatch, +) -> None: + class _Result: + def __init__(self, *, stdout: str = "", returncode: int = 0) -> None: + self.stdout = stdout + self.stderr = "" + self.returncode = returncode + + neighbors = "\n".join( + ( + "192.168.1.11 dev eth0 lladdr 04:e9:e5:11:22:33 REACHABLE", + "192.168.1.12 dev eth0 lladdr 04:e9:e5:44:55:66 STALE", + "192.168.1.50 dev eth0 lladdr aa:bb:cc:dd:ee:ff REACHABLE", + "192.168.4.11 dev eth0 lladdr 04:e9:e5:77:88:99 REACHABLE", + ) + ) + + def fake_run(command: list[str], **kwargs: object) -> _Result: + del kwargs + if command[:3] == ["ip", "neigh", "show"]: + return _Result(stdout=neighbors) + if command[0] == "ping": + return _Result(returncode=0 if command[-1] == "192.168.1.12" else 1) + raise AssertionError(command) + + monkeypatch.setattr(trossen_eth.subprocess, "run", fake_run) + + assert trossen_eth.ping_reachable_controller_ips(["192.168.1"]) == ["192.168.1.12"] + + +def _fake_net_iface( + root: pathlib.Path, name: str, *, dev_type: str, operstate: str, wireless: bool = False +) -> None: + iface = root / name + iface.mkdir(parents=True) + (iface / "type").write_text(f"{dev_type}\n") + (iface / "operstate").write_text(f"{operstate}\n") + if wireless: + (iface / "wireless").mkdir() + + +def test_wired_up_interfaces_excludes_wireless_loopback_and_down(tmp_path: pathlib.Path) -> None: + _fake_net_iface(tmp_path, "eth0", dev_type="1", operstate="up") + _fake_net_iface(tmp_path, "eth1", dev_type="1", operstate="down") + _fake_net_iface(tmp_path, "wlan0", dev_type="1", operstate="up", wireless=True) + _fake_net_iface(tmp_path, "lo", dev_type="772", operstate="unknown") + _fake_net_iface(tmp_path, "can0", dev_type="280", operstate="up") + + assert trossen_eth.wired_up_interfaces(tmp_path) == ["eth0"] + + +def test_free_host_ip_skips_arm_candidate_octets_and_addresses_in_use( + monkeypatch: pytest.MonkeyPatch, +) -> None: + in_use = {"192.168.1.200", "192.168.1.201"} + monkeypatch.setattr(trossen_eth, "ip_in_use", lambda ip, interface=None: ip in in_use) + + assert trossen_eth.free_host_ip("192.168.1", "eth0") == "192.168.1.202" + # The arm provisioning candidates (.211/.212/.221/.222) are never offered + # as host addresses even when free. + assert trossen_eth.HOST_OCTET_START == 200 + in_use.update(f"192.168.1.{octet}" for octet in range(200, 215)) + assert trossen_eth.free_host_ip("192.168.1", "eth0") == "192.168.1.215" + monkeypatch.setattr(trossen_eth, "ip_in_use", lambda ip, interface=None: True) + assert trossen_eth.free_host_ip("192.168.1", "eth0") is None + + +def test_temporary_address_add_and_remove_run_ip_addr(monkeypatch: pytest.MonkeyPatch) -> None: + commands: list[list[str]] = [] + + class _Result: + stdout = "" + stderr = "" + returncode = 0 + + monkeypatch.setattr( + trossen_eth.subprocess, + "run", + lambda command, **kwargs: commands.append(command) or _Result(), + ) + + trossen_eth.add_temporary_address("eth0", "192.168.1.200/24") + trossen_eth.remove_temporary_address("eth0", "192.168.1.200/24") + + assert commands == [ + ["sudo", "ip", "addr", "add", "192.168.1.200/24", "dev", "eth0"], + ["sudo", "ip", "addr", "del", "192.168.1.200/24", "dev", "eth0"], + ] + + +def test_add_temporary_address_raises_on_failure(monkeypatch: pytest.MonkeyPatch) -> None: + class _Result: + stdout = "" + stderr = "RTNETLINK answers: Operation not permitted" + returncode = 2 + + monkeypatch.setattr(trossen_eth.subprocess, "run", lambda command, **kwargs: _Result()) + + with pytest.raises(RuntimeError, match="Operation not permitted"): + trossen_eth.add_temporary_address("eth0", "192.168.1.200/24") + + +def test_add_persistent_address_runs_nmcli_modify_and_up(monkeypatch: pytest.MonkeyPatch) -> None: + commands: list[list[str]] = [] + + class _Result: + def __init__(self, stdout: str = "", returncode: int = 0) -> None: + self.stdout = stdout + self.stderr = "" + self.returncode = returncode + + def fake_run(command: list[str], **kwargs: object) -> _Result: + commands.append(command) + if command[:4] == ["nmcli", "-g", "GENERAL.CONNECTION", "device"]: + return _Result(stdout="Wired connection 1\n") + return _Result() + + monkeypatch.setattr(trossen_eth.shutil, "which", lambda name: "/usr/bin/nmcli") + monkeypatch.setattr(trossen_eth.subprocess, "run", fake_run) + + trossen_eth.add_persistent_address("eth0", "192.168.1.200/24") + + assert commands[1] == [ + "sudo", "nmcli", "connection", "modify", "Wired connection 1", + "+ipv4.addresses", "192.168.1.200/24", + ] + assert commands[2] == ["sudo", "nmcli", "connection", "up", "Wired connection 1"] + + +def test_add_persistent_address_requires_active_nm_connection( + monkeypatch: pytest.MonkeyPatch, +) -> None: + class _Result: + stdout = "--\n" + stderr = "" + returncode = 0 + + monkeypatch.setattr(trossen_eth.shutil, "which", lambda name: "/usr/bin/nmcli") + monkeypatch.setattr(trossen_eth.subprocess, "run", lambda command, **kwargs: _Result()) + + with pytest.raises(RuntimeError, match="no active NetworkManager connection"): + trossen_eth.add_persistent_address("eth0", "192.168.1.200/24") diff --git a/uv.lock b/uv.lock index 33eec5d..6f319f7 100644 --- a/uv.lock +++ b/uv.lock @@ -11,7 +11,7 @@ name = "cffi" version = "2.0.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pycparser" }, + { name = "pycparser", marker = "implementation_name != 'PyPy'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/eb/56/b1ba7935a17738ae8453301356628e8147c79dbb825bcbc73dc7401f9846/cffi-2.0.0.tar.gz", hash = "sha256:44d1b5909021139fe36001ae048dbdde8214afa20200eda0f64c068cac5d5529", size = 523588, upload-time = "2025-09-08T23:24:04.541Z" } wheels = [ @@ -333,13 +333,15 @@ wheels = [ [[package]] name = "openpi-control" -version = "0.1.0" +version = "0.1.1" source = { editable = "." } dependencies = [ { name = "numpy", version = "2.4.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.12'" }, { name = "numpy", version = "2.5.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.12'" }, + { name = "pyserial" }, { name = "python-can" }, { name = "pyzmq" }, + { name = "trossen-arm" }, ] [package.optional-dependencies] @@ -354,11 +356,13 @@ dev = [ requires-dist = [ { name = "hypothesis", marker = "extra == 'dev'", specifier = ">=6" }, { name = "numpy", specifier = ">=1.26" }, + { name = "pyserial", specifier = ">=3.5" }, { name = "pytest", marker = "extra == 'dev'", specifier = ">=8" }, { name = "pytest-cov", marker = "extra == 'dev'", specifier = ">=5" }, { name = "python-can", specifier = ">=4.3" }, { name = "pyzmq", specifier = ">=25" }, { name = "ruff", marker = "extra == 'dev'", specifier = ">=0.6" }, + { name = "trossen-arm", specifier = "==1.10.0" }, ] provides-extras = ["dev"] @@ -398,6 +402,15 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/f4/7e/a72dd26f3b0f4f2bf1dd8923c85f7ceb43172af56d63c7383eb62b332364/pygments-2.20.0-py3-none-any.whl", hash = "sha256:81a9e26dd42fd28a23a2d169d86d7ac03b46e2f8b59ed4698fb4785f946d0176", size = 1231151, upload-time = "2026-03-29T13:29:30.038Z" }, ] +[[package]] +name = "pyserial" +version = "3.5" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/1e/7d/ae3f0a63f41e4d2f6cb66a5b57197850f919f59e558159a4dd3a818f5082/pyserial-3.5.tar.gz", hash = "sha256:3c77e014170dfffbd816e6ffc205e9842efb10be9f58ec16d3e8675b4925cddb", size = 159125, upload-time = "2020-11-23T03:59:15.045Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/07/bc/587a445451b253b285629263eb51c2d8e9bcea4fc97826266d186f96f558/pyserial-3.5-py2.py3-none-any.whl", hash = "sha256:c4451db6ba391ca6ca299fb3ec7bae67a5c55dde170964c7a14ceefec02f2cf0", size = 90585, upload-time = "2020-11-23T03:59:13.41Z" }, +] + [[package]] name = "pytest" version = "9.1.1" @@ -588,6 +601,26 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/7b/61/cceae43728b7de99d9b847560c262873a1f6c98202171fd5ed62640b494b/tomli-2.4.1-py3-none-any.whl", hash = "sha256:0d85819802132122da43cb86656f8d1f8c6587d54ae7dcaf30e90533028b49fe", size = 14583, upload-time = "2026-03-25T20:22:03.012Z" }, ] +[[package]] +name = "trossen-arm" +version = "1.10.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy", version = "2.4.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.12'" }, + { name = "numpy", version = "2.5.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.12'" }, +] +wheels = [ + { url = "https://files.pythonhosted.org/packages/0a/32/13307c7eef88cc656a00d41ea1925b749e7762fac6da7252ea7938c1b7d2/trossen_arm-1.10.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:76dbfae7841fad136e747be3921c2101801d25ac2f4ca176eec3027a9b1a5e74", size = 729263, upload-time = "2026-05-05T21:13:26.789Z" }, + { url = "https://files.pythonhosted.org/packages/6c/a4/5e0f49b18d2533aee4536666d4b0117c9aeb5d3230f35b233e3c106a2b8a/trossen_arm-1.10.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e145bd36ffb53cdf1c6e6de4d4d2e8e5302ea599db0d716e75f97f61925631b7", size = 1026542, upload-time = "2026-05-05T21:13:28.287Z" }, + { url = "https://files.pythonhosted.org/packages/7a/bb/e0f2c80a1018f507b4dd252603d62196acdc558104cb3a7349c0cb35e72c/trossen_arm-1.10.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:2ebd6775ca42dc75ebd718ea080450b335daedc2eedbd6223cbdfe47dbe35cb1", size = 1134241, upload-time = "2026-05-05T21:13:29.604Z" }, + { url = "https://files.pythonhosted.org/packages/bc/35/4b9e702dbe5a844171e76d9e20e57f7d2ebce539dc25866a7dc665e24277/trossen_arm-1.10.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:8c7653ebeb8a0240ba0f6de99cfb63ef470d8d2cff13844f9a994e48d4c68095", size = 698112, upload-time = "2026-05-05T21:13:31.316Z" }, + { url = "https://files.pythonhosted.org/packages/55/78/54758216c0b3fea040edabc452facf7e8363752e113e97e58bbc817b76f0/trossen_arm-1.10.0-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:1ecf68daf9c56829c6b8b0b8ea08b5d9ef8364424e847b32c321b31395d9fbb3", size = 1027422, upload-time = "2026-05-05T21:13:32.72Z" }, + { url = "https://files.pythonhosted.org/packages/cb/e7/d6b208b9f48d4549ca39f992a6ef8ea9175fa675ed1ba9a61dcfc3cb6782/trossen_arm-1.10.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:40c595fd535012f50e1e802cd23cde3daa4ad2c65a15907868cbe5b6969a3e51", size = 1137443, upload-time = "2026-05-05T21:13:34.122Z" }, + { url = "https://files.pythonhosted.org/packages/10/37/11a89f928de15bc79e8151579b7aff1347842175cd02c9cf26c76a3a6bbb/trossen_arm-1.10.0-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:d873626e07d2b734b4fa986911e19d7551379ce0b64167205abbbddb2d4de116", size = 698155, upload-time = "2026-05-05T21:13:35.287Z" }, + { url = "https://files.pythonhosted.org/packages/68/39/ee18ee48aa32878da42244365dbdb91904dc082422a947686ee8d0953e40/trossen_arm-1.10.0-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:396a01a85313b49c4456a7425528d46a4cc58520709c18312228a7c3f051cf19", size = 1027412, upload-time = "2026-05-05T21:13:36.261Z" }, + { url = "https://files.pythonhosted.org/packages/ed/fe/1f45d3f05b88b394df6974e660ae9479956d99e41f10d9859a1f675f66eb/trossen_arm-1.10.0-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8b4218b51eece5bfadaa54885bf182dc94325f7a1f8071f225a8e80fe2bbd31f", size = 1137637, upload-time = "2026-05-05T21:13:37.492Z" }, +] + [[package]] name = "typing-extensions" version = "4.16.0"