Debug expected num joints on 7-dof arms

include/elevated_control/interface_arm.hpp

@@ -34,7 +34,12 @@ class ArmInterface : public SynapticonBase {
  public:
   struct Config : SynapticonBaseConfig {
     std::string elevate_config_yaml = "/home/elevate/Desktop/elevate_config.yaml";
-    Config() { expected_slave_count = kNumJoints; }
+    Config() {
+      // Bus layout: positions 1..kNumJoints are SOMANET joint drives, and the
+      // final slave is the user-interface device (not a joint).
+      expected_slave_count = kNumJoints + 1;
+      num_joints = kNumJoints;
+    }
   };
 
   explicit ArmInterface(const Config& config);

include/elevated_control/interface_base.hpp

@@ -40,6 +40,11 @@ struct InSomanetSnapshot {
 struct SynapticonBaseConfig {
   std::string network_interface = "eno0";
   std::size_t expected_slave_count = 0;  // 0 = auto-detect, >0 = verify
+  // Number of joint slaves on the bus. Joints are assumed to occupy EtherCAT
+  // positions 1..num_joints; any additional trailing slaves (e.g. a
+  // user-interface device) are left alone by the driver. 0 = use total slave
+  // count (every detected slave is treated as a joint).
+  std::size_t num_joints = 0;
   std::string joint_limits_yaml;
 };
 

src/interface_base.cpp

@@ -155,14 +155,24 @@ std::expected<void, Error> SynapticonBase::Initialize() {
         Error{ErrorCode::kEtherCATError, "No EtherCAT slaves found"});
   }
 
-  num_joints_ = static_cast<std::size_t>(ec_slavecount);
+  const std::size_t total_slaves = static_cast<std::size_t>(ec_slavecount);
   if (base_config_.expected_slave_count > 0 &&
-      num_joints_ != base_config_.expected_slave_count) {
+      total_slaves != base_config_.expected_slave_count) {
     ec_close();
     return std::unexpected(Error{
         ErrorCode::kEtherCATError,
         "Expected " + std::to_string(base_config_.expected_slave_count) +
-            " slaves but found " + std::to_string(num_joints_)});
+            " slaves but found " + std::to_string(total_slaves)});
+  }
+  num_joints_ = base_config_.num_joints > 0 ? base_config_.num_joints
+                                            : total_slaves;
+  if (num_joints_ > total_slaves) {
+    ec_close();
+    return std::unexpected(Error{
+        ErrorCode::kEtherCATError,
+        "Configured num_joints (" + std::to_string(num_joints_) +
+            ") exceeds detected slave count (" + std::to_string(total_slaves) +
+            ")"});
   }
 
   // Resize all per-joint containers
@@ -335,7 +345,14 @@ std::expected<void, Error> SynapticonBase::Initialize() {
   });
 
   initialized_ = true;
-  spdlog::info("SynapticonBase initialized with {} joints", num_joints_);
+  if (total_slaves == num_joints_) {
+    spdlog::info("SynapticonBase initialized with {} joints", num_joints_);
+  } else {
+    spdlog::info(
+        "SynapticonBase initialized with {} joints ({} total EtherCAT slaves, "
+        "{} trailing non-joint slave(s))",
+        num_joints_, total_slaves, total_slaves - num_joints_);
+  }
   return {};
 }