SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(motor): initialize motor groups by device entry

Browse Source
This commit is contained in:
lgv 2026-09-04 13:05:14 +08:00
parent 708c585f28
commit f768960ff2
22 changed files with 122 additions and 353 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cmvr-es/config/devices/arm/arm.pb.txt
View File

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

4
cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt
View File

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

4
cmvr-es/config/devices/arm/arm_mujoco.pb.txt
View File

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

4
cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt
View File

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

4
cmvr-es/config/devices/arm/arm_qp.pb.txt
View File

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

2
cmvr-es/config/devices/motor/ethercat_motors.pb.txt
View File

@ -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

2
cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt
View File

@ -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

2
cmvr-es/config/devices/motor/mujoco_motors.pb.txt
View File

@ -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

2
cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt
View File

@ -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

8
cmvr-es/config/devices/motor/ti5_motors.pb.txt
View File

@ -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

29
cmvr-es/config/manager/device_manager.pb.txt
View File

@ -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

14
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp
View File

@ -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_;

15
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp
View File

@ -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;
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());
motor_system_ = std::make_shared<MotorManager>(
"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_;

4
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp vendored
View File

@ -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<int, 4> 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);

16
cmvr-es/devices/motor/manager/include/motor_manager.h
View File

@ -5,7 +5,6 @@
#include <memory>
#include <mutex>
#include <string>
#include <unordered_set>
#include <unordered_map>
#include <vector>
@ -32,7 +31,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 +57,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 +81,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 +91,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

105
cmvr-es/devices/motor/manager/src/motor_manager.cpp
View File

@ -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)
: 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<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()) {
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<config::MotorConfigItem> 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<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
{

2
cmvr-es/manager/device_manager/include/device_manager.h
View File

@ -8,7 +8,6 @@
#include <list>
#include <mutex>
#include <string>
#include <unordered_set>
#include <unordered_map>
#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_();
};

25
cmvr-es/manager/device_manager/src/device_factory.cpp
View File

@ -228,11 +228,11 @@ DeviceFactory::DeviceFactory()
registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM,
[](const auto& entry) {
if (entry.id().empty()) {
CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required";
CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id is required";
return DeviceRecord{};
}
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{};
}
config::MotorRootConfig root_cfg;
@ -240,8 +240,27 @@ DeviceFactory::DeviceFactory()
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<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();

191
cmvr-es/manager/device_manager/src/device_manager.cpp
View File

@ -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<std::string, std::unordered_set<std::string>>;
using MotorJointSelections = std::unordered_map<std::string, GroupJointSelection>;
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<AbstractAGV> DeviceManager::getDevice(const std::string& device_id);
@ -125,7 +83,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_();
@ -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<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()) {

8
cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp
View File

@ -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<cmvr::device::MotorManager>("mujoco_motors");
auto motor_system = device_manager.getDevice<cmvr::device::MotorManager>("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;

2
dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf vendored
View File

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

4
protos/cmvr/config/device_manager_config/device_manager_config.proto
View File

@ -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;