feat(manager): initialize EtherCAT motors from DeviceManager

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

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

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

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

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

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

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

@@ -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: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 24 joint_name: "L_SHOULDER_R" }
+      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" }
+      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" }
+      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" }
+      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" }
+      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" }
+      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: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 17 joint_name: "R_SHOULDER_R" }
+      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" }
+      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" }
+      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" }
+      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" }
+      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" }
+      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: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 30 joint_name: "HEAD_P" }
+      motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 31 joint_name: "HEAD_R" }
+      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: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
-      motors { id: 15 joint_name: "WAIST_P" }
+      motors { id: 15 joint_name: "WAIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
     }
   }
 }

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

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

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

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

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

@@ -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 "devices/motor/bus_runtime/abstract_motor_bus_runtime.h"
-#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
-#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
-#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
+#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
-#include "motor/drivers/mujoco/include/mujoco_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
-#include "motor/drivers/ti5_canopen/include/ti5_motor.h"
+#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
-#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.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:
+        case config::MOTOR_BUS_ETHERCAT: {
-            return std::make_shared<EthercatMotorBusRuntime>();
+            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="
+    auto protocol = std::make_shared<Cia402Protocol>(
-                    << config::MotorVendor_Name(group_cfg.vendor())
+        ethercat_bus_runtime, group_cfg.ethercat().cia402());
-                    << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+
-                    << ", group=" << group_cfg.id();
+    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

cmvr-es/manager/device_manager/src/device_manager.cpp

@@ -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>>;
+    MotorJointSelections selections;
-    std::unordered_map<std::string, GroupJointSelection> 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));

protos/cmvr/config/device_manager_config/device_manager_config.proto

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