refactor(motor): introduce MotorManager and bus runtimes

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

@@ -2,16 +2,35 @@ motor {
   id: "mujoco_motors"
 
   motor_groups {
-    id: "right_arm_mujoco"
+    id: "mujoco_right_arm"
     bus_type: MOTOR_BUS_MUJOCO
+    vendor: MOTOR_VENDOR_MUJOCO
+    protocol: MOTOR_PROTOCOL_MUJOCO
     tool_frame: "R_FINGER_TIP"
+    mujoco {
+      world_id: "mujoco_world"
+    }
 
-    motors { id: 1 joint_name: "R_SHOULDER_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
+    joint_limits {
-    motors { id: 2 joint_name: "R_SHOULDER_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
+      enable: true
-    motors { id: 3 joint_name: "R_SHOULDER_Y" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
+      source: JOINT_LIMIT_SOURCE_CUSTOM
-    motors { id: 4 joint_name: "R_ELBOW_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
+      joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-    motors { id: 5 joint_name: "R_WRIST_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
+      joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
-    motors { id: 6 joint_name: "R_WRIST_Y" limit_q_lb: -1.102 limit_q_ub: 1.02 limit_qd: 3.0 }
+      joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-    motors { id: 7 joint_name: "R_WRIST_R" limit_q_lb: -0.293 limit_q_ub: 1.57079 limit_qd: 3.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" }
+    }
   }
 }

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

@@ -4,31 +4,39 @@ motor {
   motor_groups {
     id: "left_arm_can"
     bus_type: MOTOR_BUS_CAN
+    vendor: MOTOR_VENDOR_TI5
+    protocol: MOTOR_PROTOCOL_CANOPEN
     tool_frame: "L_FINGER_TIP"
     can {
       channel_id: 0
     }
     joint_limits {
+      enable: true
       source: JOINT_LIMIT_SOURCE_URDF
       urdf_path: "model/xiaoyan_description/dual_arm.urdf"
     }
-    motors { id: 23 joint_name: "L_SHOULDER_P" }
+    motors {
-    motors { id: 24 joint_name: "L_SHOULDER_R" }
+      motors { id: 23 joint_name: "L_SHOULDER_P" }
-    motors { id: 25 joint_name: "L_SHOULDER_Y" }
+      motors { id: 24 joint_name: "L_SHOULDER_R" }
-    motors { id: 26 joint_name: "L_ELBOW_R" }
+      motors { id: 25 joint_name: "L_SHOULDER_Y" }
-    motors { id: 27 joint_name: "L_WRIST_P" }
+      motors { id: 26 joint_name: "L_ELBOW_R" }
-    motors { id: 28 joint_name: "L_WRIST_Y" }
+      motors { id: 27 joint_name: "L_WRIST_P" }
-    motors { id: 29 joint_name: "L_WRIST_R" }
+      motors { id: 28 joint_name: "L_WRIST_Y" }
+      motors { id: 29 joint_name: "L_WRIST_R" }
+    }
   }
 
   motor_groups {
     id: "right_arm_can"
     bus_type: MOTOR_BUS_CAN
+    vendor: MOTOR_VENDOR_TI5
+    protocol: MOTOR_PROTOCOL_CANOPEN
     tool_frame: "R_FINGER_TIP"
     can {
       channel_id: 1
     }
     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 }
@@ -38,44 +46,56 @@ motor {
       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 { id: 16 joint_name: "R_SHOULDER_P" }
+    motors {
-    motors { id: 17 joint_name: "R_SHOULDER_R" }
+      motors { id: 16 joint_name: "R_SHOULDER_P" }
-    motors { id: 18 joint_name: "R_SHOULDER_Y" }
+      motors { id: 17 joint_name: "R_SHOULDER_R" }
-    motors { id: 19 joint_name: "R_ELBOW_R" }
+      motors { id: 18 joint_name: "R_SHOULDER_Y" }
-    motors { id: 20 joint_name: "R_WRIST_P" }
+      motors { id: 19 joint_name: "R_ELBOW_R" }
-    motors { id: 21 joint_name: "R_WRIST_Y" }
+      motors { id: 20 joint_name: "R_WRIST_P" }
-    motors { id: 22 joint_name: "R_WRIST_R" }
+      motors { id: 21 joint_name: "R_WRIST_Y" }
+      motors { id: 22 joint_name: "R_WRIST_R" }
+    }
   }
 
   motor_groups {
     id: "head_can"
     bus_type: MOTOR_BUS_CAN
+    vendor: MOTOR_VENDOR_TI5
+    protocol: MOTOR_PROTOCOL_CANOPEN
     can {
       channel_id: 2
     }
     joint_limits {
+      enable: true
       source: JOINT_LIMIT_SOURCE_CUSTOM
       joints { joint_name: "HEAD_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
       joints { joint_name: "HEAD_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
       joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
     }
-    motors { id: 32 joint_name: "HEAD_Y" }
+    motors {
-    motors { id: 30 joint_name: "HEAD_P" }
+      motors { id: 32 joint_name: "HEAD_Y" }
-    motors { id: 31 joint_name: "HEAD_R" }
+      motors { id: 30 joint_name: "HEAD_P" }
+      motors { id: 31 joint_name: "HEAD_R" }
+    }
   }
 
   motor_groups {
     id: "waist_can"
     bus_type: MOTOR_BUS_CAN
+    vendor: MOTOR_VENDOR_TI5
+    protocol: MOTOR_PROTOCOL_CANOPEN
     can {
       channel_id: 3
     }
     joint_limits {
+      enable: true
       source: JOINT_LIMIT_SOURCE_CUSTOM
       joints { joint_name: "WAIST_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
       joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
     }
-    motors { id: 4 joint_name: "WAIST_Y" }
+    motors {
-    motors { id: 15 joint_name: "WAIST_P" }
+      motors { id: 4 joint_name: "WAIST_Y" }
+      motors { id: 15 joint_name: "WAIST_P" }
+    }
   }
 }

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

@@ -3,6 +3,41 @@ device_manager {
   version: "0.1"
   description: "cmvr edge system version 0.1"
 
+  devices {
+    id: "mujoco_world"
+    type: DEVICE_TYPE_MUJOCO_WORLD
+    config_file: "devices/mujoco/mujoco_world.pb.txt"
+    enable: true
+  }
+
+  devices {
+    id: "mujoco_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/mujoco_motors.pb.txt"
+    enable: true
+  }
+
+  devices {
+    id: "mujoco_right_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/arm_mujoco.pb.txt"
+    enable: true
+  }
+
+  devices {
+    id: "mujoco_viewer"
+    type: DEVICE_TYPE_MUJOCO_VIEWER
+    config_file: "devices/mujoco/mujoco_viewer.pb.txt"
+    enable: true
+  }
+
+  devices {
+    id: "mujoco_hand_cam"
+    type: DEVICE_TYPE_CAMERA
+    config_file: "devices/camera/camera.pb.txt"
+    enable: false
+  }
+
   devices {
     id: "right_hand_cam"
     type: DEVICE_TYPE_CAMERA

cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt

@@ -11,7 +11,8 @@ target_link_libraries(motor_robot_arm
         cmvr_es::algorithms::arm_motion
         cmvr_es::ik_solver
         cmvr_es::algorithms::arm_control
-        cmvr_es::device::motor_system
+        cmvr_es::device::motor_manager
+        cmvr_es::device::mujoco_motor_driver
         glog
 )
 
@@ -25,8 +26,8 @@ add_executable(motor_robot_arm_mujoco_test
 target_link_libraries(motor_robot_arm_mujoco_test
         PRIVATE
         cmvr_es::device::motor_robot_arm
-        cmvr_es::device::motor_system
+        cmvr_es::device::motor_manager
-        cmvr_es::device::mujoco_motor
+        cmvr_es::device::mujoco_motor_driver
         cmvr_es::mujoco_viewer
         cmvr_es::proto
         gtest

cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h

@@ -15,7 +15,7 @@
 #include "cmvr/config/arm_config/arm_config.pb.h"
 #include "devices/arm/robot_arm.h"
 #include "algorithms/kinematics/ik_solver/common/include/ik_solver.h"
-#include "motor/motor_manager.h"
+#include "motor/manager/include/motor_manager.h"
 
 namespace cmvr::device {
 
@@ -105,8 +105,7 @@ private:
 
     static CartesianVelocityController::Config toCartesianVelocityControllerConfig_(
         const config::CartesianVelocityControllerConfig& config);
-    static std::vector<double> withDefaultQdMax_(const std::vector<double>& qd_max,
+    std::vector<double> moveLJointVelocityLimits_(const std::vector<double>& qd_max) const;
-                                                 std::size_t dof);
     static Result unsupported_(const std::string& name);
 
 private:

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp

@@ -2,7 +2,6 @@
 
 #include <chrono>
 #include <Eigen/Dense>
-#include <sstream>
 #include <stdexcept>
 #include <thread>
 #include <utility>
@@ -11,13 +10,17 @@
 #include "algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h"
 #include "algorithms/kinematics/ik_solver/ik_solver_factory.h"
 #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
+#include "common/base/protobuf_utils.h"
+#include "common/base/string_utils.h"
 #include "common/base/logging/logger.h"
 #include "common/math/transform_math.h"
 #include "motor/abstract_motor.h"
-#include "motor/motor_system/include/motor_system.h"
+#include "motor/manager/include/motor_manager.h"
 
 namespace cmvr::device {
 
+using cmvr::common::joinStrings;
+using cmvr::common::repeatedToVector;
 namespace {
 
 struct BusyGuard {
@@ -25,24 +28,6 @@ struct BusyGuard {
     ~BusyGuard() { busy.store(false); }
 };
 
-std::string joinStrings(const std::vector<std::string>& values)
-{
-    std::ostringstream oss;
-    for (std::size_t i = 0; i < values.size(); ++i) {
-        if (i > 0) {
-            oss << ", ";
-        }
-        oss << values[i];
-    }
-    return oss.str();
-}
-
-std::vector<std::string> repeatedToVector(
-    const google::protobuf::RepeatedPtrField<std::string>& values)
-{
-    return {values.begin(), values.end()};
-}
-
 } // namespace
 
 MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
@@ -99,10 +84,10 @@ bool MotorRobotArm::init()
                    << "], joints=[" << joinStrings(joint_names_) << "]";
 
     if (!motor_manager_) {
-        motor_manager_ = MotorSystem::managerFor(motor_system_id_);
+        motor_manager_ = MotorManager::managerFor(motor_system_id_);
     }
     if (!motor_manager_) {
-        CMVR_LOG(ERROR) << "[MotorRobotArm] MotorSystem is not initialized: " << motor_system_id_;
+        CMVR_LOG(ERROR) << "[MotorRobotArm] MotorManager is not initialized: " << motor_system_id_;
         CMVR_LOG(ERROR) << "[MotorRobotArm] (init): Arm '" << id_
                         << "' initialized motors=[], missing motors=["
                         << joinStrings(joint_names_) << "]";
@@ -394,7 +379,7 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
     CartesianJointTrajectory trajectory;
     if (!cartesian_planner_->planMoveL(target,
                                        q_start,
-                                       withDefaultQdMax_(options.joint_velocity_limits, getDof()),
+                                       moveLJointVelocityLimits_(options.joint_velocity_limits),
                                        options.velocity,
                                        options.acceleration,
                                        options.jerk,
@@ -402,6 +387,12 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
                                        trajectory)) {
         return Result::failure(ArmErrorCode::CommandFailed, "moveL planner failed");
     }
+    if (trajectory.truncated) {
+        CMVR_LOG(WARNING) << "[MotorRobotArm][moveL] planned_path_m="
+                          << trajectory.planned_path_length
+                          << ", truncated_reason=" << trajectory.truncation_reason
+                          << ", executable_path_m=" << trajectory.executable_path_length;
+    }
 
     return executeMoveLTrajectory_(trajectory) ? Result::success()
                                                : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
@@ -690,8 +681,10 @@ bool MotorRobotArm::configureAlgorithms_()
     }
     const auto* speed_l_config = CartesianMotionPlannerFactory::speedLConfig(speed_l);
     const auto* move_l_config = CartesianMotionPlannerFactory::moveLConfig(move_l);
-    if (!speed_l_config || !move_l_config ||
+    if (!speed_l_config || !move_l_config) {
-        !cartesian_motion->configureMoveL(*move_l_config) ||
+        return false;
+    }
+    if (!cartesian_motion->configureMoveL(*move_l_config) ||
         !cartesian_motion->configureSpeedL(*speed_l_config, getDof())) {
         return false;
     }
@@ -749,16 +742,26 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController
     result.stop_measured_velocity_norm =
         config.stop_measured_velocity_norm() > 0.0 ? config.stop_measured_velocity_norm()
                                                    : result.stop_measured_velocity_norm;
+    result.stop_acceleration =
+        config.stop_acceleration() > 0.0 ? config.stop_acceleration()
+                                         : result.stop_acceleration;
     return result;
 }
 
-std::vector<double> MotorRobotArm::withDefaultQdMax_(const std::vector<double>& qd_max,
+std::vector<double> MotorRobotArm::moveLJointVelocityLimits_(
-                                                     const std::size_t dof)
+    const std::vector<double>& qd_max) const
 {
     if (!qd_max.empty()) {
         return qd_max;
     }
-    return std::vector<double>(dof, 2.5);
+    const auto pinocchio_solver = std::dynamic_pointer_cast<cmvr::PinocchioIKBase>(ik_solver_);
+    if (pinocchio_solver) {
+        const auto& limits = pinocchio_solver->jointLimitPolicy().limits();
+        if (!limits.enable()) {
+            return {};
+        }
+    }
+    return {};
 }
 
 Result MotorRobotArm::unsupported_(const std::string& name)

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp

@@ -17,9 +17,9 @@
 #include <gtest/gtest.h>
 
 #include "common/io/proto_file_io.h"
-#include "devices/motor/mujoco_motor/include/mujoco_joint_bridge.h"
+#include "devices/motor/manager/include/motor_manager.h"
-#include "devices/motor/motor_system/include/motor_system.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
 
 namespace cmvr::device {
 namespace {
@@ -54,84 +54,6 @@ std::filesystem::path findProjectRoot()
     return search(std::filesystem::path(__FILE__).parent_path());
 }
 
-class MotorRobotArmViewer final : public MuJocoViewer {
-public:
-    MotorRobotArmViewer(const std::string& model_path,
-                        std::shared_ptr<MujocoJointBridge> bridge)
-        : MuJocoViewer(model_path.c_str()), bridge_(std::move(bridge))
-    {
-        position_actuator_ids_.fill(-1);
-        qpos_ids_.fill(-1);
-        qvel_ids_.fill(-1);
-    }
-
-protected:
-    void initOnce(mjModel* model, mjData* data) override
-    {
-        setupCamera(2.5, -160.0, -25.0);
-        bool valid = true;
-        for (std::size_t i = 0; i < kDof; ++i) {
-            const std::string actuator_name = std::string(kJointNames[i]) + "_pos";
-            position_actuator_ids_[i] = mj_name2id(model, mjOBJ_ACTUATOR, actuator_name.c_str());
-            const int joint_id = mj_name2id(model, mjOBJ_JOINT, kJointNames[i]);
-            if (position_actuator_ids_[i] < 0 || joint_id < 0) {
-                valid = false;
-                continue;
-            }
-            qpos_ids_[i] = model->jnt_qposadr[joint_id];
-            qvel_ids_[i] = model->jnt_dofadr[joint_id];
-            position_reference_[i] = data->qpos[qpos_ids_[i]];
-        }
-        bridge_->markReady(valid);
-    }
-
-    void controlCallback(mjModel* model, mjData* data) override
-    {
-        std::vector<double> measured_position(kDof, 0.0);
-        std::vector<double> measured_velocity(kDof, 0.0);
-        for (std::size_t i = 0; i < kDof; ++i) {
-            if (qpos_ids_[i] >= 0) {
-                measured_position[i] = data->qpos[qpos_ids_[i]];
-            }
-            if (qvel_ids_[i] >= 0) {
-                measured_velocity[i] = data->qvel[qvel_ids_[i]];
-            }
-        }
-        bridge_->publishMeasured(measured_position, measured_velocity);
-
-        const auto commands = bridge_->commands();
-        for (std::size_t i = 0; i < kDof; ++i) {
-            const int actuator_id = position_actuator_ids_[i];
-            if (actuator_id < 0) {
-                continue;
-            }
-
-            if (commands.mode[i] == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
-                if (last_mode_[i] != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY && qpos_ids_[i] >= 0) {
-                    position_reference_[i] = data->qpos[qpos_ids_[i]];
-                }
-                position_reference_[i] += commands.velocity[i] * model->opt.timestep;
-            } else {
-                position_reference_[i] = commands.position[i];
-            }
-
-            const double lower = model->actuator_ctrlrange[2 * actuator_id];
-            const double upper = model->actuator_ctrlrange[2 * actuator_id + 1];
-            position_reference_[i] = std::clamp(position_reference_[i], lower, upper);
-            data->ctrl[actuator_id] = position_reference_[i];
-            last_mode_[i] = commands.mode[i];
-        }
-    }
-
-private:
-    std::shared_ptr<MujocoJointBridge> bridge_;
-    std::array<int, kDof> position_actuator_ids_{};
-    std::array<int, kDof> qpos_ids_{};
-    std::array<int, kDof> qvel_ids_{};
-    std::array<double, kDof> position_reference_{};
-    std::array<msgs::RunMode, kDof> last_mode_{};
-};
-
 double maxPositionError(const std::vector<double>& actual,
                         const std::vector<double>& expected)
 {
@@ -208,6 +130,18 @@ protected:
         project_root_ = findProjectRoot();
         ASSERT_FALSE(project_root_.empty());
 
+        config::MujocoWorldRootConfig world_root_config;
+        ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
+            (project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(),
+            &world_root_config));
+        ASSERT_GT(world_root_config.worlds_size(), 0);
+        auto world_config = world_root_config.worlds(0);
+        world_config.set_model_path(
+            (project_root_ / "model/xiaoyan_description/dual_arm.xml").string());
+        world_device_ = std::make_shared<simulate::MujocoWorldDevice>(world_config);
+        ASSERT_TRUE(world_device_->init());
+        ASSERT_TRUE(world_device_->start());
+
         config::MotorRootConfig motor_root_config;
         ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
             (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(),
@@ -217,14 +151,15 @@ protected:
         for (const auto* joint_name : kJointNames) {
             right_arm_joints.insert(joint_name);
         }
-        MotorSystem::clearActiveJoints();
+        MotorManager::clearActiveJoints();
-        MotorSystem::setActiveJoints(
+        MotorManager::setActiveJoints(
-            "mujoco_motors", {{"right_arm_mujoco", std::move(right_arm_joints)}});
+            "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}});
 
-        motor_system_ = std::make_shared<MotorSystem>("mujoco_motors", motor_root_config.motor());
+        motor_system_ = std::make_shared<MotorManager>("mujoco_motors", motor_root_config.motor());
         ASSERT_NO_THROW(motor_system_->init());
-        bridge_ = MotorSystem::mujocoBridgeFor("mujoco_motors");
+        world_ = MotorManager::mujocoWorldFor("mujoco_motors");
-        ASSERT_TRUE(bridge_);
+        ASSERT_TRUE(world_);
+        ASSERT_TRUE(world_->isLoaded());
 
         config::ArmRootConfig root_config;
         ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
@@ -249,12 +184,16 @@ protected:
         if (motor_system_) {
             motor_system_->stop();
         }
-        MotorSystem::clearActiveJoints();
+        if (world_device_) {
+            world_device_->stop();
+        }
+        MotorManager::clearActiveJoints();
     }
 
     std::filesystem::path project_root_;
-    std::shared_ptr<MotorSystem> motor_system_;
+    std::shared_ptr<simulate::MujocoWorldDevice> world_device_;
-    std::shared_ptr<MujocoJointBridge> bridge_;
+    std::shared_ptr<MotorManager> motor_system_;
+    std::shared_ptr<simulate::MujocoWorld> world_;
     std::unique_ptr<MotorRobotArm> arm_;
 };
 
@@ -262,14 +201,14 @@ TEST_P(MotorRobotArmMujocoTest, MoveJ)
 {
     MotorRobotArm& arm = *arm_;
 
-    MotorRobotArmViewer viewer(
+    MuJocoViewer viewer(world_);
-        (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_);
+    viewer.setupCamera(2.5, -160.0, -25.0);
     ScenarioOutcome outcome;
 
     std::thread scenario([&] {
         try {
-            if (!bridge_->waitUntilReady(std::chrono::seconds(10))) {
+            if (!world_ || !world_->isRunning()) {
-                throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready");
+                throw std::runtime_error("MuJoCo world is not running");
             }
             std::this_thread::sleep_for(std::chrono::milliseconds(300));
 
@@ -310,14 +249,14 @@ TEST_P(MotorRobotArmMujocoTest, MoveL)
 {
     MotorRobotArm& arm = *arm_;
 
-    MotorRobotArmViewer viewer(
+    MuJocoViewer viewer(world_);
-        (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_);
+    viewer.setupCamera(2.5, -160.0, -25.0);
     ScenarioOutcome outcome;
 
     std::thread scenario([&] {
         try {
-            if (!bridge_->waitUntilReady(std::chrono::seconds(10))) {
+            if (!world_ || !world_->isRunning()) {
-                throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready");
+                throw std::runtime_error("MuJoCo world is not running");
             }
             std::this_thread::sleep_for(std::chrono::milliseconds(300));
 
@@ -379,14 +318,14 @@ TEST_P(MotorRobotArmMujocoTest, SpeedL)
 {
     MotorRobotArm& arm = *arm_;
 
-    MotorRobotArmViewer viewer(
+    MuJocoViewer viewer(world_);
-        (project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_);
+    viewer.setupCamera(2.5, -160.0, -25.0);
     ScenarioOutcome outcome;
 
     std::thread scenario([&] {
         try {
-            if (!bridge_->waitUntilReady(std::chrono::seconds(10))) {
+            if (!world_ || !world_->isRunning()) {
-                throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready");
+                throw std::runtime_error("MuJoCo world is not running");
             }
             std::this_thread::sleep_for(std::chrono::milliseconds(300));
 
@@ -446,9 +385,8 @@ INSTANTIATE_TEST_SUITE_P(
     ArmPlannerCombinations,
     MotorRobotArmMujocoTest,
     ::testing::Values(
-        ArmMujocoConfigCase{"DlsIkDlsMotion", "arm_mujoco_dls_ik_dls_motion.pb.txt"},
+        ArmMujocoConfigCase{"DlsIk", "arm_mujoco.pb.txt"},
-        ArmMujocoConfigCase{"DlsIkQpMotion", "arm_mujoco_dls_ik_qp_motion.pb.txt"},
+        ArmMujocoConfigCase{"QpIk", "arm_mujoco_qp.pb.txt"}),
-        ArmMujocoConfigCase{"QpIkQpMotion", "arm_mujoco_qp_ik_qp_motion.pb.txt"}),
     [](const ::testing::TestParamInfo<ArmMujocoConfigCase>& info) {
         return std::string(info.param.name);
     });

cmvr-es/devices/motor/CMakeLists.txt

@@ -1,31 +1,15 @@
-add_subdirectory(ti5_motor)
+add_library(motor_core INTERFACE)
-add_subdirectory(mujoco_motor)
-add_subdirectory(motor_system)
 
-# --------------------------------------------------------
+target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices)
-# Unit test
-# --------------------------------------------------------
-include_directories(
-        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
-)
 
-link_directories(
+target_link_libraries(motor_core
-        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
+        INTERFACE
-)
-
-
-add_executable(motor_manager_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/motor_manager_test.cpp
-)
-
-
-target_link_libraries(motor_manager_test
-        PRIVATE
-        cmvr_es::device::canbus
-        cmvr_es::device::ti5motor
-        gtest
-        gtest_main
-        pthread
-        glog
         cmvr_es::proto
 )
+
+add_library(cmvr_es::device::motor_core ALIAS motor_core)
+
+add_subdirectory(drivers/ti5_canopen)
+add_subdirectory(drivers/mujoco)
+add_subdirectory(bus_runtime)
+add_subdirectory(manager)

cmvr-es/devices/motor/abstract_motor.h

@@ -7,7 +7,7 @@
 
 #pragma once
 
-#include "../abstract_device.h"
+#include "devices/abstract_device.h"
 #include "common/base/logging/logger.h"
 #include "motor/motor_protocol_interface.h"
 #include <mutex>
@@ -66,7 +66,7 @@ namespace cmvr::device{
             return protocol_->getMode(node_id_);
         }
 
-        void torqueOff() {
+        virtual void torqueOff() {
             std::scoped_lock lock(mtx_);
             if (!protocol_) {
                 CMVR_LOG(ERROR) << "Protocol not set for motor";

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

@@ -0,0 +1,20 @@
+add_library(motor_bus_runtime SHARED
+        can/src/can_motor_bus_runtime.cpp
+        mujoco/src/mujoco_motor_bus_runtime.cpp
+        ethercat/src/ethercat_motor_bus_runtime.cpp
+)
+
+target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(motor_bus_runtime
+        PUBLIC
+        cmvr_es::proto
+        cmvr_es::device::motor_core
+        cmvr_es::mujoco_world
+        PRIVATE
+        cmvr_es::device::canbus
+        glog
+)
+
+add_library(cmvr_es::device::motor_bus_runtime ALIAS motor_bus_runtime)
+install(TARGETS motor_bus_runtime LIBRARY DESTINATION lib)

cmvr-es/devices/motor/bus_runtime/abstract_motor_bus_runtime.h

@@ -0,0 +1,30 @@
+#ifndef CMVR_ES_ABSTRACT_MOTOR_BUS_RUNTIME_H
+#define CMVR_ES_ABSTRACT_MOTOR_BUS_RUNTIME_H
+
+#include <memory>
+
+#include "cmvr/config/motor_config/motor_config.pb.h"
+
+namespace cmvr::simulate {
+
+class MujocoWorld;
+
+} // namespace cmvr::simulate
+
+namespace cmvr::device {
+
+class AbstractMotorBusRuntime {
+public:
+    virtual ~AbstractMotorBusRuntime() = default;
+
+    virtual bool init(const config::MotorGroupConfig& group_cfg) = 0;
+    virtual bool start() = 0;
+    virtual void stop() = 0;
+    virtual config::MotorBusType busType() const = 0;
+
+    virtual std::shared_ptr<simulate::MujocoWorld> mujocoWorld() const { return nullptr; }
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_ABSTRACT_MOTOR_BUS_RUNTIME_H

cmvr-es/devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h

@@ -0,0 +1,45 @@
+#ifndef CMVR_ES_CAN_MOTOR_BUS_RUNTIME_H
+#define CMVR_ES_CAN_MOTOR_BUS_RUNTIME_H
+
+#include <memory>
+#include <string>
+
+#include "cmvr/msgs/robot_detail.pb.h"
+#include "../../abstract_motor_bus_runtime.h"
+
+namespace cmvr::device {
+
+class AbstractCanbus;
+
+template <typename SensorType>
+class CanReceiver;
+
+template <typename SensorType>
+class CanSender;
+
+template <typename SensorType>
+class MessageManager;
+
+class CanMotorBusRuntime final : public AbstractMotorBusRuntime {
+public:
+    bool init(const config::MotorGroupConfig& group_cfg) override;
+    bool start() override;
+    void stop() override;
+    config::MotorBusType busType() const override { return config::MOTOR_BUS_CAN; }
+
+    const std::string& id() const { return id_; }
+    std::shared_ptr<CanSender<msgs::RobotDetail>> sender() const { return sender_; }
+    std::shared_ptr<MessageManager<msgs::RobotDetail>> messageManager() const { return message_manager_; }
+
+private:
+    std::string id_;
+    std::shared_ptr<AbstractCanbus> client_;
+    std::shared_ptr<CanSender<msgs::RobotDetail>> sender_;
+    std::shared_ptr<CanReceiver<msgs::RobotDetail>> receiver_;
+    std::shared_ptr<MessageManager<msgs::RobotDetail>> message_manager_;
+    bool started_{false};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CAN_MOTOR_BUS_RUNTIME_H

cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp

@@ -0,0 +1,102 @@
+#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h"
+
+#include "canbus/can_client/socket/socket_can_client_raw.h"
+#include "canbus/can_comm/can_receiver.h"
+#include "canbus/can_comm/can_sender.h"
+#include "canbus/can_comm/message_manager.h"
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+
+bool CanMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
+{
+    id_ = group_cfg.id();
+    if (id_.empty()) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] group id is empty";
+        return false;
+    }
+    if (group_cfg.bus_type() != config::MOTOR_BUS_CAN) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] invalid bus type for group: " << id_;
+        return false;
+    }
+    if (!group_cfg.has_can()) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] missing can config: " << id_;
+        return false;
+    }
+
+    client_ = std::make_shared<SocketCanClientRaw>(group_cfg.can());
+    sender_ = std::make_shared<CanSender<msgs::RobotDetail>>();
+    receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail>>();
+    message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail>>();
+    if (!client_ || !sender_ || !receiver_ || !message_manager_) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] invalid runtime: " << id_;
+        return false;
+    }
+
+    if (!client_->init()) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to init CAN client: " << id_;
+        return false;
+    }
+
+    auto ret = sender_->Init(client_.get(), false);
+    if (ret != ErrorCode::OK) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to init CAN sender: " << id_;
+        return false;
+    }
+
+    ret = receiver_->Init(client_.get(), message_manager_.get(), false);
+    if (ret != ErrorCode::OK) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to init CAN receiver: " << id_;
+        return false;
+    }
+
+    return true;
+}
+
+bool CanMotorBusRuntime::start()
+{
+    if (started_) {
+        return true;
+    }
+    if (!client_ || !sender_ || !receiver_) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] runtime is not initialized: " << id_;
+        return false;
+    }
+
+    if (!client_->start()) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN client: " << id_;
+        return false;
+    }
+
+    auto ret = sender_->Start();
+    if (ret != ErrorCode::OK) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
+        stop();
+        return false;
+    }
+
+    ret = receiver_->Start();
+    if (ret != ErrorCode::OK) {
+        CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
+        stop();
+        return false;
+    }
+    started_ = true;
+    return true;
+}
+
+void CanMotorBusRuntime::stop()
+{
+    if (receiver_) {
+        receiver_->Stop();
+    }
+    if (sender_) {
+        sender_->Stop();
+    }
+    if (client_) {
+        client_->stop();
+    }
+    started_ = false;
+}
+
+} // namespace cmvr::device

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

@@ -0,0 +1,31 @@
+#ifndef CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
+#define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
+
+#include <string>
+#include <unordered_map>
+
+#include "../../abstract_motor_bus_runtime.h"
+
+namespace cmvr::device {
+
+class EthercatMotorBusRuntime final : public AbstractMotorBusRuntime {
+public:
+    bool init(const config::MotorGroupConfig& group_cfg) override;
+    bool start() override;
+    void stop() override;
+    config::MotorBusType busType() const override { return config::MOTOR_BUS_ETHERCAT; }
+
+    const std::string& id() const { return id_; }
+    const config::EtherCATConfig& config() const { return config_; }
+    const config::EthercatSlaveConfig* slaveForMotor(int motor_id) const;
+
+private:
+    std::string id_;
+    config::EtherCATConfig config_;
+    std::unordered_map<int, const config::EthercatSlaveConfig*> slaves_by_motor_id_;
+    bool started_{false};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H

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

@@ -0,0 +1,78 @@
+#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+
+bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
+{
+    id_ = group_cfg.id();
+    if (id_.empty()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] group id is empty";
+        return false;
+    }
+    if (group_cfg.bus_type() != config::MOTOR_BUS_ETHERCAT) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid bus type for group: " << id_;
+        return false;
+    }
+    if (!group_cfg.has_ethercat()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing ethercat config: " << id_;
+        return false;
+    }
+
+    config_ = group_cfg.ethercat();
+    if (config_.master_id().empty()) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] master_id is empty: " << id_;
+        return false;
+    }
+    if (config_.cycle_us() <= 0) {
+        CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] cycle_us must be positive: " << id_;
+        return false;
+    }
+
+    slaves_by_motor_id_.clear();
+    for (const auto& slave : config_.slaves()) {
+        if (slave.motor_id() <= 0) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid motor_id in slave config: " << id_;
+            return false;
+        }
+        if (slave.slave_index() < 0) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid slave_index for motor "
+                            << slave.motor_id() << " in group: " << id_;
+            return false;
+        }
+        if (slaves_by_motor_id_.count(slave.motor_id()) > 0) {
+            CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] duplicate slave motor_id: "
+                            << slave.motor_id() << " in group: " << id_;
+            return false;
+        }
+        slaves_by_motor_id_[slave.motor_id()] = &slave;
+    }
+
+    return true;
+}
+
+bool EthercatMotorBusRuntime::start()
+{
+    if (started_) {
+        return true;
+    }
+    CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] EtherCAT master is not implemented yet: " << id_;
+    return false;
+}
+
+void EthercatMotorBusRuntime::stop()
+{
+    started_ = false;
+}
+
+const config::EthercatSlaveConfig* EthercatMotorBusRuntime::slaveForMotor(const int motor_id) const
+{
+    const auto it = slaves_by_motor_id_.find(motor_id);
+    if (it == slaves_by_motor_id_.end()) {
+        return nullptr;
+    }
+    return it->second;
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h

@@ -0,0 +1,36 @@
+#ifndef CMVR_ES_MUJOCO_MOTOR_BUS_RUNTIME_H
+#define CMVR_ES_MUJOCO_MOTOR_BUS_RUNTIME_H
+
+#include <memory>
+#include <string>
+
+#include "../../abstract_motor_bus_runtime.h"
+
+namespace cmvr::simulate {
+
+class MujocoWorld;
+
+} // namespace cmvr::simulate
+
+namespace cmvr::device {
+
+class MujocoMotorBusRuntime final : public AbstractMotorBusRuntime {
+public:
+    bool init(const config::MotorGroupConfig& group_cfg) override;
+    bool start() override;
+    void stop() override;
+    config::MotorBusType busType() const override { return config::MOTOR_BUS_MUJOCO; }
+    std::shared_ptr<simulate::MujocoWorld> mujocoWorld() const override { return world_; }
+
+    const std::string& id() const { return id_; }
+
+private:
+    std::string id_;
+    std::string world_id_;
+    std::shared_ptr<simulate::MujocoWorld> world_;
+    bool started_{false};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_MUJOCO_MOTOR_BUS_RUNTIME_H

cmvr-es/devices/motor/bus_runtime/mujoco/src/mujoco_motor_bus_runtime.cpp

@@ -0,0 +1,71 @@
+#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
+
+#include "common/base/logging/logger.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
+
+namespace cmvr::device {
+
+bool MujocoMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg)
+{
+    id_ = group_cfg.id();
+    if (id_.empty()) {
+        CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] group id is empty";
+        return false;
+    }
+    if (group_cfg.bus_type() != config::MOTOR_BUS_MUJOCO) {
+        CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] invalid bus type for group: " << id_;
+        return false;
+    }
+    if (!group_cfg.has_mujoco()) {
+        CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] missing mujoco config: " << id_;
+        return false;
+    }
+
+    const auto& mujoco_cfg = group_cfg.mujoco();
+    if (mujoco_cfg.world_id().empty()) {
+        CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] missing world_id: " << id_;
+        return false;
+    }
+    world_id_ = mujoco_cfg.world_id();
+
+    world_ = simulate::MujocoWorldDevice::worldFor(world_id_);
+    if (!world_) {
+        CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] world not found: "
+                        << world_id_ << " for group: " << id_;
+        return false;
+    }
+    if (!world_->isLoaded()) {
+        CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] world is not loaded: "
+                        << world_id_;
+        return false;
+    }
+    return true;
+}
+
+bool MujocoMotorBusRuntime::start()
+{
+    if (started_) {
+        return true;
+    }
+    if (!world_) {
+        CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] runtime is not initialized: " << id_;
+        return false;
+    }
+    if (!world_->isRunning() && !world_->start()) {
+        CMVR_LOG(ERROR) << "[MujocoMotorBusRuntime] failed to start world: "
+                        << world_id_ << ", error=" << world_->lastError();
+        return false;
+    }
+    started_ = true;
+    return true;
+}
+
+void MujocoMotorBusRuntime::stop()
+{
+    if (world_) {
+        world_->stop();
+    }
+    started_ = false;
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/mujoco/CMakeLists.txt

@@ -0,0 +1,15 @@
+add_library(mujoco_motor_driver SHARED
+        src/mujoco_motor.cpp
+)
+
+target_include_directories(mujoco_motor_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(mujoco_motor_driver
+        PUBLIC
+        cmvr_es::proto
+        cmvr_es::device::motor_core
+        cmvr_es::mujoco_world
+)
+
+add_library(cmvr_es::device::mujoco_motor_driver ALIAS mujoco_motor_driver)
+install(TARGETS mujoco_motor_driver LIBRARY DESTINATION lib)

cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h

@@ -5,24 +5,25 @@
 #include <cstdint>
 #include <memory>
 #include <string>
+#include <vector>
 
-#include "devices/motor/abstract_motor.h"
+#include "motor/abstract_motor.h"
-#include "devices/motor/mujoco_motor/include/mujoco_joint_bridge.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
 
 namespace cmvr::device {
 
 class MujocoMotor final : public AbstractMotor {
 public:
-    MujocoMotor(std::size_t joint_index,
+    MujocoMotor(std::string joint_name,
-                std::string joint_name,
+                std::shared_ptr<simulate::MujocoWorld> world,
-                std::shared_ptr<MujocoJointBridge> bridge,
                 std::uint8_t node_id = 0);
 
     std::string typeName() const override { return "MujocoMotor"; }
-    bool init() override { return true; }
+    bool init() override;
 
     void setMode(msgs::RunMode mode) override;
     msgs::RunMode getMode() override;
+    void torqueOff() override;
 
     void setLimitQ(double ub, double lb) override;
     void setLimitQd(double qd) override;
@@ -38,11 +39,21 @@ public:
     double getQ() override;
     double getQd() override;
 
+    static bool setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
+                                 const std::vector<double>& positions,
+                                 const std::vector<double>& velocities);
+
 private:
-    std::size_t joint_index_{0};
+    double clampQ_(double q) const;
-    std::shared_ptr<MujocoJointBridge> bridge_;
+    double clampQd_(double qd) const;
+    std::shared_ptr<simulate::MujocoWorld> worldLocked_() const;
+
+    std::weak_ptr<simulate::MujocoWorld> world_;
+    msgs::RunMode mode_{msgs::RUN_MODE_UNSPECIFIED};
+    double target_q_{0.0};
     double limit_qdd_upper_{0.0};
     double limit_qdd_lower_{0.0};
+    bool initialized_{false};
 };
 
 } // namespace cmvr::device

cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp

@@ -0,0 +1,249 @@
+#include "motor/drivers/mujoco/include/mujoco_motor.h"
+
+#include <algorithm>
+#include <cmath>
+#include <stdexcept>
+#include <utility>
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+
+MujocoMotor::MujocoMotor(std::string joint_name,
+                         std::shared_ptr<simulate::MujocoWorld> world,
+                         const std::uint8_t node_id)
+    : AbstractMotor(node_id),
+      world_(std::move(world))
+{
+    info_.id = id();
+    info_.joint_name = std::move(joint_name);
+    id_ = info_.joint_name;
+}
+
+bool MujocoMotor::init()
+{
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        CMVR_LOG(ERROR) << "[MujocoMotor] world is null for joint: " << info_.joint_name;
+        return false;
+    }
+    if (!world->isLoaded()) {
+        CMVR_LOG(ERROR) << "[MujocoMotor] world is not loaded for joint: " << info_.joint_name;
+        return false;
+    }
+    if (!world->hasJoint(info_.joint_name)) {
+        CMVR_LOG(ERROR) << "[MujocoMotor] joint not found in world: " << info_.joint_name;
+        return false;
+    }
+    world->getJointPosition(info_.joint_name, target_q_);
+    mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+    initialized_ = true;
+    return true;
+}
+
+void MujocoMotor::setMode(const msgs::RunMode mode)
+{
+    std::scoped_lock lock(mtx_);
+    mode_ = mode;
+}
+
+msgs::RunMode MujocoMotor::getMode()
+{
+    std::scoped_lock lock(mtx_);
+    return mode_;
+}
+
+void MujocoMotor::torqueOff()
+{
+    brake();
+    std::scoped_lock lock(mtx_);
+    mode_ = msgs::RUN_MODE_UNSPECIFIED;
+}
+
+void MujocoMotor::setLimitQ(const double ub, const double lb)
+{
+    std::scoped_lock lock(mtx_);
+    info_.limit_q_ub = ub;
+    info_.limit_q_lb = lb;
+}
+
+void MujocoMotor::setLimitQd(const double qd)
+{
+    std::scoped_lock lock(mtx_);
+    info_.limit_qd = std::abs(qd);
+}
+
+void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd)
+{
+    std::scoped_lock lock(mtx_);
+    limit_qdd_upper_ = std::abs(u_qdd);
+    limit_qdd_lower_ = -std::abs(l_qdd);
+    info_.limit_qdd = std::max(limit_qdd_upper_, std::abs(limit_qdd_lower_));
+}
+
+void MujocoMotor::brake()
+{
+    const auto world = worldLocked_();
+    double q = 0.0;
+    if (!world || !world->getJointPosition(info_.joint_name, q)) {
+        return;
+    }
+    std::scoped_lock lock(mtx_);
+    target_q_ = q;
+    mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+    world->setJointTargetState(info_.joint_name, q, 0.0);
+}
+
+void MujocoMotor::setQ(const double q)
+{
+    setTarget(q, 0.0);
+}
+
+void MujocoMotor::setTarget(const double q, const double qd)
+{
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return;
+    }
+    target_q_ = clampQ_(q);
+    world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd));
+}
+
+void MujocoMotor::setTarget(const double qd)
+{
+    setQd(qd);
+}
+
+bool MujocoMotor::calibrateZeroQ()
+{
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return false;
+    }
+    target_q_ = clampQ_(0.0);
+    return world->setJointPosition(info_.joint_name, target_q_);
+}
+
+bool MujocoMotor::reachedTargetQ()
+{
+    double q = 0.0;
+    {
+        std::scoped_lock lock(mtx_);
+        const auto world = worldLocked_();
+        if (!world || !world->getJointPosition(info_.joint_name, q)) {
+            return false;
+        }
+        return std::abs(q - target_q_) < 1e-3;
+    }
+}
+
+void MujocoMotor::setQd(const double qd)
+{
+    std::scoped_lock lock(mtx_);
+    const auto world = worldLocked_();
+    if (!world) {
+        return;
+    }
+    world->setJointTargetVelocity(info_.joint_name, clampQd_(qd));
+}
+
+double MujocoMotor::getQ()
+{
+    const auto world = worldLocked_();
+    double q = 0.0;
+    if (!world || !world->getJointPosition(info_.joint_name, q)) {
+        return 0.0;
+    }
+    return q;
+}
+
+double MujocoMotor::getQd()
+{
+    const auto world = worldLocked_();
+    double qd = 0.0;
+    if (!world || !world->getJointVelocity(info_.joint_name, qd)) {
+        return 0.0;
+    }
+    return qd;
+}
+
+bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
+                                   const std::vector<double>& positions,
+                                   const std::vector<double>& velocities)
+{
+    if (motors.size() != positions.size() || motors.size() != velocities.size() || motors.empty()) {
+        return false;
+    }
+
+    std::shared_ptr<simulate::MujocoWorld> world;
+    std::vector<std::string> joint_names;
+    std::vector<double> clamped_positions;
+    std::vector<double> clamped_velocities;
+    joint_names.reserve(motors.size());
+    clamped_positions.reserve(motors.size());
+    clamped_velocities.reserve(motors.size());
+
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        const auto& motor = motors[i];
+        if (!motor) {
+            return false;
+        }
+
+        std::scoped_lock lock(motor->mtx_);
+        auto motor_world = motor->worldLocked_();
+        if (!motor_world) {
+            return false;
+        }
+        if (!world) {
+            world = motor_world;
+        } else if (world.get() != motor_world.get()) {
+            CMVR_LOG(ERROR) << "[MujocoMotor] batch target motors belong to different worlds";
+            return false;
+        }
+
+        joint_names.push_back(motor->info_.joint_name);
+        clamped_positions.push_back(motor->clampQ_(positions[i]));
+        clamped_velocities.push_back(motor->clampQd_(velocities[i]));
+    }
+
+    if (!world || !world->setJointTargetStates(joint_names, clamped_positions, clamped_velocities)) {
+        if (world) {
+            CMVR_LOG(ERROR) << "[MujocoMotor] failed to set batch joint targets: " << world->lastError();
+        }
+        return false;
+    }
+
+    for (std::size_t i = 0; i < motors.size(); ++i) {
+        std::scoped_lock lock(motors[i]->mtx_);
+        motors[i]->target_q_ = clamped_positions[i];
+        motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
+    }
+    return true;
+}
+
+double MujocoMotor::clampQ_(const double q) const
+{
+    if (std::isfinite(info_.limit_q_lb) && std::isfinite(info_.limit_q_ub) &&
+        info_.limit_q_ub > info_.limit_q_lb) {
+        return std::clamp(q, info_.limit_q_lb, info_.limit_q_ub);
+    }
+    return q;
+}
+
+double MujocoMotor::clampQd_(const double qd) const
+{
+    if (std::isfinite(info_.limit_qd) && info_.limit_qd > 0.0) {
+        return std::clamp(qd, -info_.limit_qd, info_.limit_qd);
+    }
+    return qd;
+}
+
+std::shared_ptr<simulate::MujocoWorld> MujocoMotor::worldLocked_() const
+{
+    return world_.lock();
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/drivers/ti5_canopen/CMakeLists.txt

@@ -0,0 +1,24 @@
+add_library(ti5_canopen_motor_driver SHARED
+        src/protocol/ti5_motor_sdo_response.cpp
+        src/protocol/ti5_motor_tpdo1.cpp
+        src/protocol/ti5_motor_tpdo2.cpp
+        src/protocol/ti5_motor_rpdo1.cpp
+        src/protocol/ti5_motor_rpdo2.cpp
+        src/ti5_motor_canopen_protocol.cpp
+        src/ti5_motor.cpp
+)
+
+target_include_directories(ti5_canopen_motor_driver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+
+add_library(cmvr_es::device::ti5_canopen_motor_driver ALIAS ti5_canopen_motor_driver)
+
+target_link_libraries(ti5_canopen_motor_driver
+        PUBLIC
+        cmvr_es::device::motor_core
+        PRIVATE
+        cmvr_es::device::canbus
+        cmvr_es::proto
+        glog
+)
+
+install(TARGETS ti5_canopen_motor_driver LIBRARY DESTINATION lib)

cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h

@@ -2,10 +2,10 @@
 // Created by lgv on 2025/8/1.
 //
 
-#include "../abstract_motor.h"
+#include "motor/abstract_motor.h"
 #include "common/base/logging/logger.h"
 #include "motor/motor_protocol_interface.h"
-#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
+#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
 #include "cmvr/config/motor_config/motor_config.pb.h"
 
 /**
@@ -15,7 +15,7 @@ namespace  cmvr {
     namespace device {
         class Ti5Motor : public AbstractMotor {
         public:
-            explicit Ti5Motor(const config::Ti5MotorConfig &config) {
+            explicit Ti5Motor(const config::MotorConfigItem &config) {
                 info_.id = config.id();
                 info_.joint_name = config.joint_name();
                 info_.limit_q_lb = config.limit_q_lb();

cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h

@@ -5,7 +5,7 @@
 #pragma once
 #include "cmvr/msgs/motor.pb.h"
 #include "motor/motor_protocol_interface.h"
-#include "../../../canbus/abstract_canbus.h"
+#include "canbus/abstract_canbus.h"
 #include "canbus/can_comm/can_receiver.h"
 #include "canbus/can_comm/can_sender.h"
 #include "canbus/can_comm/message_manager.h"
@@ -15,8 +15,8 @@
 #include "canbus/canopen/sdo_request_protocol.h"
 #include "canbus/canopen/sync_protocol.h"
 #include "canbus/canopen/nmt_request_protocol.h"
-#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h"
-#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h"
 #include <cmath>
 
 namespace cmvr {

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo1.cpp

@@ -2,7 +2,7 @@
 // Created by lgv on 2025/7/28.
 //
 
-#include  "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h"
 #include "common/base/logging/logger.h"
 
 using namespace cmvr::msgs;

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo2.cpp

@@ -2,7 +2,7 @@
 // Created by lgv on 2026/3/3.
 //
 
-#include  "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h"
 #include "common/base/logging/logger.h"
 
 using namespace cmvr::msgs;

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp

@@ -3,7 +3,7 @@
 // Created by lgv on 2025/7/24.
 //
 
-#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h"
 
 using namespace cmvr::device::motor;
 using namespace cmvr::msgs;

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo1.cpp

@@ -2,7 +2,7 @@
 // Created by lgv on 2025/7/25.
 //
 
-#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h"
 #include "common/base/logging/logger.h"
 #include "canbus/canopen/register.h"
 using namespace cmvr::msgs;

cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp

@@ -2,7 +2,7 @@
 // Created by lgv on 2025/7/25.
 //
 
-#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo2.h"
 #include "common/base/logging/logger.h"
 
 using namespace cmvr::device::motor;

cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor.cpp

@@ -0,0 +1,5 @@
+//
+// Created by lgv on 2025/8/1.
+//
+
+#include "motor/drivers/ti5_canopen/include/ti5_motor.h"

cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp

@@ -3,11 +3,11 @@
 // Created by lgv on 2025/8/1.
 //
 
-#include "ti5_motor_canopen_protocol.h"
+#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
 #include "canbus/canopen/register.h"
-#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h"
-#include "motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo2.h"
-#include "motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h"
+#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h"
 #include "canbus/canopen/nmt_response_protocol.h"
 using namespace cmvr::device;
 using namespace cmvr::msgs;

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

@@ -0,0 +1,20 @@
+add_library(motor_manager SHARED
+        src/motor_manager.cpp
+)
+
+target_include_directories(motor_manager PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(motor_manager
+        PUBLIC
+        cmvr_es::proto
+        cmvr_es::device::motor_core
+        cmvr_es::device::motor_bus_runtime
+        PRIVATE
+        cmvr_es::device::ti5_canopen_motor_driver
+        cmvr_es::device::mujoco_motor_driver
+        cmvr_es::ik_solver
+        glog
+)
+
+add_library(cmvr_es::device::motor_manager ALIAS motor_manager)
+install(TARGETS motor_manager LIBRARY DESTINATION lib)

cmvr-es/devices/motor/manager/include/motor_manager.h

@@ -0,0 +1,98 @@
+#ifndef CMVR_ES_MOTOR_MANAGER_H
+#define CMVR_ES_MOTOR_MANAGER_H
+
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_set>
+#include <unordered_map>
+#include <vector>
+
+#include "devices/abstract_device.h"
+#include "cmvr/config/motor_config/motor_config.pb.h"
+#include "motor/abstract_motor.h"
+
+namespace cmvr::msgs {
+
+class RobotDetail;
+
+} // namespace cmvr::msgs
+
+namespace cmvr::simulate {
+
+class MujocoWorld;
+
+} // namespace cmvr::simulate
+
+namespace cmvr::device {
+
+class AbstractMotorBusRuntime;
+
+class MotorManager final : public AbstractDevice,
+                           public std::enable_shared_from_this<MotorManager> {
+public:
+    MotorManager(std::string id, const config::MotorConfig& cfg);
+    ~MotorManager() override;
+
+    DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; }
+    std::string typeName() const override { return "MotorManager"; }
+
+    bool init() override;
+    bool stop() override;
+
+    bool addMotor(const std::shared_ptr<AbstractMotor>& motor);
+    std::shared_ptr<AbstractMotor> getMotor(std::uint8_t node_id) const;
+    std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const;
+    const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const;
+
+    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_(
+        const config::MotorGroupConfig& group_cfg) const;
+    std::vector<std::shared_ptr<AbstractMotor>> createMotors_(
+        const config::MotorGroupConfig& group_cfg,
+        const std::vector<config::MotorConfigItem>& motor_cfgs,
+        const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
+    std::vector<std::shared_ptr<AbstractMotor>> createCanMotors_(
+        const config::MotorGroupConfig& group_cfg,
+        const std::vector<config::MotorConfigItem>& motor_cfgs,
+        const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
+    std::vector<std::shared_ptr<AbstractMotor>> createMujocoMotors_(
+        const config::MotorGroupConfig& group_cfg,
+        const std::vector<config::MotorConfigItem>& motor_cfgs,
+        const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
+    std::vector<std::shared_ptr<AbstractMotor>> createEthercatMotors_(
+        const config::MotorGroupConfig& group_cfg,
+        const std::vector<config::MotorConfigItem>& motor_cfgs,
+        const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const;
+
+private:
+    config::MotorConfig cfg_;
+    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_;
+    std::unordered_map<std::string, std::shared_ptr<AbstractMotor>> motors_by_joint_;
+    bool initialized_{false};
+
+    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
+
+#endif // CMVR_ES_MOTOR_MANAGER_H

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

@@ -0,0 +1,552 @@
+#include "motor/manager/include/motor_manager.h"
+
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#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"
+
+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)
+{
+    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;
+
+bool MotorManager::init()
+{
+    if (initialized_) {
+        return true;
+    }
+    if (id_.empty()) {
+        CMVR_LOG(ERROR) << "[MotorManager] id is empty";
+        return false;
+    }
+
+    bus_runtimes_.clear();
+    bus_runtimes_.reserve(static_cast<std::size_t>(cfg_.motor_groups_size()));
+    {
+        std::lock_guard<std::mutex> lock(motors_mutex_);
+        motors_by_id_.clear();
+        motors_by_joint_.clear();
+    }
+
+    bool all_ok = true;
+    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;
+            continue;
+        }
+        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) {
+            continue;
+        }
+
+        if (!applyConfiguredJointLimits_(motor_group_cfg, selected_motor_cfgs)) {
+            all_ok = false;
+            continue;
+        }
+
+        auto bus_runtime = createBusRuntime_(motor_group_cfg);
+        if (!bus_runtime) {
+            all_ok = false;
+            continue;
+        }
+        if (!bus_runtime->init(motor_group_cfg)) {
+            bus_runtime->stop();
+            all_ok = false;
+            continue;
+        }
+        if (!bus_runtime->start()) {
+            bus_runtime->stop();
+            all_ok = false;
+            continue;
+        }
+
+        auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime);
+        if (motors.empty()) {
+            CMVR_LOG(ERROR) << "[MotorManager] motor group created no motors: " << group_name;
+            bus_runtime->stop();
+            all_ok = false;
+            continue;
+        }
+        bool group_ok = true;
+        for (auto& motor : motors) {
+            if (!addMotor(motor)) {
+                group_ok = false;
+                break;
+            }
+        }
+        if (!group_ok) {
+            bus_runtime->stop();
+            all_ok = false;
+            continue;
+        }
+
+        bus_runtimes_.push_back(std::move(bus_runtime));
+    }
+
+    if (!all_ok) {
+        for (auto& bus_runtime : bus_runtimes_) {
+            if (bus_runtime) {
+                bus_runtime->stop();
+            }
+        }
+        CMVR_LOG(ERROR) << "[MotorManager] motor initialization failed: " << id_;
+        return false;
+    }
+
+    initialized_ = true;
+    {
+        std::lock_guard<std::mutex> lock(registry_mutex_);
+        managers_[id_] = shared_from_this();
+        mujoco_world_registry_.erase(id_);
+        for (const auto& bus_runtime : bus_runtimes_) {
+            if (!bus_runtime) {
+                continue;
+            }
+            auto world = bus_runtime->mujocoWorld();
+            if (world) {
+                mujoco_world_registry_[id_] = std::move(world);
+                break;
+            }
+        }
+    }
+    CMVR_LOG(INFO) << "[MotorManager] Initialized motor manager: " << id_;
+    return true;
+}
+
+bool MotorManager::stop()
+{
+    {
+        std::lock_guard<std::mutex> lock(motors_mutex_);
+        for (const auto& [joint_name, motor] : motors_by_joint_) {
+            (void)joint_name;
+            if (motor) {
+                motor->torqueOff();
+            }
+        }
+    }
+    for (auto& bus_runtime : bus_runtimes_) {
+        if (bus_runtime) {
+            bus_runtime->stop();
+        }
+    }
+    return true;
+}
+
+bool MotorManager::addMotor(const std::shared_ptr<AbstractMotor>& motor)
+{
+    if (!motor) {
+        CMVR_LOG(ERROR) << "[MotorManager] Cannot register null motor";
+        return false;
+    }
+    if (motor->jointName().empty()) {
+        CMVR_LOG(ERROR) << "[MotorManager] Cannot register motor with empty joint name";
+        return false;
+    }
+
+    std::lock_guard<std::mutex> lock(motors_mutex_);
+    const auto node_id = motor->id();
+    const auto& joint_name = motor->jointName();
+    if (motors_by_id_.count(node_id) > 0) {
+        CMVR_LOG(ERROR) << "[MotorManager] Duplicate motor id: "
+                        << static_cast<int>(node_id);
+        return false;
+    }
+    if (motors_by_joint_.count(joint_name) > 0) {
+        CMVR_LOG(ERROR) << "[MotorManager] Duplicate motor joint: " << joint_name;
+        return false;
+    }
+    motors_by_id_[node_id] = motor;
+    motors_by_joint_[joint_name] = motor;
+    return true;
+}
+
+std::shared_ptr<AbstractMotor> MotorManager::getMotor(const std::uint8_t node_id) const
+{
+    std::lock_guard<std::mutex> lock(motors_mutex_);
+    const auto it = motors_by_id_.find(node_id);
+    if (it == motors_by_id_.end()) {
+        return nullptr;
+    }
+    return it->second;
+}
+
+std::shared_ptr<AbstractMotor> MotorManager::getMotor(const std::string& joint_name) const
+{
+    std::lock_guard<std::mutex> lock(motors_mutex_);
+    const auto it = motors_by_joint_.find(joint_name);
+    if (it == motors_by_joint_.end()) {
+        return nullptr;
+    }
+    return it->second;
+}
+
+const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& MotorManager::motorsMap() const
+{
+    return motors_by_joint_;
+}
+
+std::shared_ptr<MotorManager> MotorManager::managerFor(const std::string& id)
+{
+    std::lock_guard<std::mutex> lock(registry_mutex_);
+    const auto it = managers_.find(id);
+    if (it == managers_.end()) {
+        return nullptr;
+    }
+    return it->second.lock();
+}
+
+std::shared_ptr<simulate::MujocoWorld> MotorManager::mujocoWorldFor(const std::string& id)
+{
+    std::lock_guard<std::mutex> lock(registry_mutex_);
+    const auto it = mujoco_world_registry_.find(id);
+    if (it == mujoco_world_registry_.end()) {
+        return nullptr;
+    }
+    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
+{
+    if (selected.empty() || !group_cfg.has_joint_limits()) {
+        return true;
+    }
+
+    const auto& limits_cfg = group_cfg.joint_limits();
+    if (!limits_cfg.enable()) {
+        if (group_cfg.bus_type() == config::MOTOR_BUS_MUJOCO) {
+            constexpr double kDisabledLimit = 1e6;
+            for (auto& motor_cfg : selected) {
+                motor_cfg.set_limit_q_lb(-kDisabledLimit);
+                motor_cfg.set_limit_q_ub(kDisabledLimit);
+                motor_cfg.set_limit_qd(kDisabledLimit);
+                motor_cfg.set_limit_qdd(kDisabledLimit);
+            }
+        }
+        return true;
+    }
+
+    std::unordered_map<std::string, config::JointLimitConfig> limits_by_joint;
+    if (limits_cfg.source() == config::JOINT_LIMIT_SOURCE_CUSTOM) {
+        limits_by_joint.reserve(static_cast<std::size_t>(limits_cfg.joints_size()));
+        for (const auto& limit : limits_cfg.joints()) {
+            if (!limit.joint_name().empty()) {
+                limits_by_joint[limit.joint_name()] = limit;
+            }
+        }
+    } else if (limits_cfg.source() == config::JOINT_LIMIT_SOURCE_URDF) {
+        const auto urdf_path =
+            cmvr::ConfigHelper::resolveResourceFile(limits_cfg.urdf_path());
+        if (urdf_path.empty()) {
+            CMVR_LOG(ERROR) << "[MotorManager] joint_limits.urdf_path is empty for motor group: "
+                            << group_cfg.id();
+            return false;
+        }
+
+        UrdfParser parser;
+        std::string error;
+        if (!parser.loadModel(urdf_path, &error)) {
+            CMVR_LOG(ERROR) << "[MotorManager] Failed to load joint limit URDF: " << error;
+            return false;
+        }
+        const auto& model = parser.model();
+        for (pinocchio::JointIndex joint_id = 1; joint_id < model.njoints; ++joint_id) {
+            if (model.nqs[joint_id] != 1 || model.nvs[joint_id] != 1) {
+                continue;
+            }
+            config::JointLimitConfig limit;
+            limit.set_joint_name(model.names[joint_id]);
+            limit.set_q_lb(model.lowerPositionLimit[model.idx_qs[joint_id]]);
+            limit.set_q_ub(model.upperPositionLimit[model.idx_qs[joint_id]]);
+            limit.set_qd(model.velocityLimit[model.idx_vs[joint_id]]);
+            limits_by_joint[limit.joint_name()] = limit;
+        }
+    } else {
+        CMVR_LOG(ERROR) << "[MotorManager] unsupported joint limit source";
+        return false;
+    }
+
+    for (auto& motor_cfg : selected) {
+        const auto it = limits_by_joint.find(motor_cfg.joint_name());
+        if (it == limits_by_joint.end()) {
+            CMVR_LOG(ERROR) << "[MotorManager] missing joint limit for " << motor_cfg.joint_name()
+                            << " in motor group: " << group_cfg.id();
+            return false;
+        }
+        const auto& limit = it->second;
+        if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
+            !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
+            limit.qd() <= 0.0 || !std::isfinite(limit.qdd()) ||
+            limit.qdd() < 0.0) {
+            CMVR_LOG(ERROR) << "[MotorManager] invalid joint limit for " << motor_cfg.joint_name();
+            return false;
+        }
+        motor_cfg.set_limit_q_lb(limit.q_lb());
+        motor_cfg.set_limit_q_ub(limit.q_ub());
+        motor_cfg.set_limit_qd(std::abs(limit.qd()));
+        if (limit.qdd() > 0.0) {
+            motor_cfg.set_limit_qdd(std::abs(limit.qdd()));
+        }
+    }
+    return true;
+}
+
+std::shared_ptr<AbstractMotorBusRuntime> MotorManager::createBusRuntime_(
+    const config::MotorGroupConfig& group_cfg) const
+{
+    switch (group_cfg.bus_type()) {
+        case config::MOTOR_BUS_CAN:
+            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>();
+        default:
+            CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
+                            << config::MotorBusType_Name(group_cfg.bus_type())
+                            << ", group=" << group_cfg.id();
+            return nullptr;
+    }
+}
+
+std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createMotors_(
+    const config::MotorGroupConfig& group_cfg,
+    const std::vector<config::MotorConfigItem>& motor_cfgs,
+    const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const
+{
+    if (group_cfg.vendor() == config::MOTOR_VENDOR_UNKNOWN ||
+        group_cfg.protocol() == config::MOTOR_PROTOCOL_UNKNOWN) {
+        CMVR_LOG(ERROR) << "[MotorManager] missing vendor/protocol: " << group_cfg.id();
+        return {};
+    }
+
+    switch (group_cfg.bus_type()) {
+        case config::MOTOR_BUS_CAN:
+            return createCanMotors_(group_cfg, motor_cfgs, bus_runtime);
+        case config::MOTOR_BUS_MUJOCO:
+            return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime);
+        case config::MOTOR_BUS_ETHERCAT:
+            return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime);
+        default:
+            CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
+                            << config::MotorBusType_Name(group_cfg.bus_type())
+                            << ", group=" << group_cfg.id();
+            return {};
+    }
+}
+
+std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createCanMotors_(
+    const config::MotorGroupConfig& group_cfg,
+    const std::vector<config::MotorConfigItem>& motor_cfgs,
+    const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const
+{
+    auto can_bus_runtime = std::dynamic_pointer_cast<CanMotorBusRuntime>(bus_runtime);
+    if (!can_bus_runtime || !can_bus_runtime->sender() || !can_bus_runtime->messageManager()) {
+        CMVR_LOG(ERROR) << "[MotorManager] missing CAN bus runtime: " << group_cfg.id();
+        return {};
+    }
+    if (!group_cfg.has_can()) {
+        CMVR_LOG(ERROR) << "[MotorManager] missing CAN config: " << group_cfg.id();
+        return {};
+    }
+    if (group_cfg.vendor() != config::MOTOR_VENDOR_TI5 ||
+        group_cfg.protocol() != config::MOTOR_PROTOCOL_CANOPEN) {
+        CMVR_LOG(ERROR) << "[MotorManager] unsupported CAN motor: vendor="
+                        << config::MotorVendor_Name(group_cfg.vendor())
+                        << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+                        << ", group=" << group_cfg.id();
+        return {};
+    }
+
+    auto protocol = std::make_shared<Ti5MotorCanopenProtocol>(
+        can_bus_runtime->sender(), can_bus_runtime->messageManager());
+
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
+    motors.reserve(motor_cfgs.size());
+    for (const auto& cfg : motor_cfgs) {
+        auto motor = std::make_shared<Ti5Motor>(cfg);
+        motor->setProtocol(protocol);
+        if (!motor->init()) {
+            CMVR_LOG(ERROR) << "[MotorManager] failed to init TI5 motor: "
+                            << cfg.joint_name();
+            return {};
+        }
+        motors.push_back(std::move(motor));
+    }
+    return motors;
+}
+
+std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createMujocoMotors_(
+    const config::MotorGroupConfig& group_cfg,
+    const std::vector<config::MotorConfigItem>& motor_cfgs,
+    const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const
+{
+    auto mujoco_bus_runtime = std::dynamic_pointer_cast<MujocoMotorBusRuntime>(bus_runtime);
+    if (!mujoco_bus_runtime || !mujoco_bus_runtime->mujocoWorld()) {
+        CMVR_LOG(ERROR) << "[MotorManager] missing MuJoCo bus runtime: " << group_cfg.id();
+        return {};
+    }
+    if (!group_cfg.has_mujoco()) {
+        CMVR_LOG(ERROR) << "[MotorManager] missing MuJoCo config: " << group_cfg.id();
+        return {};
+    }
+    if (group_cfg.vendor() != config::MOTOR_VENDOR_MUJOCO ||
+        group_cfg.protocol() != config::MOTOR_PROTOCOL_MUJOCO) {
+        CMVR_LOG(ERROR) << "[MotorManager] unsupported MuJoCo motor: vendor="
+                        << config::MotorVendor_Name(group_cfg.vendor())
+                        << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
+                        << ", group=" << group_cfg.id();
+        return {};
+    }
+
+    std::vector<std::shared_ptr<AbstractMotor>> motors;
+    motors.reserve(motor_cfgs.size());
+    for (const auto& cfg : motor_cfgs) {
+        auto motor = std::make_shared<MujocoMotor>(
+            cfg.joint_name(),
+            mujoco_bus_runtime->mujocoWorld(),
+            static_cast<std::uint8_t>(cfg.id()));
+        motor->setLimitQ(cfg.limit_q_ub(), cfg.limit_q_lb());
+        motor->setLimitQd(cfg.limit_qd());
+        if (cfg.limit_qdd() > 0.0) {
+            motor->setLimitQdd(cfg.limit_qdd(), -cfg.limit_qdd());
+        }
+        if (!motor->init()) {
+            CMVR_LOG(ERROR) << "[MotorManager] failed to init MuJoCo motor: "
+                            << cfg.joint_name();
+            return {};
+        }
+        motors.push_back(std::move(motor));
+    }
+    return motors;
+}
+
+std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
+    const config::MotorGroupConfig& group_cfg,
+    const std::vector<config::MotorConfigItem>& motor_cfgs,
+    const std::shared_ptr<AbstractMotorBusRuntime>& bus_runtime) const
+{
+    auto ethercat_bus_runtime = std::dynamic_pointer_cast<EthercatMotorBusRuntime>(bus_runtime);
+    if (!ethercat_bus_runtime) {
+        CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT bus runtime: " << group_cfg.id();
+        return {};
+    }
+    if (!group_cfg.has_ethercat()) {
+        CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << 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 "
+                            << motor_cfg.id() << " in group: " << group_cfg.id();
+            return {};
+        }
+    }
+
+    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 {};
+}
+
+} // namespace cmvr::device

cmvr-es/devices/motor/motor_manager.h

@@ -1,57 +0,0 @@
-//
-// Created by lgv on 2025/8/1.
-//
-
-
-#pragma once
-
-#include "abstract_motor.h"
-
-#include <unordered_map>
-#include <memory>
-#include <string>
-#include <mutex>
-
-namespace cmvr {
-    namespace device {
-
-        class MotorManager {
-        public:
-            void addMotor(uint8_t node_id, std::shared_ptr<AbstractMotor> motor) {
-                std::lock_guard<std::mutex> lock(mutex_);
-                motors_[node_id] = std::move(motor);
-            }
-
-            void addMotor(std::shared_ptr<AbstractMotor> motor) {
-                std::lock_guard<std::mutex> lock(mutex_);
-                motors_map_[motor->jointName()] = std::move(motor);
-            }
-
-            std::shared_ptr<AbstractMotor> getMotor(uint8_t node_id) const {
-                auto it = motors_.find(node_id);
-                if (it != motors_.end()) {
-                    return it->second;
-                }
-                return nullptr;
-            }
-
-            std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const {
-                auto it = motors_map_.find(joint_name);
-                if (it != motors_map_.end()) {
-                    return it->second;
-                }
-                return nullptr;
-            }
-
-            const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const {
-                return motors_map_;
-            }
-
-        private:
-            mutable std::mutex mutex_;
-            std::unordered_map<uint8_t, std::shared_ptr<AbstractMotor>> motors_;
-            std::unordered_map<std::string, std::shared_ptr<AbstractMotor>> motors_map_;
-        };
-
-    } // namespace device
-} // namespace cmvr

cmvr-es/devices/motor/motor_manager_test.cpp

@@ -1,120 +0,0 @@
-#include "common/base/logging/logger.h"
-//
-// Created by lgv on 2025/8/1.
-//
-
-#include "motor/motor_manager.h"
-#include "gtest/gtest.h"
-#include "motor/ti5_motor/ti5_motor.h"
-#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
-
-
-#include "canbus/can_comm/can_sender.h"
-#include "canbus/can_comm/message_manager.h"
-#include "canbus/can_client/socket/socket_can_client_raw.h"
-
-using namespace cmvr::device;
-using namespace cmvr::msgs;
-
-TEST(MotorMangerTest,MyTest) {
-
-    uint8_t id = 22;
-    cmvr::config::SocketCanConfig can_cfg;
-    can_cfg.set_channel_id(0);
-    cmvr::config::Ti5MotorConfig motor_cfg;
-    motor_cfg.set_id(id);
-    motor_cfg.set_joint_name("test_motor");
-    motor_cfg.set_limit_q_lb(-3.14);
-    motor_cfg.set_limit_q_ub(3.14);
-    motor_cfg.set_limit_qd(3.0);
-
-    // 1 === 初始化公共组件 ===
-    auto can_client = std::make_shared<SocketCanClientRaw>(can_cfg);
-    auto sender = std::make_shared<CanSender<RobotDetail>>();
-    auto receiver = std::make_shared<CanReceiver<RobotDetail>>();
-    auto message_manager = std::make_shared<MessageManager<RobotDetail>>();
-
-
-    can_client->init();
-    auto ret = sender->Init(can_client.get(), false);
-    if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "Failed to init can sender.";
-    }
-
-
-    ret = receiver->Init(can_client.get(), message_manager.get(), false);
-    if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "Failed to init can receiver.";
-    }
-
-    // 2 == 创建协议 ===
-    auto canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(sender, message_manager);
-
-    // 3 === 创建电机 ===
-    auto motor = std::make_shared<Ti5Motor>(motor_cfg);
-    motor->setProtocol(canopen_protocol);
-
-    // 4 === 添加电机到  MotorManager===
-    auto manager = std::make_shared<MotorManager>();
-    manager->addMotor(id,motor);
-
-
-    // 5 ===  启动通讯 ===
-    can_client->start();
-    ret = sender->Start();
-    if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "Failed to start can sender.";
-    }
-
-    ret = receiver->Start();
-    if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "Failed to start can receiver.";
-    }
-
-    // 6 ===  控制电机 ===
-    auto motor_3 = manager->getMotor(id);
-    motor_3->init();
-
-    // motor_3->calibrateZeroQ();
-
-
-
-
-    // motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY);
-    // motor_3->setTarget(-0.5);
-    // std::this_thread::sleep_for(std::chrono::milliseconds(5000));
-    motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
-    // motor_3->setTarget(0,1.6);
-
-    // std::this_thread::sleep_for(std::chrono::milliseconds(5000));
-    //
-    // motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
-    // motor_3->setTarget(0,1.6);
-    // motor_3->calibrateZeroQ();
-    // motor_3->setMode(RUN_MODE_PROFILE_POSITION);
-    // motor_3->setLimitQ(30.14,-40.14);
-    // motor_3->setQ(-30);
-    // std::this_thread::sleep_for(std::chrono::milliseconds(5000));
-    // motor_3->brake();
-    // std::this_thread::sleep_for(std::chrono::milliseconds(5000));
-    // motor_3->setQ(-30);
-    // motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
-    // std::this_thread::sleep_for(std::chrono::milliseconds(5000));
-    // motor_3->setQ(-30.14);
-    // manager->init(id);
-    // manager->setMode(id,RUN_MODE_CYCLIC_SYNC_POSITION);
-    // manager->setQ(id,3.14);
-
-    while (true) {
-
-        auto mode = motor_3->getMode();
-        auto q = motor_3->getQ();
-        auto qd = motor_3->getQd();
-        std::cout << q << ", " << qd << std::endl;
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-        // motor_3->getQ(q);
-
-    }
-
-    CMVR_LOG(INFO) << "Testing MotorManger";
-}

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

@@ -1,19 +0,0 @@
-add_library(motor_system SHARED
-        src/motor_system.cpp
-)
-
-target_include_directories(motor_system PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-target_link_libraries(motor_system
-        PUBLIC
-        cmvr_es::proto
-        PRIVATE
-        cmvr_es::device::canbus
-        cmvr_es::device::ti5motor
-        cmvr_es::device::mujoco_motor
-        cmvr_es::ik_solver
-        glog
-)
-
-add_library(cmvr_es::device::motor_system ALIAS motor_system)
-install(TARGETS motor_system LIBRARY DESTINATION lib)

cmvr-es/devices/motor/motor_system/include/motor_system.h

@@ -1,81 +0,0 @@
-#ifndef CMVR_ES_MOTOR_SYSTEM_H
-#define CMVR_ES_MOTOR_SYSTEM_H
-
-#include <memory>
-#include <mutex>
-#include <string>
-#include <unordered_set>
-#include <unordered_map>
-#include <vector>
-
-#include "devices/abstract_device.h"
-#include "devices/canbus/abstract_canbus.h"
-#include "canbus/can_comm/can_receiver.h"
-#include "canbus/can_comm/can_sender.h"
-#include "canbus/can_comm/message_manager.h"
-#include "cmvr/config/motor_config/motor_config.pb.h"
-#include "cmvr/msgs/robot_detail.pb.h"
-#include "motor/motor_manager.h"
-
-namespace cmvr::device {
-
-class MujocoJointBridge;
-
-class MotorSystem final : public AbstractDevice {
-public:
-    MotorSystem(std::string id, const config::MotorConfig& cfg);
-    ~MotorSystem() override = default;
-
-    DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; }
-    std::string typeName() const override { return "MotorSystem"; }
-
-    bool init() override;
-    bool stop() override;
-
-    std::shared_ptr<MotorManager> manager() const { return motor_manager_; }
-    static std::shared_ptr<MotorManager> managerFor(const std::string& id);
-    static std::shared_ptr<MujocoJointBridge> mujocoBridgeFor(const std::string& id);
-    static void setActiveJoints(const std::string& motor_system_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>>;
-
-    struct MotorGroupRuntime {
-        std::string name;
-        config::MotorBusType bus_type{config::MOTOR_BUS_UNKNOWN};
-        bool enabled{false};
-        std::shared_ptr<AbstractCanbus> client;
-        std::shared_ptr<CanSender<msgs::RobotDetail>> sender;
-        std::shared_ptr<CanReceiver<msgs::RobotDetail>> receiver;
-        std::shared_ptr<MessageManager<msgs::RobotDetail>> message_manager;
-        std::shared_ptr<MujocoJointBridge> mujoco_bridge;
-        std::vector<config::Ti5MotorConfig> motor_cfgs;
-    };
-
-    bool initMotorGroup_(MotorGroupRuntime& group);
-    bool initMotors_(const MotorGroupRuntime& group);
-    bool selectActiveMotors_(const std::string& group_name,
-                             const google::protobuf::RepeatedPtrField<config::Ti5MotorConfig>& source,
-                             std::vector<config::Ti5MotorConfig>& selected) const;
-    bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
-                                     std::vector<config::Ti5MotorConfig>& selected) const;
-    static bool makeSocketCanConfig_(const config::MotorGroupConfig& group_cfg,
-                                     config::SocketCanConfig& config);
-
-private:
-    config::MotorConfig cfg_;
-    std::vector<MotorGroupRuntime> motor_groups_;
-    std::shared_ptr<MotorManager> motor_manager_{nullptr};
-    bool initialized_{false};
-
-    static std::mutex registry_mutex_;
-    static std::unordered_map<std::string, std::weak_ptr<MotorManager>> registry_;
-    static std::unordered_map<std::string, std::weak_ptr<MujocoJointBridge>> mujoco_bridge_registry_;
-    static std::unordered_map<std::string, ActiveJointSelection> active_joints_;
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_MOTOR_SYSTEM_H

cmvr-es/devices/motor/motor_system/src/motor_system.cpp

@@ -1,411 +0,0 @@
-#include "motor/motor_system/include/motor_system.h"
-
-#include <cmath>
-#include <cstdint>
-#include <future>
-#include <stdexcept>
-#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 "devices/motor/mujoco_motor/include/mujoco_motor.h"
-#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
-#include "motor/ti5_motor/ti5_motor.h"
-
-namespace cmvr::device {
-
-std::mutex MotorSystem::registry_mutex_;
-std::unordered_map<std::string, std::weak_ptr<MotorManager>> MotorSystem::registry_;
-std::unordered_map<std::string, std::weak_ptr<MujocoJointBridge>> MotorSystem::mujoco_bridge_registry_;
-std::unordered_map<std::string, MotorSystem::ActiveJointSelection> MotorSystem::active_joints_;
-
-MotorSystem::MotorSystem(std::string id, const config::MotorConfig& cfg)
-    : cfg_(cfg)
-{
-    id_ = std::move(id);
-    if (!cfg_.id().empty() && cfg_.id() != id_) {
-        CMVR_LOG(ERROR) << "[MotorSystem] config id '" << cfg_.id()
-                        << "' does not match device id '" << id_ << "'";
-        id_.clear();
-    }
-}
-
-bool MotorSystem::init()
-{
-    if (initialized_) {
-        return true;
-    }
-
-    motor_manager_ = std::make_shared<MotorManager>();
-    if (id_.empty()) {
-        CMVR_LOG(ERROR) << "[MotorSystem] id is empty";
-        return false;
-    }
-    motor_groups_.clear();
-    motor_groups_.reserve(cfg_.motor_groups_size());
-
-    bool all_ok = true;
-    for (const auto& motor_group_cfg : cfg_.motor_groups()) {
-        MotorGroupRuntime group;
-        group.name = motor_group_cfg.id();
-        if (group.name.empty()) {
-            CMVR_LOG(ERROR) << "[MotorSystem] motor group id is empty in system: " << id_;
-            continue;
-        }
-        group.bus_type = motor_group_cfg.bus_type();
-        group.enabled = selectActiveMotors_(group.name, motor_group_cfg.motors(), group.motor_cfgs);
-        const bool selected_active_group = group.enabled;
-        if (group.enabled && !applyConfiguredJointLimits_(motor_group_cfg, group.motor_cfgs)) {
-            group.enabled = false;
-        }
-        if (group.enabled) {
-            switch (group.bus_type) {
-                case config::MOTOR_BUS_CAN: {
-                    config::SocketCanConfig can_config;
-                    if (!makeSocketCanConfig_(motor_group_cfg, can_config)) {
-                        group.enabled = false;
-                        break;
-                    }
-                    group.client = std::make_shared<SocketCanClientRaw>(can_config);
-                    group.sender = std::make_shared<CanSender<msgs::RobotDetail>>();
-                    group.receiver = std::make_shared<CanReceiver<msgs::RobotDetail>>();
-                    group.message_manager = std::make_shared<MessageManager<msgs::RobotDetail>>();
-                    break;
-                }
-                case config::MOTOR_BUS_MUJOCO:
-                    group.mujoco_bridge = std::make_shared<MujocoJointBridge>(group.motor_cfgs.size());
-                    break;
-                case config::MOTOR_BUS_ETHERCAT:
-                    CMVR_LOG(ERROR) << "[MotorSystem] EtherCAT motor group is configured but runtime is not implemented: " << group.name;
-                    group.enabled = false;
-                    break;
-                case config::MOTOR_BUS_UNKNOWN:
-                default:
-                    CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor bus type for group: " << group.name;
-                    group.enabled = false;
-                    break;
-            }
-        }
-        if (selected_active_group && !group.enabled) {
-            all_ok = false;
-        }
-        motor_groups_.emplace_back(std::move(group));
-    }
-
-    std::vector<std::future<bool>> tasks;
-    for (auto& group : motor_groups_) {
-        if (!group.enabled) {
-            continue;
-        }
-        if (group.bus_type != config::MOTOR_BUS_MUJOCO) {
-            if (!initMotorGroup_(group)) {
-                group.enabled = false;
-                all_ok = false;
-                continue;
-            }
-        }
-        tasks.push_back(std::async(std::launch::async, [this, &group]() {
-            return initMotors_(group);
-        }));
-    }
-
-    for (auto& task : tasks) {
-        all_ok = task.get() && all_ok;
-    }
-    if (!all_ok) {
-        CMVR_LOG(ERROR) << "[MotorSystem] motor initialization failed: " << id_;
-        return false;
-    }
-
-    initialized_ = true;
-    {
-        std::lock_guard<std::mutex> lock(registry_mutex_);
-        registry_[id_] = motor_manager_;
-        for (const auto& group : motor_groups_) {
-            if (group.mujoco_bridge) {
-                mujoco_bridge_registry_[id_] = group.mujoco_bridge;
-                break;
-            }
-        }
-    }
-    CMVR_LOG(INFO) << "[MotorSystem] Initialized motor system: " << id_;
-    return true;
-}
-
-bool MotorSystem::stop()
-{
-    if (!motor_manager_) {
-        return true;
-    }
-    for (const auto& [joint_name, motor] : motor_manager_->motorsMap()) {
-        if (motor) {
-            motor->torqueOff();
-        }
-    }
-    return true;
-}
-
-std::shared_ptr<MotorManager> MotorSystem::managerFor(const std::string& id)
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    const auto it = registry_.find(id);
-    if (it == registry_.end()) {
-        return nullptr;
-    }
-    return it->second.lock();
-}
-
-std::shared_ptr<MujocoJointBridge> MotorSystem::mujocoBridgeFor(const std::string& id)
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    const auto it = mujoco_bridge_registry_.find(id);
-    if (it == mujoco_bridge_registry_.end()) {
-        return nullptr;
-    }
-    return it->second.lock();
-}
-
-void MotorSystem::setActiveJoints(const std::string& motor_system_id,
-                                  ActiveJointSelection group_joints)
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    active_joints_[motor_system_id] = std::move(group_joints);
-}
-
-void MotorSystem::clearActiveJoints()
-{
-    std::lock_guard<std::mutex> lock(registry_mutex_);
-    active_joints_.clear();
-}
-
-bool MotorSystem::selectActiveMotors_(
-    const std::string& group_name,
-    const google::protobuf::RepeatedPtrField<config::Ti5MotorConfig>& source,
-    std::vector<config::Ti5MotorConfig>& 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) << "[MotorSystem] No active joints selected for motor system " << 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) << "[MotorSystem] active joint '" << joint_name
-                                << "' not found in motor group '" << group_name << "'";
-                return false;
-            }
-        }
-    }
-    return !selected.empty();
-}
-
-bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
-                                              std::vector<config::Ti5MotorConfig>& selected) const
-{
-    if (selected.empty() || !group_cfg.has_joint_limits()) {
-        return true;
-    }
-
-    const auto& limits_cfg = group_cfg.joint_limits();
-    const auto source = limits_cfg.source();
-    if (source == config::JOINT_LIMIT_SOURCE_UNKNOWN) {
-        return true;
-    }
-
-    std::unordered_map<std::string, config::JointLimitConfig> limits_by_joint;
-    if (source == config::JOINT_LIMIT_SOURCE_CUSTOM) {
-        limits_by_joint.reserve(static_cast<std::size_t>(limits_cfg.joints_size()));
-        for (const auto& limit : limits_cfg.joints()) {
-            if (!limit.joint_name().empty()) {
-                limits_by_joint[limit.joint_name()] = limit;
-            }
-        }
-    } else if (source == config::JOINT_LIMIT_SOURCE_URDF) {
-        const auto urdf_path =
-            cmvr::ConfigHelper::resolveResourceFile(limits_cfg.urdf_path());
-        if (urdf_path.empty()) {
-            CMVR_LOG(ERROR) << "[MotorSystem] joint_limits.urdf_path is empty for motor group: "
-                            << group_cfg.id();
-            return false;
-        }
-
-        UrdfParser parser;
-        std::string error;
-        if (!parser.loadModel(urdf_path, &error)) {
-            CMVR_LOG(ERROR) << "[MotorSystem] Failed to load joint limit URDF: " << error;
-            return false;
-        }
-        const auto& model = parser.model();
-        for (pinocchio::JointIndex joint_id = 1; joint_id < model.njoints; ++joint_id) {
-            if (model.nqs[joint_id] != 1 || model.nvs[joint_id] != 1) {
-                continue;
-            }
-            config::JointLimitConfig limit;
-            limit.set_joint_name(model.names[joint_id]);
-            limit.set_q_lb(model.lowerPositionLimit[model.idx_qs[joint_id]]);
-            limit.set_q_ub(model.upperPositionLimit[model.idx_qs[joint_id]]);
-            limit.set_qd(model.velocityLimit[model.idx_vs[joint_id]]);
-            limits_by_joint[limit.joint_name()] = limit;
-        }
-    } else {
-        CMVR_LOG(ERROR) << "[MotorSystem] unsupported joint limit source";
-        return false;
-    }
-
-    for (auto& motor_cfg : selected) {
-        const auto it = limits_by_joint.find(motor_cfg.joint_name());
-        if (it == limits_by_joint.end()) {
-            CMVR_LOG(ERROR) << "[MotorSystem] missing joint limit for " << motor_cfg.joint_name()
-                            << " in motor group: " << group_cfg.id();
-            return false;
-        }
-        const auto& limit = it->second;
-        if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
-            !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
-            limit.qd() <= 0.0 || !std::isfinite(limit.qdd()) ||
-            limit.qdd() < 0.0) {
-            CMVR_LOG(ERROR) << "[MotorSystem] invalid joint limit for " << motor_cfg.joint_name();
-            return false;
-        }
-        motor_cfg.set_limit_q_lb(limit.q_lb());
-        motor_cfg.set_limit_q_ub(limit.q_ub());
-        motor_cfg.set_limit_qd(std::abs(limit.qd()));
-        if (limit.qdd() > 0.0) {
-            motor_cfg.set_limit_qdd(std::abs(limit.qdd()));
-        }
-    }
-    return true;
-}
-
-bool MotorSystem::makeSocketCanConfig_(const config::MotorGroupConfig& group_cfg,
-                                       config::SocketCanConfig& config)
-{
-    if (!group_cfg.has_can()) {
-        CMVR_LOG(ERROR) << "[MotorSystem] CAN motor group missing can config: " << group_cfg.id();
-        return false;
-    }
-    config = group_cfg.can();
-    return true;
-}
-
-bool MotorSystem::initMotorGroup_(MotorGroupRuntime& group)
-{
-    if (group.bus_type != config::MOTOR_BUS_CAN) {
-        CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor group runtime: " << group.name;
-        return false;
-    }
-    if (!group.client || !group.sender || !group.receiver || !group.message_manager) {
-        CMVR_LOG(ERROR) << "[MotorSystem] Invalid motor group runtime: " << group.name;
-        return false;
-    }
-
-    if (!group.client->init()) {
-        CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN client: " << group.name;
-        return false;
-    }
-
-    auto ret = group.sender->Init(group.client.get(), false);
-    if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN sender: " << group.name;
-        return false;
-    }
-
-    ret = group.receiver->Init(group.client.get(), group.message_manager.get(), false);
-    if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[MotorSystem] Failed to init CAN receiver: " << group.name;
-        return false;
-    }
-
-    if (!group.client->start()) {
-        CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN client: " << group.name;
-        return false;
-    }
-
-    ret = group.sender->Start();
-    if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN sender: " << group.name;
-        return false;
-    }
-
-    ret = group.receiver->Start();
-    if (ret != ErrorCode::OK) {
-        CMVR_LOG(ERROR) << "[MotorSystem] Failed to start CAN receiver: " << group.name;
-        return false;
-    }
-    return true;
-}
-
-bool MotorSystem::initMotors_(const MotorGroupRuntime& group)
-{
-    switch (group.bus_type) {
-        case config::MOTOR_BUS_CAN: {
-            auto protocol = std::make_shared<Ti5MotorCanopenProtocol>(group.sender, group.message_manager);
-            for (const auto& cfg : group.motor_cfgs) {
-                auto motor = std::make_shared<Ti5Motor>(cfg);
-                motor->setProtocol(protocol);
-                if (!motor->init()) {
-                    CMVR_LOG(ERROR) << "[MotorSystem] Failed to init motor: " << cfg.joint_name();
-                    return false;
-                }
-                motor_manager_->addMotor(motor);
-            }
-            return true;
-        }
-        case config::MOTOR_BUS_MUJOCO: {
-            if (!group.mujoco_bridge) {
-                CMVR_LOG(ERROR) << "[MotorSystem] MuJoCo motor group missing joint bridge: " << group.name;
-                return false;
-            }
-            for (std::size_t i = 0; i < group.motor_cfgs.size(); ++i) {
-                const auto& cfg = group.motor_cfgs[i];
-                auto motor = std::make_shared<MujocoMotor>(
-                    i,
-                    cfg.joint_name(),
-                    group.mujoco_bridge,
-                    static_cast<std::uint8_t>(cfg.id()));
-                motor->setLimitQ(cfg.limit_q_ub(), cfg.limit_q_lb());
-                motor->setLimitQd(cfg.limit_qd());
-                motor_manager_->addMotor(motor);
-            }
-            return true;
-        }
-        case config::MOTOR_BUS_ETHERCAT:
-        case config::MOTOR_BUS_UNKNOWN:
-        default:
-            CMVR_LOG(ERROR) << "[MotorSystem] Unsupported motor init runtime: " << group.name;
-            return false;
-    }
-}
-
-} // namespace cmvr::device

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

@@ -1,13 +0,0 @@
-add_library(mujoco_motor SHARED
-        src/mujoco_motor.cpp
-)
-
-target_include_directories(mujoco_motor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-target_link_libraries(mujoco_motor
-        PUBLIC
-        cmvr_es::proto
-)
-
-add_library(cmvr_es::device::mujoco_motor ALIAS mujoco_motor)
-install(TARGETS mujoco_motor LIBRARY DESTINATION lib)

cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h

@@ -1,57 +0,0 @@
-#ifndef CMVR_ES_MUJOCO_JOINT_BRIDGE_H
-#define CMVR_ES_MUJOCO_JOINT_BRIDGE_H
-
-#include <chrono>
-#include <condition_variable>
-#include <cstddef>
-#include <mutex>
-#include <vector>
-
-#include "cmvr/msgs/motor.pb.h"
-
-namespace cmvr::device {
-
-class MujocoJointBridge {
-public:
-    struct CommandState {
-        std::vector<msgs::RunMode> mode;
-        std::vector<double> position;
-        std::vector<double> velocity;
-    };
-
-    explicit MujocoJointBridge(std::size_t dof);
-
-    std::size_t dof() const noexcept { return dof_; }
-
-    void setMode(std::size_t index, msgs::RunMode mode);
-    msgs::RunMode getMode(std::size_t index) const;
-
-    void setPositionTarget(std::size_t index, double position, double velocity);
-    void setVelocityTarget(std::size_t index, double velocity);
-
-    CommandState commands() const;
-
-    void publishMeasured(const std::vector<double>& position,
-                         const std::vector<double>& velocity);
-    double position(std::size_t index) const;
-    double velocity(std::size_t index) const;
-
-    void markReady(bool valid);
-    bool waitUntilReady(std::chrono::milliseconds timeout);
-
-private:
-    void validateIndex_(std::size_t index) const;
-
-    const std::size_t dof_{0};
-    mutable std::mutex mutex_;
-    std::condition_variable ready_cv_;
-    CommandState commands_;
-    std::vector<double> measured_position_;
-    std::vector<double> measured_velocity_;
-    bool ready_{false};
-    bool valid_{false};
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_MUJOCO_JOINT_BRIDGE_H

cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp

@@ -1,225 +0,0 @@
-#include "devices/motor/mujoco_motor/include/mujoco_motor.h"
-
-#include <cmath>
-#include <stdexcept>
-#include <utility>
-
-#include "common/base/logging/logger.h"
-
-namespace cmvr::device {
-
-MujocoJointBridge::MujocoJointBridge(const std::size_t dof)
-    : dof_(dof)
-{
-    if (dof_ == 0) {
-        CMVR_LOG(ERROR) << "MujocoJointBridge dof must be positive";
-        return;
-    }
-    commands_.mode.assign(dof_, msgs::RUN_MODE_UNSPECIFIED);
-    commands_.position.assign(dof_, 0.0);
-    commands_.velocity.assign(dof_, 0.0);
-    measured_position_.assign(dof_, 0.0);
-    measured_velocity_.assign(dof_, 0.0);
-}
-
-void MujocoJointBridge::validateIndex_(const std::size_t index) const
-{
-    if (index >= dof_) {
-        CMVR_LOG(ERROR) << "MujocoJointBridge joint index out of range";
-    }
-}
-
-void MujocoJointBridge::setMode(const std::size_t index, const msgs::RunMode mode)
-{
-    validateIndex_(index);
-    if (index >= dof_) {
-        return;
-    }
-    std::lock_guard<std::mutex> lock(mutex_);
-    commands_.mode[index] = mode;
-}
-
-msgs::RunMode MujocoJointBridge::getMode(const std::size_t index) const
-{
-    validateIndex_(index);
-    if (index >= dof_) {
-        return msgs::RUN_MODE_UNSPECIFIED;
-    }
-    std::lock_guard<std::mutex> lock(mutex_);
-    return commands_.mode[index];
-}
-
-void MujocoJointBridge::setPositionTarget(const std::size_t index,
-                                          const double position,
-                                          const double velocity)
-{
-    validateIndex_(index);
-    if (index >= dof_) {
-        return;
-    }
-    std::lock_guard<std::mutex> lock(mutex_);
-    commands_.position[index] = position;
-    commands_.velocity[index] = velocity;
-}
-
-void MujocoJointBridge::setVelocityTarget(const std::size_t index, const double velocity)
-{
-    validateIndex_(index);
-    if (index >= dof_) {
-        return;
-    }
-    std::lock_guard<std::mutex> lock(mutex_);
-    commands_.velocity[index] = velocity;
-}
-
-MujocoJointBridge::CommandState MujocoJointBridge::commands() const
-{
-    std::lock_guard<std::mutex> lock(mutex_);
-    return commands_;
-}
-
-void MujocoJointBridge::publishMeasured(const std::vector<double>& position,
-                                        const std::vector<double>& velocity)
-{
-    if (position.size() != dof_ || velocity.size() != dof_) {
-        CMVR_LOG(ERROR) << "MujocoJointBridge measured vectors size mismatch";
-        return;
-    }
-    std::lock_guard<std::mutex> lock(mutex_);
-    measured_position_ = position;
-    measured_velocity_ = velocity;
-}
-
-double MujocoJointBridge::position(const std::size_t index) const
-{
-    validateIndex_(index);
-    if (index >= dof_) {
-        return 0.0;
-    }
-    std::lock_guard<std::mutex> lock(mutex_);
-    return measured_position_[index];
-}
-
-double MujocoJointBridge::velocity(const std::size_t index) const
-{
-    validateIndex_(index);
-    if (index >= dof_) {
-        return 0.0;
-    }
-    std::lock_guard<std::mutex> lock(mutex_);
-    return measured_velocity_[index];
-}
-
-void MujocoJointBridge::markReady(const bool valid)
-{
-    {
-        std::lock_guard<std::mutex> lock(mutex_);
-        ready_ = true;
-        valid_ = valid;
-    }
-    ready_cv_.notify_all();
-}
-
-bool MujocoJointBridge::waitUntilReady(const std::chrono::milliseconds timeout)
-{
-    std::unique_lock<std::mutex> lock(mutex_);
-    return ready_cv_.wait_for(lock, timeout, [this] { return ready_; }) && valid_;
-}
-
-MujocoMotor::MujocoMotor(const std::size_t joint_index,
-                         std::string joint_name,
-                         std::shared_ptr<MujocoJointBridge> bridge,
-                         const std::uint8_t node_id)
-    : AbstractMotor(node_id == 0 ? static_cast<std::uint8_t>(joint_index + 1) : node_id),
-      joint_index_(joint_index),
-      bridge_(std::move(bridge))
-{
-    if (!bridge_) {
-        CMVR_LOG(ERROR) << "MujocoMotor bridge is null";
-        return;
-    }
-    if (joint_index_ >= bridge_->dof()) {
-        CMVR_LOG(ERROR) << "MujocoMotor joint index out of range";
-        bridge_.reset();
-        return;
-    }
-
-    info_.id = id();
-    info_.joint_name = std::move(joint_name);
-    id_ = info_.joint_name;
-}
-
-void MujocoMotor::setMode(const msgs::RunMode mode)
-{
-    bridge_->setMode(joint_index_, mode);
-}
-
-msgs::RunMode MujocoMotor::getMode()
-{
-    return bridge_->getMode(joint_index_);
-}
-
-void MujocoMotor::setLimitQ(const double ub, const double lb)
-{
-    info_.limit_q_ub = ub;
-    info_.limit_q_lb = lb;
-}
-
-void MujocoMotor::setLimitQd(const double qd)
-{
-    info_.limit_qd = qd;
-}
-
-void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd)
-{
-    limit_qdd_upper_ = u_qdd;
-    limit_qdd_lower_ = l_qdd;
-}
-
-void MujocoMotor::brake()
-{
-    bridge_->setMode(joint_index_, msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
-    bridge_->setPositionTarget(joint_index_, bridge_->position(joint_index_), 0.0);
-}
-
-void MujocoMotor::setQ(const double q)
-{
-    bridge_->setPositionTarget(joint_index_, q, 0.0);
-}
-
-void MujocoMotor::setTarget(const double q, const double qd)
-{
-    bridge_->setPositionTarget(joint_index_, q, qd);
-}
-
-void MujocoMotor::setTarget(const double qd)
-{
-    bridge_->setVelocityTarget(joint_index_, qd);
-}
-
-bool MujocoMotor::calibrateZeroQ()
-{
-    return true;
-}
-
-bool MujocoMotor::reachedTargetQ()
-{
-    return true;
-}
-
-void MujocoMotor::setQd(const double qd)
-{
-    bridge_->setVelocityTarget(joint_index_, qd);
-}
-
-double MujocoMotor::getQ()
-{
-    return bridge_->position(joint_index_);
-}
-
-double MujocoMotor::getQd()
-{
-    return bridge_->velocity(joint_index_);
-}
-
-} // namespace cmvr::device

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

@@ -1,24 +0,0 @@
-add_library(ti5motor SHARED
-        ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_sdo_response.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_tpdo1.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_tpdo2.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_rpdo1.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/canopen/protocol/ti5_motor_rpdo2.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/canopen/ti5_motor_canopen_protocol.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/ti5_motor.cpp
-
-)
-
-
-target_include_directories(ti5motor PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-add_library(cmvr_es::device::ti5motor ALIAS ti5motor)
-
-target_link_libraries(ti5motor
-        PRIVATE
-        cmvr_es::device::canbus
-        cmvr_es::proto
-        glog
-)
-
-install(TARGETS ti5motor LIBRARY DESTINATION lib)

cmvr-es/devices/motor/ti5_motor/ti5_motor.cpp

@@ -1,5 +0,0 @@
-//
-// Created by lgv on 2025/8/1.
-//
-
-#include "ti5_motor.h"

cmvr-es/manager/device_manager/CMakeLists.txt

@@ -13,8 +13,9 @@ target_link_libraries(device_manager PRIVATE
         cmvr_es::device::microphone
         cmvr_es::device::dexhand
         cmvr::device::head_esp32
-        cmvr_es::device::motor_system
+        cmvr_es::device::motor_manager
         cmvr_es::device::arm
+        cmvr_es::mujoco_viewer
         osqp
 #        cmvr_es::device::aubo_robot
 )

cmvr-es/manager/device_manager/include/device_manager.h

@@ -51,6 +51,7 @@ namespace cmvr::device {
         void log_device_plan_() const;
         void pre_scan_robot_arm_dependencies_() const;
         void init_devices_();
+        void configure_mujoco_viewer_pip_();
     };
 } // cmvr
 

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

@@ -10,7 +10,7 @@
 #include "devices/camera/camera_factory.h"
 #include "devices/dexhand/dexhand_factory.h"
 #include "devices/microphone/microphone_factory.h"
-#include "devices/motor/motor_system/include/motor_system.h"
+#include "devices/motor/manager/include/motor_manager.h"
 #include "devices/speaker/speaker_factory.h"
 
 #include "common/config/config_files.h"
@@ -21,8 +21,11 @@
 #include "cmvr/config/dexhand_config/dexhand_config.pb.h"
 #include "cmvr/config/microphone_config/microphone_config.pb.h"
 #include "cmvr/config/motor_config/motor_config.pb.h"
+#include "cmvr/config/mujoco_config/mujoco_world_config.pb.h"
 #include "cmvr/config/speaker_config/speaker_config.pb.h"
 #include "common/base/logging/logger.h"
+#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
 
 using namespace std;
 using namespace cmvr::device;
@@ -225,11 +228,11 @@ DeviceFactory::DeviceFactory()
     registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM,
 	                    [](const auto& entry) {
 	                        if (entry.id().empty()) {
-	                            CMVR_LOG(ERROR) << "[DeviceFactory]: MotorSystem id is required";
+	                            CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required";
 	                            return DeviceRecord{};
 	                        }
 	                        if (entry.config_file().empty()) {
-	                            CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorSystem ID: " << entry.id();
+	                            CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorManager ID: " << entry.id();
 	                            return DeviceRecord{};
 	                        }
 	                        config::MotorRootConfig root_cfg;
@@ -238,7 +241,7 @@ DeviceFactory::DeviceFactory()
 	                            return DeviceRecord{};
 	                        }
                         CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success";
-                        auto device = std::make_shared<MotorSystem>(entry.id(), root_cfg.motor());
+                        auto device = std::make_shared<MotorManager>(entry.id(), root_cfg.motor());
                         DeviceRecord record;
                         record.id = entry.id();
                         record.kind = device->kind();
@@ -249,6 +252,30 @@ DeviceFactory::DeviceFactory()
 
     registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM, createRobotArmRecord);
 
+    registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD,
+                    [](const auto& entry) {
+                        return createFactoryRecordFromCollection<config::MujocoWorldRootConfig>(
+                            entry,
+                            [](const auto& root) -> decltype(auto) {
+                                return (root.worlds());
+                            },
+                            [](const auto& cfg) {
+                                return std::make_shared<cmvr::simulate::MujocoWorldDevice>(cfg);
+                            });
+                    });
+
+    registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_VIEWER,
+                    [](const auto& entry) {
+                        return createFactoryRecordFromCollection<config::MujocoViewerRootConfig>(
+                            entry,
+                            [](const auto& root) -> decltype(auto) {
+                                return (root.viewers());
+                            },
+                            [](const auto& cfg) {
+                                return std::make_shared<cmvr::MujocoViewerDevice>(cfg);
+                            });
+                    });
+
     registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_BIO_HEAD_ROBOT,
                     [](const auto& entry) {
                         return createSingleDeviceRecord<config::BioHeadRobotRootConfig, config::BioHeadRobotConfig, BioHeadRobot>(

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

@@ -10,11 +10,13 @@
 #include "devices/battery/abstract_battery.h"
 #include "devices/biohead/abstract_biohead.h"
 #include "devices/camera/abstract_camera.h"
+#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
 #include "devices/dexhand/abstract_dexhand.h"
 #include "devices/gripper/abstract_gripper.h"
 #include "devices/microphone/abstract_microphone.h"
-#include "devices/motor/motor_system/include/motor_system.h"
+#include "devices/motor/manager/include/motor_manager.h"
 #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"
@@ -48,12 +50,27 @@ const char* deviceTypeToString(const cmvr::config::DeviceConfigEntry::DeviceType
             return "DEVICE_TYPE_SPEAKER";
         case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_AGV:
             return "DEVICE_TYPE_AGV";
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD:
+            return "DEVICE_TYPE_MUJOCO_WORLD";
+        case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_VIEWER:
+            return "DEVICE_TYPE_MUJOCO_VIEWER";
         case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN:
         default:
             return "DEVICE_TYPE_UNKNOWN";
     }
 }
 
+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;
+}
+
 } // namespace
 
 template std::shared_ptr<AbstractAGV> DeviceManager::getDevice(const std::string& device_id);
@@ -65,7 +82,8 @@ template std::shared_ptr<AbstractGripper> DeviceManager::getDevice(const std::st
 template std::shared_ptr<AbstractMicrophone> DeviceManager::getDevice(const std::string& device_id);
 template std::shared_ptr<AbstractSpeaker> DeviceManager::getDevice(const std::string& device_id);
 template std::shared_ptr<AbstractBiohead> DeviceManager::getDevice(const std::string& device_id);
-template std::shared_ptr<MotorSystem> DeviceManager::getDevice(const std::string& device_id);
+template std::shared_ptr<MotorManager> DeviceManager::getDevice(const std::string& device_id);
+template std::shared_ptr<cmvr::MujocoViewerDevice> DeviceManager::getDevice(const std::string& device_id);
 
 
 std::shared_ptr<DeviceManager> DeviceManager::instance_ = nullptr;
@@ -81,6 +99,7 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) {
     pre_scan_robot_arm_dependencies_();
     logSection("Initialize Devices");
     init_devices_();
+    configure_mujoco_viewer_pip_();
 }
 
 DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) {
@@ -102,7 +121,7 @@ DeviceManager& DeviceManager::getInstance() {
 void DeviceManager::destroyInstance() {
     std::lock_guard lock(init_mutex_);
     instance_.reset();
-    MotorSystem::clearActiveJoints();
+    MotorManager::clearActiveJoints();
 }
 
 void DeviceManager::start(){
@@ -246,11 +265,11 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
             continue;
         }
         if (entry.id().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorSystem device id is empty";
+            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty";
             return;
         }
         if (entry.config_file().empty()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorSystem config_file is empty: " << entry.id();
+            CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id();
             return;
         }
 
@@ -260,7 +279,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
             return;
         }
         if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) {
-            CMVR_LOG(ERROR) << "[DeviceManager]: MotorSystem entry id '" << entry.id()
+            CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id()
                             << "' does not match config id '" << root_cfg.motor().id() << "'";
             return;
         }
@@ -324,7 +343,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         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 MotorSystem: "
+                            << "' depends on disabled or missing MotorManager: "
                             << motor_config.motor_system_id();
             return;
         }
@@ -352,13 +371,8 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
                 if (allowed_groups.count(group_id) == 0) {
                     continue;
                 }
-                for (const auto& motor : motor_group.motors()) {
+                if (motorGroupHasJoint(motor_group, joint_name)) {
-                    if (motor.joint_name() == joint_name) {
+                    matched_group = group_id;
-                        matched_group = group_id;
-                        break;
-                    }
-                }
-                if (!matched_group.empty()) {
                     break;
                 }
             }
@@ -373,9 +387,9 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
         }
     }
 
-    MotorSystem::clearActiveJoints();
+    MotorManager::clearActiveJoints();
     for (auto& [motor_system_id, group_selection] : selections) {
-        MotorSystem::setActiveJoints(motor_system_id, std::move(group_selection));
+        MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));
     }
 }
 
@@ -423,3 +437,40 @@ void DeviceManager::init_devices_() {
         devices_.emplace(record.id, std::move(record));
     }
 }
+
+void DeviceManager::configure_mujoco_viewer_pip_()
+{
+    for (const auto& [camera_id, camera_record] : devices_) {
+        if (camera_record.type_name != "MujocoCamera") {
+            continue;
+        }
+
+        auto camera = std::dynamic_pointer_cast<MujocoCamera>(camera_record.device);
+        if (!camera) {
+            continue;
+        }
+
+        const auto& camera_config = camera->config();
+        if (!camera_config.viewer_pip().enable()) {
+            continue;
+        }
+
+        for (const auto& [viewer_id, viewer_record] : devices_) {
+            if (viewer_record.type_name != "MujocoViewerDevice") {
+                continue;
+            }
+
+            auto viewer = getDevice<cmvr::MujocoViewerDevice>(viewer_id);
+            if (viewer && viewer->setPiPCameraConfig(camera_config)) {
+                camera->setFetchRgbdFn([viewer](std::vector<unsigned char>& rgb,
+                                                std::vector<float>& depth,
+                                                int& width,
+                                                int& height,
+                                                uint64_t& frame_id) {
+                    return viewer->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
+                });
+                break;
+            }
+        }
+    }
+}

docs/ethercat_motor_tutorial.md

@@ -0,0 +1,237 @@
+# EtherCAT 电机接入教程
+
+这份文档只说明新增一种 EtherCAT 电机需要改哪里、怎么写。
+
+## 1. 增加 vendor
+
+修改 `protos/cmvr/config/motor_config/motor_config.proto`：
+
+```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. 写电机配置
+
+新增配置文件：
+
+```text
+cmvr-es/config/devices/motor/ethercat_motors.pb.txt
+```
+
+示例：
+
+```proto
+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. 注册设备
+
+修改：
+
+```text
+cmvr-es/config/manager/device_manager.pb.txt
+```
+
+加入：
+
+```proto
+devices {
+  id: "ethercat_motors"
+  type: MOTOR_SYSTEM
+  enable: true
+  config_file: "devices/motor/ethercat_motors.pb.txt"
+}
+```
+
+## 4. 机械臂使用 EtherCAT group
+
+修改机械臂配置，例如：
+
+```text
+cmvr-es/config/devices/arm/arm.pb.txt
+```
+
+把 motor backend 改成：
+
+```proto
+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 总线资源放在：
+
+```text
+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` 负责：
+
+```text
+读取 ethercat 配置
+初始化 EtherCAT master
+扫描/校验 slave_index、vendor_id、product_code
+启动 cyclic loop
+保存 command/feedback buffer
+停止 cyclic loop
+```
+
+bus runtime 不创建具体电机，也不关心厂商；它只保存总线连接、线程和数据缓存。
+
+## 6. 增加具体电机 driver
+
+新增目录：
+
+```text
+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 电机
+
+修改：
+
+```text
+cmvr-es/devices/motor/manager/src/motor_manager.cpp
+```
+
+在 `MotorManager::createEthercatMotors_()` 里按 `vendor + protocol` 创建具体电机：
+
+```cpp
+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
+
+修改：
+
+```text
+cmvr-es/devices/motor/manager/CMakeLists.txt
+```
+
+给 `motor_manager` 链接新增的具体 EtherCAT 电机 target。
+
+修改：
+
+```text
+cmvr-es/devices/motor/CMakeLists.txt
+```
+
+加入：
+
+```cmake
+add_subdirectory(drivers/xxx_ethercat)
+```
+
+## 9. 验证配置
+
+先验证 proto：
+
+```bash
+./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
+```
+
+再编译：
+
+```bash
+cmake --build cmake-build-debug --target mujoco_manual_ui_test
+```
+
+真机联调时先只验证初始化日志：master 打开、slave 数量、vendor/product 校验、cyclic loop 启动、每个 motor 注册成功。然后再下发运动命令。

protos/cmvr/config/arm_config/arm_config.proto

@@ -1,12 +1,11 @@
 syntax = "proto3";
 package cmvr.config;
 
-import "cmvr/common/geometry.proto";
-import "cmvr/config/joint_limits_config.proto";
 import "cmvr/config/lawba_ik_config.proto";
 import "cmvr/config/pinocchio_dls_ik_config.proto";
 import "cmvr/config/pinocchio_qp_ik_config.proto";
 import "cmvr/config/srs_ik_config.proto";
+import "cmvr/config/cartesian_motion_validation_config.proto";
 
 enum ToppraPathType {
   TOPPRA_PATH_TYPE_UNKNOWN = 0;
@@ -45,23 +44,6 @@ message VendorRobotArmBackendConfig {
   string password = 10;
 }
 
-message QpJointLimitAvoidanceConfig {
-  bool enable = 1;
-  double margin_ratio = 2;
-  double gain = 3;
-  double max_push = 4;
-  double weight = 5;
-}
-
-message CartesianVelocityQpConfig {
-  JointLimitsConfig joint_limits = 1;
-  .cmvr.common.Vec6 twist_tracking_weight = 2;
-  double qdot_regularization = 3;
-  double prev_qdot_regularization = 4;
-  double solver_eps = 5;
-  QpJointLimitAvoidanceConfig joint_limit_avoidance = 6;
-}
-
 message SpeedLPlannerConfig {
   double linear_velocity_max = 1;
   double linear_acceleration_max = 2;
@@ -69,16 +51,14 @@ message SpeedLPlannerConfig {
   double angular_velocity_max = 4;
   double angular_acceleration_max = 5;
   double angular_jerk_max = 6;
-  repeated double joint_acceleration_max = 7;
   double linear_target_replan_threshold = 8;
   double angular_target_replan_threshold = 9;
   double linear_reverse_cos_threshold = 10;
   double linear_reverse_switch_speed_threshold = 11;
-  double normal_direction_deviation_deg = 12;
+  optional bool enforce_joint_acceleration_limits = 17;
-  double mild_direction_deviation_deg = 13;
+  CartesianLineDeviationCheckConfig line_deviation_check = 18;
-  double severe_direction_deviation_deg = 14;
+  JointVelocityCheckConfig joint_velocity_check = 19;
-  double linear_min_speed_ratio = 15;
+  CartesianVelocityFeasibilityCheckConfig cartesian_velocity_feasibility_check = 20;
-  CartesianVelocityQpConfig qp = 16;
 }
 
 message CartesianVelocityControllerConfig {
@@ -86,6 +66,7 @@ message CartesianVelocityControllerConfig {
   double stop_twist_norm = 2;
   double stop_command_velocity_norm = 3;
   double stop_measured_velocity_norm = 4;
+  double stop_acceleration = 5;
 }
 
 message ToppraJointMotionPlannerConfig {
@@ -105,13 +86,14 @@ message MoveLPlannerConfig {
   double sample_period_s = 1;
   double position_gain = 2;
   double rotation_gain = 3;
-  CartesianVelocityQpConfig qp = 4;
+  CartesianLineDeviationCheckConfig line_deviation_check = 4;
+  JointContinuityCheckConfig joint_continuity_check = 5;
+  CartesianStepFeasibilityCheckConfig cartesian_step_feasibility_check = 6;
 }
 
 message MoveLConfig {
   oneof algorithm {
-    MoveLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1;
+    MoveLPlannerConfig pinocchio_cartesian_motion_planner = 1;
-    MoveLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2;
   }
 }
 
@@ -123,8 +105,7 @@ message SpeedLControllerConfig {
 
 message SpeedLConfig {
   oneof algorithm {
-    SpeedLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1;
+    SpeedLPlannerConfig pinocchio_cartesian_motion_planner = 1;
-    SpeedLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2;
   }
 
   SpeedLControllerConfig speed_l_controller = 10;
@@ -142,13 +123,11 @@ message ArmKinematicsConfig {
 message ArmMotionConfig {
   MoveJConfig move_j = 1;
   MoveLConfig move_l = 2;
-  reserved 3;
   SpeedLConfig speed_l = 4;
 }
 
 message RobotArmConfig {
   string id = 1;
-  reserved 2;
 
   oneof backend {
     MotorRobotArmBackendConfig motor = 10;

protos/cmvr/config/device_manager_config/device_manager_config.proto

@@ -19,6 +19,8 @@ message DeviceConfigEntry {
     DEVICE_TYPE_MICROPHONE = 15;
     DEVICE_TYPE_SPEAKER = 16;
     DEVICE_TYPE_AGV = 17;
+    DEVICE_TYPE_MUJOCO_WORLD = 18;
+    DEVICE_TYPE_MUJOCO_VIEWER = 19;
   }
 
   string id = 1;

protos/cmvr/config/joint_limits_config.proto

@@ -2,7 +2,7 @@ syntax = "proto3";
 package cmvr.config;
 
 enum JointLimitSource {
-  JOINT_LIMIT_SOURCE_UNKNOWN = 0;
+  JOINT_LIMIT_SOURCE_UNSPECIFIED = 0;
   JOINT_LIMIT_SOURCE_URDF = 1;
   JOINT_LIMIT_SOURCE_CUSTOM = 2;
 }
@@ -19,4 +19,25 @@ message JointLimitsConfig {
   JointLimitSource source = 1;
   repeated JointLimitConfig joints = 2;
   string urdf_path = 3;
+  bool enable = 4;
+}
+
+message JointSoftLimitConfig {
+  bool enable = 1;
+  double margin_ratio = 2;
+  double min_margin_rad = 3;
+}
+
+message JointLimitAvoidanceConfig {
+  bool enable = 1;
+  double gain = 2;
+  double margin_ratio = 3;
+  double max_push = 4;
+  double weight = 5;
+}
+
+message JointLimitPolicyConfig {
+  JointLimitsConfig limits = 1;
+  JointSoftLimitConfig soft_limit = 2;
+  JointLimitAvoidanceConfig avoidance = 3;
 }

protos/cmvr/config/motor_config/motor_config.proto

@@ -3,7 +3,7 @@ package cmvr.config;
 
 import "cmvr/config/joint_limits_config.proto";
 
-message Ti5MotorConfig {
+message MotorConfigItem {
   int32 id = 1;
   string joint_name = 2;
   double limit_q_lb = 3;
@@ -12,6 +12,17 @@ message Ti5MotorConfig {
   double limit_qdd = 6;
 }
 
+message MotorList {
+  repeated MotorConfigItem motors = 1;
+}
+
+message EthercatSlaveConfig {
+  int32 motor_id = 1;
+  int32 slave_index = 2;
+  uint32 vendor_id = 3;
+  uint32 product_code = 4;
+}
+
 message SocketCanConfig {
   string dev_id = 1;
   int32 channel_id = 2;
@@ -20,6 +31,11 @@ message SocketCanConfig {
 message EtherCATConfig {
   string master_id = 1;
   int32 cycle_us = 2;
+  repeated EthercatSlaveConfig slaves = 10;
+}
+
+message MujocoMotorGroupConfig {
+  string world_id = 1;
 }
 
 enum MotorBusType {
@@ -29,18 +45,34 @@ enum MotorBusType {
   MOTOR_BUS_MUJOCO = 3;
 }
 
+enum MotorVendor {
+  MOTOR_VENDOR_UNKNOWN = 0;
+  MOTOR_VENDOR_TI5 = 1;
+  MOTOR_VENDOR_MUJOCO = 2;
+}
+
+enum MotorProtocol {
+  MOTOR_PROTOCOL_UNKNOWN = 0;
+  MOTOR_PROTOCOL_CANOPEN = 1;
+  MOTOR_PROTOCOL_ETHERCAT_CIA402 = 2;
+  MOTOR_PROTOCOL_MUJOCO = 3;
+}
+
 message MotorGroupConfig {
   string id = 1;
   MotorBusType bus_type = 2;
-  string tool_frame = 3;
+  MotorVendor vendor = 3;
+  MotorProtocol protocol = 4;
+  string tool_frame = 5;
 
   oneof bus_config {
     SocketCanConfig can = 10;
     EtherCATConfig ethercat = 11;
+    MujocoMotorGroupConfig mujoco = 12;
   }
 
-  repeated Ti5MotorConfig motors = 20;
   JointLimitsConfig joint_limits = 30;
+  MotorList motors = 40;
 }
 
 message MotorConfig {