Trossen WidowX AI and SO-ARM101 support

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")

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()

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.

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

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:
+};
+

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 <typename T>
+    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<T>();
+        } 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.

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;
+};

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<Topic> p_topic,
+                            std::shared_ptr<Driver> 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;
+};
+