From f768960ff2265b97820cfc69a92f3bf0821e098a Mon Sep 17 00:00:00 2001 From: lgv Date: Fri, 4 Sep 2026 13:05:14 +0800 Subject: [PATCH] feat(motor): initialize motor groups by device entry --- cmvr-es/config/devices/arm/arm.pb.txt | 4 +- .../config/devices/arm/arm_gen2_mujoco.pb.txt | 4 +- cmvr-es/config/devices/arm/arm_mujoco.pb.txt | 4 +- .../config/devices/arm/arm_mujoco_qp.pb.txt | 4 +- cmvr-es/config/devices/arm/arm_qp.pb.txt | 4 +- .../devices/motor/ethercat_motors.pb.txt | 2 +- .../ethercat_motors_four_real_test.pb.txt | 2 +- .../config/devices/motor/mujoco_motors.pb.txt | 2 +- .../devices/motor/mujoco_motors_gen2.pb.txt | 2 +- .../config/devices/motor/ti5_motors.pb.txt | 8 +- cmvr-es/config/manager/device_manager.pb.txt | 29 ++- .../src/motor_robot_arm_gen2_mujoco_test.cpp | 14 +- .../src/motor_robot_arm_mujoco_test.cpp | 15 +- .../eyou_motor_device_manager_real_test.cpp | 4 +- .../motor/manager/include/motor_manager.h | 16 +- .../motor/manager/src/motor_manager.cpp | 105 +++------- .../device_manager/include/device_manager.h | 2 - .../device_manager/src/device_factory.cpp | 49 +++-- .../device_manager/src/device_manager.cpp | 191 ------------------ .../src/touch_screen_task_test.cpp | 8 +- .../ethercat/v1.7.0/etc/ethercat.conf | 2 +- .../device_manager_config.proto | 4 +- 22 files changed, 122 insertions(+), 353 deletions(-) diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt index 714c5c9b..6dd5fca9 100644 --- a/cmvr-es/config/devices/arm/arm.pb.txt +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -3,8 +3,8 @@ arm { id: "right_arm" motor { - motor_system_id: "ti5_motors" - motor_group_ids: "right_arm_can" + motor_system_id: "right_arm_can_motors" + motor_group_ids: "right_arm_can_motors" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" diff --git a/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt index f8c0a3b8..bf9eae37 100644 --- a/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt +++ b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt @@ -3,8 +3,8 @@ arm { id: "mujoco_right_arm" motor { - motor_system_id: "mujoco_motors" - motor_group_ids: "mujoco_right_arm" + motor_system_id: "right_arm_mujoco_motors" + motor_group_ids: "right_arm_mujoco_motors" dof: 7 joint_names: "right_arm_J1" joint_names: "right_arm_J2" diff --git a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt index 0d5604bd..31361224 100644 --- a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt +++ b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt @@ -3,8 +3,8 @@ arm { id: "mujoco_right_arm" motor { - motor_system_id: "mujoco_motors" - motor_group_ids: "mujoco_right_arm" + motor_system_id: "right_arm_mujoco_motors" + motor_group_ids: "right_arm_mujoco_motors" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" diff --git a/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt index ef729d58..3960aacf 100644 --- a/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt +++ b/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt @@ -3,8 +3,8 @@ arm { id: "mujoco_right_arm" motor { - motor_system_id: "mujoco_motors" - motor_group_ids: "mujoco_right_arm" + motor_system_id: "right_arm_mujoco_motors" + motor_group_ids: "right_arm_mujoco_motors" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" diff --git a/cmvr-es/config/devices/arm/arm_qp.pb.txt b/cmvr-es/config/devices/arm/arm_qp.pb.txt index 9e25391f..6e952b48 100644 --- a/cmvr-es/config/devices/arm/arm_qp.pb.txt +++ b/cmvr-es/config/devices/arm/arm_qp.pb.txt @@ -3,8 +3,8 @@ arm { id: "right_arm" motor { - motor_system_id: "ti5_motors" - motor_group_ids: "right_arm_can" + motor_system_id: "right_arm_can_motors" + motor_group_ids: "right_arm_can_motors" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt index 9fda778a..0d9432ec 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt +++ b/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 diff --git a/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt index 8e69905a..4f649ab1 100644 --- a/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt +++ b/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 diff --git a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt index 7271eba6..448270e3 100644 --- a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt +++ b/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 diff --git a/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt index 8bd88329..a45bafae 100644 --- a/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt +++ b/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 diff --git a/cmvr-es/config/devices/motor/ti5_motors.pb.txt b/cmvr-es/config/devices/motor/ti5_motors.pb.txt index 20a28aff..173d7b1c 100644 --- a/cmvr-es/config/devices/motor/ti5_motors.pb.txt +++ b/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 diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index a024bfd1..8aeb0d25 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -2,8 +2,6 @@ device_manager { name: "cmvr_es" version: "0.1" description: "cmvr edge system version 0.1" - init_all_motors_when_no_active_joints: true - devices { id: "mujoco_world" type: DEVICE_TYPE_MUJOCO_WORLD @@ -12,7 +10,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: true @@ -76,14 +74,35 @@ device_manager { } devices { - id: "ti5_motors" + 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 diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp index ea3fd7aa..9feebb00 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp @@ -13,7 +13,6 @@ #include #include #include -#include #include #include @@ -159,18 +158,10 @@ protected: "cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt").string(), &motor_root_config)); - std::unordered_set 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( - "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_; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp index 5ec14ee0..82e473f9 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include @@ -147,17 +146,10 @@ protected: (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(), &motor_root_config)); - std::unordered_set 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("mujoco_motors", motor_root_config.motor()); + motor_system_ = std::make_shared( + "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors"); 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_; diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp index 3b877f00..140f7c15 100644 --- a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp @@ -21,7 +21,7 @@ namespace cmvr::device { namespace { -constexpr const char* kMotorManagerId = "ethercat_motors"; +constexpr const char* kMotorManagerId = "right_arm_ethercat_motors"; constexpr const char* kMotorConfigFile = "devices/motor/ethercat_motors_four_real_test.pb.txt"; constexpr std::array kFourMotorIds{1, 2, 3, 4}; @@ -169,8 +169,6 @@ config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig() config::DeviceManagerConfig config; config.set_name("eyou_motor_device_manager_real_test"); config.set_version("test"); - config.set_init_all_motors_when_no_active_joints(true); - auto* motor_entry = config.add_devices(); motor_entry->set_id(kMotorManagerId); motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index 3673355a..45e8ab3e 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -5,7 +5,6 @@ #include #include #include -#include #include #include @@ -32,7 +31,9 @@ class AbstractMotorBusRuntime; class MotorManager final : public AbstractDevice, public std::enable_shared_from_this { 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 +57,7 @@ public: static std::shared_ptr managerFor(const std::string& id); static std::shared_ptr mujocoWorldFor(const std::string& id); - static void setActiveJoints(const std::string& motor_manager_id, - std::unordered_map> group_joints); - static void clearActiveJoints(); - private: - using ActiveJointSelection = std::unordered_map>; - - bool selectActiveMotors_(const std::string& group_name, - const google::protobuf::RepeatedPtrField& source, - std::vector& selected) const; bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, std::vector& selected) const; std::shared_ptr createBusRuntime_( @@ -89,6 +81,7 @@ private: private: config::MotorConfig cfg_; + std::string selected_group_id_; std::vector> bus_runtimes_; mutable std::mutex motors_mutex_; std::unordered_map> motors_by_id_; @@ -98,7 +91,6 @@ private: static std::mutex registry_mutex_; static std::unordered_map> managers_; static std::unordered_map> mujoco_world_registry_; - static std::unordered_map active_joints_; }; } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index 3a6b5118..cf4f5e0a 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -28,17 +28,14 @@ namespace cmvr::device { std::mutex MotorManager::registry_mutex_; std::unordered_map> MotorManager::managers_; std::unordered_map> MotorManager::mujoco_world_registry_; -std::unordered_map MotorManager::active_joints_; -MotorManager::MotorManager(std::string id, const config::MotorConfig& cfg) - : cfg_(cfg) +MotorManager::MotorManager(std::string id, + 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(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()) { - CMVR_LOG(ERROR) << "[MotorManager] motor group id is empty in manager: " << id_; - all_ok = false; + if (group_name != selected_group_id_) { 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 selected_motor_cfgs; - const bool selected_active_group = selectActiveMotors_( - group_name, motor_group_cfg.motors().motors(), selected_motor_cfgs); - if (!selected_active_group) { + if (motor_group_cfg.motors().motors_size() == 0) { + CMVR_LOG(ERROR) << "[MotorManager] motor group has no motors: " << group_name; + all_ok = false; continue; } + std::vector 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 MotorManager::mujocoWorldFor(const std::s return it->second.lock(); } -void MotorManager::setActiveJoints(const std::string& motor_manager_id, - ActiveJointSelection group_joints) -{ - std::lock_guard lock(registry_mutex_); - active_joints_[motor_manager_id] = std::move(group_joints); -} - -void MotorManager::clearActiveJoints() -{ - std::lock_guard lock(registry_mutex_); - active_joints_.clear(); -} - -bool MotorManager::selectActiveMotors_( - const std::string& group_name, - const google::protobuf::RepeatedPtrField& source, - std::vector& selected) const -{ - selected.clear(); - - ActiveJointSelection selection; - { - std::lock_guard 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 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& selected) const { diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index d680dded..58bedc74 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -8,7 +8,6 @@ #include #include #include -#include #include #include "device_factory.h" #include "cmvr/config/device_manager_config/device_manager_config.pb.h" @@ -49,7 +48,6 @@ namespace cmvr::device { explicit DeviceManager(const config::DeviceManagerConfig &cfg); void log_device_plan_() const; - void pre_scan_robot_arm_dependencies_() const; void init_devices_(); void configure_mujoco_viewer_pip_(); }; diff --git a/cmvr-es/manager/device_manager/src/device_factory.cpp b/cmvr-es/manager/device_manager/src/device_factory.cpp index b1bfe45e..853df6d5 100644 --- a/cmvr-es/manager/device_manager/src/device_factory.cpp +++ b/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) { - if (entry.id().empty()) { - CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required"; - return DeviceRecord{}; - } - if (entry.config_file().empty()) { - CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorManager ID: " << entry.id(); - return DeviceRecord{}; - } - config::MotorRootConfig root_cfg; - if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { - CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; - return DeviceRecord{}; - } + [](const auto& entry) { + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id is required"; + return DeviceRecord{}; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for motor group ID: " << entry.id(); + return DeviceRecord{}; + } + config::MotorRootConfig root_cfg; + if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; + 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(entry.id(), root_cfg.motor()); + auto device = std::make_shared( + entry.id(), root_cfg.motor(), entry.id()); DeviceRecord record; record.id = entry.id(); record.kind = device->kind(); diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index 88e7a501..32e8319d 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -18,17 +18,12 @@ #include "devices/speaker/abstract_speaker.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" #include "common/config/config_files.h" -#include "cmvr/config/arm_config/arm_config.pb.h" -#include "cmvr/config/motor_config/motor_config.pb.h" using namespace std; using namespace cmvr::device; namespace { -using GroupJointSelection = std::unordered_map>; -using MotorJointSelections = std::unordered_map; - void logSection(const char* title) { CMVR_LOG(INFO) << "---------------- " << title << " ----------------"; @@ -63,43 +58,6 @@ const char* deviceTypeToString(const cmvr::config::DeviceConfigEntry::DeviceType } } -bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group, - const std::string& joint_name) -{ - for (const auto& motor : motor_group.motors().motors()) { - if (motor.joint_name() == joint_name) { - return true; - } - } - return false; -} - -void addAllMotorJoints(const std::string& motor_system_id, - const cmvr::config::MotorRootConfig& root_cfg, - MotorJointSelections& selections) -{ - auto& group_selection = selections[motor_system_id]; - for (const auto& motor_group : root_cfg.motor().motor_groups()) { - if (motor_group.id().empty()) { - continue; - } - - auto& selected_joints = group_selection[motor_group.id()]; - for (const auto& motor : motor_group.motors().motors()) { - if (!motor.joint_name().empty()) { - selected_joints.insert(motor.joint_name()); - } - } - if (selected_joints.empty()) { - group_selection.erase(motor_group.id()); - } - } - - if (group_selection.empty()) { - selections.erase(motor_system_id); - } -} - } // namespace template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); @@ -125,7 +83,6 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { dev_factory_ = std::make_unique(); logSection("Device Plan"); log_device_plan_(); - pre_scan_robot_arm_dependencies_(); logSection("Initialize Devices"); init_devices_(); configure_mujoco_viewer_pip_(); @@ -150,7 +107,6 @@ DeviceManager& DeviceManager::getInstance() { void DeviceManager::destroyInstance() { std::lock_guard lock(init_mutex_); instance_.reset(); - MotorManager::clearActiveJoints(); } void DeviceManager::start(){ @@ -283,153 +239,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 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 allowed_groups; - allowed_groups.reserve(static_cast(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()) { diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp index 7e14ed09..62800082 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp @@ -292,7 +292,7 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { world_entry->set_config_file("devices/mujoco/mujoco_world.pb.txt"); world_entry->set_enable(true); auto* motor_entry = device_manager_config.add_devices(); - motor_entry->set_id("mujoco_motors"); + motor_entry->set_id("right_arm_mujoco_motors"); motor_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); motor_entry->set_config_file("devices/motor/mujoco_motors.pb.txt"); motor_entry->set_enable(true); @@ -308,12 +308,12 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { dexhand_entry->set_enable(true); auto& device_manager = cmvr::device::DeviceManager::getInstance(device_manager_config); - auto motor_system = device_manager.getDevice("mujoco_motors"); + auto motor_system = device_manager.getDevice("right_arm_mujoco_motors"); if (!motor_system) { - CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: mujoco_motors"; + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: right_arm_mujoco_motors"; return; } - auto world = cmvr::device::MotorManager::mujocoWorldFor("mujoco_motors"); + auto world = cmvr::device::MotorManager::mujocoWorldFor("right_arm_mujoco_motors"); if (!world || !world->isLoaded() || !world->isRunning()) { CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco world is not ready: mujoco_world"; return; diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf index 9ec7a2ce..21dc7c70 100644 --- a/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf @@ -1,3 +1,3 @@ -MASTER0_DEVICE="a0:ad:9f:c4:c2:2c" +MASTER0_DEVICE="42:e6:6d:44:c1:0f" DEVICE_MODULES="generic" UPDOWN_INTERFACES="eno1" diff --git a/protos/cmvr/config/device_manager_config/device_manager_config.proto b/protos/cmvr/config/device_manager_config/device_manager_config.proto index d36d328b..751e6f9a 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -23,6 +23,7 @@ message DeviceConfigEntry { DEVICE_TYPE_MUJOCO_VIEWER = 19; } + // For DEVICE_TYPE_MOTOR_SYSTEM, this is the motor_group id selected from config_file. string id = 1; DeviceType type = 2; string config_file = 3; @@ -30,11 +31,12 @@ message DeviceConfigEntry { } message DeviceManagerConfig { + reserved 20; + string name = 1; string version = 2; string description = 3; repeated DeviceConfigEntry devices = 4; - bool init_all_motors_when_no_active_joints = 20; } message DeviceManagerRootConfig { DeviceManagerConfig device_manager = 1;