merge lgv_dev code

cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h

@@ -53,6 +53,8 @@ public:
 private:
     void ensureWorkerStarted_();
     void workerLoop_();
+    void requestStop_(std::optional<double> acceleration = std::nullopt);
+    void abortCommand_();
     void sendZero_();
 
     static double velocityNorm_(const std::vector<double>& velocity);

cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp

@@ -61,8 +61,13 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
     if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
         return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
     }
-    if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
+    if (!worker_ || !worker_->joinable()) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
+        if (busy_.exchange(true)) {
+            return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
+        }
+    } else {
+        // speedL is a streaming command: an existing worker may receive a new target.
+        busy_.store(true);
     }
 
     ensureWorkerStarted_();
@@ -103,15 +108,7 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
-    {
+    requestStop_(acceleration);
-        std::lock_guard<std::mutex> lock(mutex_);
-        target_twist_ = {};
-        target_frame_ = FrameType::Base;
-        target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
-        command_active_ = true;
-        ++command_version_;
-    }
-    cv_.notify_all();
     return Result::success();
 }
 
@@ -192,27 +189,26 @@ void CartesianVelocityController::workerLoop_()
             }
 
             if (!planner_->updateSpeedLAcceleration(acceleration)) {
-                if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                    std::lock_guard<std::mutex> lock(mutex_);
+                    abortCommand_();
-                    command_active_ = false;
-                    sendZero_();
-                    busy_.store(false);
                     break;
                 }
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
                                 << acceleration;
-                sendZero_();
+                requestStop_();
-                busy_.store(false);
+                continue;
-                return;
             }
 
             std::vector<double> q_now;
             std::vector<double> qd_now;
             if (!read_state_(q_now, qd_now)) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             std::vector<double> qd_cmd;
@@ -222,9 +218,12 @@ void CartesianVelocityController::workerLoop_()
                                 << target_twist.vz << ", " << target_twist.wx << ", "
                                 << target_twist.wy << ", " << target_twist.wz
                                 << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             JointVelocityCommand velocity_command;
@@ -233,9 +232,12 @@ void CartesianVelocityController::workerLoop_()
             if (!send_result.ok()) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
                                 << send_result.message;
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             if (twistNorm_(target_twist) < config_.stop_twist_norm &&
@@ -260,6 +262,32 @@ void CartesianVelocityController::workerLoop_()
     busy_.store(false);
 }
 
+void CartesianVelocityController::requestStop_(const std::optional<double> acceleration)
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+        target_acceleration_ = acceleration.has_value() ? *acceleration
+                                                         : config_.stop_acceleration;
+        command_active_ = true;
+        ++command_version_;
+    }
+    cv_.notify_all();
+}
+
+void CartesianVelocityController::abortCommand_()
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        command_active_ = false;
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+    }
+    sendZero_();
+    busy_.store(false);
+}
+
 void CartesianVelocityController::sendZero_()
 {
     if (!send_velocity_) {

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp

@@ -932,7 +932,11 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         qdot = applyJointAccelerationLimits_(qdot, reference, dt);
     }
     const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
-    if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
+    // During a stop, the limiter intentionally commands a near-zero residual
+    // twist while the measured arm can still be moving in a different direction.
+    // Direction and speed-ratio checks are not meaningful for that transient.
+    if (!is_stop_command &&
+        !validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
                                                      achieved_twist_base,
                                                      toEigenVector(q_measured),
                                                      qdot)) {

cmvr-es/config/devices/motor/ethercat_motors.pb.txt

@@ -2,7 +2,7 @@ motor {
   id: "ethercat_motors"
 
   motor_groups {
-    id: "right_arm_ethercat"
+    id: "right_arm_ethercat_motors"
     bus_type: MOTOR_BUS_ETHERCAT
     vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402

cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt

@@ -2,7 +2,7 @@ motor {
   id: "ethercat_motors"
 
   motor_groups {
-    id: "right_arm_ethercat"
+    id: "right_arm_ethercat_motors"
     bus_type: MOTOR_BUS_ETHERCAT
     vendor: MOTOR_VENDOR_EYOU
     protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402

cmvr-es/config/devices/motor/mujoco_motors.pb.txt

@@ -2,7 +2,7 @@ motor {
   id: "mujoco_motors"
 
   motor_groups {
-    id: "mujoco_right_arm"
+    id: "right_arm_mujoco_motors"
     bus_type: MOTOR_BUS_MUJOCO
     vendor: MOTOR_VENDOR_MUJOCO
     protocol: MOTOR_PROTOCOL_MUJOCO

cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt

@@ -2,7 +2,7 @@ motor {
   id: "mujoco_motors"
 
   motor_groups {
-    id: "mujoco_right_arm"
+    id: "right_arm_mujoco_motors"
     bus_type: MOTOR_BUS_MUJOCO
     vendor: MOTOR_VENDOR_MUJOCO
     protocol: MOTOR_PROTOCOL_MUJOCO

cmvr-es/config/devices/motor/ti5_motors.pb.txt

@@ -2,7 +2,7 @@ motor {
   id: "ti5_motors"
 
   motor_groups {
-    id: "left_arm_can"
+    id: "left_arm_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -26,7 +26,7 @@ motor {
   }
 
   motor_groups {
-    id: "right_arm_can"
+    id: "right_arm_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -56,7 +56,7 @@ motor {
   }
 
   motor_groups {
-    id: "head_can"
+    id: "head_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN
@@ -78,7 +78,7 @@ motor {
   }
 
   motor_groups {
-    id: "waist_can"
+    id: "waist_can_motors"
     bus_type: MOTOR_BUS_CAN
     vendor: MOTOR_VENDOR_TI5
     protocol: MOTOR_PROTOCOL_CANOPEN

cmvr-es/config/manager/device_manager.pb.txt

@@ -12,7 +12,7 @@ device_manager {
   }
 
   devices {
-    id: "mujoco_motors"
+    id: "right_arm_mujoco_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/mujoco_motors.pb.txt"
     enable: false
@@ -69,14 +69,42 @@ device_manager {
   }
 
   devices {
-    id: "ti5_motors"
+    id: "mujoco_zero_touch_dexhand"
+    type: DEVICE_TYPE_DEXHAND
+    config_file: "devices/dexhand/dexhand.pb.txt"
+    enable: true
+  }
+
+  devices {
+    id: "left_arm_can_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ti5_motors.pb.txt"
     enable: false
   }
 
   devices {
-    id: "ethercat_motors"
+    id: "right_arm_can_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/ti5_motors.pb.txt"
+    enable: false
+  }
+
+    devices {
+      id: "head_can_motors"
+      type: DEVICE_TYPE_MOTOR_SYSTEM
+      config_file: "devices/motor/ti5_motors.pb.txt"
+      enable: false
+    }
+
+    devices {
+      id: "waist_can_motors"
+      type: DEVICE_TYPE_MOTOR_SYSTEM
+      config_file: "devices/motor/ti5_motors.pb.txt"
+      enable: false
+    }
+
+  devices {
+    id: "right_arm_ethercat_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ethercat_motors.pb.txt"
     enable: false

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp

@@ -13,7 +13,6 @@
 #include <stdexcept>
 #include <string>
 #include <thread>
-#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -159,18 +158,10 @@ protected:
              "cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt").string(),
             &motor_root_config));
 
-        std::unordered_set<std::string> right_arm_joints;
-        for (const auto* joint_name : kJointNames) {
-            right_arm_joints.insert(joint_name);
-        }
-        MotorManager::clearActiveJoints();
-        MotorManager::setActiveJoints(
-            "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}});
-
         motor_system_ = std::make_shared<MotorManager>(
-            "mujoco_motors", motor_root_config.motor());
+            "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
         ASSERT_TRUE(motor_system_->init());
-        world_ = MotorManager::mujocoWorldFor("mujoco_motors");
+        world_ = MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
         ASSERT_TRUE(world_);
         ASSERT_TRUE(world_->isLoaded());
 
@@ -208,7 +199,6 @@ protected:
             world_device_->stop();
         }
         DeviceManager::destroyInstance();
-        MotorManager::clearActiveJoints();
     }
 
     std::filesystem::path project_root_;

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp

@@ -10,7 +10,6 @@
 #include <memory>
 #include <string>
 #include <thread>
-#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -147,17 +146,10 @@ protected:
             (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(),
             &motor_root_config));
 
-        std::unordered_set<std::string> right_arm_joints;
+        motor_system_ = std::make_shared<MotorManager>(
-        for (const auto* joint_name : kJointNames) {
+            "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
-            right_arm_joints.insert(joint_name);
-        }
-        MotorManager::clearActiveJoints();
-        MotorManager::setActiveJoints(
-            "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}});
-
-        motor_system_ = std::make_shared<MotorManager>("mujoco_motors", motor_root_config.motor());
         ASSERT_NO_THROW(motor_system_->init());
-        world_ = MotorManager::mujocoWorldFor("mujoco_motors");
+        world_ = MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
         ASSERT_TRUE(world_);
         ASSERT_TRUE(world_->isLoaded());
 
@@ -187,7 +179,6 @@ protected:
         if (world_device_) {
             world_device_->stop();
         }
-        MotorManager::clearActiveJoints();
     }
 
     std::filesystem::path project_root_;

cmvr-es/devices/arm/robot_arm_factory.h

@@ -6,7 +6,7 @@
 
 #include "cmvr/config/arm_config/arm_config.pb.h"
 #include "common/base/logging/logger.h"
-#include "devices/arm/aubo_arm/aubo_arm.h"
+#include "devices/arm/aubo_arm/include/aubo_arm.h"
 #include "devices/arm/huayan_arm/huayan_arm.h"
 #include "devices/arm/motor_robot_arm/include/motor_robot_arm.h"
 

cmvr-es/devices/motor/manager/include/motor_manager.h

@@ -32,7 +32,9 @@ class AbstractMotorBusRuntime;
 class MotorManager final : public AbstractDevice,
                            public std::enable_shared_from_this<MotorManager> {
 public:
-    MotorManager(std::string id, const config::MotorConfig& cfg);
+    MotorManager(std::string id,
+                 const config::MotorConfig& cfg,
+                 std::string selected_group_id);
     ~MotorManager() override;
 
     DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; }
@@ -56,16 +58,7 @@ public:
 
     static std::shared_ptr<MotorManager> managerFor(const std::string& id);
     static std::shared_ptr<simulate::MujocoWorld> mujocoWorldFor(const std::string& id);
-    static void setActiveJoints(const std::string& motor_manager_id,
-                                std::unordered_map<std::string, std::unordered_set<std::string>> group_joints);
-    static void clearActiveJoints();
-
 private:
-    using ActiveJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
-
-    bool selectActiveMotors_(const std::string& group_name,
-                             const google::protobuf::RepeatedPtrField<config::MotorConfigItem>& source,
-                             std::vector<config::MotorConfigItem>& selected) const;
     bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
                                      std::vector<config::MotorConfigItem>& selected) const;
     std::shared_ptr<AbstractMotorBusRuntime> createBusRuntime_(
@@ -89,6 +82,7 @@ private:
 
 private:
     config::MotorConfig cfg_;
+    std::string selected_group_id_;
     std::vector<std::shared_ptr<AbstractMotorBusRuntime>> bus_runtimes_;
     mutable std::mutex motors_mutex_;
     std::unordered_map<std::uint8_t, std::shared_ptr<AbstractMotor>> motors_by_id_;
@@ -98,7 +92,6 @@ private:
     static std::mutex registry_mutex_;
     static std::unordered_map<std::string, std::weak_ptr<MotorManager>> managers_;
     static std::unordered_map<std::string, std::weak_ptr<simulate::MujocoWorld>> mujoco_world_registry_;
-    static std::unordered_map<std::string, ActiveJointSelection> active_joints_;
 };
 
 } // namespace cmvr::device

cmvr-es/devices/motor/manager/src/motor_manager.cpp

@@ -28,17 +28,14 @@ namespace cmvr::device {
 std::mutex MotorManager::registry_mutex_;
 std::unordered_map<std::string, std::weak_ptr<MotorManager>> MotorManager::managers_;
 std::unordered_map<std::string, std::weak_ptr<simulate::MujocoWorld>> MotorManager::mujoco_world_registry_;
-std::unordered_map<std::string, MotorManager::ActiveJointSelection> MotorManager::active_joints_;
 
-MotorManager::MotorManager(std::string id, const config::MotorConfig& cfg)
+MotorManager::MotorManager(std::string id,
-    : cfg_(cfg)
+                           const config::MotorConfig& cfg,
+                           std::string selected_group_id)
+    : cfg_(cfg),
+      selected_group_id_(std::move(selected_group_id))
 {
     id_ = std::move(id);
-    if (!cfg_.id().empty() && cfg_.id() != id_) {
-        CMVR_LOG(ERROR) << "[MotorManager] config id '" << cfg_.id()
-                        << "' does not match device id '" << id_ << "'";
-        id_.clear();
-    }
 }
 
 MotorManager::~MotorManager() = default;
@@ -52,6 +49,14 @@ bool MotorManager::init()
         CMVR_LOG(ERROR) << "[MotorManager] id is empty";
         return false;
     }
+    if (selected_group_id_.empty()) {
+        CMVR_LOG(ERROR) << "[MotorManager] selected motor group id is empty: " << id_;
+        return false;
+    }
+    if (cfg_.motor_groups_size() == 0) {
+        CMVR_LOG(ERROR) << "[MotorManager] no motor groups configured: " << id_;
+        return false;
+    }
 
     bus_runtimes_.clear();
     bus_runtimes_.reserve(static_cast<std::size_t>(cfg_.motor_groups_size()));
@@ -62,25 +67,27 @@ bool MotorManager::init()
     }
 
     bool all_ok = true;
+    std::size_t selected_group_count = 0;
     for (const auto& motor_group_cfg : cfg_.motor_groups()) {
         const auto& group_name = motor_group_cfg.id();
-        if (group_name.empty()) {
+        if (group_name != selected_group_id_) {
-            CMVR_LOG(ERROR) << "[MotorManager] motor group id is empty in manager: " << id_;
-            all_ok = false;
             continue;
         }
+        ++selected_group_count;
         if (!motor_group_cfg.has_motors()) {
             CMVR_LOG(ERROR) << "[MotorManager] motor group missing motors: " << group_name;
             all_ok = false;
             continue;
         }
 
-        std::vector<config::MotorConfigItem> selected_motor_cfgs;
+        if (motor_group_cfg.motors().motors_size() == 0) {
-        const bool selected_active_group = selectActiveMotors_(
+            CMVR_LOG(ERROR) << "[MotorManager] motor group has no motors: " << group_name;
-            group_name, motor_group_cfg.motors().motors(), selected_motor_cfgs);
+            all_ok = false;
-        if (!selected_active_group) {
             continue;
         }
+        std::vector<config::MotorConfigItem> selected_motor_cfgs(
+            motor_group_cfg.motors().motors().begin(),
+            motor_group_cfg.motors().motors().end());
 
         if (!applyConfiguredJointLimits_(motor_group_cfg, selected_motor_cfgs)) {
             all_ok = false;
@@ -137,6 +144,12 @@ bool MotorManager::init()
         bus_runtimes_.push_back(std::move(bus_runtime));
     }
 
+    if (selected_group_count != 1) {
+        CMVR_LOG(ERROR) << "[MotorManager] selected motor group '" << selected_group_id_
+                        << "' must occur exactly once in config: " << id_;
+        all_ok = false;
+    }
+
     if (!all_ok) {
         for (auto& bus_runtime : bus_runtimes_) {
             if (bus_runtime) {
@@ -299,68 +312,6 @@ std::shared_ptr<simulate::MujocoWorld> MotorManager::mujocoWorldFor(const std::s
     return it->second.lock();
 }
 
-void MotorManager::setActiveJoints(const std::string& motor_manager_id,
-                                  ActiveJointSelection group_joints)
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    active_joints_[motor_manager_id] = std::move(group_joints);
-}
-
-void MotorManager::clearActiveJoints()
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    active_joints_.clear();
-}
-
-bool MotorManager::selectActiveMotors_(
-    const std::string& group_name,
-    const google::protobuf::RepeatedPtrField<config::MotorConfigItem>& source,
-    std::vector<config::MotorConfigItem>& selected) const
-{
-    selected.clear();
-
-    ActiveJointSelection selection;
-    {
-        std::lock_guard<std::mutex> lock(registry_mutex_);
-        const auto it = active_joints_.find(id_);
-        if (it != active_joints_.end()) {
-            selection = it->second;
-        }
-    }
-
-    if (selection.empty()) {
-        CMVR_LOG(WARNING) << "[MotorManager] No active joints selected for motor manager " << id_
-                          << ", motor group " << group_name << " will not initialize motors.";
-        return false;
-    }
-
-    const auto group_it = selection.find(group_name);
-    if (group_it == selection.end() || group_it->second.empty()) {
-        return false;
-    }
-
-    for (const auto& motor_cfg : source) {
-        if (group_it->second.count(motor_cfg.joint_name()) > 0) {
-            selected.push_back(motor_cfg);
-        }
-    }
-
-    if (selected.size() != group_it->second.size()) {
-        std::unordered_set<std::string> found;
-        for (const auto& motor_cfg : selected) {
-            found.insert(motor_cfg.joint_name());
-        }
-        for (const auto& joint_name : group_it->second) {
-            if (found.count(joint_name) == 0) {
-                CMVR_LOG(ERROR) << "[MotorManager] active joint '" << joint_name
-                                << "' not found in motor group '" << group_name << "'";
-                return false;
-            }
-        }
-    }
-    return !selected.empty();
-}
-
 bool MotorManager::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
                                               std::vector<config::MotorConfigItem>& selected) const
 {

cmvr-es/manager/device_manager/src/device_factory.cpp

@@ -226,22 +226,41 @@ DeviceFactory::DeviceFactory()
                     });
 
     registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM,
-	                    [](const auto& entry) {
+                    [](const auto& entry) {
-	                        if (entry.id().empty()) {
+                        if (entry.id().empty()) {
-	                            CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required";
+                            CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id is required";
-	                            return DeviceRecord{};
+                            return DeviceRecord{};
-	                        }
+                        }
-	                        if (entry.config_file().empty()) {
+                        if (entry.config_file().empty()) {
-	                            CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorManager ID: " << entry.id();
+                            CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for motor group ID: " << entry.id();
-	                            return DeviceRecord{};
+                            return DeviceRecord{};
-	                        }
+                        }
-	                        config::MotorRootConfig root_cfg;
+                        config::MotorRootConfig root_cfg;
-	                        if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
+                        if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
-	                            CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail";
+                            CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail";
-	                            return DeviceRecord{};
+                            return DeviceRecord{};
-	                        }
+                        }
+
+                        const config::MotorGroupConfig* selected_group = nullptr;
+                        for (const auto& group_cfg : root_cfg.motor().motor_groups()) {
+                            if (group_cfg.id() != entry.id()) {
+                                continue;
+                            }
+                            if (selected_group != nullptr) {
+                                CMVR_LOG(ERROR) << "[DeviceFactory]: Duplicate motor group id '"
+                                                << entry.id() << "' in config: " << entry.config_file();
+                                return DeviceRecord{};
+                            }
+                            selected_group = &group_cfg;
+                        }
+                        if (selected_group == nullptr) {
+                            CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id '" << entry.id()
+                                            << "' not found in config: " << entry.config_file();
+                            return DeviceRecord{};
+                        }
                         CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success";
-                        auto device = std::make_shared<MotorManager>(entry.id(), root_cfg.motor());
+                        auto device = std::make_shared<MotorManager>(
+                            entry.id(), root_cfg.motor(), entry.id());
                         DeviceRecord record;
                         record.id = entry.id();
                         record.kind = device->kind();

cmvr-es/manager/device_manager/src/device_manager.cpp

@@ -128,7 +128,6 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) {
     dev_factory_ = std::make_unique<DeviceFactory>();
     logSection("Device Plan");
     log_device_plan_();
-    pre_scan_robot_arm_dependencies_();
     logSection("Initialize Devices");
     init_devices_();
     configure_mujoco_viewer_pip_();
@@ -153,7 +152,6 @@ DeviceManager& DeviceManager::getInstance() {
 void DeviceManager::destroyInstance() {
     std::lock_guard lock(init_mutex_);
     instance_.reset();
-    MotorManager::clearActiveJoints();
 }
 
 void DeviceManager::start(){
@@ -354,153 +352,6 @@ void DeviceManager::log_device_plan_() const
     CMVR_LOG(INFO) << "[DeviceManager]: Device plan end";
 }
 
-void DeviceManager::pre_scan_robot_arm_dependencies_() const
-{
-    MotorJointSelections selections;
-    std::unordered_map<std::string, config::MotorRootConfig> motor_roots;
-
-    for (const auto& entry : cfg_.devices()) {
-        if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) {
-            continue;
-        }
-        if (entry.id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty";
-            return;
-        }
-        if (entry.config_file().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id();
-            return;
-        }
-
-        config::MotorRootConfig root_cfg;
-        if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file();
-            return;
-        }
-        if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id()
-                            << "' does not match config id '" << root_cfg.motor().id() << "'";
-            return;
-        }
-        motor_roots.emplace(entry.id(), std::move(root_cfg));
-    }
-
-    for (const auto& entry : cfg_.devices()) {
-        if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM) {
-            continue;
-        }
-        if (entry.id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty";
-            return;
-        }
-        if (entry.config_file().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id();
-            return;
-        }
-
-        config::ArmRootConfig root_cfg;
-        if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file();
-            return;
-        }
-
-        const config::RobotArmConfig* arm_cfg = nullptr;
-        for (const auto& candidate : root_cfg.arm().robot_arms()) {
-            if (candidate.id() == entry.id()) {
-                arm_cfg = &candidate;
-                break;
-            }
-        }
-        if (!arm_cfg) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id()
-                            << "' not found in config: " << entry.config_file();
-            return;
-        }
-
-        if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) {
-            continue;
-        }
-        if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id();
-            return;
-        }
-
-        const auto& motor_config = arm_cfg->motor();
-        if (motor_config.motor_system_id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id();
-            return;
-        }
-        if (motor_config.motor_group_ids_size() == 0) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id();
-            return;
-        }
-        if (motor_config.joint_names_size() == 0) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id();
-            return;
-        }
-
-        const auto motor_root_it = motor_roots.find(motor_config.motor_system_id());
-        if (motor_root_it == motor_roots.end()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
-                            << "' depends on disabled or missing MotorManager: "
-                            << motor_config.motor_system_id();
-            return;
-        }
-
-        std::unordered_set<std::string> allowed_groups;
-        allowed_groups.reserve(static_cast<size_t>(motor_config.motor_group_ids_size()));
-        for (const auto& group_id : motor_config.motor_group_ids()) {
-            if (group_id.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id();
-                return;
-            }
-            allowed_groups.insert(group_id);
-        }
-
-        auto& group_selection = selections[motor_config.motor_system_id()];
-        for (const auto& joint_name : motor_config.joint_names()) {
-            if (joint_name.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id();
-                return;
-            }
-
-            std::string matched_group;
-            for (const auto& motor_group : motor_root_it->second.motor().motor_groups()) {
-                const auto& group_id = motor_group.id();
-                if (allowed_groups.count(group_id) == 0) {
-                    continue;
-                }
-                if (motorGroupHasJoint(motor_group, joint_name)) {
-                    matched_group = group_id;
-                    break;
-                }
-            }
-
-            if (matched_group.empty()) {
-                CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
-                                << "' joint '" << joint_name
-                                << "' not found in configured motor_group_ids";
-                return;
-            }
-            group_selection[matched_group].insert(joint_name);
-        }
-    }
-
-    if (selections.empty() && cfg_.init_all_motors_when_no_active_joints()) {
-        CMVR_LOG(INFO) << "[DeviceManager]: No active motor joints from RobotArm; "
-                       << "initialize all configured motors because "
-                       << "init_all_motors_when_no_active_joints=true";
-        for (const auto& [motor_system_id, root_cfg] : motor_roots) {
-            addAllMotorJoints(motor_system_id, root_cfg, selections);
-        }
-    }
-
-    MotorManager::clearActiveJoints();
-    for (auto& [motor_system_id, group_selection] : selections) {
-        MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));
-    }
-}
-
 void DeviceManager::init_devices_() {
     for (const auto& entry : cfg_.devices()) {
         if (!entry.enable()) {