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

feat(manager): initialize EtherCAT motors from DeviceManager

Browse Source
This commit is contained in:
lgv 2026-07-09 14:21:44 +08:00
parent dc0831eb71
commit 5b94d5c85a
9 changed files with 228 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
cmvr-es/config/devices/motor/ethercat_motors.pb.txt Normal file
View File

@ -0,0 +1,53 @@
motor {
id: "ethercat_motors"
motor_groups {
id: "right_arm_ethercat"
bus_type: MOTOR_BUS_ETHERCAT
vendor: MOTOR_VENDOR_EYOU
protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
ethercat {
master_index: 0
cycle_us: 1000
cia402 {
profile_position_trigger_delay_ms: 2
state_transition_timeout_ms: 1200
velocity_stop_timeout_ms: 2000
status_poll_period_ms: 10
stopped_velocity_tolerance_rad_s: 0.001
}
slaves { motor_id: 1 alias: 0 position: 0 }
slaves { motor_id: 2 alias: 0 position: 1 }
slaves { motor_id: 3 alias: 0 position: 2 }
slaves { motor_id: 4 alias: 0 position: 3 }
slaves { motor_id: 5 alias: 0 position: 4 }
slaves { motor_id: 6 alias: 0 position: 5 }
slaves { motor_id: 7 alias: 0 position: 6 }
}
joint_limits {
enable: true
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
motors {
motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}

44
cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt Normal file
View File

@ -0,0 +1,44 @@
motor {
id: "ethercat_motors"
motor_groups {
id: "right_arm_ethercat"
bus_type: MOTOR_BUS_ETHERCAT
vendor: MOTOR_VENDOR_EYOU
protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
ethercat {
master_index: 0
cycle_us: 1000
cia402 {
profile_position_trigger_delay_ms: 2
state_transition_timeout_ms: 1200
velocity_stop_timeout_ms: 2000
status_poll_period_ms: 10
stopped_velocity_tolerance_rad_s: 0.001
}
slaves { motor_id: 1 alias: 0 position: 0 }
slaves { motor_id: 2 alias: 0 position: 1 }
slaves { motor_id: 3 alias: 0 position: 2 }
slaves { motor_id: 4 alias: 0 position: 3 }
}
joint_limits {
enable: true
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
}
motors {
motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}

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

@ -6,7 +6,6 @@ motor {
bus_type: MOTOR_BUS_MUJOCO
vendor: MOTOR_VENDOR_MUJOCO
protocol: MOTOR_PROTOCOL_MUJOCO
tool_frame: "R_FINGER_TIP"
mujoco {
world_id: "mujoco_world"
}

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

@ -6,7 +6,6 @@ motor {
bus_type: MOTOR_BUS_CAN
vendor: MOTOR_VENDOR_TI5
protocol: MOTOR_PROTOCOL_CANOPEN
tool_frame: "L_FINGER_TIP"
can {
channel_id: 0
}
@ -16,13 +15,13 @@ motor {
urdf_path: "model/xiaoyan_description/dual_arm.urdf"
}
motors {
motors { id: 23 joint_name: "L_SHOULDER_P" }
motors { id: 24 joint_name: "L_SHOULDER_R" }
motors { id: 25 joint_name: "L_SHOULDER_Y" }
motors { id: 26 joint_name: "L_ELBOW_R" }
motors { id: 27 joint_name: "L_WRIST_P" }
motors { id: 28 joint_name: "L_WRIST_Y" }
motors { id: 29 joint_name: "L_WRIST_R" }
motors { id: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 24 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 25 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 26 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 27 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 28 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 29 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
@ -31,7 +30,6 @@ motor {
bus_type: MOTOR_BUS_CAN
vendor: MOTOR_VENDOR_TI5
protocol: MOTOR_PROTOCOL_CANOPEN
tool_frame: "R_FINGER_TIP"
can {
channel_id: 1
}
@ -47,13 +45,13 @@ motor {
joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
motors {
motors { id: 16 joint_name: "R_SHOULDER_P" }
motors { id: 17 joint_name: "R_SHOULDER_R" }
motors { id: 18 joint_name: "R_SHOULDER_Y" }
motors { id: 19 joint_name: "R_ELBOW_R" }
motors { id: 20 joint_name: "R_WRIST_P" }
motors { id: 21 joint_name: "R_WRIST_Y" }
motors { id: 22 joint_name: "R_WRIST_R" }
motors { id: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 17 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 18 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 19 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 20 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 21 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 22 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
@ -73,9 +71,9 @@ motor {
joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
}
motors {
motors { id: 32 joint_name: "HEAD_Y" }
motors { id: 30 joint_name: "HEAD_P" }
motors { id: 31 joint_name: "HEAD_R" }
motors { id: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 31 joint_name: "HEAD_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
@ -94,8 +92,8 @@ motor {
joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
}
motors {
motors { id: 4 joint_name: "WAIST_Y" }
motors { id: 15 joint_name: "WAIST_P" }
motors { id: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 15 joint_name: "WAIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}

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

@ -2,6 +2,7 @@ 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"
@ -74,6 +75,13 @@ device_manager {
enable: false
}
devices {
id: "ethercat_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ethercat_motors.pb.txt"
enable: false
}
devices {
id: "right_arm"
type: DEVICE_TYPE_ROBOT_ARM

1
cmvr-es/devices/motor/manager/CMakeLists.txt
View File

@ -12,6 +12,7 @@ target_link_libraries(motor_manager
PRIVATE
cmvr_es::device::ti5_canopen_motor_driver
cmvr_es::device::mujoco_motor_driver
cmvr_es::device::ethercat_motor_driver
cmvr_es::ik_solver
glog
)

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

@ -1,21 +1,27 @@
#include "motor/manager/include/motor_manager.h"
#include "devices/motor/manager/include/motor_manager.h"
#include <chrono>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <thread>
#include <unordered_map>
#include <utility>
#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
#include "common/base/logging/logger.h"
#include "common/config/config_files.h"
#include "../../bus_runtime/abstract_motor_bus_runtime.h"
#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h"
#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
#include "motor/drivers/mujoco/include/mujoco_motor.h"
#include "motor/drivers/ti5_canopen/include/ti5_motor.h"
#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h"
#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h"
#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
namespace cmvr::device {
@ -91,11 +97,17 @@ bool MotorManager::init()
all_ok = false;
continue;
}
if (!bus_runtime->start()) {
const bool start_before_motor_init =
motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT;
if (start_before_motor_init && !bus_runtime->start()) {
bus_runtime->stop();
all_ok = false;
continue;
}
if (start_before_motor_init) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime);
if (motors.empty()) {
@ -116,6 +128,11 @@ bool MotorManager::init()
all_ok = false;
continue;
}
if (!start_before_motor_init && !bus_runtime->start()) {
bus_runtime->stop();
all_ok = false;
continue;
}
bus_runtimes_.push_back(std::move(bus_runtime));
}
@ -398,8 +415,19 @@ std::shared_ptr<AbstractMotorBusRuntime> MotorManager::createBusRuntime_(
return std::make_shared<CanMotorBusRuntime>();
case config::MOTOR_BUS_MUJOCO:
return std::make_shared<MujocoMotorBusRuntime>();
case config::MOTOR_BUS_ETHERCAT:
return std::make_shared<EthercatMotorBusRuntime>();
case config::MOTOR_BUS_ETHERCAT: {
auto runtime = std::make_shared<EthercatMotorBusRuntime>();
if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU &&
group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
runtime->setPdoMapping(createEyouCia402PdoMapping());
return runtime;
}
CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor="
<< config::MotorVendor_Name(group_cfg.vendor())
<< ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
<< ", group=" << group_cfg.id();
return nullptr;
}
default:
CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
<< config::MotorBusType_Name(group_cfg.bus_type())
@ -534,6 +562,14 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << group_cfg.id();
return {};
}
if (group_cfg.vendor() != config::MOTOR_VENDOR_EYOU ||
group_cfg.protocol() != config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor="
<< config::MotorVendor_Name(group_cfg.vendor())
<< ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
<< ", group=" << group_cfg.id();
return {};
}
for (const auto& motor_cfg : motor_cfgs) {
if (!ethercat_bus_runtime->slaveForMotor(motor_cfg.id())) {
CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT slave config for motor id "
@ -542,11 +578,22 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
}
}
CMVR_LOG(ERROR) << "[MotorManager] EtherCAT motor creation is not implemented: vendor="
<< config::MotorVendor_Name(group_cfg.vendor())
<< ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
<< ", group=" << group_cfg.id();
return {};
auto protocol = std::make_shared<Cia402Protocol>(
ethercat_bus_runtime, group_cfg.ethercat().cia402());
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(motor_cfgs.size());
for (const auto& cfg : motor_cfgs) {
auto motor = std::make_shared<EyouMotor>(
cfg, protocol, std::make_unique<EyouMotorAdapter>(ethercat_bus_runtime));
if (!motor->init()) {
CMVR_LOG(ERROR) << "[MotorManager] failed to init EYOU EtherCAT motor: "
<< cfg.joint_name();
return {};
}
motors.push_back(std::move(motor));
}
return motors;
}
} // namespace cmvr::device

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

@ -26,6 +26,9 @@ 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 << " ----------------";
@ -71,6 +74,32 @@ bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group,
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);
@ -256,8 +285,7 @@ void DeviceManager::log_device_plan_() const
void DeviceManager::pre_scan_robot_arm_dependencies_() const
{
using GroupJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
std::unordered_map<std::string, GroupJointSelection> selections;
MotorJointSelections selections;
std::unordered_map<std::string, config::MotorRootConfig> motor_roots;
for (const auto& entry : cfg_.devices()) {
@ -387,6 +415,15 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
}
}
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));

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

@ -34,6 +34,7 @@ message DeviceManagerConfig {
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;