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

merge lgv_dev code

Browse Source
This commit is contained in:
xtkuang 2026-09-10 09:43:45 +08:00
parent 595010aada
commit eefe4883bf
16 changed files with 174 additions and 317 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h
View File

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

82
cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp
View File

@ -61,9 +61,14 @@ 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()) {
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();
}
{
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();
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<std::mutex> 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<double> q_now;
std::vector<double> 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<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_();
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<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_) {

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

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

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

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

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

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

2
cmvr-es/devices/arm/robot_arm_factory.h
View File

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

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

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

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
{

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();

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

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