diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h index 9dd0fc29..9d138abc 100644 --- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h +++ b/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 acceleration = std::nullopt); + void abortCommand_(); void sendZero_(); static double velocityNorm_(const std::vector& velocity); diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp index efad63b6..daf69fb7 100644 --- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp +++ b/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)) { - return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + if (!worker_ || !worker_->joinable()) { + 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 acceleratio if (!worker_ || !worker_->joinable()) { return Result::success(); } - { - std::lock_guard 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(); + requestStop_(acceleration); 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) { - std::lock_guard lock(mutex_); - command_active_ = false; - sendZero_(); - busy_.store(false); + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); break; } CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration=" << acceleration; - sendZero_(); - busy_.store(false); - return; + requestStop_(); + continue; } std::vector q_now; std::vector qd_now; if (!read_state_(q_now, qd_now)) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed"; - sendZero_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + break; + } + requestStop_(); + continue; } std::vector 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_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + 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_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + 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 acceleration) +{ + { + std::lock_guard 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 lock(mutex_); + command_active_ = false; + target_twist_ = {}; + target_frame_ = FrameType::Base; + } + sendZero_(); + busy_.store(false); +} + void CartesianVelocityController::sendZero_() { if (!send_velocity_) { diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp index 329c7b96..160c2570 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp +++ b/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 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)) { 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 37b1a4d3..e3e2c372 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/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 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/arm/robot_arm_factory.h b/cmvr-es/devices/arm/robot_arm_factory.h index 0a23742c..5f504392 100644 --- a/cmvr-es/devices/arm/robot_arm_factory.h +++ b/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" diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index 3673355a..c75d2fc4 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/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 { 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 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 +82,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 +92,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/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 db84586f..0f795430 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -128,7 +128,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_(); @@ -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 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()) {