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

refactor(motor): introduce MotorManager and bus runtimes

Browse Source
This commit is contained in:
lgv 2026-06-30 16:04:36 +08:00
parent 25d44d5fec
commit 80c8969ce9
57 changed files with 2011 additions and 1286 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 }
motors { id: 2 joint_name: "R_SHOULDER_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
motors { id: 3 joint_name: "R_SHOULDER_Y" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
motors { id: 4 joint_name: "R_ELBOW_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
motors { id: 5 joint_name: "R_WRIST_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
motors { id: 6 joint_name: "R_WRIST_Y" limit_q_lb: -1.102 limit_q_ub: 1.02 limit_qd: 3.0 }
motors { id: 7 joint_name: "R_WRIST_R" limit_q_lb: -0.293 limit_q_ub: 1.57079 limit_qd: 3.0 }
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" }
}
}
}

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

@ -4,14 +4,18 @@ 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 {
motors { id: 23 joint_name: "L_SHOULDER_P" }
motors { id: 24 joint_name: "L_SHOULDER_R" }
motors { id: 25 joint_name: "L_SHOULDER_Y" }
@ -20,15 +24,19 @@ motor {
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,6 +46,7 @@ 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 {
motors { id: 16 joint_name: "R_SHOULDER_P" }
motors { id: 17 joint_name: "R_SHOULDER_R" }
motors { id: 18 joint_name: "R_SHOULDER_Y" }
@ -46,36 +55,47 @@ motor {
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 {
motors { id: 32 joint_name: "HEAD_Y" }
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 {
motors { id: 4 joint_name: "WAIST_Y" }
motors { id: 15 joint_name: "WAIST_P" }
}
}
}

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

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

7
cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt
View File

@ -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::mujoco_motor
cmvr_es::device::motor_manager
cmvr_es::device::mujoco_motor_driver
cmvr_es::mujoco_viewer
cmvr_es::proto
gtest

5
cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h
View File

@ -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::size_t dof);
std::vector<double> moveLJointVelocityLimits_(const std::vector<double>& qd_max) const;
static Result unsupported_(const std::string& name);
private:

59
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
View File

@ -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 ||
!cartesian_motion->configureMoveL(*move_l_config) ||
if (!speed_l_config || !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,
const std::size_t dof)
std::vector<double> MotorRobotArm::moveLJointVelocityLimits_(
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)

146
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp
View File

@ -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/motor_system/include/motor_system.h"
#include "devices/motor/manager/include/motor_manager.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();
MotorSystem::setActiveJoints(
"mujoco_motors", {{"right_arm_mujoco", std::move(right_arm_joints)}});
MotorManager::clearActiveJoints();
MotorManager::setActiveJoints(
"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");
ASSERT_TRUE(bridge_);
world_ = MotorManager::mujocoWorldFor("mujoco_motors");
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<MujocoJointBridge> bridge_;
std::shared_ptr<simulate::MujocoWorldDevice> world_device_;
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(
(project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_);
MuJocoViewer viewer(world_);
viewer.setupCamera(2.5, -160.0, -25.0);
ScenarioOutcome outcome;
std::thread scenario([&] {
try {
if (!bridge_->waitUntilReady(std::chrono::seconds(10))) {
throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready");
if (!world_ || !world_->isRunning()) {
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(
(project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_);
MuJocoViewer viewer(world_);
viewer.setupCamera(2.5, -160.0, -25.0);
ScenarioOutcome outcome;
std::thread scenario([&] {
try {
if (!bridge_->waitUntilReady(std::chrono::seconds(10))) {
throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready");
if (!world_ || !world_->isRunning()) {
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(
(project_root_ / "model/xiaoyan_description/dual_arm.xml").string(), bridge_);
MuJocoViewer viewer(world_);
viewer.setupCamera(2.5, -160.0, -25.0);
ScenarioOutcome outcome;
std::thread scenario([&] {
try {
if (!bridge_->waitUntilReady(std::chrono::seconds(10))) {
throw std::runtime_error("MuJoCo right-arm joints or actuators are not ready");
if (!world_ || !world_->isRunning()) {
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{"DlsIkQpMotion", "arm_mujoco_dls_ik_qp_motion.pb.txt"},
ArmMujocoConfigCase{"QpIkQpMotion", "arm_mujoco_qp_ik_qp_motion.pb.txt"}),
ArmMujocoConfigCase{"DlsIk", "arm_mujoco.pb.txt"},
ArmMujocoConfigCase{"QpIk", "arm_mujoco_qp.pb.txt"}),
[](const ::testing::TestParamInfo<ArmMujocoConfigCase>& info) {
return std::string(info.param.name);
});

38
cmvr-es/devices/motor/CMakeLists.txt
View File

@ -1,31 +1,15 @@
add_subdirectory(ti5_motor)
add_subdirectory(mujoco_motor)
add_subdirectory(motor_system)
add_library(motor_core INTERFACE)
# --------------------------------------------------------
# Unit test
# --------------------------------------------------------
include_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
)
target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices)
link_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
)
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
target_link_libraries(motor_core
INTERFACE
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)

4
cmvr-es/devices/motor/abstract_motor.h
View File

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

20
cmvr-es/devices/motor/bus_runtime/CMakeLists.txt Normal file
View File

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

30
cmvr-es/devices/motor/bus_runtime/abstract_motor_bus_runtime.h Normal file
View File

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

45
cmvr-es/devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h Normal file
View File

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

102
cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp Normal file
View File

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

31
cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h Normal file
View File

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

78
cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp Normal file
View File

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

36
cmvr-es/devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h Normal file
View File

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

71
cmvr-es/devices/motor/bus_runtime/mujoco/src/mujoco_motor_bus_runtime.cpp Normal file
View File

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

15
cmvr-es/devices/motor/drivers/mujoco/CMakeLists.txt Normal file
View File

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

27
cmvr-es/devices/motor/mujoco_motor/include/mujoco_motor.h → cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h
View File

@ -5,24 +5,25 @@
#include <cstdint>
#include <memory>
#include <string>
#include <vector>
#include "devices/motor/abstract_motor.h"
#include "devices/motor/mujoco_motor/include/mujoco_joint_bridge.h"
#include "motor/abstract_motor.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,
std::string joint_name,
std::shared_ptr<MujocoJointBridge> bridge,
MujocoMotor(std::string joint_name,
std::shared_ptr<simulate::MujocoWorld> world,
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};
std::shared_ptr<MujocoJointBridge> bridge_;
double clampQ_(double q) const;
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

249
cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp Normal file
View File

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

24
cmvr-es/devices/motor/drivers/ti5_canopen/CMakeLists.txt Normal file
View File

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

0
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h → cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h
View File

0
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h → cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h
View File

0
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.h → cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h
View File

0
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.h → cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h
View File

0
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.h → cmvr-es/devices/motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo2.h
View File

6
cmvr-es/devices/motor/ti5_motor/ti5_motor.h → cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h
View File

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

6
cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h → cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h
View File

@ -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/ti5_motor/canopen/protocol/ti5_motor_rpdo2.h"
#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h"
#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h"
#include <cmath>
namespace cmvr {

2
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp → cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo1.cpp
View File

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

2
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo2.cpp → cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_rpdo2.cpp
View File

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

2
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp → cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp
View File

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

2
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp → cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo1.cpp
View File

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

2
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp → cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp
View File

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

5
cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor.cpp Normal file
View File

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

8
cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp → cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp
View File

@ -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/ti5_motor/canopen/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_tpdo1.h"
#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo2.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;

20
cmvr-es/devices/motor/manager/CMakeLists.txt Normal file
View File

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

98
cmvr-es/devices/motor/manager/include/motor_manager.h Normal file
View File

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

552
cmvr-es/devices/motor/manager/src/motor_manager.cpp Normal file
View File

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

57
cmvr-es/devices/motor/motor_manager.h
View File

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

120
cmvr-es/devices/motor/motor_manager_test.cpp
View File

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

19
cmvr-es/devices/motor/motor_system/CMakeLists.txt
View File

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

81
cmvr-es/devices/motor/motor_system/include/motor_system.h
View File

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

411
cmvr-es/devices/motor/motor_system/src/motor_system.cpp
View File

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

13
cmvr-es/devices/motor/mujoco_motor/CMakeLists.txt
View File

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

57
cmvr-es/devices/motor/mujoco_motor/include/mujoco_joint_bridge.h
View File

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

225
cmvr-es/devices/motor/mujoco_motor/src/mujoco_motor.cpp
View File

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

24
cmvr-es/devices/motor/ti5_motor/CMakeLists.txt
View File

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

5
cmvr-es/devices/motor/ti5_motor/ti5_motor.cpp
View File

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

3
cmvr-es/manager/device_manager/CMakeLists.txt
View File

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

1
cmvr-es/manager/device_manager/include/device_manager.h
View File

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

35
cmvr-es/manager/device_manager/src/device_factory.cpp
View File

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

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

@ -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,16 +371,11 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const
if (allowed_groups.count(group_id) == 0) {
continue;
}
for (const auto& motor : motor_group.motors()) {
if (motor.joint_name() == joint_name) {
if (motorGroupHasJoint(motor_group, joint_name)) {
matched_group = group_id;
break;
}
}
if (!matched_group.empty()) {
break;
}
}
if (matched_group.empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
@ -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;
}
}
}
}

237
docs/ethercat_motor_tutorial.md Normal file
View File

@ -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 注册成功。然后再下发运动命令。

43
protos/cmvr/config/arm_config/arm_config.proto
View File

@ -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;
double mild_direction_deviation_deg = 13;
double severe_direction_deviation_deg = 14;
double linear_min_speed_ratio = 15;
CartesianVelocityQpConfig qp = 16;
optional bool enforce_joint_acceleration_limits = 17;
CartesianLineDeviationCheckConfig line_deviation_check = 18;
JointVelocityCheckConfig joint_velocity_check = 19;
CartesianVelocityFeasibilityCheckConfig cartesian_velocity_feasibility_check = 20;
}
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_qp_cartesian_motion_planner = 2;
MoveLPlannerConfig pinocchio_cartesian_motion_planner = 1;
}
}
@ -123,8 +105,7 @@ message SpeedLControllerConfig {
message SpeedLConfig {
oneof algorithm {
SpeedLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1;
SpeedLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2;
SpeedLPlannerConfig pinocchio_cartesian_motion_planner = 1;
}
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;

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

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

23
protos/cmvr/config/joint_limits_config.proto
View File

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

38
protos/cmvr/config/motor_config/motor_config.proto
View File

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