SmartBench/cmvr-es

Fork 0

lgv 80c8969ce9 refactor(motor): introduce MotorManager and bus runtimes

2026-06-30 16:04:36 +08:00

6.0 KiB

Raw Blame History

EtherCAT 电机接入教程

这份文档只说明新增一种 EtherCAT 电机需要改哪里、怎么写。

1. 增加 vendor

修改 protos/cmvr/config/motor_config/motor_config.proto：

enum MotorVendor {
  MOTOR_VENDOR_UNKNOWN = 0;
  MOTOR_VENDOR_TI5 = 1;
  MOTOR_VENDOR_MUJOCO = 2;
  MOTOR_VENDOR_XXX = 3;
}

MOTOR_VENDOR_XXX 改成真实厂商名，例如 MOTOR_VENDOR_FOO。不要复用 TI5。

2. 写电机配置

新增配置文件：

cmvr-es/config/devices/motor/ethercat_motors.pb.txt

示例：

motor {
  id: "ethercat_motors"

  motor_groups {
    id: "right_arm_ethercat"
    bus_type: MOTOR_BUS_ETHERCAT
    vendor: MOTOR_VENDOR_XXX
    protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
    tool_frame: "R_FINGER_TIP"

    ethercat {
      master_id: "eth0"
      cycle_us: 1000
      slaves { motor_id: 1 slave_index: 0 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 2 slave_index: 1 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 3 slave_index: 2 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 4 slave_index: 3 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 5 slave_index: 4 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 6 slave_index: 5 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 7 slave_index: 6 vendor_id: 0x00000000 product_code: 0x00000000 }
    }

    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" }
      motors { id: 2 joint_name: "R_SHOULDER_R" }
      motors { id: 3 joint_name: "R_SHOULDER_Y" }
      motors { id: 4 joint_name: "R_ELBOW_R" }
      motors { id: 5 joint_name: "R_WRIST_P" }
      motors { id: 6 joint_name: "R_WRIST_Y" }
      motors { id: 7 joint_name: "R_WRIST_R" }
    }
  }
}

motors.motors.id 是系统内的电机逻辑 id。ethercat.slaves.motor_id 必须和它对应。

joint_limits 按 joint_name 读取，和 CAN、MuJoCo 电机保持同一个风格。

3. 注册设备

修改：

cmvr-es/config/manager/device_manager.pb.txt

加入：

devices {
  id: "ethercat_motors"
  type: MOTOR_SYSTEM
  enable: true
  config_file: "devices/motor/ethercat_motors.pb.txt"
}

4. 机械臂使用 EtherCAT group

修改机械臂配置，例如：

cmvr-es/config/devices/arm/arm.pb.txt

把 motor backend 改成：

motor {
  motor_system_id: "ethercat_motors"
  motor_group_ids: "right_arm_ethercat"
  joint_names: "R_SHOULDER_P"
  joint_names: "R_SHOULDER_R"
  joint_names: "R_SHOULDER_Y"
  joint_names: "R_ELBOW_R"
  joint_names: "R_WRIST_P"
  joint_names: "R_WRIST_Y"
  joint_names: "R_WRIST_R"
}

5. 实现 bus runtime

EtherCAT 总线资源放在：

cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h
cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp

EthercatMotorBusRuntime 负责：

读取 ethercat 配置
初始化 EtherCAT master
扫描/校验 slave_index、vendor_id、product_code
启动 cyclic loop
保存 command/feedback buffer
停止 cyclic loop

bus runtime 不创建具体电机，也不关心厂商；它只保存总线连接、线程和数据缓存。

6. 增加具体电机 driver

新增目录：

cmvr-es/devices/motor/drivers/xxx_ethercat/
  CMakeLists.txt
  include/xxx_ethercat_motor.h
  include/xxx_ethercat_motor_protocol.h
  src/xxx_ethercat_motor.cpp
  src/xxx_ethercat_motor_protocol.cpp

XxxEthercatMotor 继承 AbstractMotor。

XxxEthercatMotorProtocol 继承 MotorProtocolInterface，把 setTarget、setQd、getQ 等接口转换成 EtherCAT command/feedback。

7. 在 MotorManager 里创建 EtherCAT 电机

修改：

cmvr-es/devices/motor/manager/src/motor_manager.cpp

在 MotorManager::createEthercatMotors_() 里按 vendor + protocol 创建具体电机：

auto ethercat_bus_runtime =
    std::dynamic_pointer_cast<EthercatMotorBusRuntime>(bus_runtime);

if (group_cfg.vendor() == config::MOTOR_VENDOR_XXX &&
    group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
    auto protocol = std::make_shared<XxxEthercatMotorProtocol>(ethercat_bus_runtime);

    std::vector<std::shared_ptr<AbstractMotor>> motors;
    motors.reserve(motor_cfgs.size());
    for (const auto& cfg : motor_cfgs) {
        auto motor = std::make_shared<XxxEthercatMotor>(cfg);
        motor->setProtocol(protocol);
        if (!motor->init()) {
            return {};
        }
        motors.push_back(std::move(motor));
    }
    return motors;
}

8. 加入 CMake

修改：

cmvr-es/devices/motor/manager/CMakeLists.txt

给 motor_manager 链接新增的具体 EtherCAT 电机 target。

修改：

cmvr-es/devices/motor/CMakeLists.txt

加入：

add_subdirectory(drivers/xxx_ethercat)

9. 验证配置

先验证 proto：

./output/bin/protoc \
  --encode=cmvr.config.MotorRootConfig \
  -I protos \
  protos/cmvr/config/motor_config/motor_config.proto \
  < cmvr-es/config/devices/motor/ethercat_motors.pb.txt \
  > /tmp/ethercat_motors.pb.bin

再编译：

cmake --build cmake-build-debug --target mujoco_manual_ui_test

真机联调时先只验证初始化日志：master 打开、slave 数量、vendor/product 校验、cyclic loop 启动、每个 motor 注册成功。然后再下发运动命令。

6.0 KiB Raw Blame History Unescape Escape