Merge branch 'dev' of http://192.168.28.10:18088/SmartBench/cmvr-es into dev

CMakeLists.txt

@@ -105,6 +105,7 @@ add_subdirectory(cmvr-es)
 add_executable(cmvr_es cmvr-es/main.cpp)
 target_include_directories(cmvr_es PRIVATE ${GLOG_INCLUDE_DIRS})
 target_link_libraries(cmvr_es PRIVATE
+        cmvr_es::runtime
         cmvr_es::proto
         cmvr_es::logging
         service
@@ -113,7 +114,7 @@ target_link_libraries(cmvr_es PRIVATE
         cmvr_es::service
         cmvr_es::hardware
         cmvr_es::device::canbus
-        cmvr_es::device::ti5motor
+        cmvr_es::device::ti5_canopen_motor_driver
         cmvr_es::algorithms::controller
         cmvr_es::ik_solver
         cmvr_es::base_motion

cmvr-es/CMakeLists.txt

@@ -4,9 +4,11 @@ link_libraries(cmvr_es::logging)
 add_subdirectory(common)
 add_subdirectory(hardware)
 add_subdirectory(algorithms)
+add_subdirectory(simulate)
 add_subdirectory(devices)
 add_subdirectory(manager/device_manager)
 add_subdirectory(task)
 add_subdirectory(manager/task_manager)
 add_subdirectory(service)
-add_subdirectory(simulate)
+add_subdirectory(runtime)
+add_subdirectory(test)

cmvr-es/algorithms/controllers/CMakeLists.txt

@@ -58,29 +58,3 @@ target_link_libraries(controller PUBLIC
 add_library(cmvr_es::algorithms::controller ALIAS controller)
 
 install(TARGETS controller LIBRARY DESTINATION lib)
-
-
-# --------------------------------------------------------
-# Unit test
-# --------------------------------------------------------
-find_package(realsense2 REQUIRED)
-add_executable(controller_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/controller_test.cpp
-)
-
-target_link_libraries(controller_test
-        PRIVATE
-        cmvr_es::perception
-        cmvr_es::ik_solver
-        cmvr_es::base_motion
-        cmvr_es::proto
-        cmvr_es::mujoco_viewer
-        cmvr_es::algorithms::controller
-        cmvr_es::device::mujoco_camera
-        gtest
-        gtest_main
-        pthread
-        glog
-        ${VISP_LIBRARIES}
-        realsense2::realsense2
-)

cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h

@@ -8,6 +8,7 @@
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <thread>
 #include <vector>
 
@@ -23,6 +24,7 @@ public:
         double stop_twist_norm{1e-9};
         double stop_command_velocity_norm{1e-3};
         double stop_measured_velocity_norm{1e-2};
+        double stop_acceleration{0.5};
     };
 
     using ReadStateCallback = std::function<bool(std::vector<double>& q, std::vector<double>& qd)>;
@@ -42,7 +44,7 @@ public:
                   double acceleration,
                   double duration,
                   FrameType frame);
-    Result stop(double acceleration);
+    Result stop(std::optional<double> acceleration = std::nullopt);
     void shutdown();
 
     bool busy() const { return busy_.load(); }

cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp

@@ -26,6 +26,9 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController:
     if (config.stop_measured_velocity_norm <= 0.0) {
         config.stop_measured_velocity_norm = defaults.stop_measured_velocity_norm;
     }
+    if (config.stop_acceleration <= 0.0) {
+        config.stop_acceleration = defaults.stop_acceleration;
+    }
     return config;
 }
 
@@ -95,9 +98,8 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
     return Result::success();
 }
 
-Result CartesianVelocityController::stop(const double acceleration)
+Result CartesianVelocityController::stop(const std::optional<double> acceleration)
 {
-    (void)acceleration;
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
@@ -105,6 +107,7 @@ Result CartesianVelocityController::stop(const double acceleration)
         std::lock_guard<std::mutex> lock(mutex_);
         target_twist_ = {};
         target_frame_ = FrameType::Base;
+        target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
         command_active_ = true;
         ++command_version_;
     }
@@ -189,6 +192,13 @@ void CartesianVelocityController::workerLoop_()
             }
 
             if (!planner_->updateSpeedLAcceleration(acceleration)) {
+                if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
+                    std::lock_guard<std::mutex> lock(mutex_);
+                    command_active_ = false;
+                    sendZero_();
+                    busy_.store(false);
+                    break;
+                }
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
                                 << acceleration;
                 sendZero_();

cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h

@@ -209,18 +209,6 @@ public:
      */
     void resetTwistCommandState();
 
-    /**
-     * @brief 设置关节限位回避参数。
-     * @param enable 是否启用。
-     * @param gain 回避增益。
-     * @param margin_ratio 触发限位回避的边界比例。
-     * @param max_push 单关节最大推回速度。
-     */
-    void setJointLimitAvoidance(bool enable,
-                                double gain = 0.2,
-                                double margin_ratio = 0.05,
-                                double max_push = 0.25);
-
     /**
      * @brief 设置从 `AbstractCamera` 相机坐标系 `cam` 到 ViSP 相机坐标系 `c` 的旋转矩阵。
      * @param R_cv 旋转矩阵。
@@ -316,11 +304,6 @@ private:
     // 相机 twist 一阶低通滤波系数；默认 1.0 表示不过滤。
     double twist_lpf_alpha_{1.0};
 
-    bool limit_avoidance_enabled_{false};
-    double limit_avoidance_gain_{0.2};
-    double limit_avoidance_margin_ratio_{0.15};
-    double limit_avoidance_max_push_{0.25};
-
     Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()};
 
     Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};

cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp

@@ -114,12 +114,6 @@ bool IbvsController::init(std::shared_ptr<cmvr::PinocchioIKBase> solver,
     camera_frame_name_ = camera_link;
     initialized_ = solver_ != nullptr && !camera_frame_name_.empty();
     if (initialized_) {
-        if (auto dls_solver = std::dynamic_pointer_cast<PinocchioDlsIKSolver>(solver_)) {
-            dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_,
-                                               limit_avoidance_gain_,
-                                               limit_avoidance_margin_ratio_,
-                                               limit_avoidance_max_push_);
-        }
         has_joint_position_limits_ =
             solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_);
     } else {
@@ -555,23 +549,6 @@ void IbvsController::resetTwistCommandState() {
     has_v_camera_cmd_prev_ = true;
 }
 
-void IbvsController::setJointLimitAvoidance(bool enable,
-                                            double gain,
-                                            double margin_ratio,
-                                            double max_push) {
-    limit_avoidance_enabled_ = enable;
-    limit_avoidance_gain_ = std::max(0.0, gain);
-    limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49);
-    limit_avoidance_max_push_ = max_push;
-
-    if (auto dls_solver = std::dynamic_pointer_cast<PinocchioDlsIKSolver>(solver_)) {
-        dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_,
-                                           limit_avoidance_gain_,
-                                           limit_avoidance_margin_ratio_,
-                                           limit_avoidance_max_push_);
-    }
-}
-
 void IbvsController::setAlignCameraToVisp(const Eigen::Matrix3d& R_cv) {
     R_cv_ = R_cv;
 }

cmvr-es/algorithms/controllers/tests/src/controller_test.cpp

@@ -1,869 +0,0 @@
-//
-// Created by lgv on 2026/2/10.
-//  TEST(contrller_test, visp_test){
-#include "gtest/gtest.h"
-
-#include <algorithm>
-#include <array>
-#include <atomic>
-#include <chrono>
-#include <cmath>
-#include <limits>
-#include <memory>
-#include <mutex>
-#include <string>
-#include <thread>
-#include <vector>
-
-
-#include "algorithms/controllers/ibvs/include/ibvs_controller.h"
-#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
-#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
-using namespace cmvr;
-
-
-
-
-#include "gtest/gtest.h"
-
-#include <array>
-#include <cmath>
-#include <cstdio>
-#include <vector>
-
-#include <Eigen/Dense>
-
-#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
-
-// ---- ViSP ----
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/vs/vpServo.h>
-
-// Created by lgv on 2026/2/10.
-
-#include "gtest/gtest.h"
-
-#include <array>
-#include <cmath>
-#include <cstdio>
-#include <vector>
-
-#include <Eigen/Dense>
-
-#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
-
-// ---- ViSP ----
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/vs/vpServo.h>
-
-using namespace cmvr;
-
-namespace {
-
-// clamp helper
-static inline double clamp(double x, double lo, double hi) {
-  return std::max(lo, std::min(hi, x));
-}
-
-// MuJoCo xmat(9) -> Eigen::Matrix3d (row-major)
-static inline Eigen::Matrix3d xmat_to_R(const mjtNum* xmat9) {
-  Eigen::Matrix3d R;
-  R << xmat9[0], xmat9[1], xmat9[2],
-       xmat9[3], xmat9[4], xmat9[5],
-       xmat9[6], xmat9[7], xmat9[8];
-  return R;
-}
-
-// Build vpHomogeneousMatrix from Eigen R,t
-static inline vpHomogeneousMatrix make_cMo_from_Eigen(const Eigen::Matrix3d& R,
-                                                      const Eigen::Vector3d& t) {
-  vpRotationMatrix vR;
-  for (int r = 0; r < 3; ++r)
-    for (int c = 0; c < 3; ++c)
-      vR[r][c] = R(r, c);
-
-  vpTranslationVector vt(t(0), t(1), t(2));
-  vpHomogeneousMatrix cMo(vt, vR);
-  return cMo;
-}
-
-} // namespace
-
-class IBVSHomingViewer : public MuJocoViewer {
-public:
-  using MuJocoViewer::MuJocoViewer;
-
-protected:
-  enum class Mode { HOMING, IBVS };
-
-  void initOnce(mjModel* m, mjData* d) override {
-    // 1) 主视角自由相机 + PiP显示 hand_cam
-    setupCamera(3.0, -170.0, -40.0);
-    enablePiPCamera("hand_cam", 405, 1200, 320, 240);
-
-    // 2) 右臂 7DOF actuator/joint
-    const char* act_names[7] = {
-      "R_SHOULDER_P_pos",
-      "R_SHOULDER_R_pos",
-      "R_SHOULDER_Y_pos",
-      "R_ELBOW_R_pos",
-      "R_WRIST_P_pos",
-      "R_WRIST_Y_pos",
-      "R_WRIST_R_pos"
-    };
-    const char* jnt_names[7] = {
-      "R_SHOULDER_P",
-      "R_SHOULDER_R",
-      "R_SHOULDER_Y",
-      "R_ELBOW_R",
-      "R_WRIST_P",
-      "R_WRIST_Y",
-      "R_WRIST_R"
-    };
-
-    for (int i = 0; i < 7; ++i) {
-      act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
-      jnt_ids_[i] = mj_name2id(m, mjOBJ_JOINT,    jnt_names[i]);
-
-      if (act_ids_[i] < 0) std::fprintf(stderr, "Cannot find actuator %s\n", act_names[i]);
-      if (jnt_ids_[i] < 0) std::fprintf(stderr, "Cannot find joint %s\n", jnt_names[i]);
-
-      if (jnt_ids_[i] >= 0) {
-        qpos_adr_[i] = m->jnt_qposadr[jnt_ids_[i]];
-        dof_adr_[i]  = m->jnt_dofadr[jnt_ids_[i]];
-      }
-    }
-
-    // 3) 找相机 site + tag
-    cam_site_id_ = mj_name2id(m, mjOBJ_SITE, "R_CAM_SITE");
-    tag_body_id_ = mj_name2id(m, mjOBJ_BODY, "tag_board");
-
-    std::printf("[IBVS] R_CAM_SITE id=%d, tag_board id=%d\n",
-                cam_site_id_, tag_body_id_);
-
-    if (cam_site_id_ < 0 || tag_body_id_ < 0) {
-      std::fprintf(stderr, "[IBVS] Missing R_CAM_SITE or tag_board. Check XML names.\n");
-      ready_ = false;
-      return;
-    }
-
-    // 4) 初始姿态（保证 PiP 里能看到 tag）
-    q_home_ = { 0.25, 1.00, M_PI/2 - 0.2, M_PI/2 - 0.2, -M_PI + 0.5, 0.0, 0.0 };
-
-    // 5) 坐标轴对齐：site相机系 -> ViSP相机系
-    //  ViSP（+X 右、+Y 下、+Z 前）
-    // R_CAM_SITE  +X 右、+Y 上、+Z 后
-    R_cv_ = (Eigen::Matrix3d() <<
-      1, 0, 0,
-       0,-1, 0,
-       0, 0, -1).finished();
-
-    // 6) ViSP IBVS 参数
-    tag_half_ = 0.06;   // 12cm tag -> half 6cm
-    Z_des_    = 0.34;
-    lambda_   = 0.7;
-
-    task_.setServo(vpServo::EYEINHAND_CAMERA);
-    task_.setInteractionMatrixType(vpServo::CURRENT);
-    task_.setLambda(lambda_);
-
-    obj_pts_[0].setWorldCoordinates(-tag_half_, -tag_half_, 0.0);
-    obj_pts_[1].setWorldCoordinates( tag_half_, -tag_half_, 0.0);
-    obj_pts_[2].setWorldCoordinates( tag_half_,  tag_half_, 0.0);
-    obj_pts_[3].setWorldCoordinates(-tag_half_,  tag_half_, 0.0);
-
-    // desired: 正对 + 距离 Z_des
-    {
-      vpTranslationVector t_des(0.0, 0.0, Z_des_);
-
-      // tag 在（相机坐标系visp 相机）下的朝向
-      vpRotationMatrix R_des;
-      R_des.buildFrom(M_PI, 0, 0);
-      vpHomogeneousMatrix cMo_des(t_des, R_des);
-
-      for (int i = 0; i < 4; ++i) {
-        obj_pts_[i].track(cMo_des);
-        s_star_[i].buildFrom(obj_pts_[i].get_x(),
-                             obj_pts_[i].get_y(),
-                             obj_pts_[i].get_Z());
-
-        s_cur_[i].buildFrom(0.0, 0.0, 1.0);
-        task_.addFeature(s_cur_[i], s_star_[i]);
-      }
-    }
-
-    // 初始化 q_cmd 为当前 qpos，避免突变
-    for (int i = 0; i < 7; ++i) {
-      q_cmd_[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
-    }
-
-    mode_ = Mode::HOMING;
-    home_hold_acc_ = 0.0;
-    step_count_ = 0;
-    ready_ = true;
-  }
-
-  void controlCallback(mjModel* m, mjData* d) override {
-    if (!ready_) return;
-    const double dt = m->opt.timestep;
-
-    // ======================
-    // A) HOMING
-    // ======================
-    if (mode_ == Mode::HOMING) {
-      double max_err = 0.0;
-      for (int i = 0; i < 7; ++i) {
-        if (act_ids_[i] < 0 || qpos_adr_[i] < 0) continue;
-        d->ctrl[act_ids_[i]] = q_home_[i];
-        const double qi = d->qpos[qpos_adr_[i]];
-        max_err = std::max(max_err, std::abs(qi - q_home_[i]));
-      }
-
-      if (max_err < home_tol_) home_hold_acc_ += dt;
-      else home_hold_acc_ = 0.0;
-
-      if (home_hold_acc_ > home_hold_time_) {
-        for (int i = 0; i < 7; ++i) {
-          if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]];
-        }
-        mode_ = Mode::IBVS;
-        std::printf("[IBVS] switch HOMING -> IBVS\n");
-      }
-      return;
-    }
-
-    // ======================
-    // B) IBVS
-    // ======================
-
-    // 1) camera(site) pose in world
-    const mjtNum* pc = d->site_xpos + 3 * cam_site_id_;
-    const mjtNum* Rc9 = d->site_xmat + 9 * cam_site_id_;
-    Eigen::Vector3d p_cw(pc[0], pc[1], pc[2]);
-    Eigen::Matrix3d R_cw = xmat_to_R(Rc9);      // site(cam)->world
-    Eigen::Matrix3d R_wc = R_cw.transpose();    // world->site(cam)
-    if ((step_count_ % 120) == 0) {
-      const Eigen::Vector3d x_w = R_cw.col(0);
-      const Eigen::Vector3d y_w = R_cw.col(1);
-      const Eigen::Vector3d z_w = R_cw.col(2);
-      std::printf("[IBVS] site axes(world): x=[%.3f %.3f %.3f] y=[%.3f %.3f %.3f] z=[%.3f %.3f %.3f]\n",
-                  x_w.x(), x_w.y(), x_w.z(),
-                  y_w.x(), y_w.y(), y_w.z(),
-                  z_w.x(), z_w.y(), z_w.z());
-
-      const int cam_id = mj_name2id(m, mjOBJ_CAMERA, "hand_cam");
-      if (cam_id >= 0) {
-        const mjtNum* Rc9_cam = d->cam_xmat + 9 * cam_id;
-        Eigen::Matrix3d R_cam_w = xmat_to_R(Rc9_cam);  // cam->world
-        Eigen::Matrix3d R_rel = R_cam_w.transpose() * R_cw; // cam <- site
-        std::printf("[IBVS] cam<-site:\n"
-                    "%.3f %.3f %.3f\n"
-                    "%.3f %.3f %.3f\n"
-                    "%.3f %.3f %.3f\n",
-                    R_rel(0,0), R_rel(0,1), R_rel(0,2),
-                    R_rel(1,0), R_rel(1,1), R_rel(1,2),
-                    R_rel(2,0), R_rel(2,1), R_rel(2,2));
-      } else {
-        std::printf("[IBVS] cam<-site: hand_cam not found\n");
-      }
-    }
-
-    // 2) tag_board body pose in world
-    const mjtNum* po = d->xpos + 3 * tag_body_id_;
-    const mjtNum* Ro9 = d->xmat + 9 * tag_body_id_;
-    Eigen::Vector3d p_ow(po[0], po[1], po[2]);
-    Eigen::Matrix3d R_ow = xmat_to_R(Ro9);
-
-    // 3) object->camera(site)
-    Eigen::Matrix3d R_co_site = R_wc * R_ow;
-    Eigen::Vector3d t_co_site = R_wc * (p_ow - p_cw);
-
-    // 4) site相机系 -> ViSP相机系（对齐）
-    Eigen::Matrix3d R_co = R_cv_ * R_co_site;
-    Eigen::Vector3d t_co = R_cv_ * t_co_site;
-
-    if ((step_count_ % 120) == 0) {
-      const Eigen::Vector3d z_site = R_co_site.col(2); // tag +Z in site(cam) frame
-      const Eigen::Vector3d z_visp = R_co.col(2);      // tag +Z in ViSP frame
-      std::printf("[IBVS] tag +Z (site) = [%.3f %.3f %.3f]\n",
-                  z_site.x(), z_site.y(), z_site.z());
-      std::printf("[IBVS] tag +Z (visp) = [%.3f %.3f %.3f]\n",
-                  z_visp.x(), z_visp.y(), z_visp.z());
-    }
-
-    if ((step_count_ % 60) == 0) {
-      std::printf("[IBVS] t_co = [%.3f %.3f %.3f]\n", t_co.x(), t_co.y(), t_co.z());
-    }
-
-    vpHomogeneousMatrix cMo = make_cMo_from_Eigen(R_co, t_co);
-
-    // 5) current features
-    for (int i = 0; i < 4; ++i) {
-      obj_pts_[i].track(cMo);
-      double x = obj_pts_[i].get_x();
-      double y = obj_pts_[i].get_y();
-      double Z = std::max(obj_pts_[i].get_Z(), 0.05);
-      s_cur_[i].buildFrom(x, y, Z);
-    }
-
-    // 6) ViSP control law -> v_c (ViSP camera frame)
-    vpColVector v_c = task_.computeControlLaw();
-
-    // 限幅（关键）
-    for (int k = 0; k < 6; ++k) {
-      v_c[k] = clamp(v_c[k], -vmax6_[k], vmax6_[k]);
-    }
-
-    if ((step_count_ % 60) == 0) {
-      std::printf("[IBVS] v_c = [%+.3f %+.3f %+.3f %+.3f %+.3f %+.3f]\n",
-                  v_c[0], v_c[1], v_c[2], v_c[3], v_c[4], v_c[5]);
-    }
-
-    // 7) ViSP相机速度 -> site相机速度（逆对齐）
-    Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
-    Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
-
-    Eigen::Vector3d v_site = R_cv_.transpose() * v_visp;
-    Eigen::Vector3d w_site = R_cv_.transpose() * w_visp;
-
-    // 8) site相机速度 -> world twist（给 world Jacobian 用）
-    Eigen::Vector3d v_w = R_cw * v_site;
-    Eigen::Vector3d w_w = R_cw * w_site;
-
-    Eigen::Matrix<double,6,1> twist_w;
-    twist_w << v_w(0), v_w(1), v_w(2), w_w(0), w_w(1), w_w(2);
-
-    // 9) Jacobian for R_CAM_SITE (world)
-    std::vector<mjtNum> jacp(3 * m->nv);
-    std::vector<mjtNum> jacr(3 * m->nv);
-    mj_jacSite(m, d, jacp.data(), jacr.data(), cam_site_id_);
-
-    Eigen::Matrix<double,6,7> J;
-    J.setZero();
-    for (int j = 0; j < 7; ++j) {
-      const int dof = dof_adr_[j];
-      if (dof < 0) continue;
-
-      J(0,j) = jacp[0*m->nv + dof];
-      J(1,j) = jacp[1*m->nv + dof];
-      J(2,j) = jacp[2*m->nv + dof];
-      J(3,j) = jacr[0*m->nv + dof];
-      J(4,j) = jacr[1*m->nv + dof];
-      J(5,j) = jacr[2*m->nv + dof];
-    }
-
-    // 10) DLS inverse: qdot
-    Eigen::Matrix<double,6,6> A = J * J.transpose();
-    A += (mu_*mu_) * Eigen::Matrix<double,6,6>::Identity();
-    Eigen::Matrix<double,7,1> qdot = J.transpose() * A.inverse() * twist_w;
-
-    for (int i = 0; i < 7; ++i) {
-      qdot(i) = clamp(qdot(i), -qdot_max_, qdot_max_);
-    }
-
-    // 11) integrate -> position targets
-    for (int i = 0; i < 7; ++i) {
-      q_cmd_[i] += qdot(i) * dt;
-
-      if (jnt_ids_[i] >= 0 && m->jnt_limited[jnt_ids_[i]]) {
-        const double lo = m->jnt_range[2*jnt_ids_[i] + 0];
-        const double hi = m->jnt_range[2*jnt_ids_[i] + 1];
-        q_cmd_[i] = clamp(q_cmd_[i], lo, hi);
-      }
-
-      if (act_ids_[i] >= 0) {
-        d->ctrl[act_ids_[i]] = q_cmd_[i];
-      }
-    }
-
-    // 12) sanity: 看看机械臂是否真的在动
-    if ((step_count_ % 120) == 0 && qpos_adr_[0] >= 0 && act_ids_[0] >= 0) {
-      std::printf("[IBVS] qpos0=%.3f ctrl0=%.3f\n",
-                  d->qpos[qpos_adr_[0]], d->ctrl[act_ids_[0]]);
-    }
-
-    ++step_count_;
-  }
-
-  void onReset(mjModel* m, mjData* d) override {
-    (void)m;
-    for (int i = 0; i < 7; ++i) {
-      if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]];
-    }
-    mode_ = Mode::HOMING;
-    home_hold_acc_ = 0.0;
-    step_count_ = 0;
-  }
-
-private:
-  bool ready_{false};
-
-  std::array<int,7> act_ids_{};
-  std::array<int,7> jnt_ids_{};
-  std::array<int,7> qpos_adr_{ { -1,-1,-1,-1,-1,-1,-1 } };
-  std::array<int,7> dof_adr_{  { -1,-1,-1,-1,-1,-1,-1 } };
-
-  int cam_site_id_{-1};
-  int tag_body_id_{-1};
-
-  // frame align
-  Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
-
-  // homing
-  Mode mode_{Mode::HOMING};
-  std::array<double,7> q_home_{ {0,0,0,0,0,0,0} };
-  double home_tol_{0.02};
-  double home_hold_time_{0.3};
-  double home_hold_acc_{0.0};
-
-  // IBVS
-  double lambda_{0.7};
-  double tag_half_{0.06};
-  double Z_des_{0.60};
-
-  double mu_{0.02};
-  double qdot_max_{0.6};
-  double vmax6_[6] = {0.15, 0.15, 0.20, 0.6, 0.6, 0.6};
-
-  int step_count_{0};
-
-  vpServo task_;
-  vpPoint obj_pts_[4];
-  vpFeaturePoint s_cur_[4];
-  vpFeaturePoint s_star_[4];
-
-  std::array<double,7> q_cmd_{ {0,0,0,0,0,0,0} };
-};
-
-
-
-
-
-
-// ---- ViSP ----
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpImage.h>
-
-#include <visp3/detection/vpDetectorAprilTag.h>
-
-
-class IBVSFromMujocoCameraViewer : public MuJocoViewer {
-public:
-  using MuJocoViewer::MuJocoViewer;
-
-  void enableTwistToQdotCheck(bool enable, double tol = 1e-3) {
-    check_twist_to_qdot_ = enable;
-    qdot_check_tol_ = tol;
-    (void)qdot_check_tol_;
-  }
-
-  bool twistToQdotCheckPassed() const { return !qdot_check_failed_; }
-  double twistToQdotMaxError() const { return qdot_check_max_err_; }
-  int twistToQdotCheckSamples() const { return qdot_check_samples_; }
-
-protected:
-  enum class Mode { HOMING, IBVS };
-
-  void initOnce(mjModel* m, mjData* d) override {
-    setupCamera(3.0, -170.0, -40.0);
-    enablePiPCamera("hand_cam", 405, 1000, 320, 240);
-
-    const char* act_names[7] = {
-      "R_SHOULDER_P_pos",
-      "R_SHOULDER_R_pos",
-      "R_SHOULDER_Y_pos",
-      "R_ELBOW_R_pos",
-      "R_WRIST_P_pos",
-      "R_WRIST_Y_pos",
-      "R_WRIST_R_pos"
-    };
-    const char* jnt_names[7] = {
-      "R_SHOULDER_P",
-      "R_SHOULDER_R",
-      "R_SHOULDER_Y",
-      "R_ELBOW_R",
-      "R_WRIST_P",
-      "R_WRIST_Y",
-      "R_WRIST_R"
-    };
-
-    for (int i = 0; i < 7; ++i) {
-      act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
-      jnt_ids_[i] = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]);
-      if (act_ids_[i] < 0) {
-        std::cout << "[IBVS] Cannot find actuator " << act_names[i] << std::endl;
-      }
-      if (jnt_ids_[i] < 0) {
-        std::cout << "[IBVS] Cannot find joint " << jnt_names[i] << std::endl;
-      }
-      if (jnt_ids_[i] >= 0) {
-        qpos_adr_[i] = m->jnt_qposadr[jnt_ids_[i]];
-      }
-    }
-
-    cam_site_id_ = mj_name2id(m, mjOBJ_SITE, "R_CAM_SITE");
-    if (cam_site_id_ < 0) {
-      std::cout << "[IBVS] Missing site R_CAM_SITE in XML" << std::endl;
-      ready_ = false;
-      return;
-    }
-
-    hand_cam_id_ = mj_name2id(m, mjOBJ_CAMERA, "hand_cam");
-    if (hand_cam_id_ < 0) {
-      std::cout << "[IBVS] Missing camera hand_cam in XML" << std::endl;
-      ready_ = false;
-      return;
-    }
-    mujoco_camera_ = std::make_shared<device::MujocoCamera>(
-      [this](std::vector<unsigned char>& rgb,
-             std::vector<float>& depth,
-             int& width,
-             int& height,
-             uint64_t& frame_id) {
-        return getPiPCameraRGBD(rgb, depth, width, height, frame_id);
-      });
-    mujoco_camera_->setFovyDeg(m->cam_fovy[hand_cam_id_]);
-    // MuJoCo 渲染线程与控制线程不同步：允许复用最近一帧，避免长时间 no_new_frame。
-    mujoco_camera_->setConsumeNewFrameOnly(false);
-
-    q_home_ = {-0.2423, 1.2929,1.61, 1.58, -2.8792,  0.1150,-0.08};
-
-    ibvs_controller_ = std::make_unique<IbvsController>();
-    ibvs_controller_->setMu(mu_);
-    ibvs_controller_->setQdotMax(qdot_max_);
-    ibvs_controller_->setDepthMode(IbvsController::DepthMode::MONOCULAR);
-    ibvs_controller_->setDepthZGain(1.0);
-    ibvs_controller_->setVelocityLimit6(vmax6_);
-    ibvs_controller_->setTrackedTagId(tracked_tag_id_);
-    ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.05, 0),
-                                           Eigen::Vector3d(0.0, 0.0, 0.4));
-    ibvs_controller_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
-
-    const Eigen::Matrix3d R_align = (Eigen::Matrix3d() <<
-      1, 0, 0,
-      0, -1, 0,
-      0, 0, -1).finished();
-    ibvs_controller_->setAlignCameraToVisp(R_align);
-    ibvs_controller_->setAlignCameraToUrdf(R_align);
-
-    config::PinocchioDlsIKConfig dls_cfg;
-    dls_cfg.set_urdf_path(urdf_path_for_check_);
-    dls_cfg.set_base_frame_name("PELVIS_S");
-    dls_cfg.set_flange_frame_name("R_WRIST_R_S");
-    dls_cfg.set_tcp_frame_name(camera_frame_name_for_check_);
-    dls_cfg.set_max_iters(100);
-    dls_cfg.set_pos_eps(1e-6);
-    dls_cfg.set_rot_eps(1e-6);
-    dls_cfg.set_damping(mu_);
-    auto solver = std::make_shared<PinocchioDlsIKSolver>(dls_cfg);
-    if (!solver->init()) {
-      std::cout << "[IBVS] PinocchioDlsIKSolver init failed" << std::endl;
-      ready_ = false;
-      return;
-    }
-
-    if (!ibvs_controller_->init(solver, camera_frame_name_for_check_)) {
-      std::cout << "[IBVS] IbvsController init failed" << std::endl;
-      ready_ = false;
-      return;
-    }
-
-    perception_ = std::make_shared<cmvr::perception::AprilTagPerception>(mujoco_camera_);
-    perception_->setTagSize(tag_size_m_);
-    perception_opt_.detect_tags = true;
-    perception_opt_.fetch_encoded = false;
-    perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
-    ibvs_controller_->setPerception(perception_);
-
-    std::vector<double> q_init(7, 0.0);
-    for (int i = 0; i < 7; ++i) {
-      q_init[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
-      q_cmd_[i] = q_init[i];
-    }
-    ibvs_controller_->reset(q_init);
-
-    mode_ = Mode::HOMING;
-    home_hold_acc_ = 0.0;
-    step_count_ = 0;
-    qdot_check_max_err_ = 0.0;
-    qdot_check_samples_ = 0;
-    qdot_check_failed_ = false;
-    ready_ = true;
-
-    std::cout << "[IBVS] initOnce OK. cam_site_id=" << cam_site_id_
-              << " hand_cam_id=" << hand_cam_id_ << std::endl;
-  }
-
-  void controlCallback(mjModel* m, mjData* d) override {
-    if (!ready_) return;
-    const double dt = m->opt.timestep;
-
-    if (mode_ == Mode::HOMING) {
-      double max_err = 0.0;
-      for (int i = 0; i < 7; ++i) {
-        if (act_ids_[i] < 0 || qpos_adr_[i] < 0) continue;
-        d->ctrl[act_ids_[i]] = q_home_[i];
-        const double qi = d->qpos[qpos_adr_[i]];
-        max_err = std::max(max_err, std::abs(qi - q_home_[i]));
-      }
-
-      if (max_err < home_tol_) home_hold_acc_ += dt;
-      else home_hold_acc_ = 0.0;
-
-      if (home_hold_acc_ > home_hold_time_) {
-        std::vector<double> q_now(7, 0.0);
-        for (int i = 0; i < 7; ++i) {
-          q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
-          q_cmd_[i] = q_now[i];
-        }
-        ibvs_controller_->reset(q_now);
-        mode_ = Mode::IBVS;
-        std::cout << "[IBVS] switch HOMING -> IBVS" << std::endl;
-      }
-      return;
-    }
-
-    std::vector<double> q_now(7, 0.0);
-    for (int i = 0; i < 7; ++i) {
-      q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
-    }
-
-    if (perception_) {
-      switch (ibvs_controller_->depthMode()) {
-        case IbvsController::DepthMode::MONOCULAR:
-          perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE;
-          break;
-        case IbvsController::DepthMode::PREFER_DEPTH:
-          perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER;
-          break;
-        case IbvsController::DepthMode::DEPTH_ONLY:
-          perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE;
-          break;
-      }
-      const bool perception_ok = perception_->update(perception_opt_);
-      if (!perception_ok && (step_count_ % 60) == 0) {
-        std::cout << "[IBVS] perception update failed: "
-                  << cmvr::perception::AprilTagPerception::statusToString(perception_->lastStatus())
-                  << std::endl;
-      }
-    }
-
-    std::vector<double> q_cmd_next;
-    const bool ok = ibvs_controller_->compute(q_now, dt, q_cmd_next);
-
-    if (!ok) {
-      if ((step_count_ % 60) == 0) {
-        const auto st = ibvs_controller_->lastComputeStatus();
-        std::cout << "[IBVS] compute skipped: "
-                  << IbvsController::statusToString(st) << std::endl;
-        if (st == IbvsController::ComputeStatus::TAG_MISMATCH && perception_ && perception_->hasTags()) {
-          std::cout << "[IBVS] visible tag ids:";
-          for (const auto& t : perception_->tags()) {
-            std::cout << " " << t.id;
-          }
-          std::cout << std::endl;
-        }
-      }
-      ++step_count_;
-      return;
-    }
-
-    if (q_cmd_next.size() == 7) {
-      for (int i = 0; i < 7; ++i) {
-        q_cmd_[i] = q_cmd_next[i];
-      }
-    }
-
-    for (int i = 0; i < 7; ++i) {
-      if (jnt_ids_[i] >= 0 && m->jnt_limited[jnt_ids_[i]]) {
-        const double lo = m->jnt_range[2 * jnt_ids_[i] + 0];
-        const double hi = m->jnt_range[2 * jnt_ids_[i] + 1];
-        q_cmd_[i] = clamp(q_cmd_[i], lo, hi);
-      }
-      if (act_ids_[i] >= 0) {
-        d->ctrl[act_ids_[i]] = q_cmd_[i];
-      }
-    }
-
-    if ((step_count_ % 60) == 0) {
-      const auto& v_c = ibvs_controller_->lastCameraTwistVisp();
-      const auto& t_co = ibvs_controller_->lastTagPositionVisp();
-      std::cout << "[IBVS] v_c=[" << v_c[0] << " " << v_c[1] << " " << v_c[2]
-                << " " << v_c[3] << " " << v_c[4] << " " << v_c[5] << "]" << std::endl;
-      std::cout << "[IBVS] t_co(visp)=[" << t_co.x() << " " << t_co.y() << " " << t_co.z() << "]" << std::endl;
-      std::cout << "[IBVS] z_source="
-                << IbvsController::depthUsageToString(ibvs_controller_->lastDepthUsage()) << std::endl;
-    }
-
-    ++step_count_;
-  }
-
-  void onReset(mjModel* m, mjData* d) override {
-    (void)m;
-    std::vector<double> q_now(7, 0.0);
-    for (int i = 0; i < 7; ++i) {
-      q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
-      q_cmd_[i] = q_now[i];
-    }
-    if (ibvs_controller_) {
-      ibvs_controller_->reset(q_now);
-    }
-    mode_ = Mode::HOMING;
-    home_hold_acc_ = 0.0;
-    step_count_ = 0;
-    qdot_check_max_err_ = 0.0;
-    qdot_check_samples_ = 0;
-    qdot_check_failed_ = false;
-  }
-
-private:
-  bool ready_{false};
-
-  std::array<int, 7> act_ids_{};
-  std::array<int, 7> jnt_ids_{};
-  std::array<int, 7> qpos_adr_{{-1, -1, -1, -1, -1, -1, -1}};
-  int cam_site_id_{-1};
-  int hand_cam_id_{-1};
-
-  Mode mode_{Mode::HOMING};
-  std::array<double, 7> q_home_{{0, 0, 0, 0, 0, 0, 0}};
-  double home_tol_{0.02};
-  double home_hold_time_{0.3};
-  double home_hold_acc_{0.0};
-
-  double mu_{0.02};
-  double qdot_max_{0.6};
-  std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
-
-  bool check_twist_to_qdot_{true};
-  double qdot_check_tol_{1e-3};
-  double qdot_check_max_err_{0.0};
-  int qdot_check_samples_{0};
-  bool qdot_check_failed_{false};
-  std::string urdf_path_for_check_{
-    "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"};
-  std::string camera_frame_name_for_check_{"R_CAM"};
-  int tracked_tag_id_{0};
-  double tag_size_m_{0.12};
-
-  std::unique_ptr<IbvsController> ibvs_controller_{nullptr};
-  std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};
-  std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
-  cmvr::perception::AprilTagPerception::Options perception_opt_{};
-
-  std::array<double, 7> q_cmd_{{0, 0, 0, 0, 0, 0, 0}};
-  int step_count_{0};
-};
-
-
-
-
-
-class ControllerViewer : public MuJocoViewer {
-public:
-  using MuJocoViewer::MuJocoViewer;
-
-  void setTarget(const std::vector<double> &q_target) {
-    std::lock_guard<std::mutex> lock(mtx_);
-    q_cmd_ = q_target;
-  }
-
-protected:
-  void initOnce(mjModel *m, mjData *d) override
-  {
-    (void)d;
-    setupCamera(3.0, -170.0, -40.0);
-    // enablePiPCamera("hand_cam", 320, 240, 10);
-    enablePiPCamera("hand_cam", 405, 1200, 320, 240);
-
-    const char *act_names[7] = {
-      "R_SHOULDER_P_pos",
-      "R_SHOULDER_R_pos",
-      "R_SHOULDER_Y_pos",
-      "R_ELBOW_R_pos",
-      "R_WRIST_P_pos",
-      "R_WRIST_Y_pos",
-      "R_WRIST_R_pos"
-    };
-
-    for (int i = 0; i < 7; ++i) {
-      act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
-      if (act_ids_[i] < 0) {
-        std::cout << "Cannot find actuator " << act_names[i] << std::endl;
-      }
-    }
-
-    act_ids_inited_ = true;
-  }
-
-  void controlCallback(mjModel *m, mjData *d) override
-  {
-    (void)m;
-    if (!act_ids_inited_) return;
-
-    std::vector<double> q_local;
-    {
-      std::lock_guard<std::mutex> lock(mtx_);
-      q_local = q_cmd_;
-    }
-
-    for (int i = 0; i < 7; ++i) {
-      if (act_ids_[i] < 0) continue;
-      if (i < static_cast<int>(q_local.size())) {
-        d->ctrl[act_ids_[i]] = q_local[i];
-      }
-    }
-  }
-
-  void onReset(mjModel *m, mjData *d) override
-  {
-    (void)m;
-    (void)d;
-    std::lock_guard<std::mutex> lock(mtx_);
-    q_cmd_.assign(7, 0.0);
-  }
-
-private:
-  std::array<int, 7> act_ids_{};
-  bool act_ids_inited_{false};
-  std::vector<double> q_cmd_{7, 0.0};
-  std::mutex mtx_;
-};
-
-TEST(controller_test, mujoco_viewer_smoke)
-{
-  ControllerViewer viewer("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml");
-
-  std::vector<double> q_seed = {
-    0.25, 1.00, M_PI / 2 - 0.2, M_PI / 2 - 0.2, -M_PI+ 0.5 , 0, 0
-  };
-  viewer.setTarget(q_seed);
-  viewer.run();
-}
-
-TEST(controller_test, mujoco_ibvs_sim_gt)
-{
-  IBVSHomingViewer viewer("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml");
-  viewer.run();
-}
-
-TEST(controller_test, mujoco_camera_apriltag_ibvs_full)
-{
-  IBVSFromMujocoCameraViewer viewer(
-    "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml"
-  );
-  viewer.enableTwistToQdotCheck(true, 1e-3);
-  viewer.run();
-
-  // EXPECT_GT(viewer.twistToQdotCheckSamples(), 0)
-  //   << "no valid samples collected for twistToQdot consistency";
-  // if (viewer.twistToQdotCheckSamples() > 0) {
-  //   EXPECT_TRUE(viewer.twistToQdotCheckPassed())
-  //     << "twistToQdot mismatch, max_abs_err=" << viewer.twistToQdotMaxError();
-  // }
-}

cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt

@@ -25,44 +25,4 @@ target_link_libraries(ik_solver PUBLIC
 
 add_library(cmvr_es::ik_solver ALIAS ik_solver)
 
-install(TARGETS ik_solver LIBRARY DESTINATION lib)
+install(TARGETS ik_solver LIBRARY DESTINATION lib)
-# --------------------------------------------------------
-# Unit test
-# --------------------------------------------------------
-
-add_executable(srs_ik_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/srs_ik_test.cpp
-)
-
-
-target_link_libraries(srs_ik_test
-        PRIVATE
-        cmvr_es::ik_solver
-        cmvr_es::base_motion
-        cmvr_es::arm_motion
-		cmvr_es::proto
-        cmvr_es::mujoco_viewer
-		gtest
-		gtest_main
-        pthread
-        glog
-        matplot
-)
-
-
-
-
-add_executable(ik_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/ik_test.cpp
-)
-
-target_link_libraries(ik_test
-        PRIVATE
-        cmvr_es::ik_solver
-        cmvr_es::mujoco_viewer 
-        gtest
-        gtest_main
-        pthread
-        glog
-        cmvr_es::proto
-)

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h

@@ -54,11 +54,6 @@ public:
                            std::vector<double>& qdot_out,
                            double qdot_abs_max = std::numeric_limits<double>::infinity()) const override;
 
-    void setJointLimitAvoidance(bool enable,
-                                double gain = 0.2,
-                                double margin_ratio = 0.15,
-                                double max_push = 0.25);
-
     void setMaxIters(int iters) { max_iters_ = iters; }
     void setDamping(double d)   { damping_ = d; }
     void setEps(double pos_eps, double rot_eps) { pos_eps_ = pos_eps; rot_eps_ = rot_eps; }
@@ -68,7 +63,7 @@ public:
 private:
     Eigen::MatrixXd dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda);
 
-    bool refreshJointLimits_(const config::PinocchioDlsIKConfig& cfg);
+    bool refreshJointLimits_();
 
     Eigen::VectorXd computeJointLimitAvoidanceVelocity(const Eigen::VectorXd& q_chain) const;
 
@@ -77,11 +72,6 @@ private:
                                        const Eigen::VectorXd& secondary) const;
 
 private:
-    bool limit_avoidance_enabled_{false};
-    double limit_avoidance_gain_{0.2};
-    double limit_avoidance_margin_ratio_{0.15};
-    double limit_avoidance_max_push_{0.25};
-
     bool initialized_{false};
 
     int max_iters_;

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h

@@ -3,6 +3,7 @@
 
 #include "algorithms/kinematics/ik_solver/common/include/ik_solver.h"
 #include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
+#include "cmvr/config/joint_limits_config.pb.h"
 
 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/multibody/data.hpp>
@@ -74,6 +75,9 @@ public:
     int chainDof() const { return chain_q_dof_; }
     int chainVelocityDof() const { return chain_v_dof_; }
 
+    void setJointLimitPolicy(const config::JointLimitPolicyConfig& policy);
+    const config::JointLimitPolicyConfig& jointLimitPolicy() const { return joint_limit_policy_; }
+
     bool computeJacobianBaseAtQ(const std::vector<double>& q_chain,
                                 bool is_tcp,
                                 Eigen::MatrixXd& jacobian_base,
@@ -84,6 +88,12 @@ public:
                                 Eigen::MatrixXd& jacobian_base,
                                 Eigen::Matrix3d& base_R_ee);
 
+    bool computePoseAndJacobianBaseAtQ(const std::vector<double>& q_chain,
+                                       bool is_tcp,
+                                       Eigen::Matrix4d& pose_base,
+                                       Eigen::MatrixXd& jacobian_base,
+                                       Eigen::Matrix3d& base_R_ee);
+
     bool computeTwistBaseAtQ(const std::vector<double>& q_chain,
                              const std::vector<double>& qdot_chain,
                              bool is_tcp,
@@ -153,6 +163,13 @@ protected:
                                   Eigen::Matrix3d* base_R_ee_out = nullptr,
                                   Eigen::VectorXd* q_full_out = nullptr);
 
+    Eigen::VectorXd applyJointSoftLimitsToVelocity(const Eigen::VectorXd& q_chain,
+                                                   const Eigen::VectorXd& qdot) const;
+
+    bool jointLimitsDisabled() const;
+    void disableJointLimitCache();
+    static double disabledJointLimitAbs();
+
     /**
      * @brief 在当前 `model_` 上更新 FK 与 frame placement。
      */
@@ -198,6 +215,8 @@ protected:
     int chain_v_start_{0};
     /** @brief 当前链关节速度自由度数量。 */
     int chain_v_dof_{0};
+
+    config::JointLimitPolicyConfig joint_limit_policy_{};
 };
 
 } // namespace cmvr

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h

@@ -14,6 +14,7 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "cmvr/config/pinocchio_qp_ik_config.pb.h"
@@ -51,6 +52,8 @@ public:
     using IKSolver::update_joints_state;
 
 private:
+    bool refreshJointLimitPolicy_();
+
     // 配置
     config::PinocchioQpIKConfig config_;
     std::string urdf_path_;

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp

@@ -49,22 +49,16 @@ PinocchioDlsIKSolver::PinocchioDlsIKSolver(const config::PinocchioDlsIKConfig& c
     , damping_(cfg.damping() > 0.0 ? cfg.damping() : 1e-4)
     , config_(cfg)
 {
-    if (cfg.has_joint_limit_avoidance()) {
+    setJointLimitPolicy(cfg.joint_limit_policy());
-        const auto& avoidance = cfg.joint_limit_avoidance();
-        setJointLimitAvoidance(
-            avoidance.enable(),
-            positiveOr(avoidance.gain(), limit_avoidance_gain_),
-            positiveOr(avoidance.margin_ratio(), limit_avoidance_margin_ratio_),
-            positiveOr(avoidance.max_push(), limit_avoidance_max_push_));
-    }
 }
 
-bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfig& cfg) {
+bool PinocchioDlsIKSolver::refreshJointLimits_() {
-    const auto source = cfg.has_joint_limits()
+    const auto source = joint_limit_policy_.limits().source();
-                            ? cfg.joint_limits().source()
+    if (jointLimitsDisabled()) {
-                            : config::JOINT_LIMIT_SOURCE_URDF;
+        disableJointLimitCache();
-    if (source == config::JOINT_LIMIT_SOURCE_UNKNOWN ||
+        return true;
-        source == config::JOINT_LIMIT_SOURCE_URDF) {
+    }
+    if (source == config::JOINT_LIMIT_SOURCE_URDF) {
         return true;
     }
 
@@ -80,12 +74,11 @@ bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfi
     }
 
     std::unordered_map<std::string, config::JointLimitConfig> custom_limits;
-    if (cfg.has_joint_limits()) {
+    const auto& limits = joint_limit_policy_.limits();
-        custom_limits.reserve(static_cast<std::size_t>(cfg.joint_limits().joints_size()));
+    custom_limits.reserve(static_cast<std::size_t>(limits.joints_size()));
-        for (const auto& item : cfg.joint_limits().joints()) {
+    for (const auto& item : limits.joints()) {
-            if (!item.joint_name().empty()) {
+        if (!item.joint_name().empty()) {
-                custom_limits[item.joint_name()] = item;
+            custom_limits[item.joint_name()] = item;
-            }
         }
     }
 
@@ -101,24 +94,25 @@ bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfi
             return false;
         }
         const auto& limit = it->second;
-        if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
+        if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
-            !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
+            !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
-            limit.velocity() <= 0.0) {
+            limit.qd() <= 0.0) {
             CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] invalid custom joint limit for "
                             << limit.joint_name();
             return false;
         }
-        joint_pos_lower_limits_[i] = limit.lower();
+        joint_pos_lower_limits_[i] = limit.q_lb();
-        joint_pos_upper_limits_[i] = limit.upper();
+        joint_pos_upper_limits_[i] = limit.q_ub();
-        joint_vel_limits_[i] = std::abs(limit.velocity());
+        joint_vel_limits_[i] = std::abs(limit.qd());
     }
     return true;
 }
 
 Eigen::VectorXd PinocchioDlsIKSolver::computeJointLimitAvoidanceVelocity(
     const Eigen::VectorXd& q_chain) const {
-    if (!limit_avoidance_enabled_ ||
+    const auto& avoidance = joint_limit_policy_.avoidance();
-        limit_avoidance_gain_ <= 0.0 ||
+    if (jointLimitsDisabled() ||
+        !avoidance.enable() || avoidance.gain() <= 0.0 ||
         chain_v_dof_ != chain_q_dof_ ||
         q_chain.size() != chain_q_dof_ ||
         joint_pos_lower_limits_.size() != chain_q_dof_ ||
@@ -130,10 +124,10 @@ Eigen::VectorXd PinocchioDlsIKSolver::computeJointLimitAvoidanceVelocity(
         q_chain,
         joint_pos_lower_limits_,
         joint_pos_upper_limits_,
-        limit_avoidance_enabled_,
+        avoidance.enable(),
-        limit_avoidance_gain_,
+        positiveOr(avoidance.gain(), 0.2),
-        limit_avoidance_margin_ratio_,
+        positiveOr(avoidance.margin_ratio(), 0.15),
-        limit_avoidance_max_push_);
+        positiveOr(avoidance.max_push(), 0.25));
 }
 
 Eigen::VectorXd PinocchioDlsIKSolver::projectToNullspace(const Eigen::MatrixXd& J_pinv,
@@ -154,7 +148,7 @@ bool PinocchioDlsIKSolver::init() {
         CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] Failed to init pinocchio base: " << err;
         return false;
     }
-    if (!refreshJointLimits_(config_)) {
+    if (!refreshJointLimits_()) {
         return false;
     }
 
@@ -299,16 +293,6 @@ bool PinocchioDlsIKSolver::ik(const std::string& base_link,
     return true;
 }
 
-void PinocchioDlsIKSolver::setJointLimitAvoidance(bool enable,
-                                                  double gain,
-                                                  double margin_ratio,
-                                                  double max_push) {
-    limit_avoidance_enabled_ = enable;
-    limit_avoidance_gain_ = std::max(0.0, gain);
-    limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49);
-    limit_avoidance_max_push_ = max_push;
-}
-
 bool PinocchioDlsIKSolver::ik(const std::string& base_link,
                               const std::string& ee_link,
                               const Eigen::Matrix<double,6,1>& target_vel,
@@ -417,11 +401,11 @@ bool PinocchioDlsIKSolver::ik(const std::string& base_link,
         qdot += projectToNullspace(J_pinv, J, qdot_avoid);
     }
 
+    qdot = cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max);
+    qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
     joints_vel.resize(chain_v_dof_);
-    const Eigen::VectorXd qdot_limited =
-        cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max);
     for (int i = 0; i < chain_v_dof_; ++i) {
-        joints_vel[i] = qdot_limited[i];
+        joints_vel[i] = qdot[i];
     }
 
     return true;
@@ -454,15 +438,25 @@ bool PinocchioDlsIKSolver::solveVelocityBase(const Eigen::MatrixXd& jacobian_bas
     Eigen::VectorXd qdot = jacobian_base.transpose() * ldlt.solve(target_twist_base);
 
     const Eigen::Map<const Eigen::VectorXd> q_chain(q_chain_std.data(), chain_q_dof_);
-    const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain);
+    const auto& avoidance = joint_limit_policy_.avoidance();
-    if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
+    if (!jointLimitsDisabled() &&
-        const Eigen::Matrix<double,6,6> A_inv =
+        avoidance.enable() &&
-            ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
+        avoidance.gain() > 0.0 &&
-        const Eigen::MatrixXd J_pinv = jacobian_base.transpose() * A_inv;
+        chain_v_dof_ == chain_q_dof_ &&
-        qdot += projectToNullspace(J_pinv, jacobian_base, qdot_avoid);
+        q_chain.size() == chain_q_dof_ &&
+        joint_pos_lower_limits_.size() == chain_q_dof_ &&
+        joint_pos_upper_limits_.size() == chain_q_dof_) {
+        const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain);
+        if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
+            const Eigen::Matrix<double,6,6> A_inv =
+                ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
+            const Eigen::MatrixXd J_pinv = jacobian_base.transpose() * A_inv;
+            qdot += projectToNullspace(J_pinv, jacobian_base, qdot_avoid);
+        }
     }
 
     qdot = cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max);
+    qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
     qdot_out.resize(chain_v_dof_);
     for (int i = 0; i < chain_v_dof_; ++i) {
         qdot_out[i] = qdot[i];

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp

@@ -2,13 +2,19 @@
 
 #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
 #include "common/base/logging/logger.h"
+#include "common/config/config_files.h"
 
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/jacobian.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 
+#include <algorithm>
+#include <cmath>
+
 namespace cmvr {
 
+using cmvr::common::config::positiveOr;
+
 PinocchioIKBase::PinocchioIKBase(const std::string& urdf_path,
                                  const std::string& base_frame_name,
                                  const std::string& flange_frame_name,
@@ -116,6 +122,75 @@ const pinocchio::SE3& PinocchioIKBase::getBasePoseWorld() const {
     return base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_];
 }
 
+void PinocchioIKBase::setJointLimitPolicy(const config::JointLimitPolicyConfig& policy)
+{
+    joint_limit_policy_ = policy;
+}
+
+bool PinocchioIKBase::jointLimitsDisabled() const
+{
+    return !joint_limit_policy_.limits().enable();
+}
+
+double PinocchioIKBase::disabledJointLimitAbs()
+{
+    return 1e6;
+}
+
+void PinocchioIKBase::disableJointLimitCache()
+{
+    const double big = disabledJointLimitAbs();
+    joint_pos_lower_limits_.resize(chain_q_dof_);
+    joint_pos_upper_limits_.resize(chain_q_dof_);
+    joint_vel_limits_.resize(chain_v_dof_);
+    joint_pos_lower_limits_.setConstant(-big);
+    joint_pos_upper_limits_.setConstant(big);
+    joint_vel_limits_.setConstant(big);
+}
+
+Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
+    const Eigen::VectorXd& q_chain,
+    const Eigen::VectorXd& qdot) const
+{
+    const auto& config = joint_limit_policy_.soft_limit();
+    if (jointLimitsDisabled() || !config.enable()) {
+        return qdot;
+    }
+    if (joint_pos_lower_limits_.size() != q_chain.size() ||
+        joint_pos_upper_limits_.size() != q_chain.size() ||
+        qdot.size() != q_chain.size()) {
+        return qdot;
+    }
+
+    const double margin_ratio = positiveOr(config.margin_ratio(), 0.08);
+    const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02);
+    Eigen::VectorXd limited = qdot;
+    for (Eigen::Index i = 0; i < q_chain.size(); ++i) {
+        const double lower = joint_pos_lower_limits_[i];
+        const double upper = joint_pos_upper_limits_[i];
+        if (!std::isfinite(lower) || !std::isfinite(upper) || upper <= lower) {
+            continue;
+        }
+
+        const double span = upper - lower;
+        const double margin = std::max(min_margin_rad, margin_ratio * span);
+        if (limited[i] < 0.0 && q_chain[i] < lower + margin) {
+            const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0);
+            limited[i] *= ratio;
+            if (q_chain[i] <= lower) {
+                limited[i] = std::max(0.0, limited[i]);
+            }
+        } else if (limited[i] > 0.0 && q_chain[i] > upper - margin) {
+            const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0);
+            limited[i] *= ratio;
+            if (q_chain[i] >= upper) {
+                limited[i] = std::min(0.0, limited[i]);
+            }
+        }
+    }
+    return limited;
+}
+
 void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) {
     pinocchio::forwardKinematics(model_, *data_, q_full);
     pinocchio::updateFramePlacements(model_, *data_);
@@ -190,6 +265,52 @@ bool PinocchioIKBase::computeJacobianBaseAtQ(const std::vector<double>& q_chain,
                                 nullptr);
 }
 
+bool PinocchioIKBase::computePoseAndJacobianBaseAtQ(
+    const std::vector<double>& q_chain_std,
+    const bool is_tcp,
+    Eigen::Matrix4d& pose_base,
+    Eigen::MatrixXd& jacobian_base,
+    Eigen::Matrix3d& base_R_ee)
+{
+    if (!data_) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] computePoseAndJacobianBaseAtQ called before pinocchio init";
+        return false;
+    }
+    if (static_cast<int>(q_chain_std.size()) != chain_q_dof_) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] computePoseAndJacobianBaseAtQ: q size mismatch";
+        return false;
+    }
+
+    const Eigen::Map<const Eigen::VectorXd> q_chain(q_chain_std.data(), chain_q_dof_);
+    Eigen::VectorXd q_full;
+    if (!buildFullQFromChain(q_chain, q_full, "computePoseAndJacobianBaseAtQ")) {
+        return false;
+    }
+
+    updateKinematics(q_full);
+
+    const pinocchio::FrameIndex ee_id = (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_;
+    const pinocchio::SE3& oM_base = getBasePoseWorld();
+    const pinocchio::SE3& oM_ee = data_->oMf[ee_id];
+    const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
+    pose_base = se3ToMatrix4(base_M_ee);
+    base_R_ee = base_M_ee.rotation();
+
+    Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_world(6, model_.nv);
+    pinocchio::computeFrameJacobian(model_, *data_, q_full,
+                                    ee_id,
+                                    pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
+                                    jacobian_world);
+
+    jacobian_base = extractChainJacobian(jacobian_world);
+
+    const Eigen::Matrix3d R_bo = oM_base.rotation().transpose();
+    jacobian_base.topRows(3) = R_bo * jacobian_base.topRows(3);
+    jacobian_base.bottomRows(3) = R_bo * jacobian_base.bottomRows(3);
+
+    return true;
+}
+
 bool PinocchioIKBase::computeMeasuredTwistBase(const std::vector<double>& q_chain,
                                                const std::vector<double>& qdot_chain,
                                                const pinocchio::FrameIndex ee_id,

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp

@@ -14,6 +14,8 @@
 
 #include <algorithm>   // std::clamp, std::max, std::min
 #include <cmath>       // std::sqrt
+#include <limits>
+#include <unordered_map>
 
 namespace cmvr {
     using Eigen::Matrix4d;
@@ -36,6 +38,69 @@ namespace cmvr {
           , qp_time_limit_(positiveOr(config.qp_time_limit(), 1e-2))
           , solver_()
     {
+        setJointLimitPolicy(config.joint_limit_policy());
+    }
+
+    bool PinocchioQpIKSolver::refreshJointLimitPolicy_()
+    {
+        const auto& policy = jointLimitPolicy();
+        const auto source = policy.limits().source();
+        if (jointLimitsDisabled()) {
+            disableJointLimitCache();
+            qdd_max_global_.resize(chain_q_dof_);
+            qdd_max_global_.setConstant(disabledJointLimitAbs());
+            return true;
+        }
+        if (source == config::JOINT_LIMIT_SOURCE_URDF) {
+            return true;
+        }
+        if (source != config::JOINT_LIMIT_SOURCE_CUSTOM) {
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] unsupported joint limit source";
+            return false;
+        }
+
+        std::vector<std::string> joint_names;
+        if (!getChainJointNames(joint_names) || joint_names.empty()) {
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] failed to get chain joint names for custom limits";
+            return false;
+        }
+
+        std::unordered_map<std::string, config::JointLimitConfig> custom_limits;
+        const auto& limits = policy.limits();
+        custom_limits.reserve(static_cast<std::size_t>(limits.joints_size()));
+        for (const auto& item : limits.joints()) {
+            if (!item.joint_name().empty()) {
+                custom_limits[item.joint_name()] = item;
+            }
+        }
+
+        const auto dof = static_cast<Eigen::Index>(joint_names.size());
+        joint_pos_lower_limits_.resize(dof);
+        joint_pos_upper_limits_.resize(dof);
+        joint_vel_limits_.resize(dof);
+        qdd_max_global_.resize(dof);
+        for (Eigen::Index i = 0; i < dof; ++i) {
+            const auto& joint_name = joint_names[static_cast<std::size_t>(i)];
+            const auto it = custom_limits.find(joint_name);
+            if (it == custom_limits.end()) {
+                CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] missing custom joint limit for " << joint_name;
+                return false;
+            }
+            const auto& limit = it->second;
+            if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
+                !std::isfinite(limit.qd()) || !std::isfinite(limit.qdd()) ||
+                limit.q_ub() <= limit.q_lb() || limit.qd() <= 0.0 ||
+                limit.qdd() < 0.0) {
+                CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] invalid custom joint limit for "
+                                << limit.joint_name();
+                return false;
+            }
+            joint_pos_lower_limits_[i] = limit.q_lb();
+            joint_pos_upper_limits_[i] = limit.q_ub();
+            joint_vel_limits_[i] = std::abs(limit.qd());
+            qdd_max_global_[i] = std::abs(limit.qdd());
+        }
+        return true;
     }
 
     bool PinocchioQpIKSolver::init() {
@@ -89,6 +154,10 @@ namespace cmvr {
             expected_v += seg.nv;
         }
 
+        if (!refreshJointLimitPolicy_()) {
+            return false;
+        }
+
         if (joint_pos_lower_limits_.size() != chain_q_dof_ ||
             joint_pos_upper_limits_.size() != chain_q_dof_) {
             CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] position limits size mismatch with q_dof.";
@@ -100,8 +169,10 @@ namespace cmvr {
             joint_vel_limits_.resize(chain_v_dof_);
             joint_vel_limits_.setConstant(big);
         }
-        qdd_max_global_.resize(chain_q_dof_);
+        if (qdd_max_global_.size() != chain_q_dof_) {
-        qdd_max_global_.setConstant(big);
+            qdd_max_global_.resize(chain_q_dof_);
+            qdd_max_global_.setConstant(big);
+        }
 
         // 子链当前关节角（外部 update_joints_state 也会覆盖）
         if (cur_joints_angle_.empty()) {
@@ -316,17 +387,39 @@ namespace cmvr {
         }
 
         const int dof = chain_v_dof_;
-        MatrixXd cost(6 + dof, dof);
+        const auto& avoidance = jointLimitPolicy().avoidance();
-        VectorXd target(6 + dof);
+        const bool use_joint_limit_avoidance =
+            !jointLimitsDisabled() && avoidance.enable() && avoidance.weight() > 0.0;
+        const int avoidance_rows = use_joint_limit_avoidance ? dof : 0;
+
+        MatrixXd cost(6 + dof + avoidance_rows, dof);
+        VectorXd target(6 + dof + avoidance_rows);
         cost.setZero();
         target.setZero();
         cost.topRows(6) = jacobian_base;
         target.head(6) = target_twist_base;
-        cost.bottomRows(dof) = std::sqrt(lambda_) * MatrixXd::Identity(dof, dof);
+        cost.middleRows(6, dof) =
+            std::sqrt(lambda_) * MatrixXd::Identity(dof, dof);
+        target.segment(6, dof).setZero();
 
         VectorXd lower(dof);
         VectorXd upper(dof);
         const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof);
+        if (use_joint_limit_avoidance) {
+            const VectorXd qdot_avoid =
+                cmvr::kinematics::computeJointLimitAvoidanceVelocity(
+                    q_chain,
+                    joint_pos_lower_limits_,
+                    joint_pos_upper_limits_,
+                    true,
+                    positiveOr(avoidance.gain(), 0.2),
+                    positiveOr(avoidance.margin_ratio(), 0.15),
+                    positiveOr(avoidance.max_push(), 0.25));
+            const double sqrt_weight = std::sqrt(positiveOr(avoidance.weight(), 0.05));
+            cost.middleRows(6 + dof, dof) =
+                sqrt_weight * MatrixXd::Identity(dof, dof);
+            target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid;
+        }
         for (int i = 0; i < dof; ++i) {
             double limit = std::numeric_limits<double>::infinity();
             if (joint_vel_limits_.size() == dof) {
@@ -382,6 +475,7 @@ namespace cmvr {
         if (qdot.size() != dof) {
             return false;
         }
+        qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
         qdot_out.assign(qdot.data(), qdot.data() + qdot.size());
         return true;
     }

cmvr-es/algorithms/kinematics/ik_solver/tests/src/ik_test.cpp

@@ -1,661 +0,0 @@
-//
-// Created by lgv on 11/28/25.
-//
-
-
-
-#include <iostream>
-#include <pinocchio/parsers/urdf.hpp>
-#include <pinocchio/algorithm/model.hpp>
-#include <pinocchio/algorithm/kinematics.hpp>
-#include <pinocchio/algorithm/frames.hpp>
-#include <pinocchio/algorithm/jacobian.hpp>
-#include "pinocchio/multibody/sample-models.hpp"
-#include <Eigen/Dense>
-#include "gtest/gtest.h"
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h"
-#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h"
-#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
-using namespace cmvr;
-
-namespace {
-
-constexpr const char* kDefaultUrdf =
-    "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf";
-constexpr const char* kDefaultBaseFrame = "PELVIS_S";
-constexpr const char* kDefaultFlangeFrame = "R_WRIST_R_S";
-constexpr const char* kDefaultTcpFrame = "R_FINGER_TIP_FIXED";
-
-config::PinocchioDlsIKConfig makeDlsConfig()
-{
-    config::PinocchioDlsIKConfig cfg;
-    cfg.set_urdf_path(kDefaultUrdf);
-    cfg.set_base_frame_name(kDefaultBaseFrame);
-    cfg.set_flange_frame_name(kDefaultFlangeFrame);
-    cfg.set_tcp_frame_name(kDefaultTcpFrame);
-    cfg.set_max_iters(100);
-    cfg.set_pos_eps(1e-6);
-    cfg.set_rot_eps(1e-6);
-    cfg.set_damping(1e-4);
-    return cfg;
-}
-
-config::PinocchioQpIKConfig makeQpConfig()
-{
-    config::PinocchioQpIKConfig cfg;
-    cfg.set_urdf_path(kDefaultUrdf);
-    cfg.set_base_frame_name(kDefaultBaseFrame);
-    cfg.set_flange_frame_name(kDefaultFlangeFrame);
-    cfg.set_tcp_frame_name(kDefaultTcpFrame);
-    cfg.set_lambda(1e-4);
-    cfg.set_w_posrot(0.5);
-    cfg.set_max_iters(100);
-    cfg.set_tol(1e-6);
-    cfg.set_qp_time_limit(1e-2);
-    return cfg;
-}
-
-} // namespace
-
-
-class DualArmViewer : public MuJocoViewer {
-public:
-    using Vec7 = std::vector<double>;
-
-    explicit DualArmViewer(const char *model_path)
-        : MuJocoViewer(model_path),
-          q_cmd_(7, 0.0) {                         // 初始 7 维全 0
-    }
-
-    // 更新右臂 7 关节的目标角度（rad）
-    // 顺序：
-    // [R_SHOULDER_P, R_SHOULDER_R, R_SHOULDER_Y,
-    //  R_ELBOW_R,    R_WRIST_P,    R_WRIST_Y,    R_WRIST_R]
-    void moveJ(const Vec7 &q_target) {
-        std::lock_guard<std::mutex> lk(mtx_);
-        q_cmd_ = q_target; // 只更新目标，不直接改 d->ctrl
-    }
-
-protected:
-    // 只在第一次进控制循环时调用
-    void initOnce(mjModel *m, mjData *d) override {
-        UNUSED_VARIABLE(d);
-
-        // actuator 名（已经有）
-        const char *act_names[7] = {
-            "R_SHOULDER_P_pos",
-            "R_SHOULDER_R_pos",
-            "R_SHOULDER_Y_pos",
-            "R_ELBOW_R_pos",
-            "R_WRIST_P_pos",
-            "R_WRIST_Y_pos",
-            "R_WRIST_R_pos"
-        };
-
-        // 对应的 joint 名
-        const char *jnt_names[7] = {
-            "R_SHOULDER_P",
-            "R_SHOULDER_R",
-            "R_SHOULDER_Y",
-            "R_ELBOW_R",
-            "R_WRIST_P",
-            "R_WRIST_Y",
-            "R_WRIST_R"
-        };
-
-        for (int i = 0; i < 7; ++i) {
-            // 1. 保存 actuator id
-            int act_id = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
-            right_act_ids_[i] = act_id;
-            if (act_id < 0) {
-                std::cerr << "[DualArmViewer] actuator not found: "
-                          << act_names[i] << std::endl;
-            }
-
-            // 2. 保存 joint id
-            int jnt_id = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]);
-            right_jnt_ids_[i] = jnt_id;
-            if (jnt_id < 0) {
-                std::cerr << "[DualArmViewer] joint not found: "
-                          << jnt_names[i] << std::endl;
-            }
-        }
-
-        act_ids_inited_ = true;
-    }
-
-    // 每个 mj_step 前会被 physics 线程调用
-    void controlCallback(mjModel *m, mjData *d) override {
-        if (!act_ids_inited_) return;
-
-        Vec7 q_local(7, 0.0);
-        {
-            std::lock_guard<std::mutex> lk(mtx_);
-            q_local = q_cmd_;  // 拷贝一份当前目标，避免长时间持锁
-        }
-
-        for (int i = 0; i < 7; ++i) {
-            int act_id = right_act_ids_[i];
-            if (act_id < 0) continue;
-            d->ctrl[act_id] = q_local[i];
-        }
-
-        // // 示例：读取第 5 个关节（R_WRIST_P）的实际角度并打印
-        // int jnt_id = right_jnt_ids_[4];
-        // if (jnt_id >= 0) {
-        //     int qpos_adr = m->jnt_qposadr[jnt_id];
-        //     double q_actual = d->qpos[qpos_adr];
-        //     std::cout << "cmd: " << q_local[4]
-        //               << "  act: " << q_actual << std::endl;
-        // }
-    }
-
-    void onReset(mjModel *m, mjData *d) override {
-        UNUSED_VARIABLE(m);
-        UNUSED_VARIABLE(d);
-        std::lock_guard<std::mutex> lk(mtx_);
-        q_cmd_.assign(7, 0.0); // reset 时把目标清零，保持 7 维
-    }
-
-private:
-    std::array<int, 7> right_act_ids_{}; // 右臂 7 个 actuator id
-    std::array<int, 7> right_jnt_ids_{}; // 右臂 7 个 joint id（新加）
-    bool act_ids_inited_{false};
-
-    Vec7 q_cmd_;               // 当前命令目标角（始终 7 维）
-    mutable std::mutex mtx_;   // 保护 q_cmd_
-};
-
-
-// ---------- 工具函数：计算均值 + 标准差 ----------
-static void computeMeanStd(const std::vector<double> &data,
-                           double &mean, double &stddev)
-{
-    if (data.empty()) {
-        mean = stddev = std::numeric_limits<double>::quiet_NaN();
-        return;
-    }
-    double sum = 0.0;
-    for (double x : data) sum += x;
-    mean = sum / static_cast<double>(data.size());
-
-    double var = 0.0;
-    if (data.size() > 1) {
-        for (double x : data) {
-            double d = x - mean;
-            var += d * d;
-        }
-        var /= static_cast<double>(data.size() - 1);
-    }
-    stddev = std::sqrt(var);
-}
-
-// 姿态误差：两旋转矩阵之间的角度（弧度制）
-static double rotationError(const Eigen::Matrix3d &R_des,
-                            const Eigen::Matrix3d &R_cur)
-{
-    Eigen::Matrix3d R_err = R_des.transpose() * R_cur;
-    double cos_theta = (R_err.trace() - 1.0) * 0.5;
-    if (cos_theta > 1.0)  cos_theta = 1.0;
-    if (cos_theta < -1.0) cos_theta = -1.0;
-    return std::acos(cos_theta); // rad
-}
-
-struct SolverStats {
-    std::string name;
-    std::vector<double> times_ms;   // 单次成功求解耗时
-    std::vector<double> pos_errors; // 末端位置误差 (m)
-    std::vector<double> ori_errors; // 姿态误差 (rad)
-    int attempts = 0;               // 尝试次数（有 target_pose 就算一次）
-    int success  = 0;               // 成功次数（误差在阈值内）
-};
-
-void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
-{
-    // ========== 1. 7 个关节限位，直接用你给的 joints_limits_ 定义 ==========
-    const std::array<std::pair<double,double>, 7> joints_limits_ = {{
-        {-0.26,   1.57},
-        {-0.78,   1.57},
-        {-3.1415, 3.1415},
-        { 0.0,    2.05},
-        {-3.1415, 3.1415},
-        {-0.78,   0.78},
-        {-0.26,   1.57},
-    }};
-
-    constexpr int N_SAMPLES = 10;
-
-    // ========== 2. 创建三个求解器实例 ==========
-
-    // 数值优化类 QP IK
-    PinocchioQpIKSolver qp_solver(makeQpConfig());
-
-    // 基于广义逆雅可比矩阵的数值增量 IK
-    PinocchioDlsIKSolver pinv_solver(makeDlsConfig());
-
-    // 你的专利方法：臂角 ψ + 可行域 + 势场
-    LawbaIKSolver psi_solver(config::LawbaIKConfig{});
-
-    // 统一容器，方便 for 循环
-    std::vector<IKSolver*> solvers = {
-        &psi_solver,
-        &pinv_solver,
-        &qp_solver
-    };
-
-
-    std::vector<SolverStats> stats = {
-        {"OptPsiLimitBias (Patent)", {}, {}, {}, 0, 0},
-        {"Pinocchio Jacobian IK",     {}, {}, {}, 0, 0},
-        {"Pinocchio QP IK",           {}, {}, {}, 0, 0}
-    };
-
-    // ========== 3. 初始化（如果类里有 init 就调一下） ==========
-    for (auto *solver : solvers) {
-        if (!solver->init()) {
-            std::cerr << "Warning: solver init() failed.\n";
-        }
-    }
-
-    // ========== 4. 随机数发生器 ==========
-    std::mt19937 rng(42);               // 固定种子，结果可复现
-    std::uniform_real_distribution<double> dist01(0.0, 1.0);
-
-    // ========== 5. 主循环：随机关节角 → FK → 三种 IK ==========
-    // 这里用 PinocchioDlsIKSolver (pinv_solver) 的 FK 作为“真值”
-    for (int i = 0; i < N_SAMPLES; ++i) {
-        std::cout << "[ik_compare] sample " << (i + 1) << "/" << N_SAMPLES << std::endl;
-
-        // 5.1 随机生成一组 q_true
-        std::vector<double> q_true(7);
-        for (int j = 0; j < 7; ++j) {
-            double r = dist01(rng);  // [0, 1]
-            double qmin = joints_limits_[j].first;
-            double qmax = joints_limits_[j].second;
-            q_true[j] = qmin + r * (qmax - qmin);
-        }
-
-        // 5.3 为这一组样本生成一个“统一的初始解” q_init（和 q_true 无关）
-        std::vector<double> q_init(7);
-        for (int j = 0; j < 7; ++j) {
-            double r = dist01(rng);
-            double qmin = joints_limits_[j].first;
-            double qmax = joints_limits_[j].second;
-            q_init[j] = qmin + r * (qmax - qmin);
-        }
-
-        // std::vector<double> q_cur(7, 0.0);
-        // // q_true = q_cur;
-        // Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity();
-        // if (!psi_solver.fk(q_true, target_pose, true)) {
-        //     // FK 失败的话，这个样本就跳过
-        //     continue;
-        // }
-        //
-        // Eigen::Matrix4d target_pose1 = Eigen::Matrix4d::Identity();
-        // if (!pinv_solver.fk(q_true, target_pose1, true)) {
-        //     // FK 失败的话，这个样本就跳过
-        //     continue;
-        // }
-        //
-        // Eigen::Matrix4d target_pose2 = Eigen::Matrix4d::Identity();
-        // if (!qp_solver.fk(q_true, target_pose2, true)) {
-        //     // FK 失败的话，这个样本就跳过
-        //     continue;
-        // }
-
-
-
-        // 5.3 三个 IK 分别求解
-        for (size_t idx = 0; idx < solvers.size(); ++idx) {
-
-            IKSolver *solver = solvers[idx];
-            SolverStats &s   = stats[idx];
-            s.attempts++;
-
-            // 5.2 用 pinv_solver 的 FK 计算目标末端位姿 target_pose
-
-            Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity();
-            if (!solver->fk(q_true, target_pose, false)) {
-                // FK 失败的话，这个样本就跳过
-                continue;
-            }
-
-            // 先用真实关节角更新内部状态（按你之前的约定）
-            // std::vector<double> q_cur(7, 0.0);
-            solver->update_joints_state(q_init);
-
-            std::vector<double> q_sol;
-            q_sol.reserve(7);
-
-            auto t0 = std::chrono::steady_clock::now();
-            bool ok = solver->ik(target_pose, q_sol, false);   // is_tcp = true
-            auto t1 = std::chrono::steady_clock::now();
-
-            if (!ok || q_sol.size() != 7) {
-                // 求解失败，不计入成功统计
-                continue;
-            }
-
-            viewer.moveJ(q_sol);
-            std::this_thread::sleep_for(std::chrono::milliseconds(500));
-
-            double dt_ms = std::chrono::duration_cast<
-                std::chrono::microseconds>(t1 - t0).count() / 1000.0;
-
-            // 5.4 用该求解器自己的 FK 得到实际末端位姿 pose_sol
-            Eigen::Matrix4d pose_sol = Eigen::Matrix4d::Identity();
-            if (!solver->fk(q_sol, pose_sol, false)) {
-                continue;
-            }
-
-            // 5.5 计算末端位置误差 + 姿态误差
-            Eigen::Vector3d p_des = target_pose.block<3,1>(0,3);
-            Eigen::Vector3d p_cur = pose_sol.block<3,1>(0,3);
-            double pos_err = (p_cur - p_des).norm(); // m
-
-            Eigen::Matrix3d R_des = target_pose.block<3,3>(0,0);
-            Eigen::Matrix3d R_cur = pose_sol.block<3,3>(0,0);
-            double ori_err = rotationError(R_des, R_cur); // rad
-
-            // 5.6 判定是否“成功解”
-            // 阈值可以按你机械臂精度需求调
-            const double POS_THRESH = 1e-3;  // 0.1 mm
-            const double ORI_THRESH = 1e-3;  // ≈ 0.057°
-            if (pos_err < POS_THRESH && ori_err < ORI_THRESH) {
-                s.success++;
-                s.times_ms.push_back(dt_ms);
-                s.pos_errors.push_back(pos_err);
-                s.ori_errors.push_back(ori_err);
-            } else {
-                // 这里是“收敛但精度不够”的情况，如果你想也可以单独统计
-            }
-        }
-
-
-    }
-
-    // ========== 6. 输出统计结果：耗时/误差（均值 + 标准差）+ 成功率 ==========
-    for (const auto &s : stats) {
-        std::cout << "========== Solver: " << s.name << " ==========\n";
-        std::cout << "Attempts: " << s.attempts
-                  << ", Success: " << s.success;
-        if (s.attempts > 0) {
-            double succ_rate =
-                100.0 * static_cast<double>(s.success) /
-                static_cast<double>(s.attempts);
-            std::cout << " (Success rate: " << succ_rate << "%)\n";
-        } else {
-            std::cout << " (No attempts)\n";
-        }
-
-        double mean_t, std_t;
-        computeMeanStd(s.times_ms, mean_t, std_t);
-
-        double mean_ep, std_ep;
-        computeMeanStd(s.pos_errors, mean_ep, std_ep);
-
-        double mean_er, std_er;
-        computeMeanStd(s.ori_errors, mean_er, std_er);
-
-        std::cout << "Time   (ms)  : mean = " << mean_t
-                  << ", std = " << std_t << "\n";
-        std::cout << "PosErr (m)   : mean = " << mean_ep
-                  << ", std = " << std_ep << "\n";
-        std::cout << "OriErr (rad) : mean = " << mean_er
-                  << ", std = " << std_er << "\n\n";
-    }
-}
-
-
-
-
-void benchmarkIkSolversRandomJoints()
-{
-    // ========== 1. 7 个关节限位，直接用你给的 joints_limits_ 定义 ==========
-    const std::array<std::pair<double,double>, 7> joints_limits_ = {{
-
-            {-0.26,   1.57},
-     {-0.78,   1.57},
-     {-M_PI, M_PI},
-     { 0.0,    2.05},
-     {-M_PI, M_PI},
-     {-0.78,   0.78},
-     {-0.26,   1.57},
-
-    }};
-
-    constexpr int N_SAMPLES = 10;
-
-    // ========== 2. 创建三个求解器实例 ==========
-
-    // 数值优化类 QP IK
-    PinocchioQpIKSolver qp_solver(makeQpConfig());
-
-    // 基于广义逆雅可比矩阵的数值增量 IK
-    PinocchioDlsIKSolver pinv_solver(makeDlsConfig());
-
-    // 你的专利方法：臂角 ψ + 可行域 + 势场
-    LawbaIKSolver psi_solver(config::LawbaIKConfig{});
-
-    // 统一容器，方便 for 循环
-    std::vector<IKSolver*> solvers = {
-        &psi_solver,
-        &pinv_solver,
-        &qp_solver
-    };
-
-
-    std::vector<SolverStats> stats = {
-        {"OptPsiLimitBias (Patent)", {}, {}, {}, 0, 0},
-        {"Pinocchio Jacobian IK",     {}, {}, {}, 0, 0},
-        {"Pinocchio QP IK",           {}, {}, {}, 0, 0}
-    };
-
-    // ========== 3. 初始化（如果类里有 init 就调一下） ==========
-    for (auto *solver : solvers) {
-        if (!solver->init()) {
-            std::cerr << "Warning: solver init() failed.\n";
-        }
-    }
-
-    // ========== 4. 随机数发生器 ==========
-    std::mt19937 rng(42);               // 固定种子，结果可复现
-    std::uniform_real_distribution<double> dist01(0.0, 1.0);
-
-    // ========== 5. 主循环：随机关节角 → FK → 三种 IK ==========
-    // 这里用 PinocchioDlsIKSolver (pinv_solver) 的 FK 作为“真值”
-    for (int i = 0; i < N_SAMPLES; ++i) {
-        std::cout << "[ik_compare] sample " << (i + 1) << "/" << N_SAMPLES << std::endl;
-
-        // 5.1 随机生成一组 q_true
-        std::vector<double> q_true(7);
-        for (int j = 0; j < 7; ++j) {
-            double r = dist01(rng);  // [0, 1]
-            double qmin = joints_limits_[j].first;
-            double qmax = joints_limits_[j].second;
-            q_true[j] = qmin + r * (qmax - qmin);
-        }
-
-        // 5.3 为这一组样本生成一个“统一的初始解” q_init（和 q_true 无关）
-        std::vector<double> q_init(7);
-        for (int j = 0; j < 7; ++j) {
-            double r = dist01(rng);
-            double qmin = joints_limits_[j].first;
-            double qmax = joints_limits_[j].second;
-            q_init[j] = qmin + r * (qmax - qmin);
-        }
-
-        // std::vector<double> q_cur(7, 0.0);
-        // // q_true = q_cur;
-        // Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity();
-        // if (!psi_solver.fk(q_true, target_pose, true)) {
-        //     // FK 失败的话，这个样本就跳过
-        //     continue;
-        // }
-        //
-        // Eigen::Matrix4d target_pose1 = Eigen::Matrix4d::Identity();
-        // if (!pinv_solver.fk(q_true, target_pose1, true)) {
-        //     // FK 失败的话，这个样本就跳过
-        //     continue;
-        // }
-        //
-        // Eigen::Matrix4d target_pose2 = Eigen::Matrix4d::Identity();
-        // if (!qp_solver.fk(q_true, target_pose2, true)) {
-        //     // FK 失败的话，这个样本就跳过
-        //     continue;
-        // }
-
-
-
-        // 5.3 三个 IK 分别求解
-        for (size_t idx = 0; idx < solvers.size(); ++idx) {
-
-            IKSolver *solver = solvers[idx];
-            SolverStats &s   = stats[idx];
-            s.attempts++;
-
-            // 5.2 用 pinv_solver 的 FK 计算目标末端位姿 target_pose
-
-            Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity();
-            if (!solver->fk(q_true, target_pose, false)) {
-                // FK 失败的话，这个样本就跳过
-                continue;
-            }
-
-            // 先用真实关节角更新内部状态（按你之前的约定）
-            // std::vector<double> q_cur(7, 0.0);
-            solver->update_joints_state(q_init);
-
-            std::vector<double> q_sol;
-            q_sol.reserve(7);
-
-            auto t0 = std::chrono::steady_clock::now();
-            bool ok = solver->ik(target_pose, q_sol, false);   // is_tcp = true
-            auto t1 = std::chrono::steady_clock::now();
-
-            if (!ok || q_sol.size() != 7) {
-                // 求解失败，不计入成功统计
-                continue;
-            }
-
-
-            double dt_ms = std::chrono::duration_cast<
-                std::chrono::microseconds>(t1 - t0).count() / 1000.0;
-
-            // 5.4 用该求解器自己的 FK 得到实际末端位姿 pose_sol
-            Eigen::Matrix4d pose_sol = Eigen::Matrix4d::Identity();
-            if (!solver->fk(q_sol, pose_sol, false)) {
-                continue;
-            }
-
-            // 5.5 计算末端位置误差 + 姿态误差
-            Eigen::Vector3d p_des = target_pose.block<3,1>(0,3);
-            Eigen::Vector3d p_cur = pose_sol.block<3,1>(0,3);
-            double pos_err = (p_cur - p_des).norm(); // m
-
-            Eigen::Matrix3d R_des = target_pose.block<3,3>(0,0);
-            Eigen::Matrix3d R_cur = pose_sol.block<3,3>(0,0);
-            double ori_err = rotationError(R_des, R_cur); // rad
-
-            // 5.6 判定是否“成功解”
-            // 阈值可以按你机械臂精度需求调
-            const double POS_THRESH = 1e-3;  // 0.1 mm
-            const double ORI_THRESH = 1e-3;  // ≈ 0.057°
-            if (pos_err < POS_THRESH && ori_err < ORI_THRESH) {
-                s.success++;
-                s.times_ms.push_back(dt_ms);
-                s.pos_errors.push_back(pos_err);
-                s.ori_errors.push_back(ori_err);
-            } else {
-                // 这里是“收敛但精度不够”的情况，如果你想也可以单独统计
-            }
-        }
-
-
-    }
-
-    // ========== 6. 输出统计结果：耗时/误差（均值 + 标准差）+ 成功率 ==========
-    for (const auto &s : stats) {
-        std::cout << "========== Solver: " << s.name << " ==========\n";
-        std::cout << "Attempts: " << s.attempts
-                  << ", Success: " << s.success;
-        if (s.attempts > 0) {
-            double succ_rate =
-                100.0 * static_cast<double>(s.success) /
-                static_cast<double>(s.attempts);
-            std::cout << " (Success rate: " << succ_rate << "%)\n";
-        } else {
-            std::cout << " (No attempts)\n";
-        }
-
-        double mean_t, std_t;
-        computeMeanStd(s.times_ms, mean_t, std_t);
-
-        double mean_ep, std_ep;
-        computeMeanStd(s.pos_errors, mean_ep, std_ep);
-
-        double mean_er, std_er;
-        computeMeanStd(s.ori_errors, mean_er, std_er);
-
-        std::cout << "Time   (ms)  : mean = " << mean_t
-                  << ", std = " << std_t << "\n";
-        std::cout << "PosErr (m)   : mean = " << mean_ep
-                  << ", std = " << std_ep << "\n";
-        std::cout << "OriErr (rad) : mean = " << mean_er
-                  << ", std = " << std_er << "\n\n";
-    }
-}
-
-TEST(ik_test,pinocchio_lib_test) {
-    PinocchioQpIKSolver solver(makeQpConfig());
-    // PinocchioDlsIKSolver solver(makeDlsConfig());
-    ASSERT_TRUE(solver.init());
-
-
-    // 真实关节角（比如从控制器读回来）
-    std::vector<double> q_cur(7, 0.0);
-    q_cur[5] = 0.236;
-    q_cur[4] = -0.236;
-    q_cur[3] = 0.156;
-    q_cur[2] = 0.036;
-    q_cur[1] = 0.236;
-    solver.update_joints_state(q_cur);
-
-    // 1) 先求当前 TCP 位姿
-    Eigen::Matrix4d cur_tcp_pose = Eigen::Matrix4d::Identity();
-    ASSERT_TRUE(solver.fk(q_cur, cur_tcp_pose, false));
-    std::cout <<  cur_tcp_pose<< std::endl;
-
-
-    Eigen::Matrix4d target_tcp_pose = cur_tcp_pose;
-
-
-    std::vector<double> q_target;
-    ASSERT_TRUE(solver.ik(target_tcp_pose, q_target, false));
-
-    for (double q: q_target) {
-        std::cout << q << std::endl;
-    }
-
-    Eigen::Matrix4d cur_flange_pose = Eigen::Matrix4d::Identity();
-    ASSERT_TRUE(solver.fk(q_target, cur_flange_pose, false));
-    std::cout <<  cur_flange_pose<< std::endl;
-}
-
-TEST(ik_test,ik_compare) {
-    constexpr const char* model_path =
-        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml";
-
-    DualArmViewer viewer(model_path);
-    std::thread benchmark_thread([&viewer]() {
-        // viewer.run() 在主线程创建仿真和渲染资源，稍后再开始发送关节目标。
-        std::this_thread::sleep_for(std::chrono::seconds(1));
-        benchmarkIkSolversRandomJoints(viewer);
-        std::cout << "[ik_compare] benchmark finished; close the viewer to exit." << std::endl;
-    });
-
-    viewer.run();
-    benchmark_thread.join();
-}

cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt

@@ -1,6 +1,5 @@
 add_library(arm_motion SHARED
-        cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp
+        cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp
-        cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp
         joint_motion/toppra/src/toppra_joint_motion_planner.cpp
 )
 

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h

@@ -1,6 +1,7 @@
 #ifndef CMVR_ES_CARTESIAN_MOTION_PLANNER_H
 #define CMVR_ES_CARTESIAN_MOTION_PLANNER_H
 
+#include <string>
 #include <vector>
 
 #include "cmvr/config/arm_config/arm_config.pb.h"
@@ -12,6 +13,10 @@ struct CartesianJointTrajectory {
     std::vector<std::vector<double>> position;
     std::vector<std::vector<double>> velocity;
     std::vector<double> time;
+    double planned_path_length{0.0};
+    double executable_path_length{0.0};
+    bool truncated{false};
+    std::string truncation_reason;
 };
 
 class CartesianMotionPlanner {

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h

@@ -3,10 +3,8 @@
 
 #include <memory>
 
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
 #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
-#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h"
+#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h"
-#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h"
 #include "algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h"
 #include "cmvr/config/arm_config/arm_config.pb.h"
 
@@ -23,21 +21,12 @@ public:
             return nullptr;
         }
         switch (move_l.algorithm_case()) {
-            case config::MoveLConfig::kPinocchioQpCartesianMotionPlanner:
+            case config::MoveLConfig::kPinocchioCartesianMotionPlanner:
                 if (speed_l.algorithm_case() !=
-                    config::SpeedLConfig::kPinocchioQpCartesianMotionPlanner) {
+                    config::SpeedLConfig::kPinocchioCartesianMotionPlanner) {
                     return nullptr;
                 }
-                return std::make_shared<PinocchioQpCartesianMotionPlanner>(solver);
+                return std::make_shared<PinocchioCartesianMotionPlanner>(solver);
-            case config::MoveLConfig::kPinocchioDlsCartesianMotionPlanner:
-                if (speed_l.algorithm_case() !=
-                    config::SpeedLConfig::kPinocchioDlsCartesianMotionPlanner) {
-                    return nullptr;
-                }
-                if (auto dls_solver = std::dynamic_pointer_cast<cmvr::PinocchioDlsIKSolver>(solver)) {
-                    return std::make_shared<PinocchioDlsCartesianMotionPlanner>(dls_solver);
-                }
-                return nullptr;
             case config::MoveLConfig::ALGORITHM_NOT_SET:
             default:
                 return nullptr;
@@ -48,10 +37,8 @@ public:
         const config::SpeedLConfig& cfg)
     {
         switch (cfg.algorithm_case()) {
-        case config::SpeedLConfig::kPinocchioQpCartesianMotionPlanner:
+        case config::SpeedLConfig::kPinocchioCartesianMotionPlanner:
-            return &cfg.pinocchio_qp_cartesian_motion_planner();
+            return &cfg.pinocchio_cartesian_motion_planner();
-        case config::SpeedLConfig::kPinocchioDlsCartesianMotionPlanner:
-            return &cfg.pinocchio_dls_cartesian_motion_planner();
         case config::SpeedLConfig::ALGORITHM_NOT_SET:
         default:
             return nullptr;
@@ -62,10 +49,8 @@ public:
         const config::MoveLConfig& cfg)
     {
         switch (cfg.algorithm_case()) {
-        case config::MoveLConfig::kPinocchioQpCartesianMotionPlanner:
+        case config::MoveLConfig::kPinocchioCartesianMotionPlanner:
-            return &cfg.pinocchio_qp_cartesian_motion_planner();
+            return &cfg.pinocchio_cartesian_motion_planner();
-        case config::MoveLConfig::kPinocchioDlsCartesianMotionPlanner:
-            return &cfg.pinocchio_dls_cartesian_motion_planner();
         case config::MoveLConfig::ALGORITHM_NOT_SET:
         default:
             return nullptr;

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h

@@ -0,0 +1,107 @@
+#ifndef CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H
+#define CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H
+
+#include <Eigen/Core>
+
+#include <memory>
+#include <vector>
+
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
+#include "../../cartesian_motion_planner.h"
+#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
+
+namespace cmvr::device {
+
+class PinocchioCartesianMotionPlanner final : public CartesianMotionPlanner {
+public:
+    explicit PinocchioCartesianMotionPlanner(std::shared_ptr<cmvr::PinocchioIKBase> solver);
+
+    bool configureSpeedL(const config::SpeedLPlannerConfig& config,
+                         std::size_t dof) override;
+    bool configureMoveL(const config::MoveLPlannerConfig& config) override;
+
+    bool planMoveL(const CartesianPose& target,
+                   const std::vector<double>& q_start,
+                   const std::vector<double>& qd_max,
+                   double velocity,
+                   double acceleration,
+                   double jerk,
+                   FrameType frame,
+                   CartesianJointTrajectory& trajectory) override;
+
+    bool speedLStep(const CartesianVelocity& target_velocity,
+                    double dt,
+                    const std::vector<double>& q_measured,
+                    const std::vector<double>& qd_measured,
+                    std::vector<double>& qd_command,
+                    FrameType frame) override;
+
+    bool updateSpeedLAcceleration(double acceleration) override;
+    CartesianVelocity getSpeedLCommandTwistBase() const override;
+
+private:
+    bool refreshJointAccelerationLimits_();
+    Eigen::VectorXd applyJointAccelerationLimits_(const Eigen::VectorXd& qdot,
+                                                  const Eigen::VectorXd& reference,
+                                                  double dt) const;
+    bool checkMoveLPlanLineDeviation_(const Eigen::Vector3d& p_start,
+                                      const Eigen::Vector3d& line_direction,
+                                      const Eigen::Vector3d& p_current,
+                                      double traveled,
+                                      bool& deviation_warned,
+                                      bool& direction_warned,
+                                      std::string& stop_reason) const;
+    bool checkMoveLPlanJointContinuity_(const Eigen::VectorXd& q_current,
+                                        const Eigen::VectorXd& q_next,
+                                        const Eigen::VectorXd& qdot,
+                                        const Eigen::VectorXd& prev_qdot,
+                                        double dt,
+                                        double path_position,
+                                        std::string& stop_reason) const;
+    bool checkMoveLPlanJointPositionLimits_(const Eigen::VectorXd& q_current,
+                                            const Eigen::VectorXd& q_next,
+                                            const Eigen::VectorXd& qdot,
+                                            double dt,
+                                            double path_position,
+                                            std::string& stop_reason) const;
+    bool checkMoveLPlanCartesianStepFeasibility_(
+        const Eigen::Matrix<double, 6, 1>& desired_twist,
+        const Eigen::Matrix<double, 6, 1>& achieved_twist,
+        double path_position,
+        std::string& stop_reason) const;
+    bool updateAndValidateSpeedLLineDeviation_(const std::vector<double>& q_measured,
+                                               bool is_stop_command,
+                                               const Eigen::Vector3d& command_linear_base);
+    bool validateSpeedLJointVelocityCommand_(const Eigen::VectorXd& qdot,
+                                             const Eigen::VectorXd& reference,
+                                             double dt) const;
+    bool validateSpeedLPredictedJointPositionLimits_(const std::vector<double>& q_measured,
+                                                     const Eigen::VectorXd& qdot,
+                                                     double dt) const;
+    bool validateSpeedLCartesianVelocityFeasibility_(
+        const Eigen::Matrix<double, 6, 1>& desired_twist,
+        const Eigen::Matrix<double, 6, 1>& achieved_twist,
+        const Eigen::VectorXd& q_current,
+        const Eigen::VectorXd& qdot) const;
+    std::string describeJointLimitCandidates_(const Eigen::VectorXd& q_current,
+                                              const Eigen::VectorXd& qdot) const;
+
+    std::shared_ptr<cmvr::PinocchioIKBase> solver_{nullptr};
+    config::MoveLPlannerConfig movel_config_{};
+    config::SpeedLPlannerConfig speedl_config_{};
+    cmvr::CartesianTwistLimiter twist_limiter_{};
+    Eigen::VectorXd joint_acceleration_limits_;
+    std::vector<double> prev_qdot_command_;
+    Eigen::Matrix<double, 6, 1> speedl_command_twist_base_{Eigen::Matrix<double, 6, 1>::Zero()};
+    Eigen::Vector3d speedl_line_start_tcp_base_{Eigen::Vector3d::Zero()};
+    Eigen::Vector3d speedl_line_direction_base_{Eigen::Vector3d::Zero()};
+    double speedl_applied_acceleration_{0.25};
+    bool speedl_line_check_active_{false};
+    bool speedl_line_deviation_warned_{false};
+    bool speedl_line_direction_warned_{false};
+    bool speedl_configured_{false};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h

@@ -1,66 +0,0 @@
-#ifndef CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H
-#define CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H
-
-#include <Eigen/Core>
-
-#include <memory>
-#include <vector>
-
-#include "../../cartesian_motion_planner.h"
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
-#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
-
-namespace cmvr::device {
-
-class PinocchioDlsCartesianMotionPlanner final : public CartesianMotionPlanner {
-public:
-    explicit PinocchioDlsCartesianMotionPlanner(std::shared_ptr<cmvr::PinocchioDlsIKSolver> solver);
-
-    bool configureSpeedL(const config::SpeedLPlannerConfig& config,
-                         std::size_t dof) override;
-    bool configureMoveL(const config::MoveLPlannerConfig& config) override;
-
-    bool planMoveL(const CartesianPose& target,
-                   const std::vector<double>& q_start,
-                   const std::vector<double>& qd_max,
-                   double velocity,
-                   double acceleration,
-                   double jerk,
-                   FrameType frame,
-                   CartesianJointTrajectory& trajectory) override;
-
-    bool speedLStep(const CartesianVelocity& target_velocity,
-                    double dt,
-                    const std::vector<double>& q_measured,
-                    const std::vector<double>& qd_measured,
-                    std::vector<double>& qd_command,
-                    FrameType frame) override;
-
-    bool updateSpeedLAcceleration(double acceleration) override;
-    CartesianVelocity getSpeedLCommandTwistBase() const override;
-
-private:
-    bool refreshJointLimits_();
-    Eigen::VectorXd applyJointVelocityLimits_(const Eigen::VectorXd& qdot) const;
-    Eigen::VectorXd applyJointSoftLimits_(const Eigen::VectorXd& q,
-                                          const Eigen::VectorXd& qdot);
-    Eigen::VectorXd applyJointAccelerationLimits_(const Eigen::VectorXd& qdot,
-                                                  const Eigen::VectorXd& reference,
-                                                  double dt) const;
-
-    std::shared_ptr<cmvr::PinocchioDlsIKSolver> solver_{nullptr};
-    config::MoveLPlannerConfig movel_config_{};
-    config::SpeedLPlannerConfig speedl_config_{};
-    cmvr::CartesianTwistLimiter twist_limiter_{};
-    Eigen::VectorXd joint_lower_limits_;
-    Eigen::VectorXd joint_upper_limits_;
-    Eigen::VectorXd joint_velocity_limits_;
-    std::vector<double> prev_qdot_command_;
-    Eigen::Matrix<double, 6, 1> speedl_command_twist_base_{Eigen::Matrix<double, 6, 1>::Zero()};
-    double speedl_applied_acceleration_{0.25};
-    bool speedl_configured_{false};
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp

@@ -1,408 +0,0 @@
-#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h"
-
-#include <Eigen/Geometry>
-
-#include <algorithm>
-#include <cmath>
-#include <limits>
-#include <utility>
-
-#include "algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h"
-#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h"
-#include "common/math/cartesian_motion_math.h"
-#include "common/math/joint_limits.h"
-#include "common/config/config_files.h"
-#include "common/math/transform_math.h"
-
-namespace cmvr::device {
-
-using cmvr::common::config::positiveOr;
-using cmvr::device::cartesian_motion::clamp;
-using cmvr::device::cartesian_motion::directionDeviationDeg;
-using cmvr::device::cartesian_motion::rotationVector;
-using cmvr::device::cartesian_motion::toEigenVector;
-using cmvr::device::cartesian_motion::toStdVector;
-
-PinocchioDlsCartesianMotionPlanner::PinocchioDlsCartesianMotionPlanner(
-    std::shared_ptr<cmvr::PinocchioDlsIKSolver> solver)
-    : solver_(std::move(solver))
-{
-}
-
-bool PinocchioDlsCartesianMotionPlanner::refreshJointLimits_()
-{
-    if (!solver_) {
-        return false;
-    }
-    if (!solver_->getJointPositionLimits(joint_lower_limits_, joint_upper_limits_)) {
-        return false;
-    }
-    if (!solver_->getJointVelocityLimits(joint_velocity_limits_)) {
-        return false;
-    }
-    return true;
-}
-
-Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointVelocityLimits_(
-    const Eigen::VectorXd& qdot) const
-{
-    return cmvr::kinematics::scaleToVelocityLimits(qdot, joint_velocity_limits_);
-}
-
-Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointSoftLimits_(
-    const Eigen::VectorXd& q,
-    const Eigen::VectorXd& qdot)
-{
-    if (joint_lower_limits_.size() != q.size() ||
-        joint_upper_limits_.size() != q.size() ||
-        qdot.size() != q.size()) {
-        return qdot;
-    }
-
-    Eigen::VectorXd limited = qdot;
-    for (Eigen::Index i = 0; i < q.size(); ++i) {
-        const double lower = joint_lower_limits_[i];
-        const double upper = joint_upper_limits_[i];
-        if (!std::isfinite(lower) || !std::isfinite(upper) || upper <= lower) {
-            continue;
-        }
-
-        const double span = upper - lower;
-        const double margin = std::max(0.02, 0.08 * span);
-        if (limited[i] < 0.0 && q[i] < lower + margin) {
-            const double ratio = clamp((q[i] - lower) / margin, 0.0, 1.0);
-            limited[i] *= ratio;
-            if (q[i] <= lower) {
-                limited[i] = std::max(0.0, limited[i]);
-            }
-        } else if (limited[i] > 0.0 && q[i] > upper - margin) {
-            const double ratio = clamp((upper - q[i]) / margin, 0.0, 1.0);
-            limited[i] *= ratio;
-            if (q[i] >= upper) {
-                limited[i] = std::min(0.0, limited[i]);
-            }
-        }
-    }
-    return limited;
-}
-
-Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointAccelerationLimits_(
-    const Eigen::VectorXd& qdot,
-    const Eigen::VectorXd& reference,
-    const double dt) const
-{
-    if (reference.size() != qdot.size() || dt <= 0.0) {
-        return qdot;
-    }
-
-    Eigen::VectorXd limited = qdot;
-    for (Eigen::Index i = 0; i < qdot.size(); ++i) {
-        double acc_limit = 8.0;
-        if (i < speedl_config_.joint_acceleration_max_size() &&
-            speedl_config_.joint_acceleration_max(static_cast<int>(i)) > 0.0) {
-            acc_limit = speedl_config_.joint_acceleration_max(static_cast<int>(i));
-        }
-
-        const double delta_max = acc_limit * dt;
-        const double delta = clamp(qdot[i] - reference[i], -delta_max, delta_max);
-        limited[i] = reference[i] + delta;
-    }
-    return limited;
-}
-
-bool PinocchioDlsCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlannerConfig& config,
-                                                         const std::size_t dof)
-{
-    if (!solver_ || dof == 0) {
-        return false;
-    }
-
-    const auto solver_dof = static_cast<std::size_t>(std::max(0, solver_->chainVelocityDof()));
-    if (solver_dof != 0 && solver_dof != dof) {
-        return false;
-    }
-
-    speedl_config_ = config;
-
-    if (!refreshJointLimits_()) {
-        return false;
-    }
-
-    cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_);
-
-    prev_qdot_command_.assign(dof, 0.0);
-    speedl_command_twist_base_.setZero();
-    speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0);
-    speedl_configured_ = true;
-    return true;
-}
-
-bool PinocchioDlsCartesianMotionPlanner::configureMoveL(
-    const config::MoveLPlannerConfig& config)
-{
-    if (!std::isfinite(config.sample_period_s()) ||
-        !std::isfinite(config.position_gain()) ||
-        !std::isfinite(config.rotation_gain())) {
-        return false;
-    }
-    movel_config_ = config;
-    return true;
-}
-
-bool PinocchioDlsCartesianMotionPlanner::planMoveL(const CartesianPose& target,
-                                                   const std::vector<double>& q_start,
-                                                   const std::vector<double>& qd_max,
-                                                   const double velocity,
-                                                   const double acceleration,
-                                                   const double jerk,
-                                                   const FrameType frame,
-                                                   CartesianJointTrajectory& trajectory)
-{
-    trajectory = {};
-    const double dt = positiveOr(movel_config_.sample_period_s(), 0.001);
-    if (!solver_ || q_start.empty() ||
-        velocity <= 0.0 || acceleration <= 0.0 || jerk <= 0.0) {
-        return false;
-    }
-    if (static_cast<int>(q_start.size()) != solver_->chainDof()) {
-        return false;
-    }
-    if (!qd_max.empty() && qd_max.size() != q_start.size()) {
-        return false;
-    }
-    if (!refreshJointLimits_()) {
-        return false;
-    }
-
-    Eigen::Matrix4d start_pose_base = Eigen::Matrix4d::Identity();
-    if (!solver_->fk(q_start, start_pose_base, true)) {
-        return false;
-    }
-
-    const Eigen::Matrix4d target_pose_input = common::math::poseToMatrix(target);
-    const Eigen::Matrix4d target_pose_base =
-        frame == FrameType::Tool ? start_pose_base * target_pose_input : target_pose_input;
-
-    const Eigen::Vector3d p_start = start_pose_base.block<3, 1>(0, 3);
-    const Eigen::Vector3d p_target = target_pose_base.block<3, 1>(0, 3);
-    const Eigen::Vector3d dp = p_target - p_start;
-    const double linear_distance = dp.norm();
-
-    const Eigen::Matrix3d R_start = start_pose_base.block<3, 3>(0, 0);
-    const Eigen::Matrix3d R_target = target_pose_base.block<3, 3>(0, 0);
-    const Eigen::Vector3d total_rotation_vector = rotationVector(R_target * R_start.transpose());
-    const double angular_distance = total_rotation_vector.norm();
-
-    const double path_length = linear_distance > 1e-9 ? linear_distance : angular_distance;
-    trajectory.position.push_back(q_start);
-    trajectory.velocity.push_back(std::vector<double>(q_start.size(), 0.0));
-    trajectory.time.push_back(0.0);
-    if (path_length <= 1e-9) {
-        return true;
-    }
-
-    cmvr::SCurve curve(velocity, acceleration, jerk);
-    const cmvr::SCurveProfile profile = curve.calculateProfile(0.0, path_length, 0.0, 0.0);
-    if (profile.total_time <= 0.0) {
-        return false;
-    }
-
-    Eigen::VectorXd q_current = toEigenVector(q_start);
-    Eigen::Vector3d linear_direction = Eigen::Vector3d::Zero();
-    if (linear_distance > 1e-9) {
-        linear_direction = dp / linear_distance;
-    }
-    const Eigen::Quaterniond q_start_rot(R_start);
-    const Eigen::Quaterniond q_target_rot(R_target);
-    const double position_gain = positiveOr(movel_config_.position_gain(), 4.0);
-    const double rotation_gain = positiveOr(movel_config_.rotation_gain(), 4.0);
-
-    double previous_time = 0.0;
-    for (double t = std::min(dt, profile.total_time);
-         t <= profile.total_time + 1e-9;
-         t = std::min(t + dt, profile.total_time)) {
-        const double step_dt = std::max(1e-6, t - previous_time);
-        previous_time = t;
-
-        const double s = clamp(curve.getPositionAtTime(profile, t), 0.0, path_length);
-        const double sd = std::max(0.0, curve.getVelocityAtTime(profile, t));
-        const double ratio = clamp(s / path_length, 0.0, 1.0);
-
-        const std::vector<double> q_std = toStdVector(q_current);
-        Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity();
-        if (!solver_->fk(q_std, current_pose_base, true)) {
-            return false;
-        }
-
-        const Eigen::Vector3d p_current = current_pose_base.block<3, 1>(0, 3);
-        const Eigen::Vector3d p_desired = p_start + ratio * dp;
-        Eigen::Matrix<double, 6, 1> target_twist_base = Eigen::Matrix<double, 6, 1>::Zero();
-        target_twist_base.head<3>() =
-            linear_direction * sd + position_gain * (p_desired - p_current);
-
-        if (angular_distance > 1e-9) {
-            const Eigen::Matrix3d R_current = current_pose_base.block<3, 3>(0, 0);
-            const Eigen::Matrix3d R_desired =
-                q_start_rot.slerp(ratio, q_target_rot).toRotationMatrix();
-            const Eigen::Vector3d rotation_error = rotationVector(R_desired * R_current.transpose());
-            target_twist_base.tail<3>() =
-                (total_rotation_vector / path_length) * sd + rotation_gain * rotation_error;
-        }
-
-        Eigen::MatrixXd jacobian_base;
-        Eigen::Matrix3d base_R_tool;
-        if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) {
-            return false;
-        }
-
-        std::vector<double> qdot_std;
-        if (!solver_->solveVelocityBase(jacobian_base,
-                                        target_twist_base,
-                                        q_std,
-                                        qdot_std,
-                                        std::numeric_limits<double>::infinity())) {
-            return false;
-        }
-
-        Eigen::VectorXd qdot = applyJointVelocityLimits_(toEigenVector(qdot_std));
-        if (!qd_max.empty()) {
-            double scale = 1.0;
-            for (Eigen::Index i = 0; i < qdot.size(); ++i) {
-                const double limit = std::abs(qd_max[static_cast<std::size_t>(i)]);
-                if (limit <= 0.0 || !std::isfinite(limit)) {
-                    continue;
-                }
-                const double value = std::abs(qdot[i]);
-                if (value > limit) {
-                    scale = std::min(scale, limit / value);
-                }
-            }
-            qdot *= scale;
-        }
-        qdot = applyJointSoftLimits_(q_current, qdot);
-        q_current += qdot * step_dt;
-
-        if (joint_lower_limits_.size() == q_current.size() &&
-            joint_upper_limits_.size() == q_current.size()) {
-            q_current = q_current.cwiseMax(joint_lower_limits_).cwiseMin(joint_upper_limits_);
-        }
-
-        trajectory.position.push_back(toStdVector(q_current));
-        trajectory.velocity.push_back(toStdVector(qdot));
-        trajectory.time.push_back(t);
-
-        if (t >= profile.total_time - 1e-9) {
-            break;
-        }
-    }
-
-    return true;
-}
-
-bool PinocchioDlsCartesianMotionPlanner::speedLStep(const CartesianVelocity& target_velocity,
-                                                    const double dt,
-                                                    const std::vector<double>& q_measured,
-                                                    const std::vector<double>& qd_measured,
-                                                    std::vector<double>& qd_command,
-                                                    const FrameType frame)
-{
-    qd_command.clear();
-    if (!solver_ || !speedl_configured_ || dt <= 0.0) {
-        return false;
-    }
-    if (static_cast<int>(q_measured.size()) != solver_->chainDof() ||
-        static_cast<int>(qd_measured.size()) != solver_->chainVelocityDof()) {
-        return false;
-    }
-
-    Eigen::Matrix<double, 6, 1> measured_twist_base = Eigen::Matrix<double, 6, 1>::Zero();
-    Eigen::MatrixXd jacobian_base;
-    Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-    if (!solver_->computeTwistBaseAtQ(q_measured,
-                                      qd_measured,
-                                      true,
-                                      measured_twist_base,
-                                      &jacobian_base,
-                                      &base_R_tool)) {
-        return false;
-    }
-
-    const Eigen::Matrix<double, 6, 1> target_twist = common::math::velocityToVector(target_velocity);
-    if (target_twist.squaredNorm() <= 1e-12) {
-        twist_limiter_.synchronize(measured_twist_base, dt, true);
-    } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
-        twist_limiter_.initialize(Eigen::Matrix<double, 6, 1>::Zero());
-    }
-    twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame));
-    speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool);
-
-    std::vector<double> qdot_std;
-    if (!solver_->solveVelocityBase(jacobian_base,
-                                    speedl_command_twist_base_,
-                                    q_measured,
-                                    qdot_std,
-                                    std::numeric_limits<double>::infinity())) {
-        return false;
-    }
-
-    Eigen::VectorXd qdot = applyJointVelocityLimits_(toEigenVector(qdot_std));
-    qdot = applyJointSoftLimits_(toEigenVector(q_measured), qdot);
-
-    Eigen::VectorXd reference = toEigenVector(qd_measured);
-    if (prev_qdot_command_.size() == qdot.size()) {
-        reference = toEigenVector(prev_qdot_command_);
-    }
-    qdot = applyJointAccelerationLimits_(qdot, reference, dt);
-
-    const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
-    const Eigen::Vector3d desired_linear = speedl_command_twist_base_.head<3>();
-    const Eigen::Vector3d achieved_linear = achieved_twist_base.head<3>();
-    const double desired_linear_norm = desired_linear.norm();
-    const double achieved_linear_norm = achieved_linear.norm();
-    const double direction_check_min_speed =
-        std::max(1e-4, positiveOr(speedl_config_.linear_reverse_switch_speed_threshold(), 1e-3));
-    if (desired_linear_norm > direction_check_min_speed) {
-        const double linear_min_speed_ratio =
-            clamp(positiveOr(speedl_config_.linear_min_speed_ratio(), 0.2), 0.0, 1.0);
-        const double speed_ratio = achieved_linear_norm / desired_linear_norm;
-        if (speed_ratio < linear_min_speed_ratio) {
-            return false;
-        }
-        if (achieved_linear_norm > direction_check_min_speed) {
-            const double deviation_deg = directionDeviationDeg(desired_linear, achieved_linear);
-            const double severe_direction_deviation_deg =
-                positiveOr(speedl_config_.severe_direction_deviation_deg(), 45.0);
-            if (deviation_deg >= severe_direction_deviation_deg) {
-                return false;
-            }
-        }
-    }
-
-    qd_command = toStdVector(qdot);
-    prev_qdot_command_ = qd_command;
-    return true;
-}
-
-bool PinocchioDlsCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration)
-{
-    if (!speedl_configured_ || acceleration <= 0.0) {
-        return false;
-    }
-    if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) {
-        return true;
-    }
-
-    cartesian_motion::updateTwistLimiterAcceleration(
-        twist_limiter_,
-        speedl_config_,
-        acceleration);
-    speedl_applied_acceleration_ = acceleration;
-    return true;
-}
-
-CartesianVelocity PinocchioDlsCartesianMotionPlanner::getSpeedLCommandTwistBase() const
-{
-    return common::math::vectorToVelocity(speedl_command_twist_base_);
-}
-
-} // namespace cmvr::device

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h

@@ -1,76 +0,0 @@
-#ifndef CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H
-#define CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H
-
-#include <Eigen/Core>
-
-#include <memory>
-#include <vector>
-
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
-#include "../../cartesian_motion_planner.h"
-#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
-#include "common/math/qp_solver.h"
-
-namespace cmvr::device {
-
-class PinocchioQpCartesianMotionPlanner final : public CartesianMotionPlanner {
-public:
-    explicit PinocchioQpCartesianMotionPlanner(std::shared_ptr<cmvr::PinocchioIKBase> solver);
-
-    bool configureSpeedL(const config::SpeedLPlannerConfig& config,
-                         std::size_t dof) override;
-    bool configureMoveL(const config::MoveLPlannerConfig& config) override;
-
-    bool planMoveL(const CartesianPose& target,
-                   const std::vector<double>& q_start,
-                   const std::vector<double>& qd_max,
-                   double velocity,
-                   double acceleration,
-                   double jerk,
-                   FrameType frame,
-                   CartesianJointTrajectory& trajectory) override;
-
-    bool speedLStep(const CartesianVelocity& target_velocity,
-                    double dt,
-                    const std::vector<double>& q_measured,
-                    const std::vector<double>& qd_measured,
-                    std::vector<double>& qd_command,
-                    FrameType frame) override;
-
-    bool updateSpeedLAcceleration(double acceleration) override;
-    CartesianVelocity getSpeedLCommandTwistBase() const override;
-
-private:
-    bool refreshJointLimits_(const config::CartesianVelocityQpConfig& config);
-    bool configureQpSolver_(Eigen::Index dof, double solver_eps);
-    bool solveVelocityQp_(const Eigen::MatrixXd& jacobian_base,
-                          const Eigen::Matrix<double, 6, 1>& target_twist_base,
-                          const Eigen::VectorXd& q_measured,
-                          const Eigen::VectorXd& qd_reference,
-                          double dt,
-                          const std::vector<double>& qd_max,
-                          bool enforce_acceleration_limits,
-                          const config::CartesianVelocityQpConfig& qp_config,
-                          Eigen::VectorXd& qdot);
-    bool validateAchievedLinearTwist_(const Eigen::MatrixXd& jacobian_base,
-                                      const Eigen::VectorXd& qdot) const;
-
-    std::shared_ptr<cmvr::PinocchioIKBase> solver_{nullptr};
-    config::MoveLPlannerConfig movel_config_{};
-    config::SpeedLPlannerConfig speedl_config_{};
-    cmvr::CartesianTwistLimiter twist_limiter_{};
-    cmvr::QPSolver qp_solver_;
-    int qp_solver_dof_{0};
-    double qp_solver_eps_{0.0};
-    Eigen::VectorXd joint_lower_limits_;
-    Eigen::VectorXd joint_upper_limits_;
-    Eigen::VectorXd joint_velocity_limits_;
-    std::vector<double> prev_qdot_command_;
-    Eigen::Matrix<double, 6, 1> speedl_command_twist_base_{Eigen::Matrix<double, 6, 1>::Zero()};
-    double speedl_applied_acceleration_{0.25};
-    bool speedl_configured_{false};
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp

@@ -1,590 +0,0 @@
-#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h"
-
-#include <algorithm>
-#include <cmath>
-#include <Eigen/Geometry>
-#include <limits>
-#include <unordered_map>
-#include <utility>
-
-#include "algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h"
-#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h"
-#include "common/base/logging/logger.h"
-#include "common/math/cartesian_motion_math.h"
-#include "common/math/joint_limits.h"
-#include "common/config/config_files.h"
-#include "common/math/proto_geometry.h"
-#include "common/math/transform_math.h"
-
-namespace cmvr::device {
-
-using cmvr::common::config::positiveOr;
-using cmvr::device::cartesian_motion::clamp;
-using cmvr::device::cartesian_motion::directionDeviationDeg;
-using cmvr::device::cartesian_motion::rotationVector;
-using cmvr::device::cartesian_motion::toEigenVector;
-using cmvr::device::cartesian_motion::toStdVector;
-
-namespace {
-
-const config::CartesianVelocityQpConfig& qpConfigOrDefault(
-    const config::CartesianVelocityQpConfig& config)
-{
-    static const config::CartesianVelocityQpConfig defaults;
-    return config.ByteSizeLong() > 0 ? config : defaults;
-}
-
-Eigen::Matrix<double, 6, 1> twistTrackingWeightOrDefault(
-    const cmvr::common::Vec6& value)
-{
-    Eigen::Matrix<double, 6, 1> defaults;
-    defaults << 1.0, 1.0, 1.0, 0.5, 0.5, 0.5;
-    Eigen::Matrix<double, 6, 1> weight =
-        cmvr::common::math::toEigenVec6(value, defaults);
-    for (int i = 0; i < weight.size(); ++i) {
-        if (!std::isfinite(weight[i]) || weight[i] <= 0.0) {
-            weight[i] = defaults[i];
-        }
-    }
-    return weight;
-}
-
-} // namespace
-
-PinocchioQpCartesianMotionPlanner::PinocchioQpCartesianMotionPlanner(
-    std::shared_ptr<cmvr::PinocchioIKBase> solver)
-    : solver_(std::move(solver))
-{
-}
-
-bool PinocchioQpCartesianMotionPlanner::refreshJointLimits_(
-    const config::CartesianVelocityQpConfig& config)
-{
-    if (!solver_) {
-        return false;
-    }
-    const auto source = config.has_joint_limits()
-                            ? config.joint_limits().source()
-                            : config::JOINT_LIMIT_SOURCE_URDF;
-    if (source == config::JOINT_LIMIT_SOURCE_CUSTOM) {
-        std::vector<std::string> joint_names;
-        if (!solver_->getChainJointNames(joint_names) || joint_names.empty()) {
-            return false;
-        }
-        std::unordered_map<std::string, config::JointLimitConfig> custom_limits;
-        if (config.has_joint_limits()) {
-            custom_limits.reserve(
-                static_cast<std::size_t>(config.joint_limits().joints_size()));
-            for (const auto& item : config.joint_limits().joints()) {
-                if (!item.joint_name().empty()) {
-                    custom_limits[item.joint_name()] = item;
-                }
-            }
-        }
-
-        const auto dof = static_cast<Eigen::Index>(joint_names.size());
-        joint_lower_limits_.resize(dof);
-        joint_upper_limits_.resize(dof);
-        joint_velocity_limits_.resize(dof);
-        for (Eigen::Index i = 0; i < dof; ++i) {
-            const auto it = custom_limits.find(joint_names[static_cast<std::size_t>(i)]);
-            if (it == custom_limits.end()) {
-                CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] missing custom joint limit for "
-                                << joint_names[static_cast<std::size_t>(i)];
-                return false;
-            }
-            const auto& limit = it->second;
-            if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
-                !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
-                limit.velocity() <= 0.0) {
-                CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] invalid custom joint limit for "
-                                << limit.joint_name();
-                return false;
-            }
-            joint_lower_limits_[i] = limit.lower();
-            joint_upper_limits_[i] = limit.upper();
-            joint_velocity_limits_[i] = std::abs(limit.velocity());
-        }
-        return true;
-    }
-
-    if (!solver_->getJointPositionLimits(joint_lower_limits_, joint_upper_limits_)) {
-        return false;
-    }
-    if (!solver_->getJointVelocityLimits(joint_velocity_limits_)) {
-        return false;
-    }
-    return true;
-}
-
-bool PinocchioQpCartesianMotionPlanner::configureQpSolver_(const Eigen::Index dof,
-                                                           const double solver_eps)
-{
-    if (dof <= 0) {
-        return false;
-    }
-    const double eps = solver_eps > 0.0 ? solver_eps : 1e-3;
-    if (qp_solver_dof_ != static_cast<int>(dof) || std::abs(qp_solver_eps_ - eps) > 1e-12) {
-        qp_solver_.Setup(static_cast<int>(dof), static_cast<int>(dof), eps);
-        qp_solver_.ResetIsFirst();
-        qp_solver_dof_ = static_cast<int>(dof);
-        qp_solver_eps_ = eps;
-    }
-    return true;
-}
-
-bool PinocchioQpCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlannerConfig& config,
-                                                        const std::size_t dof)
-{
-    if (!solver_ || dof == 0) {
-        return false;
-    }
-    const auto solver_dof = static_cast<std::size_t>(std::max(0, solver_->chainVelocityDof()));
-    if (solver_dof != 0 && solver_dof != dof) {
-        return false;
-    }
-    speedl_config_ = config;
-
-    const auto& qp_config = qpConfigOrDefault(speedl_config_.qp());
-    if (!refreshJointLimits_(qp_config)) {
-        return false;
-    }
-
-    cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_);
-
-    prev_qdot_command_.assign(dof, 0.0);
-    speedl_command_twist_base_.setZero();
-    speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0);
-    if (!configureQpSolver_(static_cast<Eigen::Index>(dof),
-                            positiveOr(qp_config.solver_eps(), 1e-3))) {
-        return false;
-    }
-    speedl_configured_ = true;
-    return true;
-}
-
-bool PinocchioQpCartesianMotionPlanner::configureMoveL(
-    const config::MoveLPlannerConfig& config)
-{
-    if (!std::isfinite(config.sample_period_s()) ||
-        !std::isfinite(config.position_gain()) ||
-        !std::isfinite(config.rotation_gain())) {
-        return false;
-    }
-    movel_config_ = config;
-    return true;
-}
-
-bool PinocchioQpCartesianMotionPlanner::planMoveL(const CartesianPose& target,
-                                                  const std::vector<double>& q_start,
-                                                  const std::vector<double>& qd_max,
-                                                  const double velocity,
-                                                  const double acceleration,
-                                                  const double jerk,
-                                                  const FrameType frame,
-                                                  CartesianJointTrajectory& trajectory)
-{
-    trajectory = {};
-    const double dt = positiveOr(movel_config_.sample_period_s(), 0.001);
-    if (!solver_ || q_start.empty() ||
-        velocity <= 0.0 || acceleration <= 0.0 || jerk <= 0.0) {
-        return false;
-    }
-    if (static_cast<int>(q_start.size()) != solver_->chainDof()) {
-        return false;
-    }
-    if (!qd_max.empty() && qd_max.size() != q_start.size()) {
-        return false;
-    }
-    const auto& qp_config = qpConfigOrDefault(movel_config_.qp());
-    if (!refreshJointLimits_(qp_config)) {
-        return false;
-    }
-
-    Eigen::Matrix4d start_pose_base = Eigen::Matrix4d::Identity();
-    if (!solver_->fk(q_start, start_pose_base, true)) {
-        return false;
-    }
-
-    const Eigen::Matrix4d target_pose_input = common::math::poseToMatrix(target);
-    const Eigen::Matrix4d target_pose_base =
-        frame == FrameType::Tool ? start_pose_base * target_pose_input : target_pose_input;
-
-    const Eigen::Vector3d p_start = start_pose_base.block<3, 1>(0, 3);
-    const Eigen::Vector3d p_target = target_pose_base.block<3, 1>(0, 3);
-    const Eigen::Vector3d dp = p_target - p_start;
-    const double linear_distance = dp.norm();
-
-    const Eigen::Matrix3d R_start = start_pose_base.block<3, 3>(0, 0);
-    const Eigen::Matrix3d R_target = target_pose_base.block<3, 3>(0, 0);
-    const Eigen::Vector3d total_rotation_vector = rotationVector(R_target * R_start.transpose());
-    const double angular_distance = total_rotation_vector.norm();
-
-    const double path_length = linear_distance > 1e-9 ? linear_distance : angular_distance;
-    trajectory.position.push_back(q_start);
-    trajectory.velocity.push_back(std::vector<double>(q_start.size(), 0.0));
-    trajectory.time.push_back(0.0);
-    if (path_length <= 1e-9) {
-        return true;
-    }
-
-    cmvr::SCurve curve(velocity, acceleration, jerk);
-    const cmvr::SCurveProfile profile = curve.calculateProfile(0.0, path_length, 0.0, 0.0);
-    if (profile.total_time <= 0.0) {
-        return false;
-    }
-
-    Eigen::VectorXd q_current = toEigenVector(q_start);
-    Eigen::VectorXd qdot_previous = Eigen::VectorXd::Zero(static_cast<Eigen::Index>(q_start.size()));
-    Eigen::Vector3d linear_direction = Eigen::Vector3d::Zero();
-    if (linear_distance > 1e-9) {
-        linear_direction = dp / linear_distance;
-    }
-    const Eigen::Quaterniond q_start_rot(R_start);
-    const Eigen::Quaterniond q_target_rot(R_target);
-    const double position_gain = positiveOr(movel_config_.position_gain(), 4.0);
-    const double rotation_gain = positiveOr(movel_config_.rotation_gain(), 4.0);
-
-    qp_solver_.ResetIsFirst();
-    double previous_time = 0.0;
-    for (double t = std::min(dt, profile.total_time);
-         t <= profile.total_time + 1e-9;
-         t = std::min(t + dt, profile.total_time)) {
-        const double step_dt = std::max(1e-6, t - previous_time);
-        previous_time = t;
-
-        const double s = clamp(curve.getPositionAtTime(profile, t), 0.0, path_length);
-        const double sd = std::max(0.0, curve.getVelocityAtTime(profile, t));
-        const double ratio = clamp(s / path_length, 0.0, 1.0);
-
-        const std::vector<double> q_std = toStdVector(q_current);
-        Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity();
-        if (!solver_->fk(q_std, current_pose_base, true)) {
-            return false;
-        }
-
-        const Eigen::Vector3d p_current = current_pose_base.block<3, 1>(0, 3);
-        const Eigen::Vector3d p_desired = p_start + ratio * dp;
-        Eigen::Matrix<double, 6, 1> target_twist_base = Eigen::Matrix<double, 6, 1>::Zero();
-        target_twist_base.head<3>() =
-            linear_direction * sd + position_gain * (p_desired - p_current);
-
-        if (angular_distance > 1e-9) {
-            const Eigen::Matrix3d R_current = current_pose_base.block<3, 3>(0, 0);
-            const Eigen::Matrix3d R_desired =
-                q_start_rot.slerp(ratio, q_target_rot).toRotationMatrix();
-            const Eigen::Vector3d rotation_error =
-                rotationVector(R_desired * R_current.transpose());
-            target_twist_base.tail<3>() =
-                (total_rotation_vector / path_length) * sd + rotation_gain * rotation_error;
-        }
-
-        Eigen::MatrixXd jacobian_base;
-        Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-        if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) {
-            return false;
-        }
-
-        Eigen::VectorXd qdot;
-        if (!solveVelocityQp_(jacobian_base,
-                              target_twist_base,
-                              q_current,
-                              qdot_previous,
-                              step_dt,
-                              qd_max,
-                              false,
-                              qp_config,
-                              qdot)) {
-            return false;
-        }
-
-        q_current += qdot * step_dt;
-        if (joint_lower_limits_.size() == q_current.size() &&
-            joint_upper_limits_.size() == q_current.size()) {
-            q_current = q_current.cwiseMax(joint_lower_limits_).cwiseMin(joint_upper_limits_);
-        }
-
-        trajectory.position.push_back(toStdVector(q_current));
-        trajectory.velocity.push_back(toStdVector(qdot));
-        trajectory.time.push_back(t);
-        qdot_previous = qdot;
-
-        if (t >= profile.total_time - 1e-9) {
-            break;
-        }
-    }
-
-    return true;
-}
-
-bool PinocchioQpCartesianMotionPlanner::solveVelocityQp_(
-    const Eigen::MatrixXd& jacobian_base,
-    const Eigen::Matrix<double, 6, 1>& target_twist_base,
-    const Eigen::VectorXd& q_measured,
-    const Eigen::VectorXd& qd_reference,
-    const double dt,
-    const std::vector<double>& qd_max,
-    const bool enforce_acceleration_limits,
-    const config::CartesianVelocityQpConfig& qp_config,
-    Eigen::VectorXd& qdot)
-{
-    const Eigen::Index dof = q_measured.size();
-    if (jacobian_base.rows() != 6 || jacobian_base.cols() != dof ||
-        qd_reference.size() != dof || dt <= 0.0) {
-        return false;
-    }
-
-    if (!configureQpSolver_(dof, positiveOr(qp_config.solver_eps(), 1e-3))) {
-        return false;
-    }
-
-    const Eigen::Matrix<double, 6, 1> twist_weight =
-        twistTrackingWeightOrDefault(qp_config.twist_tracking_weight());
-    Eigen::Matrix<double, 6, 6> task_weight = Eigen::Matrix<double, 6, 6>::Identity();
-    for (int i = 0; i < 6; ++i) {
-        task_weight(i, i) = twist_weight[i];
-    }
-    const double qdot_regularization =
-        positiveOr(qp_config.qdot_regularization(), 1e-4);
-    const double prev_qdot_regularization =
-        positiveOr(qp_config.prev_qdot_regularization(),
-                   enforce_acceleration_limits ? 2e-2 : 1e-4);
-    const bool use_joint_limit_avoidance =
-        qp_config.has_joint_limit_avoidance() &&
-        qp_config.joint_limit_avoidance().enable() &&
-        qp_config.joint_limit_avoidance().weight() > 0.0;
-    const int avoidance_rows = use_joint_limit_avoidance ? static_cast<int>(dof) : 0;
-
-    Eigen::MatrixXd cost(6 + 2 * dof + avoidance_rows, dof);
-    Eigen::VectorXd target(6 + 2 * dof + avoidance_rows);
-    cost.topRows(6) = task_weight * jacobian_base;
-    target.head(6) = task_weight * target_twist_base;
-    cost.middleRows(6, dof) = std::sqrt(qdot_regularization) * Eigen::MatrixXd::Identity(dof, dof);
-    target.segment(6, dof).setZero();
-    cost.middleRows(6 + dof, dof) =
-        std::sqrt(prev_qdot_regularization) * Eigen::MatrixXd::Identity(dof, dof);
-    target.segment(6 + dof, dof) = std::sqrt(prev_qdot_regularization) * qd_reference;
-    if (use_joint_limit_avoidance) {
-        const auto& avoidance = qp_config.joint_limit_avoidance();
-        const Eigen::VectorXd qdot_avoid =
-            cmvr::kinematics::computeJointLimitAvoidanceVelocity(
-                q_measured,
-                joint_lower_limits_,
-                joint_upper_limits_,
-                true,
-                positiveOr(avoidance.gain(), 0.2),
-                positiveOr(avoidance.margin_ratio(), 0.15),
-                positiveOr(avoidance.max_push(), 0.25));
-        const double sqrt_weight = std::sqrt(
-            positiveOr(qp_config.joint_limit_avoidance().weight(), 0.05));
-        cost.middleRows(6 + 2 * dof, dof) =
-            sqrt_weight * Eigen::MatrixXd::Identity(dof, dof);
-        target.segment(6 + 2 * dof, dof) = sqrt_weight * qdot_avoid;
-    }
-
-    Eigen::VectorXd lower(dof);
-    Eigen::VectorXd upper(dof);
-    for (Eigen::Index i = 0; i < dof; ++i) {
-        double velocity_limit = std::numeric_limits<double>::infinity();
-        if (joint_velocity_limits_.size() == dof && joint_velocity_limits_[i] > 0.0) {
-            velocity_limit = std::abs(joint_velocity_limits_[i]);
-        }
-        if (qd_max.size() == static_cast<std::size_t>(dof)) {
-            const double requested_limit = std::abs(qd_max[static_cast<std::size_t>(i)]);
-            if (std::isfinite(requested_limit) && requested_limit > 0.0) {
-                velocity_limit = std::min(velocity_limit, requested_limit);
-            }
-        }
-
-        double lb = -velocity_limit;
-        double ub = velocity_limit;
-        if (enforce_acceleration_limits) {
-            double acc_limit = 8.0;
-            if (i < speedl_config_.joint_acceleration_max_size() &&
-                speedl_config_.joint_acceleration_max(static_cast<int>(i)) > 0.0) {
-                acc_limit = speedl_config_.joint_acceleration_max(static_cast<int>(i));
-            }
-            lb = std::max(lb, qd_reference[i] - acc_limit * dt);
-            ub = std::min(ub, qd_reference[i] + acc_limit * dt);
-        }
-
-        if (joint_lower_limits_.size() == dof && joint_upper_limits_.size() == dof) {
-            lb = std::max(lb, (joint_lower_limits_[i] - q_measured[i]) / dt);
-            ub = std::min(ub, (joint_upper_limits_[i] - q_measured[i]) / dt);
-        }
-
-        if (lb > ub) {
-            CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] velocity bound infeasible at joint "
-                            << i << ": lb=" << lb << ", ub=" << ub
-                            << ", q=" << q_measured[i]
-                            << ", qd_ref=" << qd_reference[i]
-                            << ", dt=" << dt;
-            return false;
-        }
-        lower[i] = lb;
-        upper[i] = ub;
-    }
-
-    qp_solver_.SetCostFunction(cost, target);
-    qp_solver_.SetConstraintsFunction(Eigen::MatrixXd::Identity(dof, dof), lower, upper);
-    qp_solver_.SetPrimalVariable(qd_reference);
-
-    try {
-        qdot = qp_solver_.Solve();
-    } catch (const cmvr::QPSolverException& error) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] QP failed: "
-                        << error.what() << " (code=" << error.code() << ")";
-        return false;
-    } catch (const std::exception& error) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] QP failed: "
-                        << error.what();
-        return false;
-    }
-    return qdot.size() == dof;
-}
-
-bool PinocchioQpCartesianMotionPlanner::validateAchievedLinearTwist_(
-    const Eigen::MatrixXd& jacobian_base,
-    const Eigen::VectorXd& qdot) const
-{
-    const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
-    const Eigen::Vector3d desired_linear = speedl_command_twist_base_.head<3>();
-    const Eigen::Vector3d achieved_linear = achieved_twist_base.head<3>();
-    const double desired_linear_norm = desired_linear.norm();
-    const double achieved_linear_norm = achieved_linear.norm();
-    const double direction_check_min_speed =
-        std::max(1e-4, positiveOr(speedl_config_.linear_reverse_switch_speed_threshold(), 1e-3));
-    if (desired_linear_norm <= direction_check_min_speed) {
-        return true;
-    }
-
-    const double linear_min_speed_ratio =
-        clamp(positiveOr(speedl_config_.linear_min_speed_ratio(), 0.2), 0.0, 1.0);
-    const double speed_ratio = achieved_linear_norm / desired_linear_norm;
-    if (speed_ratio < linear_min_speed_ratio) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] achieved speed too low: desired_linear=["
-                        << desired_linear.x() << ", " << desired_linear.y() << ", " << desired_linear.z()
-                        << "], achieved_linear=[" << achieved_linear.x() << ", "
-                        << achieved_linear.y() << ", " << achieved_linear.z()
-                        << "], desired_norm=" << desired_linear_norm
-                        << ", achieved_norm=" << achieved_linear_norm
-                        << ", speed_ratio=" << speed_ratio
-                        << ", min_ratio=" << linear_min_speed_ratio
-                        << ", direction_check_min_speed=" << direction_check_min_speed;
-        return false;
-    }
-    if (achieved_linear_norm <= direction_check_min_speed) {
-        return true;
-    }
-    const double deviation_deg = directionDeviationDeg(desired_linear, achieved_linear);
-    const double severe_direction_deviation_deg =
-        positiveOr(speedl_config_.severe_direction_deviation_deg(), 45.0);
-    if (deviation_deg >= severe_direction_deviation_deg) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] direction deviation too large: desired_linear=["
-                        << desired_linear.x() << ", " << desired_linear.y() << ", " << desired_linear.z()
-                        << "], achieved_linear=[" << achieved_linear.x() << ", "
-                        << achieved_linear.y() << ", " << achieved_linear.z()
-                        << "], deviation_deg=" << deviation_deg
-                        << ", severe_threshold_deg=" << severe_direction_deviation_deg
-                        << ", direction_check_min_speed=" << direction_check_min_speed;
-        return false;
-    }
-    return true;
-}
-
-bool PinocchioQpCartesianMotionPlanner::speedLStep(const CartesianVelocity& target_velocity,
-                                                   const double dt,
-                                                   const std::vector<double>& q_measured,
-                                                   const std::vector<double>& qd_measured,
-                                                   std::vector<double>& qd_command,
-                                                   const FrameType frame)
-{
-    qd_command.clear();
-    if (!solver_ || !speedl_configured_ || dt <= 0.0) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] invalid state: solver="
-                        << (solver_ ? 1 : 0)
-                        << ", configured=" << (speedl_configured_ ? 1 : 0)
-                        << ", dt=" << dt;
-        return false;
-    }
-    if (static_cast<int>(q_measured.size()) != solver_->chainDof() ||
-        static_cast<int>(qd_measured.size()) != solver_->chainVelocityDof()) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] state size mismatch: q="
-                        << q_measured.size() << "/" << solver_->chainDof()
-                        << ", qd=" << qd_measured.size() << "/" << solver_->chainVelocityDof();
-        return false;
-    }
-
-    Eigen::Matrix<double, 6, 1> measured_twist_base = Eigen::Matrix<double, 6, 1>::Zero();
-    Eigen::MatrixXd jacobian_base;
-    Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-    if (!solver_->computeTwistBaseAtQ(q_measured,
-                                      qd_measured,
-                                      true,
-                                      measured_twist_base,
-                                      &jacobian_base,
-                                      &base_R_tool)) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] computeTwistBaseAtQ failed";
-        return false;
-    }
-
-    const Eigen::Matrix<double, 6, 1> target_twist = common::math::velocityToVector(target_velocity);
-    if (target_twist.squaredNorm() <= 1e-12) {
-        twist_limiter_.synchronize(measured_twist_base, dt, true);
-    } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) {
-        twist_limiter_.initialize(Eigen::Matrix<double, 6, 1>::Zero());
-    }
-    twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame));
-    speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool);
-
-    Eigen::VectorXd reference = toEigenVector(qd_measured);
-    if (prev_qdot_command_.size() == q_measured.size()) {
-        reference = toEigenVector(prev_qdot_command_);
-    }
-
-    Eigen::VectorXd qdot;
-    if (!solveVelocityQp_(jacobian_base,
-                          speedl_command_twist_base_,
-                          toEigenVector(q_measured),
-                          reference,
-                          dt,
-                          {},
-                          true,
-                          qpConfigOrDefault(speedl_config_.qp()),
-                          qdot)) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] solveVelocityQp failed";
-        return false;
-    }
-    if (!validateAchievedLinearTwist_(jacobian_base, qdot)) {
-        CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] validateAchievedLinearTwist failed";
-        return false;
-    }
-
-    qd_command = toStdVector(qdot);
-    prev_qdot_command_ = qd_command;
-    return true;
-}
-
-bool PinocchioQpCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration)
-{
-    if (!speedl_configured_ || acceleration <= 0.0) {
-        return false;
-    }
-    if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) {
-        return true;
-    }
-
-    cartesian_motion::updateTwistLimiterAcceleration(
-        twist_limiter_,
-        speedl_config_,
-        acceleration);
-    speedl_applied_acceleration_ = acceleration;
-    return true;
-}
-
-CartesianVelocity PinocchioQpCartesianMotionPlanner::getSpeedLCommandTwistBase() const
-{
-    return common::math::vectorToVelocity(speedl_command_twist_base_);
-}
-
-} // namespace cmvr::device

cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt

@@ -20,46 +20,4 @@ target_link_libraries(base_motion PUBLIC
 )
 
 add_library(cmvr_es::base_motion ALIAS base_motion)
-install(TARGETS base_motion LIBRARY DESTINATION lib)
+install(TARGETS base_motion LIBRARY DESTINATION lib)
-
-
-# --------------------------------------------------------
-# Unit test
-# --------------------------------------------------------
-
-
-
-
-add_executable(toppra_joint_trajectory_planner_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp
-)
-
-
-target_link_libraries(toppra_joint_trajectory_planner_test
-        PRIVATE
-        cmvr_es::base_motion
-        gtest
-        gtest_main
-        pthread
-        glog
-		cmvr_es::proto
-        ccd
-        fcl
-        OsqpEigen
-        
-)
-
-add_executable(cartesian_twist_limiter_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp
-)
-
-target_link_libraries(cartesian_twist_limiter_test
-        PRIVATE
-        cmvr_es::base_motion
-		gtest
-		gtest_main
-		pthread
-		glog
-		cmvr_es::proto
-        matplot
-)

cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp

@@ -1,732 +0,0 @@
-//
-// Created by lgv on 2026/3/10.
-//
-
-#include "gtest/gtest.h"
-
-#include <Eigen/Core>
-#include <matplot/matplot.h>
-
-#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
-
-namespace cmvr
-{
-namespace
-{
-
-using Twist = CartesianTwistLimiter::Twist;
-
-constexpr double kDt = 0.01;
-constexpr double kComponentEps = 1e-8;
-constexpr double kDirectionEps = 1e-6;
-constexpr double kSwitchSpeedUpperBound = 0.05;
-
-Twist makeTwist(double vx, double vy, double vz, double wx, double wy, double wz)
-{
-  Twist twist = Twist::Zero();
-  twist << vx, vy, vz, wx, wy, wz;
-  return twist;
-}
-
-Eigen::Vector3d normalizedOrZero(const Eigen::Vector3d& value)
-{
-  const double norm = value.norm();
-  if (norm <= kComponentEps) {
-    return Eigen::Vector3d::Zero();
-  }
-  return value / norm;
-}
-
-CartesianTwistLimiter makeLimiter()
-{
-  CartesianTwistLimiter limiter;
-  limiter.setLinearConstraints(1.0, 1.0, 5.0);
-  limiter.setAngularConstraints(1.0, 1.0, 5.0);
-  limiter.initialize();
-  return limiter;
-}
-
-Twist linearTargetAtTime(double t)
-{
-  if (t < 1.0) {
-    return makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-  }
-  if (t < 3.5) {
-    return makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0);
-  }
-  if (t < 6.0) {
-    return makeTwist(-0.8, 0.6, 0.2, 0.0, 0.0, 0.0);
-  }
-  if (t < 8.5) {
-    return makeTwist(-0.7, 0.0, 0.0, 0.0, 0.0, 0.0);
-  }
-  return makeTwist(-0.7, 0.0, 0.0, 0.0, 0.0, 0.0);
-}
-
-Twist angularTargetAtTime(double t)
-{
-  if (t < 1.0) {
-    return makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-  }
-  if (t < 3.5) {
-    return makeTwist(0.0, 0.0, 0.0, 0.8, 0.0, 0.0);
-  }
-  if (t < 6.0) {
-    return makeTwist(0.0, 0.0, 0.0, -0.8, 0, 0);
-  }
-  if (t < 8.5) {
-    return makeTwist(0.0, 0.0, 0.0, -0.6, 0.0, 0.0);
-  }
-  return makeTwist(0.0, 0.0, 0.0, -0.6, 0.0, 0.0);
-}
-
-Twist synchronizeTargetAtTime(double t)
-{
-  if (t < 1.0) {
-    return makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-  }
-  if (t < 3.5) {
-    return makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0);
-  }
-  if (t < 6.0) {
-    return makeTwist(0.0, 0.7, 0.0, 0.0, 0.0, 0.0);
-  }
-  if (t < 8.0) {
-    return makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
-  }
-  return makeTwist(-0.6, 0.0, 0.0, 0.0, 0.0, 0.0);
-}
-
-}  // namespace
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, LinearDirectionRemainsLockedForFixedTarget)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  const Eigen::Vector3d expected_dir = normalizedOrZero(Eigen::Vector3d(0.8, 0.6, 0.0));
-  limiter.setTargetTwist(makeTwist(0.8, 0.6, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-
-  double max_speed = 0.0;
-  for (int i = 0; i < 400; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    const Eigen::Vector3d linear = twist.head<3>();
-    max_speed = std::max(max_speed, linear.norm());
-
-    if (linear.norm() > kComponentEps) {
-      const Eigen::Vector3d dir = linear.normalized();
-      EXPECT_LT(dir.cross(expected_dir).norm(), kDirectionEps) << "step=" << i;
-    }
-  }
-
-  EXPECT_GT(max_speed, 0.5);
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, LinearReverseStopsBeforeSwitchingDirection)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  limiter.setTargetTwist(makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-
-  double speed_before_switch = 0.0;
-  for (int i = 0; i < 200; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    if (twist.head<3>().norm() > 0.2) {
-      speed_before_switch = twist.head<3>().norm();
-      break;
-    }
-  }
-  ASSERT_GT(speed_before_switch, 0.2);
-
-  limiter.setTargetTwist(makeTwist(-0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-
-  bool negative_x_seen = false;
-  double speed_when_negative_x_appears = 0.0;
-  for (int i = 0; i < 500; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    EXPECT_NEAR(twist.y(), 0.0, kComponentEps);
-    EXPECT_NEAR(twist.z(), 0.0, kComponentEps);
-
-    if (!negative_x_seen && twist.x() < -kComponentEps) {
-      negative_x_seen = true;
-      speed_when_negative_x_appears = twist.head<3>().norm();
-      break;
-    }
-
-    if (!negative_x_seen) {
-      EXPECT_GE(twist.x(), -kComponentEps);
-    }
-  }
-
-  ASSERT_TRUE(negative_x_seen);
-  EXPECT_LT(speed_when_negative_x_appears, kSwitchSpeedUpperBound);
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, LinearNonCollinearSwitchStopsBeforeChangingAxis)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  limiter.setTargetTwist(makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-
-  for (int i = 0; i < 200; ++i) {
-    if (limiter.update(kDt, base_R_tool).head<3>().norm() > 0.2) {
-      break;
-    }
-  }
-
-  limiter.setTargetTwist(makeTwist(0.0, 0.8, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-
-  bool y_seen = false;
-  double speed_when_y_appears = 0.0;
-  for (int i = 0; i < 500; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    EXPECT_NEAR(twist.z(), 0.0, kComponentEps);
-
-    if (!y_seen && std::abs(twist.y()) > kComponentEps) {
-      y_seen = true;
-      speed_when_y_appears = twist.head<3>().norm();
-      break;
-    }
-
-    if (!y_seen) {
-      EXPECT_NEAR(twist.y(), 0.0, kComponentEps);
-      EXPECT_GE(twist.x(), -kComponentEps);
-    }
-  }
-
-  ASSERT_TRUE(y_seen);
-  EXPECT_LT(speed_when_y_appears, kSwitchSpeedUpperBound);
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, AngularNonCollinearSwitchStopsBeforeChangingAxis)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  limiter.setTargetTwist(makeTwist(0.0, 0.0, 0.0, 0.8, 0.0, 0.0), CartesianFrame::Base);
-
-  for (int i = 0; i < 200; ++i) {
-    if (limiter.update(kDt, base_R_tool).tail<3>().norm() > 0.2) {
-      break;
-    }
-  }
-
-  limiter.setTargetTwist(makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.8), CartesianFrame::Base);
-
-  bool z_seen = false;
-  double speed_when_z_appears = 0.0;
-  for (int i = 0; i < 500; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    EXPECT_NEAR(twist(4), 0.0, kComponentEps);
-
-    if (!z_seen && std::abs(twist(5)) > kComponentEps) {
-      z_seen = true;
-      speed_when_z_appears = twist.tail<3>().norm();
-      break;
-    }
-
-    if (!z_seen) {
-      EXPECT_NEAR(twist(5), 0.0, kComponentEps);
-      EXPECT_GE(twist(3), -kComponentEps);
-    }
-  }
-
-  ASSERT_TRUE(z_seen);
-  EXPECT_LT(speed_when_z_appears, kSwitchSpeedUpperBound);
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, StopKeepsCurrentLinearDirection)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  limiter.setTargetTwist(makeTwist(0.8, 0.6, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-
-  Eigen::Vector3d moving_dir = Eigen::Vector3d::Zero();
-  for (int i = 0; i < 300; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    if (twist.head<3>().norm() > 0.2) {
-      moving_dir = twist.head<3>().normalized();
-      break;
-    }
-  }
-  ASSERT_GT(moving_dir.norm(), 0.5);
-
-  limiter.stop();
-
-  double final_speed = 0.0;
-  for (int i = 0; i < 400; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    final_speed = twist.head<3>().norm();
-    if (final_speed > kComponentEps) {
-      EXPECT_LT(twist.head<3>().normalized().cross(moving_dir).norm(), kDirectionEps)
-          << "step=" << i;
-    }
-  }
-
-  EXPECT_LT(final_speed, 1e-4);
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, SynchronizeWithoutKeepingTargetClearsOldCommand)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  limiter.setTargetTwist(makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-  for (int i = 0; i < 80; ++i) {
-    limiter.update(kDt, base_R_tool);
-  }
-
-  limiter.synchronize(Twist::Zero(), kDt, false);
-
-  EXPECT_NEAR(limiter.getTargetTwistBase().norm(), 0.0, kComponentEps);
-
-  for (int i = 0; i < 50; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    EXPECT_NEAR(twist.norm(), 0.0, kComponentEps) << "step=" << i;
-  }
-
-  EXPECT_FALSE(limiter.isMoving());
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, SynchronizeKeepingTargetContinuesTowardExistingCommand)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  limiter.setTargetTwist(makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-  for (int i = 0; i < 120; ++i) {
-    limiter.update(kDt, base_R_tool);
-  }
-
-  ASSERT_GT(limiter.getTwistBase().x(), 0.2);
-
-  const Twist measured_twist = makeTwist(0.15, 0.0, 0.0, 0.0, 0.0, 0.0);
-  limiter.synchronize(measured_twist, kDt, true);
-
-  EXPECT_NEAR(limiter.getTwistBase().x(), 0.15, kComponentEps);
-  EXPECT_NEAR(limiter.getTwistBase().y(), 0.0, kComponentEps);
-  EXPECT_NEAR(limiter.getTwistBase().z(), 0.0, kComponentEps);
-
-  double max_resumed_speed = measured_twist.x();
-  for (int i = 0; i < 200; ++i) {
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    EXPECT_NEAR(twist.y(), 0.0, kComponentEps);
-    EXPECT_NEAR(twist.z(), 0.0, kComponentEps);
-    max_resumed_speed = std::max(max_resumed_speed, twist.head<3>().norm());
-    if (max_resumed_speed > 0.4) {
-      break;
-    }
-  }
-
-  EXPECT_GT(max_resumed_speed, 0.4);
-  EXPECT_GT(limiter.getTargetTwistBase().x(), 0.7);
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, EmergencyStopStopsFasterThanNormalStopAndAcceptsNewTarget)
-{
-  CartesianTwistLimiter normal_limiter = makeLimiter();
-  CartesianTwistLimiter emergency_limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  const Twist forward_twist = makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0);
-  normal_limiter.setTargetTwist(forward_twist, CartesianFrame::Base);
-  emergency_limiter.setTargetTwist(forward_twist, CartesianFrame::Base);
-
-  for (int i = 0; i < 250; ++i) {
-    normal_limiter.update(kDt, base_R_tool);
-    emergency_limiter.update(kDt, base_R_tool);
-  }
-
-  const double start_speed = normal_limiter.getTwistBase().head<3>().norm();
-  ASSERT_GT(start_speed, 0.2);
-  ASSERT_NEAR(emergency_limiter.getTwistBase().head<3>().norm(), start_speed, 1e-3);
-
-  normal_limiter.stop();
-  emergency_limiter.emergencyStop(3.0, 20.0);
-
-  int normal_stop_steps = -1;
-  int emergency_stop_steps = -1;
-  for (int i = 0; i < 500; ++i) {
-    const double normal_speed = normal_limiter.update(kDt, base_R_tool).head<3>().norm();
-    const double emergency_speed = emergency_limiter.update(kDt, base_R_tool).head<3>().norm();
-
-    if (normal_stop_steps < 0 && normal_speed < 1e-3) {
-      normal_stop_steps = i;
-    }
-    if (emergency_stop_steps < 0 && emergency_speed < 1e-3) {
-      emergency_stop_steps = i;
-    }
-
-    if (normal_stop_steps >= 0 && emergency_stop_steps >= 0) {
-      break;
-    }
-  }
-
-  ASSERT_GE(normal_stop_steps, 0);
-  ASSERT_GE(emergency_stop_steps, 0);
-  EXPECT_LT(emergency_stop_steps, normal_stop_steps);
-
-  const Twist restart_twist = makeTwist(-0.4, 0.0, 0.0, 0.0, 0.0, 0.0);
-  emergency_limiter.setTargetTwist(restart_twist, CartesianFrame::Base);
-
-  double restarted_speed = 0.0;
-  double restarted_x = 0.0;
-  for (int i = 0; i < 300; ++i) {
-    const Twist twist = emergency_limiter.update(kDt, base_R_tool);
-    restarted_speed = twist.head<3>().norm();
-    restarted_x = twist.x();
-    if (restarted_speed > 0.2) {
-      break;
-    }
-  }
-
-  EXPECT_GT(restarted_speed, 0.2);
-  EXPECT_LT(restarted_x, -0.2);
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, PlotLinearVelocityComponentsAndNorm)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  std::vector<double> time;
-  std::vector<double> vx;
-  std::vector<double> vy;
-  std::vector<double> vz;
-  std::vector<double> speed;
-  std::vector<double> target_speed;
-  std::vector<double> ax;
-  std::vector<double> ay;
-  std::vector<double> az;
-  std::vector<double> accel;
-  std::vector<double> jx;
-  std::vector<double> jy;
-  std::vector<double> jz;
-  std::vector<double> jerk;
-
-  constexpr double kEmergencyTime = 8.5;
-  constexpr double kRestartTime = 10.0;
-  constexpr double kTotalTime = 14.0;
-  const int steps = static_cast<int>(kTotalTime / kDt);
-
-  time.reserve(steps + 1);
-  vx.reserve(steps + 1);
-  vy.reserve(steps + 1);
-  vz.reserve(steps + 1);
-  speed.reserve(steps + 1);
-  target_speed.reserve(steps + 1);
-  ax.reserve(steps + 1);
-  ay.reserve(steps + 1);
-  az.reserve(steps + 1);
-  accel.reserve(steps + 1);
-  jx.reserve(steps + 1);
-  jy.reserve(steps + 1);
-  jz.reserve(steps + 1);
-  jerk.reserve(steps + 1);
-
-  bool emergency_stop_called = false;
-  bool restart_called = false;
-  for (int i = 0; i <= steps; ++i) {
-    const double t = i * kDt;
-    if (t < kEmergencyTime) {
-      limiter.setTargetTwist(linearTargetAtTime(t), CartesianFrame::Base);
-    } else if (!emergency_stop_called) {
-      limiter.emergencyStop(10.0, 50.0);
-      emergency_stop_called = true;
-    } else if (t >= kRestartTime && !restart_called) {
-      limiter.setTargetTwist(makeTwist(0.0, -0.6, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base);
-      restart_called = true;
-    }
-
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    const Twist target = limiter.getTargetTwistBase();
-    const Twist acceleration = limiter.getAccelerationBase();
-    const Twist current_jerk = limiter.getJerkBase();
-
-    time.push_back(t);
-    vx.push_back(twist.x());
-    vy.push_back(twist.y());
-    vz.push_back(twist.z());
-    speed.push_back(twist.head<3>().norm());
-    target_speed.push_back(target.head<3>().norm());
-    ax.push_back(acceleration.x());
-    ay.push_back(acceleration.y());
-    az.push_back(acceleration.z());
-    accel.push_back(acceleration.head<3>().norm());
-    jx.push_back(current_jerk.x());
-    jy.push_back(current_jerk.y());
-    jz.push_back(current_jerk.z());
-    jerk.push_back(current_jerk.head<3>().norm());
-  }
-
-  using namespace matplot;
-
-  auto configure_axes = [](const axes_handle& axes) {
-    axes->line_width(1.5f);
-    grid(axes, on);
-  };
-
-  auto style_line = [](const auto& line, std::initializer_list<float> color, double width) {
-    line->line_width(width);
-    line->color(color);
-  };
-
-  auto fig = figure(true);
-  fig->size(1600, 1200);
-  fig->font_size(16);
-
-  auto ax_v = subplot(fig, std::array<float, 4>{0.07f, 0.69f, 0.88f, 0.24f});
-  hold(ax_v, on);
-  style_line(plot(ax_v, time, vx), {0.86f, 0.16f, 0.16f}, 3.0);
-  style_line(plot(ax_v, time, vy), {0.16f, 0.47f, 0.80f}, 3.0);
-  style_line(plot(ax_v, time, vz), {0.12f, 0.62f, 0.42f}, 3.0);
-  style_line(plot(ax_v, time, speed), {0.10f, 0.10f, 0.10f}, 3.5);
-  auto line_target_speed = plot(ax_v, time, target_speed);
-  line_target_speed->line_width(2.5);
-  line_target_speed->line_style("--");
-  line_target_speed->color({0.80f, 0.55f, 0.10f});
-  title(ax_v, "CartesianTwistLimiter Linear Velocity With Emergency Stop And Restart");
-  xlabel(ax_v, "time [s]");
-  ylabel(ax_v, "velocity [m/s]");
-  legend(ax_v, {"vx", "vy", "vz", "speed", "target speed"});
-  configure_axes(ax_v);
-
-  auto ax_a = subplot(fig, std::array<float, 4>{0.07f, 0.38f, 0.88f, 0.24f});
-  hold(ax_a, on);
-  style_line(plot(ax_a, time, ax), {0.86f, 0.16f, 0.16f}, 3.0);
-  style_line(plot(ax_a, time, ay), {0.16f, 0.47f, 0.80f}, 3.0);
-  style_line(plot(ax_a, time, az), {0.12f, 0.62f, 0.42f}, 3.0);
-  style_line(plot(ax_a, time, accel), {0.10f, 0.10f, 0.10f}, 3.5);
-  title(ax_a, "CartesianTwistLimiter Linear Acceleration");
-  xlabel(ax_a, "time [s]");
-  ylabel(ax_a, "acceleration [m/s^2]");
-  legend(ax_a, {"ax", "ay", "az", "acc norm"});
-  configure_axes(ax_a);
-
-  auto ax_j = subplot(fig, std::array<float, 4>{0.07f, 0.07f, 0.88f, 0.24f});
-  hold(ax_j, on);
-  style_line(plot(ax_j, time, jx), {0.86f, 0.16f, 0.16f}, 3.0);
-  style_line(plot(ax_j, time, jy), {0.16f, 0.47f, 0.80f}, 3.0);
-  style_line(plot(ax_j, time, jz), {0.12f, 0.62f, 0.42f}, 3.0);
-  style_line(plot(ax_j, time, jerk), {0.10f, 0.10f, 0.10f}, 3.5);
-  title(ax_j, "CartesianTwistLimiter Linear Jerk");
-  xlabel(ax_j, "time [s]");
-  ylabel(ax_j, "jerk [m/s^3]");
-  legend(ax_j, {"jx", "jy", "jz", "jerk norm"});
-  configure_axes(ax_j);
-
-  show(fig);
-  cla();
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, PlotAngularVelocityComponentsAndNorm)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  std::vector<double> time;
-  std::vector<double> wx;
-  std::vector<double> wy;
-  std::vector<double> wz;
-  std::vector<double> omega;
-  std::vector<double> target_omega;
-
-  constexpr double kEmergencyTime = 8.5;
-  constexpr double kRestartTime = 10.0;
-  constexpr double kTotalTime = 14.0;
-  const int steps = static_cast<int>(kTotalTime / kDt);
-
-  time.reserve(steps + 1);
-  wx.reserve(steps + 1);
-  wy.reserve(steps + 1);
-  wz.reserve(steps + 1);
-  omega.reserve(steps + 1);
-  target_omega.reserve(steps + 1);
-
-  bool emergency_stop_called = false;
-  bool restart_called = false;
-  for (int i = 0; i <= steps; ++i) {
-    const double t = i * kDt;
-    if (t < kEmergencyTime) {
-      limiter.setTargetTwist(angularTargetAtTime(t), CartesianFrame::Base);
-    } else if (!emergency_stop_called) {
-      limiter.emergencyStop(10.0, 50.0);
-      emergency_stop_called = true;
-    } else if (t >= kRestartTime && !restart_called) {
-      limiter.setTargetTwist(makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, -0.5), CartesianFrame::Base);
-      restart_called = true;
-    }
-
-    const Twist twist = limiter.update(kDt, base_R_tool);
-    const Twist target = limiter.getTargetTwistBase();
-
-    time.push_back(t);
-    wx.push_back(twist(3));
-    wy.push_back(twist(4));
-    wz.push_back(twist(5));
-    omega.push_back(twist.tail<3>().norm());
-    target_omega.push_back(target.tail<3>().norm());
-  }
-
-  using namespace matplot;
-
-  auto configure_axes = [](const axes_handle& axes) {
-    axes->line_width(1.5f);
-    grid(axes, on);
-  };
-
-  auto style_line = [](const auto& line, std::initializer_list<float> color, double width) {
-    line->line_width(width);
-    line->color(color);
-  };
-
-  auto fig = figure(true);
-  fig->size(1600, 520);
-  fig->font_size(16);
-
-  auto ax = subplot(fig, std::array<float, 4>{0.07f, 0.14f, 0.88f, 0.74f});
-  hold(ax, on);
-  style_line(plot(ax, time, wx), {0.86f, 0.16f, 0.16f}, 3.0);
-  style_line(plot(ax, time, wy), {0.16f, 0.47f, 0.80f}, 3.0);
-  style_line(plot(ax, time, wz), {0.12f, 0.62f, 0.42f}, 3.0);
-  style_line(plot(ax, time, omega), {0.10f, 0.10f, 0.10f}, 3.5);
-  auto line_target_omega = plot(ax, time, target_omega);
-  line_target_omega->line_width(2.5);
-  line_target_omega->line_style("--");
-  line_target_omega->color({0.80f, 0.55f, 0.10f});
-  title(ax, "CartesianTwistLimiter Angular Velocity With Emergency Stop And Restart");
-  xlabel(ax, "time [s]");
-  ylabel(ax, "angular velocity [rad/s]");
-  legend(ax, {"wx", "wy", "wz", "omega", "target omega"});
-  configure_axes(ax);
-
-  show(fig);
-  cla();
-}
-
-TEST(CARTESIAN_TWIST_LIMITER_TEST, PlotSynchronizeTrackingWithMeasuredTwist)
-{
-  CartesianTwistLimiter limiter = makeLimiter();
-  const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity();
-
-  Twist measured_twist = Twist::Zero();
-
-  std::vector<double> time;
-  std::vector<double> target_speed;
-  std::vector<double> commanded_speed;
-  std::vector<double> measured_speed;
-  std::vector<double> commanded_vx;
-  std::vector<double> commanded_vy;
-  std::vector<double> measured_vx;
-  std::vector<double> measured_vy;
-  std::vector<double> tracking_error;
-  std::vector<double> acc_norm;
-  std::vector<double> jerk_norm;
-
-  constexpr double kTotalTime = 10.0;
-  constexpr double kTrackingTau = 0.08;
-  const double alpha = kDt / (kTrackingTau + kDt);
-  const int steps = static_cast<int>(kTotalTime / kDt);
-
-  time.reserve(steps + 1);
-  target_speed.reserve(steps + 1);
-  commanded_speed.reserve(steps + 1);
-  measured_speed.reserve(steps + 1);
-  commanded_vx.reserve(steps + 1);
-  commanded_vy.reserve(steps + 1);
-  measured_vx.reserve(steps + 1);
-  measured_vy.reserve(steps + 1);
-  tracking_error.reserve(steps + 1);
-  acc_norm.reserve(steps + 1);
-  jerk_norm.reserve(steps + 1);
-
-  for (int i = 0; i <= steps; ++i) {
-    const double t = i * kDt;
-    const Twist target = synchronizeTargetAtTime(t);
-
-    limiter.synchronize(measured_twist, kDt, true);
-    limiter.setTargetTwist(target, CartesianFrame::Base);
-    const Twist commanded = limiter.update(kDt, base_R_tool);
-
-    measured_twist += alpha * (commanded - measured_twist);
-
-    time.push_back(t);
-    target_speed.push_back(target.head<3>().norm());
-    commanded_speed.push_back(commanded.head<3>().norm());
-    measured_speed.push_back(measured_twist.head<3>().norm());
-    commanded_vx.push_back(commanded.x());
-    commanded_vy.push_back(commanded.y());
-    measured_vx.push_back(measured_twist.x());
-    measured_vy.push_back(measured_twist.y());
-    tracking_error.push_back((commanded.head<3>() - measured_twist.head<3>()).norm());
-    acc_norm.push_back(limiter.getAccelerationBase().head<3>().norm());
-    jerk_norm.push_back(limiter.getJerkBase().head<3>().norm());
-  }
-
-  using namespace matplot;
-
-  auto configure_axes = [](const axes_handle& axes) {
-    axes->line_width(1.5f);
-    grid(axes, on);
-  };
-
-  auto style_line = [](const auto& line, std::initializer_list<float> color, double width) {
-    line->line_width(width);
-    line->color(color);
-  };
-
-  auto fig = figure(true);
-  fig->size(1600, 1200);
-  fig->font_size(16);
-
-  auto ax_speed = subplot(fig, std::array<float, 4>{0.07f, 0.69f, 0.88f, 0.24f});
-  hold(ax_speed, on);
-  auto line_target = plot(ax_speed, time, target_speed);
-  line_target->line_width(2.5);
-  line_target->line_style("--");
-  line_target->color({0.80f, 0.55f, 0.10f});
-  style_line(plot(ax_speed, time, commanded_speed), {0.10f, 0.10f, 0.10f}, 3.5);
-  style_line(plot(ax_speed, time, measured_speed), {0.55f, 0.12f, 0.72f}, 3.0);
-  title(ax_speed, "Synchronize: target / commanded / measured speed");
-  xlabel(ax_speed, "time [s]");
-  ylabel(ax_speed, "speed [m/s]");
-  legend(ax_speed, {"target speed", "commanded speed", "measured speed"});
-  configure_axes(ax_speed);
-
-  auto ax_components = subplot(fig, std::array<float, 4>{0.07f, 0.38f, 0.88f, 0.24f});
-  hold(ax_components, on);
-  style_line(plot(ax_components, time, commanded_vx), {0.86f, 0.16f, 0.16f}, 3.0);
-  auto line_measured_vx = plot(ax_components, time, measured_vx);
-  line_measured_vx->line_width(2.5);
-  line_measured_vx->line_style("--");
-  line_measured_vx->color({0.86f, 0.16f, 0.16f});
-  style_line(plot(ax_components, time, commanded_vy), {0.16f, 0.47f, 0.80f}, 3.0);
-  auto line_measured_vy = plot(ax_components, time, measured_vy);
-  line_measured_vy->line_width(2.5);
-  line_measured_vy->line_style("--");
-  line_measured_vy->color({0.16f, 0.47f, 0.80f});
-  title(ax_components, "Synchronize: commanded vs measured components");
-  xlabel(ax_components, "time [s]");
-  ylabel(ax_components, "velocity [m/s]");
-  legend(ax_components, {"cmd vx", "meas vx", "cmd vy", "meas vy"});
-  configure_axes(ax_components);
-
-  auto ax_dynamics = subplot(fig, std::array<float, 4>{0.07f, 0.07f, 0.88f, 0.24f});
-  hold(ax_dynamics, on);
-  style_line(plot(ax_dynamics, time, tracking_error), {0.10f, 0.10f, 0.10f}, 3.5);
-  style_line(plot(ax_dynamics, time, acc_norm), {0.12f, 0.62f, 0.42f}, 3.0);
-  style_line(plot(ax_dynamics, time, jerk_norm), {0.55f, 0.12f, 0.72f}, 3.0);
-  title(ax_dynamics, "Synchronize: tracking error / acceleration / jerk");
-  xlabel(ax_dynamics, "time [s]");
-  ylabel(ax_dynamics, "norm");
-  legend(ax_dynamics, {"|cmd - meas|", "acc norm", "jerk norm"});
-  configure_axes(ax_dynamics);
-
-  show(fig);
-  cla();
-}
-
-}  // namespace cmvr

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp

@@ -1,88 +0,0 @@
-//
-// Created by lgv on 11/11/25.
-//
-
-#include "gtest/gtest.h"
-#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
-
-#include <memory>
-
-using namespace cmvr;
-TEST(TOPPRA_JOINT_TRAJECTORY_PLANNER_TEST,TOPPRA_TEST) {
-
-    auto planner = std::make_shared<ToppraJointTrajectoryPlanner>();
-    planner->setPathType(PathType::Quintic);
-
-    planner->setSymmetricLimits(std::vector<double>(7, 1.5),
-                               std::vector<double>(7, 3.0));
-    TrajPtr traj;
-    std::vector<double> q0{0.0,-0.5,0.8,0.0,0.2,-0.3,0.1};
-    std::vector<double> q1{1.2,0.2,-0.6,0.7,-0.4,0.5,-0.2};
-    if (!planner->plan(q0, q1, traj)) { std::cerr << "plan failed\n"; }
-
-    // 2) 采样 0.01 s
-    auto samples = planner->sampleTrajectory(traj, 0.01);
-
-    // 3) 写 CSV
-    if (!planner->writeTrajectoryCsv("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv", samples)) {
-        std::cerr << "write csv failed\n";
-    }
-
-    std::cout << "CSV saved: traj.csv\n";
-
-}
-
-TEST(TOPPRA_JOINT_TRAJECTORY_PLANNER_TEST, TOPPRA_WAYPOINTS_TEST) {
-
-    auto planner = std::make_shared<ToppraJointTrajectoryPlanner>();
-    planner->setPathType(PathType::Quintic);
-
-    // 7 自由度对称速度 / 加速度约束
-    planner->setSymmetricLimits(std::vector<double>(7, 1.5),
-                                std::vector<double>(7, 3.0));
-
-    // ------- 1) 构造多个 q 路点 -------
-    std::vector<double> q0 { 0.0, -0.5,  0.8,  0.0,  0.2, -0.3,  0.1};
-    std::vector<double> q1 { 0.5, -0.2,  0.4,  0.3, -0.1,  0.1,  0.0};
-    std::vector<double> q2 { 0.9,  0.1, -0.3,  0.5, -0.3,  0.3, -0.1};
-    std::vector<double> q3 { 1.2,  0.2, -0.6,  0.7, -0.4,  0.5, -0.2}; // 终点
-
-    std::vector<std::vector<double>> waypoints;
-    waypoints.push_back(q0);
-    waypoints.push_back(q1);
-    waypoints.push_back(q2);
-    waypoints.push_back(q3);
-
-    // ------- 2) 调多路点 plan -------
-    TrajPtr traj;
-    if (!planner->plan(waypoints, traj)) {
-        std::cerr << "multi-waypoints plan failed\n";
-        FAIL();  // GTest 标记失败
-    }
-
-    // ------- 3) 采样并简单校验 -------
-    // 0.01 s 采样
-    auto samples = planner->sampleTrajectory(traj, 0.01);
-    ASSERT_FALSE(samples.empty());
-
-    // （下面假设 TrajSample 里有 q / pos 这样的关节角向量字段，
-    //  你按自己的结构名改一下就行）
-    const auto &q_start = samples.front().q;
-    const auto &q_end   = samples.back().q;
-
-    ASSERT_EQ(q_start.size(), q0.size());
-    ASSERT_EQ(q_end.size(),   q3.size());
-
-    for (size_t i = 0; i < q0.size(); ++i) {
-        EXPECT_NEAR(q_start[i], q0[i], 1e-4);
-        EXPECT_NEAR(q_end[i],   q3[i], 1e-4);
-    }
-
-    // ------- 4) 写 CSV 看一下轨迹 -------
-    if (!planner->writeTrajectoryCsv(
-            "/home/lgv/cmvr/cmvr-es/data/planner/traj.csv", samples)) {
-        std::cerr << "write csv failed\n";
-            } else {
-                std::cout << "CSV saved: traj.csv\n";
-            }
-}

cmvr-es/algorithms/perception/CMakeLists.txt

@@ -31,18 +31,3 @@ target_link_libraries(perception PUBLIC
 
 add_library(cmvr_es::perception ALIAS perception)
 install(TARGETS perception LIBRARY DESTINATION lib)
-
-add_executable(tag_relative_target_3d_test
-        apriltag/src/tag_relative_target_3d_test.cpp
-)
-
-target_link_libraries(tag_relative_target_3d_test
-        PRIVATE
-        cmvr_es::perception
-        cmvr_es::device::realsense_camera
-        cmvr_es::proto
-        glog
-        gtest
-        gtest_main
-        pthread
-)

cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d_test.cpp

@@ -1,711 +0,0 @@
-//
-// Created by lgv on 2026/2/26.
-//
-
-#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
-#include "devices/camera/realsense_camera/include/realsense_camera.h"
-
-#include <gtest/gtest.h>
-
-#include <algorithm>
-#include <chrono>
-#include <cmath>
-#include <cstring>
-#include <cstdint>
-#include <iomanip>
-#include <iostream>
-#include <limits>
-#include <sstream>
-#include <thread>
-#include <opencv2/highgui.hpp>
-#include <opencv2/imgproc.hpp>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpImage.h>
-
-namespace {
-
-// 直接在这里改测试参数。
-constexpr const char* kRsSerial = "243122074587";
-// constexpr const char* kRsSerial = "243122075389";
-// constexpr const char* kRsSerial = "f1421544";
-// Target pixel. Negative means image center.
-constexpr int kTargetU = -1;
-constexpr int kTargetV = -1;
-constexpr double kTagSize =  0.01975 ;
-// <= 0 means loop until user abort.
-constexpr int kTries = -1;
-constexpr int kWidth = 1280;
-constexpr int kHeight = 720;
-constexpr int kFps = 30;
-constexpr const char* kWindowName = "TagRelativeTarget3DRealSenseTest";
-constexpr int kWarmupMaxTries = 60;
-constexpr int kWarmupSleepMs = 50;
-constexpr int kFrameRetryMax = 3;
-constexpr int kFrameRetrySleepMs = 25;
-constexpr int kLogEveryNFrames = 1;
-
-struct FramePack {
-    cv::Mat color;
-    cv::Mat depth;
-    cmvr::device::Rs2Intrinsics intrinsics{};
-};
-
-bool fetchRGBFrame(const std::shared_ptr<cmvr::device::RealsenseCamera>& camera,
-                   FramePack& frame,
-                   std::string& error_out) {
-    error_out.clear();
-    for (int retry = 0; retry < kFrameRetryMax; ++retry) {
-        try {
-            camera->getRGBImage(frame.color, frame.intrinsics);
-            if (!frame.color.empty()) {
-                return true;
-            }
-            error_out = "empty_frame";
-        } catch (const std::exception& e) {
-            error_out = e.what();
-        }
-        if (retry + 1 < kFrameRetryMax) {
-            std::this_thread::sleep_for(std::chrono::milliseconds(kFrameRetrySleepMs));
-        }
-    }
-    return false;
-}
-
-bool fetchRGBDFrame(const std::shared_ptr<cmvr::device::RealsenseCamera>& camera,
-                    FramePack& frame,
-                    std::string& error_out) {
-    error_out.clear();
-    for (int retry = 0; retry < kFrameRetryMax; ++retry) {
-        try {
-            camera->getRGBDImages(frame.color, frame.depth, frame.intrinsics);
-            if (!frame.color.empty() && !frame.depth.empty()) {
-                return true;
-            }
-            error_out = "empty_rgbd_frame";
-        } catch (const std::exception& e) {
-            error_out = e.what();
-        }
-        if (retry + 1 < kFrameRetryMax) {
-            std::this_thread::sleep_for(std::chrono::milliseconds(kFrameRetrySleepMs));
-        }
-    }
-    return false;
-}
-
-bool projectPoint(const Eigen::Vector3d& p_c,
-                  const cmvr::device::Rs2Intrinsics& K,
-                  cv::Point& uv) {
-    if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z())) {
-        return false;
-    }
-    if (p_c.z() <= 1e-6) {
-        return false;
-    }
-    if (!std::isfinite(K.fx) || !std::isfinite(K.fy) ||
-        !std::isfinite(K.cx) || !std::isfinite(K.cy) ||
-        K.fx <= 0.0f || K.fy <= 0.0f) {
-        return false;
-    }
-    const double u = static_cast<double>(K.fx) * (p_c.x() / p_c.z()) + static_cast<double>(K.cx);
-    const double v = static_cast<double>(K.fy) * (p_c.y() / p_c.z()) + static_cast<double>(K.cy);
-    uv.x = static_cast<int>(std::lround(u));
-    uv.y = static_cast<int>(std::lround(v));
-    return true;
-}
-
-bool sampleDepthMetersAtPixel(const cv::Mat& depth, int u, int v, double& z_m) {
-    if (depth.empty() || depth.type() != CV_16UC1) {
-        return false;
-    }
-    if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) {
-        return false;
-    }
-
-    std::vector<double> vals;
-    vals.reserve(9);
-    constexpr int kRadius = 1; // 3x3
-    for (int yy = std::max(0, v - kRadius); yy <= std::min(depth.rows - 1, v + kRadius); ++yy) {
-        for (int xx = std::max(0, u - kRadius); xx <= std::min(depth.cols - 1, u + kRadius); ++xx) {
-            const uint16_t raw = depth.at<uint16_t>(yy, xx);
-            if (raw == 0) {
-                continue;
-            }
-            const double z = static_cast<double>(raw) * 1e-3; // mm -> m
-            if (!std::isfinite(z) || z <= 1e-4 || z >= 50.0) {
-                continue;
-            }
-            vals.push_back(z);
-        }
-    }
-    if (vals.empty()) {
-        return false;
-    }
-
-    const size_t mid = vals.size() / 2;
-    std::nth_element(vals.begin(), vals.begin() + mid, vals.end());
-    z_m = vals[mid];
-    return std::isfinite(z_m) && z_m > 0.0;
-}
-
-void drawTagAxes(cv::Mat& img,
-                 const Eigen::Matrix4d& T_c_t,
-                 const cmvr::device::Rs2Intrinsics& K,
-                 double axis_len,
-                 int tag_id) {
-    const Eigen::Matrix3d R = T_c_t.block<3,3>(0,0);
-    const Eigen::Vector3d t = T_c_t.block<3,1>(0,3);
-    const Eigen::Vector3d o_c = t;
-    const Eigen::Vector3d x_c = R * Eigen::Vector3d(axis_len, 0, 0) + t;
-    const Eigen::Vector3d y_c = R * Eigen::Vector3d(0, axis_len, 0) + t;
-    const Eigen::Vector3d z_c = R * Eigen::Vector3d(0, 0, axis_len) + t;
-
-    cv::Point o, x, y, z;
-    if (!projectPoint(o_c, K, o)) return;
-    if (projectPoint(x_c, K, x)) {
-        cv::line(img, o, x, cv::Scalar(0, 0, 255), 2); // X red
-    }
-    if (projectPoint(y_c, K, y)) {
-        cv::line(img, o, y, cv::Scalar(0, 255, 0), 2); // Y green
-    }
-    if (projectPoint(z_c, K, z)) {
-        cv::line(img, o, z, cv::Scalar(255, 0, 0), 2); // Z blue
-    }
-    cv::circle(img, o, 3, cv::Scalar(255, 255, 255), -1);
-    cv::putText(img,
-                "id=" + std::to_string(tag_id),
-                cv::Point(o.x + 6, o.y - 6),
-                cv::FONT_HERSHEY_SIMPLEX,
-                0.45,
-                cv::Scalar(255, 255, 255),
-                1);
-}
-
-struct DisplayTagPose {
-    int tag_id{-1};
-    Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
-};
-
-bool detectTagPosesForDisplay(const cv::Mat& color,
-                              const cmvr::device::Rs2Intrinsics& intrinsics,
-                              double tag_size_m,
-                              std::vector<DisplayTagPose>& out_tags) {
-    out_tags.clear();
-    if (color.empty()) {
-        return false;
-    }
-    if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) {
-        return false;
-    }
-    if (!std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
-        !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
-        intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) {
-        return false;
-    }
-
-    cv::Mat gray;
-    if (color.channels() == 1) {
-        gray = color;
-    } else if (color.channels() == 3) {
-        cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY);
-    } else {
-        cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY);
-    }
-    if (gray.empty() || gray.type() != CV_8UC1) {
-        return false;
-    }
-    if (!gray.isContinuous()) {
-        gray = gray.clone();
-    }
-
-    vpCameraParameters cam;
-    cam.initPersProjWithoutDistortion(
-        static_cast<double>(intrinsics.fx),
-        static_cast<double>(intrinsics.fy),
-        static_cast<double>(intrinsics.cx),
-        static_cast<double>(intrinsics.cy));
-
-    vpImage<unsigned char> I(gray.rows, gray.cols);
-    for (int y = 0; y < gray.rows; ++y) {
-        std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(gray.cols));
-    }
-
-    static vpDetectorAprilTag detector(vpDetectorAprilTag::TAG_36h11);
-    static bool detector_init = false;
-    if (!detector_init) {
-        detector.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
-        detector_init = true;
-    }
-
-    std::vector<vpHomogeneousMatrix> cMo_vec;
-    const bool detected = detector.detect(I, tag_size_m, cam, cMo_vec);
-    if (!detected || cMo_vec.empty()) {
-        return false;
-    }
-
-    const std::vector<int> ids = detector.getTagsId();
-    const size_t n = std::min(ids.size(), cMo_vec.size());
-    out_tags.reserve(n);
-    for (size_t i = 0; i < n; ++i) {
-        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
-        const auto& cMo = cMo_vec[i];
-        for (int r = 0; r < 3; ++r) {
-            for (int c = 0; c < 3; ++c) {
-                T(r, c) = cMo[r][c];
-            }
-        }
-        T(0, 3) = cMo[0][3];
-        T(1, 3) = cMo[1][3];
-        T(2, 3) = cMo[2][3];
-        out_tags.push_back(DisplayTagPose{ids[i], T});
-    }
-    return !out_tags.empty();
-}
-
-std::string formatVec3(const Eigen::Vector3d& p, int precision = 6) {
-    std::ostringstream oss;
-    oss << std::fixed << std::setprecision(precision)
-        << "[" << p.x() << ", " << p.y() << ", " << p.z() << "]";
-    return oss.str();
-}
-
-} // namespace
-
-
-TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
-    const std::string serial = kRsSerial;
-    if (serial.empty()) {
-        GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp";
-    }
-
-    const double tag_size = kTagSize;
-    const int tries = kTries;
-    const bool infinite = (tries <= 0);
-    const int width = kWidth;
-    const int height = kHeight;
-    const int fps = kFps;
-
-    std::cout << "[TagRelativeTarget3DTest] serial=" << serial
-              << " target_uv=[" << kTargetU << ", " << kTargetV << "]"
-              << " tag_size=" << tag_size
-              << " tries=" << tries << "\n";
-
-    cmvr::config::RealSenseCameraConfig cam_cfg;
-    cam_cfg.set_id("tag_relative_target_3d_test");
-    cam_cfg.set_serialnumber(serial);
-    cam_cfg.set_width(width);
-    cam_cfg.set_height(height);
-    cam_cfg.set_fps(fps);
-    cam_cfg.set_codec("H265");
-    cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
-    cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
-    cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
-    cam_cfg.set_buffer_size(30);
-    cam_cfg.set_sync(true);
-
-    auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
-
-    auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(camera);
-    perception->setTagSize(tag_size);
-    cmvr::perception::TagRelativeTarget3D tracker(perception);
-
-    cmvr::perception::AprilTagPerception::Options opt;
-    opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE;
-    opt.detect_tags = true;
-    tracker.setActiveTagSwitchPolicy(4, 1.2);
-    tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08);
-    tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
-
-    ASSERT_NO_THROW(camera->init());
-    ASSERT_NO_THROW(camera->start());
-    struct CameraStopGuard {
-        std::shared_ptr<cmvr::device::RealsenseCamera> cam;
-        ~CameraStopGuard() {
-            if (!cam) return;
-            try {
-                cam->stop();
-            } catch (...) {
-            }
-        }
-    } stop_guard{camera};
-
-    bool window_enabled = true;
-    try {
-        cv::namedWindow(kWindowName, cv::WINDOW_NORMAL);
-        cv::resizeWindow(kWindowName, width, height);
-        cv::imshow(kWindowName, cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)));
-        cv::waitKey(1);
-    } catch (const cv::Exception& e) {
-        window_enabled = false;
-        std::cout << "[TagRelativeTarget3DTest] window disabled: " << e.what() << "\n";
-    }
-    if (!window_enabled) {
-        GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test.";
-    }
-
-    bool ok = false;
-    bool user_abort = false;
-    int frame_fail_count = 0;
-    int plane_valid_count = 0;
-    int lock_success_count = 0;
-    int track_success_count = 0;
-    int track_fail_count = 0;
-    int tag_switch_count = 0;
-    int printed_count = 0;
-    bool target_uv_initialized = false;
-    int target_u = -1;
-    int target_v = -1;
-    bool target_locked = false;
-    FramePack last_good_frame;
-    last_good_frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
-    bool has_last_good_frame = false;
-    const auto t_loop_start = std::chrono::steady_clock::now();
-
-    // Warmup: try to get RGB frames after pipeline start.
-    bool warmup_ok = false;
-    bool warmup_abort = false;
-    for (int i = 0; i < kWarmupMaxTries; ++i) {
-        FramePack warm_frame;
-        std::string warm_error;
-        warmup_ok = fetchRGBFrame(camera, warm_frame, warm_error);
-        if (warmup_ok) {
-            break;
-        }
-        const int key = cv::waitKey(1);
-        if (key == 27 || key == 'q' || key == 'Q') {
-            warmup_abort = true;
-            break;
-        }
-        std::this_thread::sleep_for(std::chrono::milliseconds(kWarmupSleepMs));
-    }
-    if (warmup_abort) {
-        GTEST_SKIP() << "User aborted during warmup";
-    }
-    if (!warmup_ok) {
-        std::cout << "[TagRelativeTarget3DTest] warmup failed, continue running and retry per frame\n";
-    }
-
-    for (int i = 0; infinite || i < tries; ++i) {
-        FramePack frame;
-        frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
-        std::string frame_error;
-        const bool frame_ok = fetchRGBFrame(camera, frame, frame_error);
-        if (!frame_ok) {
-            ++frame_fail_count;
-            if (has_last_good_frame) {
-                frame.color = last_good_frame.color.clone();
-                frame.intrinsics = last_good_frame.intrinsics;
-            } else {
-                frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20));
-            }
-        } else {
-            last_good_frame.color = frame.color.clone();
-            last_good_frame.intrinsics = frame.intrinsics;
-            has_last_good_frame = true;
-        }
-        std::vector<DisplayTagPose> vis_tags;
-        const bool vis_tag_detected = detectTagPosesForDisplay(frame.color, frame.intrinsics, tag_size, vis_tags);
-
-        if (!target_uv_initialized) {
-            target_u = (kTargetU >= 0) ? kTargetU : (frame.color.cols / 2);
-            target_v = (kTargetV >= 0) ? kTargetV : (frame.color.rows / 2);
-            target_uv_initialized = true;
-        }
-
-        Eigen::Vector3d p_c_target = Eigen::Vector3d::Zero();
-        int used_tag_id = -1;
-        double spread_m = 0.0;
-        bool target_valid = false;
-        std::string phase = target_locked ? "track" : "lock";
-
-        if (!target_locked) {
-            perception->update(opt);
-            if (tracker.startTrackingFromPixel(target_u, target_v)) {
-                p_c_target = tracker.lastTargetInCamera();
-                used_tag_id = tracker.lastUsedTagId();
-                spread_m = tracker.lastSpread();
-                target_locked = true;
-                phase = "track";
-                target_valid = true;
-                ok = true;
-                ++plane_valid_count;
-                ++lock_success_count;
-                ++printed_count;
-
-                std::cout << "[TagRelativeTarget3DTest][LOCK] target_p_c="
-                          << formatVec3(p_c_target)
-                          << " used_tag=" << used_tag_id
-                          << " active_tag=" << tracker.activeTagId()
-                          << " spread=" << spread_m
-                          << "\n";
-                std::cout << "[TagRelativeTarget3DTest] anchors=" << tracker.anchorCount() << "\n";
-            } else if (((i + 1) % kLogEveryNFrames) == 0) {
-                std::cout << "[TagRelativeTarget3DTest] startTracking failed: "
-                          << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus())
-                          << "\n";
-            }
-        } else {
-            const int old_active_tag = tracker.activeTagId();
-            perception->update(opt);
-            if (tracker.track()) {
-                p_c_target = tracker.lastTargetInCamera();
-                used_tag_id = tracker.lastUsedTagId();
-                spread_m = tracker.lastSpread();
-                const bool switched = tracker.lastSwitched();
-
-                target_valid = true;
-                ok = true;
-                ++track_success_count;
-                ++printed_count;
-                if (switched) {
-                    ++tag_switch_count;
-                    std::cout << "[TagRelativeTarget3DTest][SWITCH] active_tag "
-                              << old_active_tag << " -> " << tracker.activeTagId() << "\n";
-                }
-                std::cout << "[TagRelativeTarget3DTest][TRACK] target_p_c="
-                          << formatVec3(p_c_target)
-                          << " used_tag=" << used_tag_id
-                          << " active_tag=" << tracker.activeTagId()
-                          << " spread=" << spread_m
-                        << "anchorCount=" << tracker.anchorCount()
-                          << std::endl;
-            } else {
-                ++track_fail_count;
-                if (((i + 1) % kLogEveryNFrames) == 0) {
-                    std::cout << "[TagRelativeTarget3DTest] track failed: "
-                              << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus())
-                              << "\n";
-                }
-            }
-        }
-        const auto status = tracker.lastStatus();
-        const char* status_str = cmvr::perception::TagRelativeTarget3D::statusToString(status);
-
-        cv::Mat vis = frame.color.clone();
-        cv::drawMarker(vis,
-                       cv::Point(target_u, target_v),
-                       cv::Scalar(0, 255, 255),
-                       cv::MARKER_CROSS,
-                       16,
-                       2);
-        cv::putText(vis,
-                    std::string("status: ") + status_str,
-                    cv::Point(12, 28),
-                    cv::FONT_HERSHEY_SIMPLEX,
-                    0.7,
-                    cv::Scalar(0, 255, 0),
-                    2);
-        cv::putText(vis,
-                    "press q / Esc to quit",
-                    cv::Point(12, 56),
-                    cv::FONT_HERSHEY_SIMPLEX,
-                    0.6,
-                    cv::Scalar(0, 255, 255),
-                    2);
-        cv::putText(vis,
-                    "phase=" + phase + " locked=" + std::to_string(target_locked ? 1 : 0) +
-                    " active_tag=" + std::to_string(tracker.activeTagId()) +
-                    " miss=" + std::to_string(tracker.activeTagMissingCount()) +
-                    " anchors=" + std::to_string(tracker.anchorCount()) +
-                    " vis_tags=" + std::to_string(vis_tag_detected ? vis_tags.size() : 0) +
-                    " frame_fail=" + std::to_string(frame_fail_count),
-                    cv::Point(12, 84),
-                    cv::FONT_HERSHEY_SIMPLEX,
-                    0.5,
-                    cv::Scalar(200, 200, 255),
-                    1);
-        if (!frame_error.empty()) {
-            cv::putText(vis,
-                        "frame error: " + frame_error,
-                        cv::Point(12, 106),
-                        cv::FONT_HERSHEY_SIMPLEX,
-                        0.45,
-                        cv::Scalar(0, 0, 255),
-                        1);
-        }
-        cv::putText(vis,
-                    std::string("target3d: ") + (target_valid ? "valid" : "invalid") +
-                    " used_tag=" + std::to_string(used_tag_id) +
-                    " spread=" + std::to_string(spread_m),
-                    cv::Point(12, 128),
-                    cv::FONT_HERSHEY_SIMPLEX,
-                    0.45,
-                    target_valid ? cv::Scalar(0, 255, 255) : cv::Scalar(0, 0, 255),
-                    1);
-        cv::putText(vis,
-                    "target_p_c=" + (target_valid ? formatVec3(p_c_target) : "invalid"),
-                    cv::Point(12, 150),
-                    cv::FONT_HERSHEY_SIMPLEX,
-                    0.45,
-                    target_valid ? cv::Scalar(0, 255, 255) : cv::Scalar(0, 0, 255),
-                    1);
-
-        if (vis_tag_detected) {
-            const double axis_len = tag_size * 0.5;
-            for (const auto& tag : vis_tags) {
-                drawTagAxes(vis, tag.T_c_t, frame.intrinsics, axis_len, tag.tag_id);
-            }
-        }
-
-        if (target_valid && std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) &&
-            std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) &&
-            p_c_target.z() > 1e-6) {
-            const double u = frame.intrinsics.fx * (p_c_target.x() / p_c_target.z()) + frame.intrinsics.cx;
-            const double v = frame.intrinsics.fy * (p_c_target.y() / p_c_target.z()) + frame.intrinsics.cy;
-            const int ui = static_cast<int>(std::lround(u));
-            const int vi = static_cast<int>(std::lround(v));
-            if (ui >= 0 && ui < vis.cols && vi >= 0 && vi < vis.rows) {
-                cv::circle(vis, cv::Point(ui, vi), 6, cv::Scalar(0, 255, 255), 2);
-                cv::putText(vis,
-                            "target",
-                            cv::Point(ui + 8, vi + 16),
-                            cv::FONT_HERSHEY_SIMPLEX,
-                            0.5,
-                            cv::Scalar(0, 255, 255),
-                            1);
-            }
-        }
-
-        try {
-            cv::imshow(kWindowName, vis);
-            const int key = cv::waitKey(1);
-            if (key == 27 || key == 'q' || key == 'Q') {
-                user_abort = true;
-                break;
-            }
-        } catch (const cv::Exception& e) {
-            std::cout << "[TagRelativeTarget3DTest] window error: " << e.what() << "\n";
-            user_abort = true;
-            break;
-        }
-
-        std::this_thread::sleep_for(std::chrono::milliseconds(30));
-    }
-
-    try {
-        cv::destroyWindow(kWindowName);
-    } catch (...) {
-        // ignore window destroy errors in headless environments
-    }
-
-    if (!user_abort && !infinite) {
-        EXPECT_TRUE(ok) << "No valid tag detected in " << tries << " tries";
-    }
-    const auto t_loop_end = std::chrono::steady_clock::now();
-    const double total_s = std::chrono::duration_cast<std::chrono::duration<double>>(t_loop_end - t_loop_start).count();
-    std::cout << "[TagRelativeTarget3DTest] summary "
-              << " plane_valid=" << plane_valid_count
-              << " lock_ok=" << lock_success_count
-              << " track_ok=" << track_success_count
-              << " track_fail=" << track_fail_count
-              << " switch=" << tag_switch_count
-              << " printed=" << printed_count
-              << " frame_fail=" << frame_fail_count
-              << " duration_s=" << total_s
-              << "\n";
-}
-
-// TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointNoDisplayMinimal) {
-//     const std::string serial = kRsSerial;
-//     if (serial.empty()) {
-//         GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp";
-//     }
-//
-//     cmvr::config::RealSenseCameraConfig cam_cfg;
-//     cam_cfg.set_id("tag_relative_target_3d_test_no_display");
-//     cam_cfg.set_serialnumber(serial);
-//     cam_cfg.set_width(kWidth);
-//     cam_cfg.set_height(kHeight);
-//     cam_cfg.set_fps(kFps);
-//     cam_cfg.set_codec("H265");
-//     cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
-//     cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
-//     cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
-//     cam_cfg.set_buffer_size(30);
-//     cam_cfg.set_sync(true);
-//
-//     auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
-//     cmvr::perception::TagRelativeTarget3D tracker(camera);
-//     ASSERT_NO_THROW(camera->init());
-//     ASSERT_NO_THROW(camera->start());
-//     struct CameraStopGuard {
-//         std::shared_ptr<cmvr::device::RealsenseCamera> cam;
-//         ~CameraStopGuard() {
-//             if (!cam) return;
-//             try {
-//                 cam->stop();
-//             } catch (...) {
-//             }
-//         }
-//     } stop_guard{camera};
-//     tracker.setTagSize(kTagSize);
-//     tracker.setActiveTagSwitchPolicy(4, 1.2);
-//     tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08);
-//     // 深度法采样：5x5 中值 + MAD 离群剔除。
-//     tracker.setDepthSamplingConfig(5, true, 2.5, 0.003);
-//     const int tries = kTries;
-//     const bool infinite = (tries <= 0);
-//     int plane_fail_count = 0;
-//     int depth_fail_count = 0;
-//     int plane_ok_count = 0;
-//     int depth_ok_count = 0;
-//     int both_ok_count = 0;
-//     int printed_count = 0;
-//     bool got_target = false;
-//     const int target_u = (kTargetU >= 0) ? kTargetU : (kWidth / 2);
-//     const int target_v = (kTargetV >= 0) ? kTargetV : (kHeight / 2);
-//
-//     for (int i = 0; infinite || i < tries; ++i) {
-//         // 用公开接口分别求解两种方法（内部自行抓帧/缓存）。
-//         tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE);
-//         const bool plane_ok = tracker.solveFromPixel(target_u, target_v);
-//         const Eigen::Vector3d p_c_plane = plane_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero();
-//         const int plane_used_tag = plane_ok ? tracker.lastUsedTagId() : -1;
-//         const double plane_spread = plane_ok ? tracker.lastSpread() : 0.0;
-//         if (plane_ok) {
-//             ++plane_ok_count;
-//         } else {
-//             ++plane_fail_count;
-//         }
-//
-//         tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE);
-//         const bool depth_ok = tracker.solveFromPixel(target_u, target_v);
-//         const Eigen::Vector3d p_c_depth = depth_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero();
-//         if (depth_ok) {
-//             ++depth_ok_count;
-//         } else {
-//             ++depth_fail_count;
-//         }
-//
-//         if (plane_ok && depth_ok) {
-//             ++both_ok_count;
-//         }
-//         if (plane_ok || depth_ok) {
-//             got_target = true;
-//         }
-//
-//         std::cout << "[TagRelativeTarget3DNoDisplay][COMPARE] "
-//                   << "uv=[" << target_u << "," << target_v << "] "
-//                   << "plane_ok=" << plane_ok
-//                   << " plane_p_c=" << (plane_ok ? formatVec3(p_c_plane) : "[invalid]")
-//                   << " used_tag=" << plane_used_tag
-//                   << " spread=" << plane_spread
-//                   << " | depth_ok=" << depth_ok
-//                   << " depth_p_c=" << (depth_ok ? formatVec3(p_c_depth) : "[invalid]");
-//         if (plane_ok && depth_ok) {
-//             std::cout << " | diff_norm=" << (p_c_plane - p_c_depth).norm();
-//         }
-//         std::cout << std::endl;
-//
-//         ++printed_count;
-//         std::this_thread::sleep_for(std::chrono::milliseconds(30));
-//     }
-//
-//     EXPECT_TRUE(got_target)
-//         << "Failed to solve target point in camera frame. "
-//         << " plane_fail=" << plane_fail_count
-//         << " depth_fail=" << depth_fail_count
-//         << " plane_ok=" << plane_ok_count
-//         << " depth_ok=" << depth_ok_count
-//         << " both_ok=" << both_ok_count
-//         << " printed=" << printed_count;
-// }

cmvr-es/common/base/protobuf_utils.h

@@ -0,0 +1,24 @@
+#ifndef CMVR_ES_PROTOBUF_UTILS_H
+#define CMVR_ES_PROTOBUF_UTILS_H
+
+#include <google/protobuf/repeated_field.h>
+
+#include <vector>
+
+namespace cmvr::common {
+
+template <typename T>
+std::vector<T> repeatedToVector(const google::protobuf::RepeatedPtrField<T>& values)
+{
+    return {values.begin(), values.end()};
+}
+
+template <typename T>
+std::vector<T> repeatedToVector(const google::protobuf::RepeatedField<T>& values)
+{
+    return {values.begin(), values.end()};
+}
+
+} // namespace cmvr::common
+
+#endif // CMVR_ES_PROTOBUF_UTILS_H

cmvr-es/common/base/string_utils.h

@@ -0,0 +1,25 @@
+#ifndef CMVR_ES_STRING_UTILS_H
+#define CMVR_ES_STRING_UTILS_H
+
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace cmvr::common {
+
+inline std::string joinStrings(const std::vector<std::string>& values,
+                               const std::string& separator = ", ")
+{
+    std::ostringstream oss;
+    for (std::size_t i = 0; i < values.size(); ++i) {
+        if (i > 0) {
+            oss << separator;
+        }
+        oss << values[i];
+    }
+    return oss.str();
+}
+
+} // namespace cmvr::common
+
+#endif // CMVR_ES_STRING_UTILS_H

cmvr-es/common/math/cartesian_motion_math.h

@@ -43,6 +43,15 @@ inline double directionDeviationDeg(const Eigen::Vector3d& desired,
     return std::acos(direction_cos) * rad_to_deg;
 }
 
+inline double lateralDistanceToLine(const Eigen::Vector3d& start,
+                                    const Eigen::Vector3d& direction,
+                                    const Eigen::Vector3d& point)
+{
+    const Eigen::Vector3d delta = point - start;
+    const Eigen::Vector3d lateral = delta - delta.dot(direction) * direction;
+    return lateral.norm();
+}
+
 inline Eigen::Vector3d rotationVector(const Eigen::Matrix3d& rotation)
 {
     Eigen::AngleAxisd angle_axis(rotation);

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

@@ -29,14 +29,30 @@ arm {
         pos_eps: 1e-6
         rot_eps: 1e-6
         damping: 1e-6
-        joint_limits {
+        joint_limit_policy {
-          source: JOINT_LIMIT_SOURCE_URDF
+          limits {
-        }
+            enable: true
-        joint_limit_avoidance {
+            source: JOINT_LIMIT_SOURCE_CUSTOM
-          enable: false
+            joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-          gain: 0.2
+            joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
-          margin_ratio: 0.15
+            joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-          max_push: 0.25
+            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 }
+          }
+          soft_limit {
+            enable: true
+            margin_ratio: 0.01
+            min_margin_rad: 0.01
+          }
+          avoidance {
+            enable: false
+            gain: 0.2
+            margin_ratio: 0.15
+            max_push: 0.25
+            weight: 0.05
+          }
         }
       }
     }
@@ -52,62 +68,73 @@ arm {
       }
 
       move_l {
-        pinocchio_qp_cartesian_motion_planner {
+        pinocchio_cartesian_motion_planner {
           sample_period_s: 0.001
           position_gain: 4.0
           rotation_gain: 4.0
-          qp {
+          line_deviation_check {
-            joint_limits {
+            enable: true
-              source: JOINT_LIMIT_SOURCE_URDF
+            line_deviation_warn_m: 0.01
-            }
+            line_deviation_stop_m: 0.03
-            twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 }
+            line_direction_warn_deg: 20.0
-            qdot_regularization: 1e-4
+            line_direction_stop_deg: 45.0
-            prev_qdot_regularization: 1e-4
+            line_direction_reset_deg: 10.0
-            solver_eps: 1e-3
+            line_check_min_distance_m: 0.01
-            joint_limit_avoidance {
+          }
-              enable: false
+          joint_continuity_check {
-              margin_ratio: 0.15
+            enable: true
-              gain: 0.2
+            max_joint_delta_rad: 0.05
-              max_push: 0.25
+            max_joint_velocity_rad_s: 10.0
-              weight: 0.05
+            max_joint_acceleration_rad_s2: 5000.0
-            }
+          }
+          cartesian_step_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 45.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 45.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
           }
         }
       }
 
       speed_l {
-        pinocchio_qp_cartesian_motion_planner {
+        pinocchio_cartesian_motion_planner {
-          qp {
-            joint_limits {
-              source: JOINT_LIMIT_SOURCE_URDF
-            }
-            twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 2e-2
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
           linear_velocity_max: 0.55
           linear_acceleration_max: 5.0
           linear_jerk_max: 10.0
           angular_velocity_max: 1.0
           angular_acceleration_max: 5.0
           angular_jerk_max: 12.0
-          joint_acceleration_max: 8.0
           linear_target_replan_threshold: 1e-4
           angular_target_replan_threshold: 1e-4
           linear_reverse_cos_threshold: -0.8660254037844386
           linear_reverse_switch_speed_threshold: 1e-3
-          normal_direction_deviation_deg: 10.0
+          enforce_joint_acceleration_limits: true
-          mild_direction_deviation_deg: 25.0
+          line_deviation_check {
-          severe_direction_deviation_deg: 45.0
+            enable: true
-          linear_min_speed_ratio: 0.2
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_velocity_check {
+            enable: true
+            max_joint_velocity_rad_s: 30.0
+            max_joint_acceleration_rad_s2: 10000.0
+          }
+          cartesian_velocity_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 5.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 5.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
         }
 
         speed_l_controller {
@@ -116,6 +143,7 @@ arm {
             stop_twist_norm: 1e-9
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
+            stop_acceleration: 10
           }
         }
       }

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

@@ -1,92 +0,0 @@
-arm {
-  robot_arms {
-    id: "right_arm"
-
-    motor {
-      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can"
-      dof: 7
-      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"
-      upd_freq: 1000
-      buffer_size: 50
-      default_vel: 1.0
-      default_acc: 2.0
-    }
-
-    kinematics {
-      pinocchio_dls_ik_solver {
-        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
-        base_frame_name: "PELVIS_S"
-        flange_frame_name: "R_WRIST_R_S"
-        tcp_frame_name: "R_FINGER_TIP_FIXED"
-        max_iters: 100
-        pos_eps: 1e-6
-        rot_eps: 1e-6
-        damping: 1e-6
-        joint_limits {
-          source: JOINT_LIMIT_SOURCE_URDF
-        }
-        joint_limit_avoidance {
-          enable: false
-          gain: 0.2
-          margin_ratio: 0.15
-          max_push: 0.25
-        }
-      }
-    }
-
-    motion {
-      move_j {
-        toppra_joint_motion_planner {
-          path_type: TOPPRA_PATH_TYPE_QUINTIC
-          sample_period_s: 0.001
-          grid_size: 150
-          high_grid_size: 300
-        }
-      }
-
-      move_l {
-        pinocchio_dls_cartesian_motion_planner {
-          sample_period_s: 0.001
-          position_gain: 4.0
-          rotation_gain: 4.0
-        }
-      }
-
-      speed_l {
-        pinocchio_dls_cartesian_motion_planner {
-          linear_velocity_max: 0.55
-          linear_acceleration_max: 5.0
-          linear_jerk_max: 10.0
-          angular_velocity_max: 1.0
-          angular_acceleration_max: 5.0
-          angular_jerk_max: 12.0
-          joint_acceleration_max: 8.0
-          linear_target_replan_threshold: 1e-4
-          angular_target_replan_threshold: 1e-4
-          linear_reverse_cos_threshold: -0.8660254037844386
-          linear_reverse_switch_speed_threshold: 1e-3
-          normal_direction_deviation_deg: 10.0
-          mild_direction_deviation_deg: 25.0
-          severe_direction_deviation_deg: 45.0
-          linear_min_speed_ratio: 0.2
-        }
-
-        speed_l_controller {
-          cartesian_velocity_controller {
-            control_period_s: 0.001
-            stop_twist_norm: 1e-9
-            stop_command_velocity_norm: 1e-3
-            stop_measured_velocity_norm: 1e-2
-          }
-        }
-      }
-    }
-  }
-}

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

@@ -1,138 +0,0 @@
-arm {
-  robot_arms {
-    id: "right_arm"
-
-    motor {
-      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can"
-      dof: 7
-      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"
-      upd_freq: 1000
-      buffer_size: 50
-      default_vel: 1.0
-      default_acc: 2.0
-    }
-
-    kinematics {
-      pinocchio_dls_ik_solver {
-        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
-        base_frame_name: "PELVIS_S"
-        flange_frame_name: "R_WRIST_R_S"
-        tcp_frame_name: "R_FINGER_TIP_FIXED"
-        max_iters: 100
-        pos_eps: 1e-6
-        rot_eps: 1e-6
-        damping: 1e-6
-        joint_limits {
-          source: JOINT_LIMIT_SOURCE_URDF
-        }
-        joint_limit_avoidance {
-          enable: false
-          gain: 0.2
-          margin_ratio: 0.15
-          max_push: 0.25
-        }
-      }
-    }
-
-    motion {
-      move_j {
-        toppra_joint_motion_planner {
-          path_type: TOPPRA_PATH_TYPE_QUINTIC
-          sample_period_s: 0.001
-          grid_size: 150
-          high_grid_size: 300
-        }
-      }
-
-      move_l {
-        pinocchio_qp_cartesian_motion_planner {
-          sample_period_s: 0.001
-          position_gain: 4.0
-          rotation_gain: 4.0
-          qp {
-            joint_limits {
-              source: JOINT_LIMIT_SOURCE_URDF
-            }
-            twist_tracking_weight {
-              x: 1.0
-              y: 1.0
-              z: 1.0
-              rx: 0.5
-              ry: 0.5
-              rz: 0.5
-            }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 1e-4
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-        }
-      }
-
-      speed_l {
-        pinocchio_qp_cartesian_motion_planner {
-          qp {
-            joint_limits {
-              source: JOINT_LIMIT_SOURCE_URDF
-            }
-            twist_tracking_weight {
-              x: 1.0
-              y: 1.0
-              z: 1.0
-              rx: 0.5
-              ry: 0.5
-              rz: 0.5
-            }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 2e-2
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-          linear_velocity_max: 0.55
-          linear_acceleration_max: 5.0
-          linear_jerk_max: 10.0
-          angular_velocity_max: 1.0
-          angular_acceleration_max: 5.0
-          angular_jerk_max: 12.0
-          joint_acceleration_max: 8.0
-          linear_target_replan_threshold: 1e-4
-          angular_target_replan_threshold: 1e-4
-          linear_reverse_cos_threshold: -0.8660254037844386
-          linear_reverse_switch_speed_threshold: 1e-3
-          normal_direction_deviation_deg: 10.0
-          mild_direction_deviation_deg: 25.0
-          severe_direction_deviation_deg: 45.0
-          linear_min_speed_ratio: 0.2
-        }
-
-        speed_l_controller {
-          cartesian_velocity_controller {
-            control_period_s: 0.001
-            stop_twist_norm: 1e-9
-            stop_command_velocity_norm: 1e-3
-            stop_measured_velocity_norm: 1e-2
-          }
-        }
-      }
-    }
-  }
-}

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

@@ -1,10 +1,10 @@
 arm {
   robot_arms {
-    id: "right_arm_mujoco"
+    id: "mujoco_right_arm"
 
     motor {
       motor_system_id: "mujoco_motors"
-      motor_group_ids: "right_arm_mujoco"
+      motor_group_ids: "mujoco_right_arm"
       dof: 7
       joint_names: "R_SHOULDER_P"
       joint_names: "R_SHOULDER_R"
@@ -29,14 +29,30 @@ arm {
         pos_eps: 1e-6
         rot_eps: 1e-6
         damping: 1e-6
-        joint_limits {
+        joint_limit_policy {
-          source: JOINT_LIMIT_SOURCE_URDF
+          limits {
-        }
+            enable: true
-        joint_limit_avoidance {
+            source: JOINT_LIMIT_SOURCE_CUSTOM
-          enable: true
+            joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 }
-          gain: 0.2
+            joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 100.0 }
-          margin_ratio: 0.15
+            joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 }
-          max_push: 0.25
+            joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 100.0 }
+            joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 }
+            joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 100.0 }
+            joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 100.0 }
+          }
+          soft_limit {
+            enable: true
+            margin_ratio: 0.01
+            min_margin_rad: 0.01
+          }
+          avoidance {
+            enable: false
+            gain: 0.2
+            margin_ratio: 0.15
+            max_push: 0.25
+            weight: 2.0
+          }
         }
       }
     }
@@ -52,62 +68,73 @@ arm {
       }
 
       move_l {
-        pinocchio_qp_cartesian_motion_planner {
+        pinocchio_cartesian_motion_planner {
           sample_period_s: 0.001
           position_gain: 4.0
           rotation_gain: 4.0
-          qp {
+          line_deviation_check {
-            joint_limits {
+            enable: true
-              source: JOINT_LIMIT_SOURCE_URDF
+            line_deviation_warn_m: 0.01
-            }
+            line_deviation_stop_m: 0.03
-            twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 }
+            line_direction_warn_deg: 20.0
-            qdot_regularization: 1e-4
+            line_direction_stop_deg: 45.0
-            prev_qdot_regularization: 1e-4
+            line_direction_reset_deg: 10.0
-            solver_eps: 1e-3
+            line_check_min_distance_m: 0.01
-            joint_limit_avoidance {
+          }
-              enable: false
+          joint_continuity_check {
-              margin_ratio: 0.15
+            enable: true
-              gain: 0.2
+            max_joint_delta_rad: 0.05
-              max_push: 0.25
+            max_joint_velocity_rad_s: 10.0
-              weight: 0.05
+            max_joint_acceleration_rad_s2: 5000.0
-            }
+          }
+          cartesian_step_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 5.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 5.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
           }
         }
       }
 
       speed_l {
-        pinocchio_qp_cartesian_motion_planner {
+        pinocchio_cartesian_motion_planner {
-          qp {
+          linear_velocity_max: 2.0
-            joint_limits {
+          linear_acceleration_max: 15.0
-              source: JOINT_LIMIT_SOURCE_URDF
+          linear_jerk_max: 60.0
-            }
-            twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 2e-2
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-          linear_velocity_max: 0.55
-          linear_acceleration_max: 5.0
-          linear_jerk_max: 10.0
           angular_velocity_max: 1.0
           angular_acceleration_max: 5.0
           angular_jerk_max: 12.0
-          joint_acceleration_max: 8.0
           linear_target_replan_threshold: 1e-4
           angular_target_replan_threshold: 1e-4
           linear_reverse_cos_threshold: -0.8660254037844386
           linear_reverse_switch_speed_threshold: 1e-3
-          normal_direction_deviation_deg: 10.0
+          enforce_joint_acceleration_limits: true
-          mild_direction_deviation_deg: 25.0
+          line_deviation_check {
-          severe_direction_deviation_deg: 45.0
+            enable: true
-          linear_min_speed_ratio: 0.2
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_velocity_check {
+            enable: true
+            max_joint_velocity_rad_s: 10.0
+            max_joint_acceleration_rad_s2: 5000.0
+          }
+          cartesian_velocity_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 5.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 5.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
         }
 
         speed_l_controller {
@@ -116,6 +143,7 @@ arm {
             stop_twist_norm: 1e-9
             stop_command_velocity_norm: 1e-3
             stop_measured_velocity_norm: 1e-2
+            stop_acceleration: 10
           }
         }
       }

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

@@ -1,92 +0,0 @@
-arm {
-  robot_arms {
-    id: "right_arm_mujoco"
-
-    motor {
-      motor_system_id: "mujoco_motors"
-      motor_group_ids: "right_arm_mujoco"
-      dof: 7
-      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"
-      upd_freq: 1000
-      buffer_size: 50
-      default_vel: 1.0
-      default_acc: 2.0
-    }
-
-    kinematics {
-      pinocchio_dls_ik_solver {
-        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
-        base_frame_name: "PELVIS_S"
-        flange_frame_name: "R_WRIST_R_S"
-        tcp_frame_name: "R_FINGER_TIP_FIXED"
-        max_iters: 100
-        pos_eps: 1e-6
-        rot_eps: 1e-6
-        damping: 1e-6
-        joint_limits {
-          source: JOINT_LIMIT_SOURCE_URDF
-        }
-        joint_limit_avoidance {
-          enable: false
-          gain: 0.2
-          margin_ratio: 0.15
-          max_push: 0.25
-        }
-      }
-    }
-
-    motion {
-      move_j {
-        toppra_joint_motion_planner {
-          path_type: TOPPRA_PATH_TYPE_QUINTIC
-          sample_period_s: 0.001
-          grid_size: 150
-          high_grid_size: 300
-        }
-      }
-
-      move_l {
-        pinocchio_dls_cartesian_motion_planner {
-          sample_period_s: 0.001
-          position_gain: 4.0
-          rotation_gain: 4.0
-        }
-      }
-
-      speed_l {
-        pinocchio_dls_cartesian_motion_planner {
-          linear_velocity_max: 0.55
-          linear_acceleration_max: 5.0
-          linear_jerk_max: 10.0
-          angular_velocity_max: 1.0
-          angular_acceleration_max: 5.0
-          angular_jerk_max: 12.0
-          joint_acceleration_max: 8.0
-          linear_target_replan_threshold: 1e-4
-          angular_target_replan_threshold: 1e-4
-          linear_reverse_cos_threshold: -0.8660254037844386
-          linear_reverse_switch_speed_threshold: 1e-3
-          normal_direction_deviation_deg: 10.0
-          mild_direction_deviation_deg: 25.0
-          severe_direction_deviation_deg: 45.0
-          linear_min_speed_ratio: 0.2
-        }
-
-        speed_l_controller {
-          cartesian_velocity_controller {
-            control_period_s: 0.001
-            stop_twist_norm: 1e-9
-            stop_command_velocity_norm: 1e-3
-            stop_measured_velocity_norm: 1e-2
-          }
-        }
-      }
-    }
-  }
-}

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

@@ -1,120 +0,0 @@
-arm {
-  robot_arms {
-    id: "right_arm_mujoco"
-
-    motor {
-      motor_system_id: "mujoco_motors"
-      motor_group_ids: "right_arm_mujoco"
-      dof: 7
-      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"
-      upd_freq: 1000
-      buffer_size: 50
-      default_vel: 1.0
-      default_acc: 2.0
-    }
-
-    kinematics {
-      pinocchio_dls_ik_solver {
-        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
-        base_frame_name: "PELVIS_S"
-        flange_frame_name: "R_WRIST_R_S"
-        tcp_frame_name: "R_FINGER_TIP_FIXED"
-        max_iters: 100
-        pos_eps: 1e-6
-        rot_eps: 1e-6
-        damping: 1e-6
-        joint_limits {
-          source: JOINT_LIMIT_SOURCE_URDF
-        }
-        joint_limit_avoidance {
-          enable: false
-          gain: 0.2
-          margin_ratio: 0.15
-          max_push: 0.25
-        }
-      }
-    }
-
-    motion {
-      move_j {
-        toppra_joint_motion_planner {
-          path_type: TOPPRA_PATH_TYPE_QUINTIC
-          sample_period_s: 0.001
-          grid_size: 150
-          high_grid_size: 300
-        }
-      }
-
-      move_l {
-        pinocchio_qp_cartesian_motion_planner {
-          sample_period_s: 0.001
-          position_gain: 4.0
-          rotation_gain: 4.0
-          qp {
-            joint_limits { source: JOINT_LIMIT_SOURCE_URDF }
-            twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 1e-4
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-        }
-      }
-
-      speed_l {
-        pinocchio_qp_cartesian_motion_planner {
-          qp {
-            joint_limits { source: JOINT_LIMIT_SOURCE_URDF }
-            twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 2e-2
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-          linear_velocity_max: 0.55
-          linear_acceleration_max: 5.0
-          linear_jerk_max: 10.0
-          angular_velocity_max: 1.0
-          angular_acceleration_max: 5.0
-          angular_jerk_max: 12.0
-          joint_acceleration_max: 8.0
-          linear_target_replan_threshold: 1e-4
-          angular_target_replan_threshold: 1e-4
-          linear_reverse_cos_threshold: -0.8660254037844386
-          linear_reverse_switch_speed_threshold: 1e-3
-          normal_direction_deviation_deg: 10.0
-          mild_direction_deviation_deg: 25.0
-          severe_direction_deviation_deg: 45.0
-          linear_min_speed_ratio: 0.2
-        }
-
-        speed_l_controller {
-          cartesian_velocity_controller {
-            control_period_s: 0.001
-            stop_twist_norm: 1e-9
-            stop_command_velocity_norm: 1e-3
-            stop_measured_velocity_norm: 1e-2
-          }
-        }
-      }
-    }
-  }
-}

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

@@ -0,0 +1,153 @@
+arm {
+  robot_arms {
+    id: "mujoco_right_arm"
+
+    motor {
+      motor_system_id: "mujoco_motors"
+      motor_group_ids: "mujoco_right_arm"
+      dof: 7
+      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"
+      upd_freq: 1000
+      buffer_size: 50
+      default_vel: 1.0
+      default_acc: 2.0
+    }
+
+    kinematics {
+      pinocchio_qp_ik_solver {
+        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
+        base_frame_name: "PELVIS_S"
+        flange_frame_name: "R_WRIST_R_S"
+        tcp_frame_name: "R_FINGER_TIP_FIXED"
+        lambda: 1e-4
+        w_posrot: 0.5
+        max_iters: 100
+        tol: 1e-6
+        qp_time_limit: 1e-2
+        joint_limit_policy {
+          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: 100.0 }
+            joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 100.0 }
+            joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 }
+            joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 100.0 }
+            joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 }
+            joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 100.0 }
+            joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 100.0 }
+          }
+          soft_limit {
+            enable: true
+            margin_ratio: 0.01
+            min_margin_rad: 0.01
+          }
+          avoidance {
+            enable: false
+            gain: 0.2
+            margin_ratio: 0.01
+            max_push: 0.02
+            weight: 0.05
+          }
+        }
+      }
+    }
+
+    motion {
+      move_j {
+        toppra_joint_motion_planner {
+          path_type: TOPPRA_PATH_TYPE_QUINTIC
+          sample_period_s: 0.001
+          grid_size: 150
+          high_grid_size: 300
+        }
+      }
+
+      move_l {
+        pinocchio_cartesian_motion_planner {
+          sample_period_s: 0.001
+          position_gain: 4.0
+          rotation_gain: 4.0
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_continuity_check {
+            enable: true
+            max_joint_delta_rad: 0.05
+            max_joint_velocity_rad_s: 10.0
+            max_joint_acceleration_rad_s2: 5000.0
+          }
+          cartesian_step_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 5.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 5.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+      }
+
+      speed_l {
+        pinocchio_cartesian_motion_planner {
+          linear_velocity_max: 2.0
+          linear_acceleration_max: 15.0
+          linear_jerk_max: 60.0
+          angular_velocity_max: 1.0
+          angular_acceleration_max: 5.0
+          angular_jerk_max: 12.0
+          linear_target_replan_threshold: 1e-4
+          angular_target_replan_threshold: 1e-4
+          linear_reverse_cos_threshold: -0.8660254037844386
+          linear_reverse_switch_speed_threshold: 1e-3
+          enforce_joint_acceleration_limits: true
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.005
+            line_deviation_stop_m: 0.005
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_velocity_check {
+            enable: true
+            max_joint_velocity_rad_s: 10.0
+            max_joint_acceleration_rad_s2: 5000.0
+          }
+          cartesian_velocity_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 20.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 20.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+
+        speed_l_controller {
+          cartesian_velocity_controller {
+            control_period_s: 0.001
+            stop_twist_norm: 1e-9
+            stop_command_velocity_norm: 1e-3
+            stop_measured_velocity_norm: 1e-2
+            stop_acceleration: 5
+          }
+        }
+      }
+    }
+  }
+}

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

@@ -1,112 +0,0 @@
-arm {
-  robot_arms {
-    id: "right_arm_mujoco"
-
-    motor {
-      motor_system_id: "mujoco_motors"
-      motor_group_ids: "right_arm_mujoco"
-      dof: 7
-      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"
-      upd_freq: 1000
-      buffer_size: 50
-      default_vel: 1.0
-      default_acc: 2.0
-    }
-
-    kinematics {
-      pinocchio_qp_ik_solver {
-        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
-        base_frame_name: "PELVIS_S"
-        flange_frame_name: "R_WRIST_R_S"
-        tcp_frame_name: "R_FINGER_TIP_FIXED"
-        lambda: 1e-4
-        w_posrot: 0.5
-        max_iters: 100
-        tol: 1e-6
-        qp_time_limit: 1e-2
-      }
-    }
-
-    motion {
-      move_j {
-        toppra_joint_motion_planner {
-          path_type: TOPPRA_PATH_TYPE_QUINTIC
-          sample_period_s: 0.001
-          grid_size: 150
-          high_grid_size: 300
-        }
-      }
-
-      move_l {
-        pinocchio_qp_cartesian_motion_planner {
-          sample_period_s: 0.001
-          position_gain: 4.0
-          rotation_gain: 4.0
-          qp {
-            joint_limits { source: JOINT_LIMIT_SOURCE_URDF }
-            twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 1e-4
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-        }
-      }
-
-      speed_l {
-        pinocchio_qp_cartesian_motion_planner {
-          qp {
-            joint_limits { source: JOINT_LIMIT_SOURCE_URDF }
-            twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 2e-2
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-          linear_velocity_max: 0.55
-          linear_acceleration_max: 5.0
-          linear_jerk_max: 10.0
-          angular_velocity_max: 1.0
-          angular_acceleration_max: 5.0
-          angular_jerk_max: 12.0
-          joint_acceleration_max: 8.0
-          linear_target_replan_threshold: 1e-4
-          angular_target_replan_threshold: 1e-4
-          linear_reverse_cos_threshold: -0.8660254037844386
-          linear_reverse_switch_speed_threshold: 1e-3
-          normal_direction_deviation_deg: 10.0
-          mild_direction_deviation_deg: 25.0
-          severe_direction_deviation_deg: 45.0
-          linear_min_speed_ratio: 0.2
-        }
-
-        speed_l_controller {
-          cartesian_velocity_controller {
-            control_period_s: 0.001
-            stop_twist_norm: 1e-9
-            stop_command_velocity_norm: 1e-3
-            stop_measured_velocity_norm: 1e-2
-          }
-        }
-      }
-    }
-  }
-}

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

@@ -0,0 +1,153 @@
+arm {
+  robot_arms {
+    id: "right_arm"
+
+    motor {
+      motor_system_id: "ti5_motors"
+      motor_group_ids: "right_arm_can"
+      dof: 7
+      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"
+      upd_freq: 1000
+      buffer_size: 50
+      default_vel: 1.0
+      default_acc: 2.0
+    }
+
+    kinematics {
+      pinocchio_qp_ik_solver {
+        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
+        base_frame_name: "PELVIS_S"
+        flange_frame_name: "R_WRIST_R_S"
+        tcp_frame_name: "R_FINGER_TIP_FIXED"
+        lambda: 1e-4
+        w_posrot: 0.5
+        max_iters: 100
+        tol: 1e-6
+        qp_time_limit: 1e-2
+        joint_limit_policy {
+          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: 100.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 }
+          }
+          soft_limit {
+            enable: true
+            margin_ratio: 0.01
+            min_margin_rad: 0.01
+          }
+          avoidance {
+            enable: false
+            gain: 0.2
+            margin_ratio: 0.01
+            max_push: 0.02
+            weight: 0.05
+          }
+        }
+      }
+    }
+
+    motion {
+      move_j {
+        toppra_joint_motion_planner {
+          path_type: TOPPRA_PATH_TYPE_QUINTIC
+          sample_period_s: 0.001
+          grid_size: 150
+          high_grid_size: 300
+        }
+      }
+
+      move_l {
+        pinocchio_cartesian_motion_planner {
+          sample_period_s: 0.001
+          position_gain: 4.0
+          rotation_gain: 4.0
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_continuity_check {
+            enable: true
+            max_joint_delta_rad: 0.05
+            max_joint_velocity_rad_s: 10.0
+            max_joint_acceleration_rad_s2: 5000.0
+          }
+          cartesian_step_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 45.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 45.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+      }
+
+      speed_l {
+        pinocchio_cartesian_motion_planner {
+          linear_velocity_max: 0.55
+          linear_acceleration_max: 5.0
+          linear_jerk_max: 10.0
+          angular_velocity_max: 1.0
+          angular_acceleration_max: 5.0
+          angular_jerk_max: 12.0
+          linear_target_replan_threshold: 1e-4
+          angular_target_replan_threshold: 1e-4
+          linear_reverse_cos_threshold: -0.8660254037844386
+          linear_reverse_switch_speed_threshold: 1e-3
+          enforce_joint_acceleration_limits: true
+          line_deviation_check {
+            enable: true
+            line_deviation_warn_m: 0.01
+            line_deviation_stop_m: 0.03
+            line_direction_warn_deg: 20.0
+            line_direction_stop_deg: 45.0
+            line_direction_reset_deg: 10.0
+            line_check_min_distance_m: 0.01
+          }
+          joint_velocity_check {
+            enable: true
+            max_joint_velocity_rad_s: 30.0
+            max_joint_acceleration_rad_s2: 10000.0
+          }
+          cartesian_velocity_feasibility_check {
+            enable: true
+            min_linear_speed_ratio: 0.2
+            max_linear_direction_deviation_deg: 5.0
+            min_angular_speed_ratio: 0.2
+            max_angular_direction_deviation_deg: 5.0
+            min_desired_linear_speed: 1e-4
+            min_desired_angular_speed: 1e-4
+          }
+        }
+
+        speed_l_controller {
+          cartesian_velocity_controller {
+            control_period_s: 0.001
+            stop_twist_norm: 1e-9
+            stop_command_velocity_norm: 1e-3
+            stop_measured_velocity_norm: 1e-2
+            stop_acceleration: 0.5
+          }
+        }
+      }
+    }
+  }
+}

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

@@ -1,130 +0,0 @@
-arm {
-  robot_arms {
-    id: "right_arm"
-
-    motor {
-      motor_system_id: "ti5_motors"
-      motor_group_ids: "right_arm_can"
-      dof: 7
-      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"
-      upd_freq: 1000
-      buffer_size: 50
-      default_vel: 1.0
-      default_acc: 2.0
-    }
-
-    kinematics {
-      pinocchio_qp_ik_solver {
-        urdf_path: "model/xiaoyan_description/dual_arm.urdf"
-        base_frame_name: "PELVIS_S"
-        flange_frame_name: "R_WRIST_R_S"
-        tcp_frame_name: "R_FINGER_TIP_FIXED"
-        lambda: 1e-4
-        w_posrot: 0.5
-        max_iters: 100
-        tol: 1e-6
-        qp_time_limit: 1e-2
-      }
-    }
-
-    motion {
-      move_j {
-        toppra_joint_motion_planner {
-          path_type: TOPPRA_PATH_TYPE_QUINTIC
-          sample_period_s: 0.001
-          grid_size: 150
-          high_grid_size: 300
-        }
-      }
-
-      move_l {
-        pinocchio_qp_cartesian_motion_planner {
-          sample_period_s: 0.001
-          position_gain: 4.0
-          rotation_gain: 4.0
-          qp {
-            joint_limits {
-              source: JOINT_LIMIT_SOURCE_URDF
-            }
-            twist_tracking_weight {
-              x: 1.0
-              y: 1.0
-              z: 1.0
-              rx: 0.5
-              ry: 0.5
-              rz: 0.5
-            }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 1e-4
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-        }
-      }
-
-      speed_l {
-        pinocchio_qp_cartesian_motion_planner {
-          qp {
-            joint_limits {
-              source: JOINT_LIMIT_SOURCE_URDF
-            }
-            twist_tracking_weight {
-              x: 1.0
-              y: 1.0
-              z: 1.0
-              rx: 0.5
-              ry: 0.5
-              rz: 0.5
-            }
-            qdot_regularization: 1e-4
-            prev_qdot_regularization: 2e-2
-            solver_eps: 1e-3
-            joint_limit_avoidance {
-              enable: false
-              margin_ratio: 0.15
-              gain: 0.2
-              max_push: 0.25
-              weight: 0.05
-            }
-          }
-          linear_velocity_max: 0.55
-          linear_acceleration_max: 5.0
-          linear_jerk_max: 10.0
-          angular_velocity_max: 1.0
-          angular_acceleration_max: 5.0
-          angular_jerk_max: 12.0
-          joint_acceleration_max: 8.0
-          linear_target_replan_threshold: 1e-4
-          angular_target_replan_threshold: 1e-4
-          linear_reverse_cos_threshold: -0.8660254037844386
-          linear_reverse_switch_speed_threshold: 1e-3
-          normal_direction_deviation_deg: 10.0
-          mild_direction_deviation_deg: 25.0
-          severe_direction_deviation_deg: 45.0
-          linear_min_speed_ratio: 0.2
-        }
-
-        speed_l_controller {
-          cartesian_velocity_controller {
-            control_period_s: 0.001
-            stop_twist_norm: 1e-9
-            stop_command_velocity_norm: 1e-3
-            stop_measured_velocity_norm: 1e-2
-          }
-        }
-      }
-    }
-  }
-}

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

@@ -3,14 +3,22 @@ camera {
     id: "cam1"
     realsense {
       serialNumber: "243122074587"
-      width: 640
-      height: 480
-      fps: 30
-      codec: "H265"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGBD
+      capture {
+        width: 640
+        height: 480
+        fps: 30
+        stream_mode: STREAM_MODE_RGBD
+      }
+      encoder {
+        width: 640
+        height: 480
+        fps: 30
+        codec: "H265"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
       align_mode: ALIGN_MODE_COLOR
-      buffer_size: 30
       sync: false
     }
   }
@@ -19,16 +27,22 @@ camera {
     id: "right_hand_cam"
     realsense {
       serialNumber: "243122072252"
-      width: 1280
-      height: 720
-      encode_width: 640
-      encode_height: 360
-      fps: 30
-      codec: "H264"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGB
+      capture {
+        width: 1280
+        height: 720
+        fps: 30
+        stream_mode: STREAM_MODE_RGB
+      }
+      encoder {
+        width: 640
+        height: 360
+        fps: 30
+        codec: "H264"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
       align_mode: ALIGN_MODE_COLOR
-      buffer_size: 30
       sync: false
     }
   }
@@ -37,44 +51,98 @@ camera {
     id: "cam3"
     realsense {
       serialNumber: "243122075614"
-      width: 640
-      height: 480
-      fps: 30
-      codec: "H265"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGBD
+      capture {
+        width: 640
+        height: 480
+        fps: 30
+        stream_mode: STREAM_MODE_RGBD
+      }
+      encoder {
+        width: 640
+        height: 480
+        fps: 30
+        codec: "H265"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
       align_mode: ALIGN_MODE_COLOR
-      buffer_size: 30
       sync: false
     }
   }
 
+  cameras {
+    id: "mujoco_hand_cam"
+    mujoco {
+      world_id: "mujoco_world"
+      camera_name: "hand_cam"
+      render {
+        width: 1280
+        height: 720
+        fps: 30
+        stream_mode: STREAM_MODE_RGBD
+      }
+      encoder {
+        width: 1280
+        height: 720
+        fps: 30
+        codec: "H264"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
+      consume_new_frame_only: false
+      viewer_pip {
+        enable: true
+        left: -10
+        bottom: 10
+        width: 320
+        height: 180
+      }
+    }
+  }
+
   cameras {
     id: "left_eye_cam"
     uvc {
       usb: "/dev/uvc_left_camera"
-      width: 640
-      height: 480
-      fps: 30
-      codec: "H265"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGB
+      capture {
-      buffer_size: 30
+        width: 640
+        height: 480
+        fps: 30
+        stream_mode: STREAM_MODE_RGB
+      }
+      encoder {
+        width: 640
+        height: 480
+        fps: 30
+        codec: "H265"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
     }
   }
 
   cameras {
     id: "cam5"
     realsense {
-      serialNumber: "243122075389"
+      serialNumber: "243122070435"
-      width: 640
-      height: 480
-      fps: 30
-      codec: "H265"
       camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGBD
+      capture {
+        width: 1280
+        height: 720
+        fps: 30
+        stream_mode: STREAM_MODE_RGB
+      }
+      encoder {
+        width: 640
+        height: 360
+        fps: 30
+        codec: "H264"
+        enable_stream_timestamp: true
+        buffer_size: 30
+      }
       align_mode: ALIGN_MODE_COLOR
-      buffer_size: 30
       sync: false
     }
   }

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

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

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

@@ -4,78 +4,98 @@ 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" }
+      motors { id: 26 joint_name: "L_ELBOW_R" }
+      motors { id: 27 joint_name: "L_WRIST_P" }
+      motors { id: 28 joint_name: "L_WRIST_Y" }
+      motors { id: 29 joint_name: "L_WRIST_R" }
     }
-    joint_limits_urdf_path: "model/xiaoyan_description/dual_arm.urdf"
-    motors { id: 23 joint_name: "L_SHOULDER_P" }
-    motors { id: 24 joint_name: "L_SHOULDER_R" }
-    motors { id: 25 joint_name: "L_SHOULDER_Y" }
-    motors { id: 26 joint_name: "L_ELBOW_R" }
-    motors { id: 27 joint_name: "L_WRIST_P" }
-    motors { id: 28 joint_name: "L_WRIST_Y" }
-    motors { id: 29 joint_name: "L_WRIST_R" }
   }
 
   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" lower: -3.14 upper: 3.14 velocity: 5.0 }
+      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" lower: -0.78 upper: 1.57 velocity: 5.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" lower: -3.14 upper: 3.14 velocity: 5.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" lower: 0 upper: 2.05 velocity: 5.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" lower: -3.14 upper: 3.14 velocity: 5.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" lower: -0.78 upper: 0.78 velocity: 5.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" lower: -0.57 upper: 1.57 velocity: 5.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" }
+      motors { id: 19 joint_name: "R_ELBOW_R" }
+      motors { id: 20 joint_name: "R_WRIST_P" }
+      motors { id: 21 joint_name: "R_WRIST_Y" }
+      motors { id: 22 joint_name: "R_WRIST_R" }
     }
-    motors { id: 16 joint_name: "R_SHOULDER_P" }
-    motors { id: 17 joint_name: "R_SHOULDER_R" }
-    motors { id: 18 joint_name: "R_SHOULDER_Y" }
-    motors { id: 19 joint_name: "R_ELBOW_R" }
-    motors { id: 20 joint_name: "R_WRIST_P" }
-    motors { id: 21 joint_name: "R_WRIST_Y" }
-    motors { id: 22 joint_name: "R_WRIST_R" }
   }
 
   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" lower: -3.14 upper: 3.14 velocity: 3.0 }
+      joints { joint_name: "HEAD_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
-      joints { joint_name: "HEAD_P" lower: -3.14 upper: 3.14 velocity: 3.0 }
+      joints { joint_name: "HEAD_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
-      joints { joint_name: "HEAD_R" lower: -3.14 upper: 3.14 velocity: 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" }
     }
-    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" lower: -3.14 upper: 3.14 velocity: 3.0 }
+      joints { joint_name: "WAIST_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
-      joints { joint_name: "WAIST_P" lower: -3.14 upper: 3.14 velocity: 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" }
     }
-    motors { id: 4 joint_name: "WAIST_Y" }
-    motors { id: 15 joint_name: "WAIST_P" }
   }
 }

cmvr-es/config/devices/mujoco/mujoco_viewer.pb.txt

@@ -0,0 +1,7 @@
+viewers {
+  id: "mujoco_viewer"
+  world_id: "mujoco_world"
+  camera_distance: 3.0
+  camera_azimuth: 0.0
+  camera_elevation: -30.0
+}

cmvr-es/config/devices/mujoco/mujoco_world.pb.txt

@@ -0,0 +1,7 @@
+worlds {
+  id: "mujoco_world"
+  model_path: "model/xiaoyan_description/dual_arm.xml"
+  timestep_s: 0.001
+  realtime_factor: 1.0
+  require_actuator: true
+}

cmvr-es/config/logger/logger.pb.txt

@@ -3,7 +3,7 @@ logger {
 
   routes {
     level: LOG_LEVEL_DEBUG
-    file: true
+    file: false
     terminal: true
   }
   routes {
@@ -13,17 +13,17 @@ logger {
   routes {
     level: LOG_LEVEL_WARNING
     terminal: true
-    file: true
+    file: false
   }
   routes {
     level: LOG_LEVEL_ERROR
     terminal: true
-    file: true
+    file: false
   }
   routes {
     level: LOG_LEVEL_FATAL
     terminal: true
-    file: true
+    file: false
   }
 
   directory: "../log"

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

@@ -3,6 +3,41 @@ device_manager {
   version: "0.1"
   description: "cmvr edge system version 0.1"
 
+  devices {
+    id: "mujoco_world"
+    type: DEVICE_TYPE_MUJOCO_WORLD
+    config_file: "devices/mujoco/mujoco_world.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "mujoco_motors"
+    type: DEVICE_TYPE_MOTOR_SYSTEM
+    config_file: "devices/motor/mujoco_motors.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "mujoco_right_arm"
+    type: DEVICE_TYPE_ROBOT_ARM
+    config_file: "devices/arm/arm_mujoco_qp.pb.txt"
+    enable: false
+  }
+
+  devices {
+    id: "mujoco_viewer"
+    type: DEVICE_TYPE_MUJOCO_VIEWER
+    config_file: "devices/mujoco/mujoco_viewer.pb.txt"
+    enable: false
+  }
+
+  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
@@ -10,6 +45,14 @@ device_manager {
     enable: false
   }
 
+    devices {
+      id: "cam5"
+      type: DEVICE_TYPE_CAMERA
+      config_file: "devices/camera/camera.pb.txt"
+      enable: false
+    }
+
+
   devices {
     id: "hand2"
     type: DEVICE_TYPE_DEXHAND

cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt

@@ -38,10 +38,6 @@ touch_screen_task {
       vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
       amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
       twist_filter_alpha: 1.0
-      enable_joint_limit_avoidance: true
-      joint_limit_avoidance_gain: 0.2
-      joint_limit_avoidance_margin_ratio: 0.15
-      joint_limit_avoidance_max_push: 0.25
       r_camera_to_visp {
         m00: 1.0 m01: 0.0 m02: 0.0
         m10: 0.0 m11: 1.0 m12: 0.0

cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt

@@ -38,10 +38,6 @@ touch_screen_task {
       vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
       amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
       twist_filter_alpha: 1.0
-      enable_joint_limit_avoidance: true
-      joint_limit_avoidance_gain: 0.2
-      joint_limit_avoidance_margin_ratio: 0.15
-      joint_limit_avoidance_max_push: 0.25
       r_camera_to_visp {
         m00: 1.0 m01: 0.0 m02: 0.0
         m10: 0.0 m11: -1.0 m12: 0.0

cmvr-es/devices/CMakeLists.txt

@@ -5,6 +5,5 @@ add_subdirectory(microphone)
 add_subdirectory(dexhand)
 add_subdirectory(biohead)
 add_subdirectory(arm)
-add_subdirectory(robot)
 add_subdirectory(canbus)
 add_subdirectory(motor)

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

@@ -4,6 +4,7 @@
 #include <atomic>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -46,7 +47,7 @@ public:
     Result stopJ(double acceleration) override;
     Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame = FrameType::Base) override;
     Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
-    Result stopL(double acceleration) override;
+    Result stopL(std::optional<double> acceleration = std::nullopt) override;
     Result stopMotion() override;
 
     Result startServoMode(const ServoOptions& options) override;

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

@@ -366,7 +366,7 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
     return unsupported_("speedL");
 }
 
-Result AuboArm::stopL(double acceleration)
+Result AuboArm::stopL(std::optional<double> acceleration)
 {
     (void)acceleration;
     return stopMotion();

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

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

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

@@ -4,6 +4,7 @@
 #include <atomic>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <unordered_set>
 #include <vector>
@@ -14,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 {
 
@@ -57,7 +58,7 @@ public:
                   double acceleration,
                   double duration,
                   FrameType frame = FrameType::Base) override;
-    Result stopL(double acceleration) override;
+    Result stopL(std::optional<double> acceleration = std::nullopt) override;
     Result stopMotion() override;
 
     Result startServoMode(const ServoOptions& options) override;
@@ -104,8 +105,7 @@ private:
 
     static CartesianVelocityController::Config toCartesianVelocityControllerConfig_(
         const config::CartesianVelocityControllerConfig& config);
-    static std::vector<double> withDefaultQdMax_(const std::vector<double>& qd_max,
+    std::vector<double> moveLJointVelocityLimits_(const std::vector<double>& qd_max) const;
-                                                 std::size_t dof);
     static Result unsupported_(const std::string& name);
 
 private:

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

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

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

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

cmvr-es/devices/arm/robot_arm.h

@@ -3,6 +3,7 @@
 
 #include <cstddef>
 #include <memory>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -53,7 +54,7 @@ public:
                           double acceleration,
                           double duration,
                           FrameType frame = FrameType::Base) = 0;
-    virtual Result stopL(double acceleration) = 0;
+    virtual Result stopL(std::optional<double> acceleration = std::nullopt) = 0;
     virtual Result stopMotion() = 0;
 
     virtual Result moveP(const CartesianPose& target,

cmvr-es/devices/camera/CMakeLists.txt

@@ -1,4 +1,5 @@
 #add_subdirectory(mechmind)
+add_subdirectory(common)
 add_subdirectory(uvc_camera)
 add_subdirectory(realsense_camera)
 add_subdirectory(mujoco_camera)
@@ -11,6 +12,8 @@ target_link_libraries(camera
         INTERFACE
         cmvr_es::device::uvc_camera
         cmvr_es::device::realsense_camera
+        cmvr_es::device::mujoco_camera
+        cmvr_es::device::camera_stream_encoder
         cmvr_es::proto
 )
 

cmvr-es/devices/camera/abstract_camera.h

@@ -8,6 +8,8 @@
 #include "cmvr/config/camera_config/camera_config.pb.h"
 
 namespace cmvr::device {
+    enum CameraMode {PHOTO_MODE, VIDEO_MODE};
+
     struct Rs2Intrinsics
     {
         float   cx;

cmvr-es/devices/camera/camera_factory.h

@@ -8,6 +8,7 @@
 #include "cmvr/config/camera_config/camera_config.pb.h"
 #include "common/base/logging/logger.h"
 #include "devices/camera/abstract_camera.h"
+#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
 #include "devices/camera/realsense_camera/include/realsense_camera.h"
 #include "devices/camera/uvc_camera/include/uvc_camera.h"
 
@@ -40,6 +41,10 @@ public:
                 return nullptr;
             }
 
+            case config::CameraDeviceConfig::kMujoco:
+                return std::make_shared<MujocoCamera>(
+                    backendWithId_(cfg.id(), cfg.mujoco()));
+
             case config::CameraDeviceConfig::BACKEND_NOT_SET:
             default:
             {

cmvr-es/devices/camera/common/CMakeLists.txt

@@ -0,0 +1,18 @@
+add_library(camera_stream_encoder SHARED src/camera_stream_encoder.cpp)
+
+target_include_directories(camera_stream_encoder PUBLIC ${CMAKE_SOURCE_DIR}/cmvr-es)
+
+target_link_libraries(camera_stream_encoder
+        PUBLIC
+        opencv_core
+        opencv_imgproc
+        avcodec
+        avformat
+        avutil
+        swscale
+        swresample
+)
+
+add_library(cmvr_es::device::camera_stream_encoder ALIAS camera_stream_encoder)
+
+install(TARGETS camera_stream_encoder LIBRARY DESTINATION lib)

cmvr-es/devices/camera/common/include/camera_stream_encoder.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <opencv2/opencv.hpp>
+
+#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
+
+namespace cmvr::device {
+
+struct FfmpegEncoderInfo {
+    std::string codec_name;
+    int width = 0;
+    int height = 0;
+    int fps = 0;
+    int64_t frame_pts = 0;
+    bool bRunning = false;
+    AVCodecContext* codec_context = nullptr;
+    AVFrame* frame = nullptr;
+    AVPacket* packet = nullptr;
+    SwsContext* sws_context = nullptr;
+
+    ~FfmpegEncoderInfo();
+};
+
+struct CameraStreamEncodeOptions {
+    bool draw_timestamp = false;
+};
+
+class CameraStreamEncoder {
+public:
+    static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
+                     const std::string& codec_name,
+                     int width,
+                     int height,
+                     int fps);
+
+    static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
+                       const cv::Mat& frame,
+                       std::vector<uint8_t>& encoded_frame,
+                       bool& is_key,
+                       const CameraStreamEncodeOptions& options = {});
+};
+
+} // namespace cmvr::device

cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp

@@ -0,0 +1,297 @@
+#include "devices/camera/common/include/camera_stream_encoder.h"
+
+#include <chrono>
+#include <ctime>
+#include <iomanip>
+#include <sstream>
+
+#include <libavutil/opt.h>
+#include <opencv2/imgproc.hpp>
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+namespace {
+
+std::string getCurrentTimeString()
+{
+    const auto now = std::chrono::system_clock::now();
+    const auto now_sec = std::chrono::time_point_cast<std::chrono::seconds>(now);
+    const std::time_t now_time = std::chrono::system_clock::to_time_t(now_sec);
+    const std::tm* tm_ptr = std::localtime(&now_time);
+    const auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - now_sec).count();
+
+    std::stringstream ss;
+    ss << std::put_time(tm_ptr, "%Y-%m-%d %H:%M:%S")
+       << "." << std::setw(3) << std::setfill('0') << ms;
+    return ss.str();
+}
+
+void drawTimeStamp(cv::Mat& image)
+{
+    if (image.empty()) {
+        return;
+    }
+
+    const std::string time_str = getCurrentTimeString();
+    constexpr int font_face = cv::FONT_HERSHEY_SIMPLEX;
+    constexpr double font_scale = 0.8;
+    constexpr int thickness = 2;
+
+    int baseline = 0;
+    const cv::Size text_size = cv::getTextSize(time_str, font_face, font_scale, thickness, &baseline);
+    const cv::Point text_pos(image.cols - text_size.width - 10, text_size.height + 10);
+
+    cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), thickness + 2);
+    cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness);
+}
+
+const AVCodec* findEncoder(const std::string& codec_name)
+{
+    if (codec_name == "h264" || codec_name == "H264") {
+        const AVCodec* codec = avcodec_find_encoder_by_name("libx264");
+        return codec ? codec : avcodec_find_encoder(AV_CODEC_ID_H264);
+    }
+    if (codec_name == "h265" || codec_name == "HEVC" || codec_name == "H265") {
+        const AVCodec* codec = avcodec_find_encoder_by_name("libx265");
+        return codec ? codec : avcodec_find_encoder(AV_CODEC_ID_HEVC);
+    }
+    return nullptr;
+}
+
+AVPixelFormat sourcePixelFormat(const cv::Mat& frame)
+{
+    if (frame.channels() == 3) {
+        return AV_PIX_FMT_BGR24;
+    }
+    if (frame.channels() == 4) {
+        return AV_PIX_FMT_BGRA;
+    }
+    if (frame.channels() == 1) {
+        return AV_PIX_FMT_GRAY8;
+    }
+    return AV_PIX_FMT_NONE;
+}
+
+} // namespace
+
+FfmpegEncoderInfo::~FfmpegEncoderInfo()
+{
+    if (frame) {
+        av_frame_free(&frame);
+        frame = nullptr;
+    }
+    if (packet) {
+        av_packet_free(&packet);
+        packet = nullptr;
+    }
+    if (codec_context) {
+        avcodec_close(codec_context);
+        avcodec_free_context(&codec_context);
+        codec_context = nullptr;
+    }
+    if (sws_context) {
+        sws_freeContext(sws_context);
+        sws_context = nullptr;
+    }
+}
+
+bool CameraStreamEncoder::init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
+                               const std::string& codec_name,
+                               const int width,
+                               const int height,
+                               const int fps)
+{
+    encoder = std::make_shared<FfmpegEncoderInfo>();
+    encoder->codec_name = codec_name;
+    encoder->width = width;
+    encoder->height = height;
+    encoder->fps = fps;
+
+    const AVCodec* codec = findEncoder(codec_name);
+    if (!codec) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to find encoder: " << codec_name;
+        return false;
+    }
+
+    encoder->codec_context = avcodec_alloc_context3(codec);
+    if (!encoder->codec_context) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to allocate codec context";
+        return false;
+    }
+
+    AVCodecContext* ctx = encoder->codec_context;
+    ctx->codec_type = AVMEDIA_TYPE_VIDEO;
+    ctx->width = (width + 1) & ~1;
+    ctx->height = (height + 1) & ~1;
+    ctx->time_base = {1, fps};
+    ctx->framerate = {fps, 1};
+    ctx->max_b_frames = 0;
+    ctx->gop_size = 10;
+
+    if (codec->id == AV_CODEC_ID_H264) {
+        av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
+        av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
+        av_opt_set(ctx->priv_data, "profile", "baseline", 0);
+        av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
+        av_opt_set(ctx->priv_data, "annexb", "1", 0);
+    } else if (codec->id == AV_CODEC_ID_HEVC) {
+        av_opt_set(ctx->priv_data, "x265-params",
+                   "keyint=10:min-keyint=10:no-open-gop=1:bframes=0:rc-lookahead=0:log-level=none",
+                   0);
+        av_opt_set_int(ctx->priv_data, "keyint", 10, 0);
+        av_opt_set_int(ctx->priv_data, "min-keyint", 10, 0);
+        av_opt_set(ctx->priv_data, "no-open-gop", "1", 0);
+    }
+
+    const AVPixelFormat* pix_fmts = codec->pix_fmts;
+    if (!pix_fmts) {
+        ctx->pix_fmt = AV_PIX_FMT_YUV420P;
+    } else {
+        ctx->pix_fmt = pix_fmts[0];
+    }
+
+    const int open_ret = avcodec_open2(ctx, codec, nullptr);
+    if (open_ret < 0) {
+        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+        av_strerror(open_ret, errbuf, sizeof(errbuf));
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to open " << codec_name << " encoder: " << errbuf;
+        return false;
+    }
+
+    encoder->frame = av_frame_alloc();
+    if (!encoder->frame) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to allocate AVFrame";
+        return false;
+    }
+    encoder->frame->format = ctx->pix_fmt;
+    encoder->frame->width = ctx->width;
+    encoder->frame->height = ctx->height;
+    if (av_frame_get_buffer(encoder->frame, 0) < 0) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to allocate AVFrame buffer";
+        av_frame_free(&encoder->frame);
+        return false;
+    }
+
+    encoder->packet = av_packet_alloc();
+    if (!encoder->packet) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to allocate AVPacket";
+        return false;
+    }
+
+    CMVR_LOG(INFO) << "[CameraStreamEncoder] initialized " << codec_name
+                   << " encoder, size=" << width << "x" << height
+                   << ", fps=" << fps;
+    return true;
+}
+
+bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
+                                 const cv::Mat& frame,
+                                 std::vector<uint8_t>& encoded_frame,
+                                 bool& is_key,
+                                 const CameraStreamEncodeOptions& options)
+{
+    encoded_frame.clear();
+    is_key = false;
+    if (!encoder || !encoder->codec_context || !encoder->frame || !encoder->packet) {
+        return false;
+    }
+
+    if (frame.cols != encoder->width || frame.rows != encoder->height) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] frame size mismatch"
+                        << ", frame=" << frame.cols << "x" << frame.rows
+                        << ", encoder=" << encoder->width << "x" << encoder->height;
+        return false;
+    }
+
+    cv::Mat frame_to_encode = frame;
+    if (options.draw_timestamp) {
+        frame_to_encode = frame.clone();
+        drawTimeStamp(frame_to_encode);
+    }
+
+    const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode);
+    if (src_pix_fmt == AV_PIX_FMT_NONE) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] unsupported channels: " << frame_to_encode.channels();
+        return false;
+    }
+
+    if (encoder->sws_context) {
+        sws_freeContext(encoder->sws_context);
+    }
+    encoder->sws_context = sws_getContext(frame_to_encode.cols,
+                                          frame_to_encode.rows,
+                                          src_pix_fmt,
+                                          encoder->codec_context->width,
+                                          encoder->codec_context->height,
+                                          encoder->codec_context->pix_fmt,
+                                          SWS_BILINEAR,
+                                          nullptr,
+                                          nullptr,
+                                          nullptr);
+    if (!encoder->sws_context) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] failed to create SwsContext";
+        return false;
+    }
+
+    const uint8_t* src_data[AV_NUM_DATA_POINTERS] = {frame_to_encode.data};
+    int src_linesize[AV_NUM_DATA_POINTERS] = {static_cast<int>(frame_to_encode.step)};
+
+    const int scale_ret = sws_scale(encoder->sws_context,
+                                    src_data,
+                                    src_linesize,
+                                    0,
+                                    frame_to_encode.rows,
+                                    encoder->frame->data,
+                                    encoder->frame->linesize);
+    if (scale_ret < 0) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] error scaling frame";
+        return false;
+    }
+
+    encoder->frame->pts = encoder->frame_pts++;
+    int ret = avcodec_send_frame(encoder->codec_context, encoder->frame);
+    if (ret < 0) {
+        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+        av_strerror(ret, errbuf, sizeof(errbuf));
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] error sending frame to encoder: " << errbuf;
+        return false;
+    }
+
+    while (true) {
+        ret = avcodec_receive_packet(encoder->codec_context, encoder->packet);
+        if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
+            break;
+        }
+        if (ret < 0) {
+            char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
+            av_strerror(ret, errbuf, sizeof(errbuf));
+            CMVR_LOG(ERROR) << "[CameraStreamEncoder] error receiving packet from encoder: " << errbuf;
+            break;
+        }
+
+        if (encoder->packet->flags & AV_PKT_FLAG_KEY) {
+            is_key = true;
+        }
+        encoded_frame.reserve(encoded_frame.size() + encoder->packet->size);
+        encoded_frame.insert(encoded_frame.end(),
+                             encoder->packet->data,
+                             encoder->packet->data + encoder->packet->size);
+        av_packet_unref(encoder->packet);
+    }
+
+    if (encoded_frame.size() < 4) {
+        return false;
+    }
+
+    const bool has_start_code =
+        (encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 1) ||
+        (encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 0 && encoded_frame[3] == 1);
+    if (!has_start_code) {
+        CMVR_LOG(ERROR) << "[CameraStreamEncoder] invalid frame: no NALU start code";
+        return false;
+    }
+    return true;
+}
+
+} // namespace cmvr::device

cmvr-es/devices/camera/mujoco_camera/CMakeLists.txt

@@ -4,8 +4,30 @@ add_library(mujoco_camera SHARED src/mujoco_camera.cpp)
 
 target_include_directories(mujoco_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-target_link_libraries(mujoco_camera PUBLIC ${OpenCV_LIBS})
+target_link_libraries(mujoco_camera
+        PUBLIC
+        ${OpenCV_LIBS}
+        cmvr_es::proto
+        cmvr_es::device::camera_stream_encoder
+        cmvr_es::mujoco_world
+        cmvr_es::device::motor_manager
+        glfw
+        mujoco
+)
 
 add_library(cmvr_es::device::mujoco_camera ALIAS mujoco_camera)
 
+add_executable(mujoco_camera_test
+        src/mujoco_camera_test.cpp
+)
+
+target_link_libraries(mujoco_camera_test
+        PRIVATE
+        cmvr_es::device::mujoco_camera
+        cmvr_es::mujoco_world
+        gtest
+        gtest_main
+        pthread
+)
+
 install(TARGETS mujoco_camera LIBRARY DESTINATION lib)

cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h

@@ -6,10 +6,19 @@
 
 #include <cstdint>
 #include <functional>
+#include <memory>
 #include <mutex>
+#include <string>
 #include <vector>
 
+#include <mujoco/mujoco.h>
+
+#include "cmvr/config/camera_config/camera_config.pb.h"
 #include "devices/camera/abstract_camera.h"
+#include "devices/camera/common/include/camera_stream_encoder.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
+
+struct GLFWwindow;
 
 namespace cmvr::device {
 
@@ -22,27 +31,67 @@ public:
                                            uint64_t& frame_id)>;
 
     explicit MujocoCamera(FetchRgbdFn fetch_rgbd_fn);
-    ~MujocoCamera() override = default;
+    explicit MujocoCamera(config::MujocoCameraConfig config);
+    ~MujocoCamera() override;
 
     std::string typeName() const override { return "MujocoCamera"; }
+    const config::MujocoCameraConfig& config() const { return config_; }
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+
+    void setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn);
     void setFovyDeg(double fovy_deg);
     void setConsumeNewFrameOnly(bool enable);
 
     void getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) override;
     void getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
     void getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) override;
+    bool startStreaming() override;
+    void stopStreaming() override;
+    bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
 
 private:
     bool fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
     void fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsics) const;
+    bool initOffscreen_();
+    void destroyOffscreen_();
+    bool renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
+    bool ensureEncoder_(int width, int height, int fps);
+    void setError_(const std::string& error);
+    static void flipRgbAndDepth_(std::vector<unsigned char>& rgb,
+                                 std::vector<float>& depth,
+                                 int width,
+                                 int height);
+    static void linearizeDepth_(const mjModel* model, std::vector<float>& depth);
 
 private:
     FetchRgbdFn fetch_rgbd_fn_;
     mutable std::mutex mtx_;
+    config::MujocoCameraConfig config_;
+    std::weak_ptr<simulate::MujocoWorld> world_;
+    GLFWwindow* window_{nullptr};
+    mjvCamera camera_{};
+    mjvOption option_{};
+    mjvPerturb perturb_{};
+    mjvScene scene_{};
+    mjrContext context_{};
+    bool scene_initialized_{false};
+    bool context_initialized_{false};
+    int camera_id_{-1};
+    int width_{640};
+    int height_{480};
+    int encode_width_{640};
+    int encode_height_{480};
+    std::string codec_{"H264"};
+    bool enable_stream_timestamp_{false};
     double fovy_deg_{60.0};
     bool consume_new_frame_only_{true};
     uint64_t last_frame_id_{0};
     bool has_last_frame_id_{false};
+    size_t stream_frame_index_{0};
+    bool streaming_{false};
+    std::shared_ptr<FfmpegEncoderInfo> rgb_encoder_;
 };
 
 } // namespace cmvr::device

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp

@@ -1,87 +1,460 @@
-//
+#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
-// Created by lgv on 2026/2/26.
-//
-
-#include "../include/mujoco_camera.h"
 
+#include <algorithm>
 #include <cmath>
+#include <cstdint>
+#include <limits>
+#include <utility>
+
+#include <GLFW/glfw3.h>
+#include <opencv2/imgproc.hpp>
+
+#include "common/base/logging/logger.h"
+#include "devices/motor/manager/include/motor_manager.h"
 
 namespace cmvr::device {
+namespace {
+
+constexpr int kDefaultWidth = 640;
+constexpr int kDefaultHeight = 480;
+constexpr int kMaxGeom = 100000;
+
+int positiveOrDefault(const int value, const int fallback)
+{
+    return value > 0 ? value : fallback;
+}
+
+} // namespace
 
 MujocoCamera::MujocoCamera(FetchRgbdFn fetch_rgbd_fn)
-    : fetch_rgbd_fn_(std::move(fetch_rgbd_fn)) {}
+    : fetch_rgbd_fn_(std::move(fetch_rgbd_fn))
+{
+    id_ = "mujoco_camera";
+}
 
-void MujocoCamera::setFovyDeg(double fovy_deg) {
+MujocoCamera::MujocoCamera(config::MujocoCameraConfig config)
+    : config_(config)
+{
+    id_ = config_.id();
+    const auto& render = config_.render();
+    const auto& encoder = config_.encoder();
+    width_ = positiveOrDefault(render.width(), kDefaultWidth);
+    height_ = positiveOrDefault(render.height(), kDefaultHeight);
+    encode_width_ = positiveOrDefault(encoder.width(), width_);
+    encode_height_ = positiveOrDefault(encoder.height(), height_);
+    codec_ = encoder.codec().empty() ? "H264" : encoder.codec();
+    enable_stream_timestamp_ = encoder.enable_stream_timestamp();
+    consume_new_frame_only_ = config_.consume_new_frame_only();
+}
+
+MujocoCamera::~MujocoCamera()
+{
+    stop();
+}
+
+bool MujocoCamera::init()
+{
+    std::lock_guard<std::mutex> lock(mtx_);
+    if (fetch_rgbd_fn_) {
+        state_.is_initialized = true;
+        state_.is_opened = true;
+        state_.fps = positiveOrDefault(config_.render().fps(), 30);
+        state_.width = width_;
+        state_.height = height_;
+        return true;
+    }
+
+    if (id_.empty()) {
+        setError_("[MujocoCamera] id is empty");
+        return false;
+    }
+    if (config_.world_id().empty()) {
+        setError_("[MujocoCamera] world_id is empty: " + id_);
+        return false;
+    }
+    if (config_.camera_name().empty()) {
+        setError_("[MujocoCamera] camera_name is empty: " + id_);
+        return false;
+    }
+
+    auto world = simulate::MujocoWorldDevice::worldFor(config_.world_id());
+    if (!world) {
+        world = MotorManager::mujocoWorldFor(config_.world_id());
+    }
+    if (!world) {
+        setError_("[MujocoCamera] MuJoCo world not found: " + config_.world_id());
+        return false;
+    }
+    if (!world->isLoaded()) {
+        setError_("[MujocoCamera] MuJoCo world is not loaded: " + config_.world_id());
+        return false;
+    }
+    world_ = world;
+
+    {
+        std::lock_guard<std::mutex> world_lock(world->mutex());
+        const mjModel* model = world->model();
+        if (model == nullptr) {
+            setError_("[MujocoCamera] world model is null: " + config_.world_id());
+            return false;
+        }
+        camera_id_ = mj_name2id(model, mjOBJ_CAMERA, config_.camera_name().c_str());
+        if (camera_id_ < 0) {
+            setError_("[MujocoCamera] camera not found in MuJoCo model: " + config_.camera_name());
+            return false;
+        }
+        fovy_deg_ = model->cam_fovy[camera_id_];
+    }
+
+    if (!initOffscreen_()) {
+        return false;
+    }
+
+    state_.is_initialized = true;
+    state_.is_opened = true;
+    state_.fps = positiveOrDefault(config_.render().fps(), 30);
+    state_.width = width_;
+    state_.height = height_;
+    clear_error_();
+    CMVR_LOG(INFO) << "[MujocoCamera] initialized, id=" << id_
+                   << ", world_id=" << config_.world_id()
+                   << ", camera=" << config_.camera_name()
+                   << ", size=" << width_ << "x" << height_;
+    return true;
+}
+
+bool MujocoCamera::start()
+{
+    if (!state_.is_initialized) {
+        if (!init()) {
+            return false;
+        }
+    }
+    std::lock_guard<std::mutex> lock(mtx_);
+    auto world = world_.lock();
+    if (world && !world->isRunning() && !world->start()) {
+        setError_("[MujocoCamera] failed to start MuJoCo world: " + world->lastError());
+        return false;
+    }
+    state_.is_streaming = true;
+    state_.is_opened = true;
+    return true;
+}
+
+bool MujocoCamera::stop()
+{
+    std::lock_guard<std::mutex> lock(mtx_);
+    state_.is_streaming = false;
+    state_.is_opened = false;
+    destroyOffscreen_();
+    return true;
+}
+
+void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn)
+{
+    std::lock_guard<std::mutex> lock(mtx_);
+    fetch_rgbd_fn_ = std::move(fetch_rgbd_fn);
+    if (fetch_rgbd_fn_) {
+        destroyOffscreen_();
+        state_.is_initialized = true;
+        state_.is_opened = true;
+        state_.fps = positiveOrDefault(config_.render().fps(), 30);
+        state_.width = width_;
+        state_.height = height_;
+        clear_error_();
+    }
+}
+
+void MujocoCamera::setFovyDeg(const double fovy_deg)
+{
     std::lock_guard<std::mutex> lock(mtx_);
     fovy_deg_ = fovy_deg;
 }
 
-void MujocoCamera::setConsumeNewFrameOnly(bool enable) {
+void MujocoCamera::setConsumeNewFrameOnly(const bool enable)
+{
     std::lock_guard<std::mutex> lock(mtx_);
     consume_new_frame_only_ = enable;
 }
 
-void MujocoCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
+void MujocoCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics)
+{
     cv::Mat depth;
     if (!fetch(color, depth, intrinsics)) {
         color.release();
     }
 }
 
-void MujocoCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
+void MujocoCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics)
+{
     cv::Mat color;
     if (!fetch(color, depth, intrinsics)) {
         depth.release();
     }
 }
 
-void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
+void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
+{
     if (!fetch(color, depth, intrinsics)) {
         color.release();
         depth.release();
     }
 }
 
-bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
+bool MujocoCamera::startStreaming()
-    std::lock_guard<std::mutex> lock(mtx_);
+{
-    if (!fetch_rgbd_fn_) return false;
+    if (!state_.is_initialized && !init()) {
-
-    std::vector<unsigned char> rgb_raw;
-    std::vector<float> depth_raw;
-    int width = 0;
-    int height = 0;
-    uint64_t frame_id = 0;
-    if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
         return false;
     }
-
+    std::lock_guard<std::mutex> lock(mtx_);
-    if (width <= 0 || height <= 0) return false;
+    streaming_ = true;
-    if ((int)rgb_raw.size() != width * height * 3) return false;
+    state_.is_streaming = true;
-    if (!depth_raw.empty() && (int)depth_raw.size() != width * height) return false;
-
-    if (consume_new_frame_only_) {
-        if (has_last_frame_id_ && frame_id == last_frame_id_) {
-            return false;
-        }
-    }
-    last_frame_id_ = frame_id;
-    has_last_frame_id_ = true;
-
-    cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
-    color = rgb.clone();
-
-    if (!depth_raw.empty()) {
-        cv::Mat dep(height, width, CV_32FC1, depth_raw.data());
-        depth = dep.clone();
-    } else {
-        depth.release();
-    }
-
-    fillIntrinsics(width, height, intrinsics);
     return true;
 }
 
-void MujocoCamera::fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsics) const {
+void MujocoCamera::stopStreaming()
+{
+    std::lock_guard<std::mutex> lock(mtx_);
+    streaming_ = false;
+    state_.is_streaming = false;
+    stream_frame_index_ = 0;
+    rgb_encoder_.reset();
+}
+
+bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index)
+{
+    if (!streaming_) {
+        return false;
+    }
+
+    cv::Mat color;
+    cv::Mat depth;
+    Rs2Intrinsics intrinsics{};
+    if (!fetch(color, depth, intrinsics) || color.empty()) {
+        return false;
+    }
+
+    cv::Mat color_to_encode = color;
+    if (encode_width_ > 0 && encode_height_ > 0 &&
+        (color.cols != encode_width_ || color.rows != encode_height_)) {
+        cv::resize(color, color_to_encode, cv::Size(encode_width_, encode_height_), 0.0, 0.0, cv::INTER_LINEAR);
+    }
+
+    const int fps = positiveOrDefault(config_.encoder().fps(), positiveOrDefault(config_.render().fps(), 30));
+    if (!ensureEncoder_(color_to_encode.cols, color_to_encode.rows, fps)) {
+        return false;
+    }
+
+    frame_data.rgbImage = color.clone();
+    CameraStreamEncodeOptions encode_options;
+    encode_options.draw_timestamp = enable_stream_timestamp_;
+    if (!CameraStreamEncoder::encode(rgb_encoder_,
+                                     color_to_encode,
+                                     frame_data.rgbFrame,
+                                     frame_data.bKey,
+                                     encode_options)) {
+        return false;
+    }
+    if (!depth.empty()) {
+        frame_data.depthImage = depth.clone();
+        const auto* depth_begin = reinterpret_cast<const uint8_t*>(depth.data);
+        const auto* depth_end = depth_begin + depth.total() * depth.elemSize();
+        frame_data.depthFrame.assign(depth_begin, depth_end);
+    }
+    frame_data.intrinsics = intrinsics;
+    frame_data.width = color_to_encode.cols;
+    frame_data.height = color_to_encode.rows;
+    frame_data.fps = fps;
+    frame_data.codec = codec_;
+    frame_data.depthKey = true;
+    next_index = ++stream_frame_index_;
+    return true;
+}
+
+bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
+{
+    std::lock_guard<std::mutex> lock(mtx_);
+    if (fetch_rgbd_fn_) {
+        std::vector<unsigned char> rgb_raw;
+        std::vector<float> depth_raw;
+        int width = 0;
+        int height = 0;
+        std::uint64_t frame_id = 0;
+        if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
+            return false;
+        }
+        if (width <= 0 || height <= 0) {
+            return false;
+        }
+        if (static_cast<int>(rgb_raw.size()) != width * height * 3) {
+            return false;
+        }
+        if (!depth_raw.empty() && static_cast<int>(depth_raw.size()) != width * height) {
+            return false;
+        }
+        if (consume_new_frame_only_ && has_last_frame_id_ && frame_id == last_frame_id_) {
+            return false;
+        }
+        last_frame_id_ = frame_id;
+        has_last_frame_id_ = true;
+
+        cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
+        cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
+        if (!depth_raw.empty()) {
+            cv::Mat dep(height, width, CV_32FC1, depth_raw.data());
+            depth = dep.clone();
+        } else {
+            depth.release();
+        }
+        fillIntrinsics(width, height, intrinsics);
+        return true;
+    }
+
+    return renderOffscreen_(color, depth, intrinsics);
+}
+
+bool MujocoCamera::initOffscreen_()
+{
+    if (window_ != nullptr && context_initialized_ && scene_initialized_) {
+        return true;
+    }
+
+    if (!glfwInit()) {
+        setError_("[MujocoCamera] glfwInit failed");
+        return false;
+    }
+
+    glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
+    glfwWindowHint(GLFW_DOUBLEBUFFER, GLFW_FALSE);
+    window_ = glfwCreateWindow(width_, height_, ("mujoco_camera_" + id_).c_str(), nullptr, nullptr);
+    if (window_ == nullptr) {
+        setError_("[MujocoCamera] glfwCreateWindow failed");
+        return false;
+    }
+
+    glfwMakeContextCurrent(window_);
+    mjv_defaultCamera(&camera_);
+    mjv_defaultOption(&option_);
+    mjv_defaultPerturb(&perturb_);
+    mjv_defaultScene(&scene_);
+    mjr_defaultContext(&context_);
+
+    auto world = world_.lock();
+    if (!world) {
+        setError_("[MujocoCamera] world expired");
+        return false;
+    }
+
+    std::lock_guard<std::mutex> world_lock(world->mutex());
+    const mjModel* model = world->model();
+    if (model == nullptr) {
+        setError_("[MujocoCamera] world model is null");
+        return false;
+    }
+    mjv_makeScene(model, &scene_, kMaxGeom);
+    scene_initialized_ = true;
+    mjr_makeContext(model, &context_, mjFONTSCALE_150);
+    context_initialized_ = true;
+    mjr_setBuffer(mjFB_OFFSCREEN, &context_);
+    if (context_.currentBuffer != mjFB_OFFSCREEN) {
+        setError_("[MujocoCamera] MuJoCo offscreen buffer is not available");
+        return false;
+    }
+    return true;
+}
+
+void MujocoCamera::destroyOffscreen_()
+{
+    if (window_ != nullptr) {
+        glfwMakeContextCurrent(window_);
+    }
+    if (context_initialized_) {
+        mjr_freeContext(&context_);
+        context_initialized_ = false;
+    }
+    if (scene_initialized_) {
+        mjv_freeScene(&scene_);
+        scene_initialized_ = false;
+    }
+    if (window_ != nullptr) {
+        glfwDestroyWindow(window_);
+        window_ = nullptr;
+    }
+}
+
+bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
+{
+    if (!state_.is_initialized) {
+        setError_("[MujocoCamera] camera is not initialized: " + id_);
+        return false;
+    }
+    if (!initOffscreen_()) {
+        return false;
+    }
+
+    auto world = world_.lock();
+    if (!world) {
+        setError_("[MujocoCamera] world expired");
+        return false;
+    }
+
+    glfwMakeContextCurrent(window_);
+    std::vector<unsigned char> rgb(static_cast<std::size_t>(width_) * height_ * 3);
+    std::vector<float> depth_raw(static_cast<std::size_t>(width_) * height_);
+
+    {
+        std::lock_guard<std::mutex> world_lock(world->mutex());
+        mjModel* model = world->model();
+        mjData* data = world->data();
+        if (model == nullptr || data == nullptr) {
+            setError_("[MujocoCamera] world model/data is null");
+            return false;
+        }
+
+        camera_.type = mjCAMERA_FIXED;
+        camera_.fixedcamid = camera_id_;
+        camera_.trackbodyid = -1;
+
+        mjrRect viewport;
+        viewport.left = 0;
+        viewport.bottom = 0;
+        viewport.width = width_;
+        viewport.height = height_;
+
+        mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
+        mjr_render(viewport, &scene_, &context_);
+        mjr_readPixels(rgb.data(), depth_raw.data(), viewport, &context_);
+        flipRgbAndDepth_(rgb, depth_raw, width_, height_);
+        linearizeDepth_(model, depth_raw);
+    }
+
+    cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data());
+    color = rgb_mat.clone();
+    cv::Mat depth_mat(height_, width_, CV_32FC1, depth_raw.data());
+    depth = depth_mat.clone();
+    fillIntrinsics(width_, height_, intrinsics);
+    ++last_frame_id_;
+    has_last_frame_id_ = true;
+    clear_error_();
+    return true;
+}
+
+bool MujocoCamera::ensureEncoder_(const int width, const int height, const int fps)
+{
+    if (rgb_encoder_ &&
+        rgb_encoder_->width == width &&
+        rgb_encoder_->height == height &&
+        rgb_encoder_->fps == fps &&
+        rgb_encoder_->codec_name == codec_) {
+        return true;
+    }
+
+    rgb_encoder_.reset();
+    return CameraStreamEncoder::init(rgb_encoder_, codec_, width, height, fps);
+}
+
+void MujocoCamera::fillIntrinsics(const int width, const int height, Rs2Intrinsics& intrinsics) const
+{
     const double fovy = fovy_deg_ * M_PI / 180.0;
     const double fy = (height * 0.5) / std::tan(fovy * 0.5);
     const double fx = fy;
@@ -95,4 +468,56 @@ void MujocoCamera::fillIntrinsics(int width, int height, Rs2Intrinsics& intrinsi
     }
 }
 
+void MujocoCamera::setError_(const std::string& error)
+{
+    state_.is_error = true;
+    state_.error_message = error;
+    CMVR_LOG(ERROR) << error;
+}
+
+void MujocoCamera::flipRgbAndDepth_(std::vector<unsigned char>& rgb,
+                                    std::vector<float>& depth,
+                                    const int width,
+                                    const int height)
+{
+    for (int row = 0; row < height / 2; ++row) {
+        auto* rgb_top = rgb.data() + 3 * width * row;
+        auto* rgb_bottom = rgb.data() + 3 * width * (height - 1 - row);
+        std::swap_ranges(rgb_top, rgb_top + 3 * width, rgb_bottom);
+
+        auto* depth_top = depth.data() + width * row;
+        auto* depth_bottom = depth.data() + width * (height - 1 - row);
+        std::swap_ranges(depth_top, depth_top + width, depth_bottom);
+    }
+}
+
+void MujocoCamera::linearizeDepth_(const mjModel* model, std::vector<float>& depth)
+{
+    if (model == nullptr) {
+        return;
+    }
+    const double znear = static_cast<double>(model->vis.map.znear) *
+                         static_cast<double>(model->stat.extent);
+    const double zfar = static_cast<double>(model->vis.map.zfar) *
+                        static_cast<double>(model->stat.extent);
+    if (znear <= 0.0 || zfar <= znear) {
+        return;
+    }
+
+    const double two_nf = 2.0 * znear * zfar;
+    const double f_plus_n = zfar + znear;
+    const double f_minus_n = zfar - znear;
+    for (float& item : depth) {
+        if (!std::isfinite(item) || item <= 0.0f || item >= 1.0f) {
+            item = std::numeric_limits<float>::infinity();
+            continue;
+        }
+        const double z_ndc = 2.0 * static_cast<double>(item) - 1.0;
+        const double denom = f_plus_n - z_ndc * f_minus_n;
+        item = denom <= 1e-12
+            ? std::numeric_limits<float>::infinity()
+            : static_cast<float>(two_nf / denom);
+    }
+}
+
 } // namespace cmvr::device

cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera_test.cpp

@@ -0,0 +1,56 @@
+#include <gtest/gtest.h>
+
+#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
+
+namespace {
+
+TEST(MujocoCameraTest, CapturesOffscreenRgbdFrame)
+{
+    cmvr::config::MujocoWorldConfig world_config;
+    world_config.set_id("mujoco_camera_test_world");
+    world_config.set_model_path("model/xiaoyan_description/dual_arm.xml");
+    world_config.set_timestep_s(0.001);
+    world_config.set_realtime_factor(1.0);
+    world_config.set_require_actuator(false);
+
+    auto world_device = std::make_shared<cmvr::simulate::MujocoWorldDevice>(world_config);
+    ASSERT_TRUE(world_device->init());
+    ASSERT_TRUE(world_device->start());
+
+    cmvr::config::MujocoCameraConfig camera_config;
+    camera_config.set_id("mujoco_camera_test_cam");
+    camera_config.set_world_id(world_config.id());
+    camera_config.set_camera_name("hand_cam");
+    auto* render = camera_config.mutable_render();
+    render->set_width(320);
+    render->set_height(240);
+    render->set_fps(30);
+    render->set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
+    auto* encoder = camera_config.mutable_encoder();
+    encoder->set_width(320);
+    encoder->set_height(240);
+    encoder->set_fps(30);
+    encoder->set_codec("H264");
+    encoder->set_enable_stream_timestamp(true);
+
+    cmvr::device::MujocoCamera camera(camera_config);
+    ASSERT_TRUE(camera.init());
+    ASSERT_TRUE(camera.start());
+
+    cv::Mat color;
+    cv::Mat depth;
+    cmvr::device::Rs2Intrinsics intrinsics{};
+    camera.getRGBDImages(color, depth, intrinsics);
+
+    EXPECT_EQ(color.cols, 320);
+    EXPECT_EQ(color.rows, 240);
+    EXPECT_EQ(color.type(), CV_8UC3);
+    EXPECT_EQ(depth.cols, 320);
+    EXPECT_EQ(depth.rows, 240);
+    EXPECT_EQ(depth.type(), CV_32FC1);
+    EXPECT_GT(intrinsics.fx, 0.0f);
+    EXPECT_GT(intrinsics.fy, 0.0f);
+}
+
+} // namespace

cmvr-es/devices/camera/realsense_camera/CMakeLists.txt

@@ -5,6 +5,7 @@ target_include_directories(realsense_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命名空间别名 OK
 
 # 链接 librealsense2
+target_link_libraries(realsense_camera PUBLIC cmvr_es::device::camera_stream_encoder)
 target_link_libraries(realsense_camera PRIVATE realsense2 opencv_core opencv_imgproc opencv_videoio cmvr_es::proto)
 target_link_libraries(realsense_camera PRIVATE
         realsense2

cmvr-es/devices/camera/realsense_camera/include/realsense_camera.h

@@ -5,7 +5,9 @@
 #ifndef REALSENSE_CAMERA_H
 #define REALSENSE_CAMERA_H
 
-#include "../../uvc_camera/include/uvc_camera.h"
+#include "camera/abstract_camera.h"
+#include "common/base/ring_buffer.h"
+#include "devices/camera/common/include/camera_stream_encoder.h"
 #include <librealsense2/rs.hpp>
 #include <librealsense2/hpp/rs_internal.hpp>
 
@@ -30,11 +32,6 @@ namespace cmvr::device{
         void pauseRecording() override;
         void resumeRecording() override;
 
-        // 初始化单个编码器的通用函数（复用逻辑，避免重复代码）
-        static bool initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,
-                              const std::string& codec_name,
-                              int width, int height, int fps);
-        static bool encodeFrameWithEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,const cv::Mat& frame,std::vector<uint8_t>& encoded_frame,bool& is_key);
         void getEncodedFrame(StreamFrameData& frame_data, size_t& index) override;
         bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
 
@@ -58,6 +55,7 @@ namespace cmvr::device{
         cv::VideoCapture cap_;
         size_t buffer_size_;
         std::string codec_;
+        bool enable_stream_timestamp_{false};
         CameraMode mode_;
         StreamMode stream_mode_;
 

cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp

@@ -76,12 +76,15 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
         state_.error_message = "empty device serial number";
         return;
     }
-    fps_ = camera_.fps();
+    const auto& capture = camera_.capture();
-    width_ = camera_.width();
+    const auto& encoder = camera_.encoder();
-    height_ = camera_.height();
+    fps_ = capture.fps();
-    encode_width_ = camera_.encode_width() > 0 ? camera_.encode_width() : width_;
+    width_ = capture.width();
-    encode_height_ = camera_.encode_height() > 0 ? camera_.encode_height() : height_;
+    height_ = capture.height();
-    buffer_size_ = camera_.buffer_size();
+    encode_width_ = encoder.width() > 0 ? encoder.width() : width_;
+    encode_height_ = encoder.height() > 0 ? encoder.height() : height_;
+    buffer_size_ = encoder.buffer_size() > 0 ? encoder.buffer_size() : 30;
+    enable_stream_timestamp_ = encoder.enable_stream_timestamp();
 
     state_.fps = fps_;
     state_.width = width_;
@@ -100,7 +103,7 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
         return;
     }
 
-    auto stream_mode = camera_.stream_mode();
+    auto stream_mode = capture.stream_mode();
     if (stream_mode == config::STREAM_MODE_RGB)
     {
         stream_mode_ = COLOR_MODE;
@@ -126,7 +129,7 @@ RealsenseCamera::RealsenseCamera(const config::RealSenseCameraConfig& camera):ca
         align_mode_ = "color";
     }
 
-    codec_ = camera_.codec();
+    codec_ = encoder.codec().empty() ? "H264" : encoder.codec();
 }
 
 RealsenseCamera::~RealsenseCamera() {
@@ -171,7 +174,7 @@ bool RealsenseCamera::init() {
 
         //初始化编码器
         // 初始化RGB编码器（示例参数：640x480，30fps，H.264）
-        if (!initSingleEncoder(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
+        if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
             CMVR_LOG(ERROR) << "[RealsenseCamera] (start): Failed to init RGB encoder!";
             state_.is_initialized = false;
             state_.is_error = true;
@@ -767,7 +770,13 @@ void RealsenseCamera::streaming_worker_() {
                            0.0,
                            cv::INTER_LINEAR);
             }
-            success = encodeFrameWithEncoder(rgbEncoder_, rgb_to_encode, frame_data.rgbFrame, frame_data.bKey);
+            CameraStreamEncodeOptions encode_options;
+            encode_options.draw_timestamp = enable_stream_timestamp_;
+            success = CameraStreamEncoder::encode(rgbEncoder_,
+                                                  rgb_to_encode,
+                                                  frame_data.rgbFrame,
+                                                  frame_data.bKey,
+                                                  encode_options);
             // 深度图编码
             // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);
             if (success) {
@@ -890,297 +899,6 @@ void RealsenseCamera::recording_worker_() {
     state_.is_recording = false;
 }
 
-// 初始化单个编码器的通用函数
-bool RealsenseCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,
-                          const std::string& codec_name,
-                          int width, int height, int fps) {
-    // 1. 创建编码器实例（不变）
-    encoder = std::make_shared<FfmpegEncoderInfo>();
-    encoder->codec_name = codec_name;
-    encoder->width = width;
-    encoder->height = height;
-    encoder->fps = fps;
-
-    // 2. 查找编码器（不变）
-    const AVCodec* codec = nullptr;
-    if (codec_name == "h264" || codec_name == "H264") {
-        codec = avcodec_find_encoder_by_name("libx264");
-        if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_H264);
-    } else if (codec_name == "h265" || codec_name == "HEVC" || codec_name == "H265") {
-        codec = avcodec_find_encoder_by_name("libx265");
-        if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_HEVC);
-    }
-    if (!codec) {
-        CMVR_LOG(ERROR) << "Failed to find " << codec_name << " encoder!";
-        return false;
-    }
-
-    // 3. 初始化编码器上下文（不变）
-    encoder->codec_context = avcodec_alloc_context3(codec);
-    if (!encoder->codec_context) {
-        CMVR_LOG(ERROR) << "Failed to allocate codec context!";
-        return false;
-    }
-
-    // 4. 设置编码器参数（不变）
-    AVCodecContext* ctx = encoder->codec_context;
-    ctx->codec_type = AVMEDIA_TYPE_VIDEO;
-    ctx->width = (width + 1) & ~1;  // 确保宽度为偶数
-    ctx->height = (height + 1) & ~1;// 确保高度为偶数
-    ctx->time_base = {1, fps};      // 时间基（1/fps）
-    ctx->framerate = {fps, 1};      // 帧率
-    ctx->max_b_frames = 0;          // 禁用B帧（降低延迟）
-    ctx->gop_size = 10;//I帧间隔
-    // 5. 设置编码器私有参数
-    if (codec->id == AV_CODEC_ID_H264) {
-        av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
-        av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
-        av_opt_set(ctx->priv_data, "profile", "baseline", 0);
-        av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
-        av_opt_set(ctx->priv_data, "annexb", "1", 0);
-    } else if (codec->id == AV_CODEC_ID_HEVC) {
-        // 降低 x265 控制台日志噪声（如 "encoded 0 frames"）。
-        av_opt_set(ctx->priv_data, "x265-params", "log-level=none", 0);
-        // H.265 的强制设置
-        // 使用 x265-params 字符串设置所有参数
-        char x265_params[256];
-        snprintf(x265_params, sizeof(x265_params),
-            "keyint=%d:"           // 关键帧间隔
-            "min-keyint=%d:"       // 最小关键帧间隔
-            "no-open-gop=1:"       // 禁用开放GOP
-            "bframes=0:"           // 禁用B帧
-            "rc-lookahead=0:"
-            "log-level=none",       // 日志级别
-            10, 10);                // 设置keyint和min-keyint为10
-
-        av_opt_set(ctx->priv_data, "x265-params", x265_params, 0);
-
-        // 或者分开设置（如果支持）
-        av_opt_set_int(ctx->priv_data, "keyint", 10, 0);
-        av_opt_set_int(ctx->priv_data, "min-keyint", 10, 0);
-        av_opt_set(ctx->priv_data, "no-open-gop", "1", 0);
-    }
-
-    // 6. 设置像素格式（不变）
-    const enum AVPixelFormat* pix_fmts = codec->pix_fmts;
-    if (!pix_fmts) {
-        CMVR_LOG(INFO) << "Using default pixel format: YUV420P";
-        ctx->pix_fmt = AV_PIX_FMT_YUV420P;
-    } else {
-        ctx->pix_fmt = pix_fmts[0];
-        CMVR_LOG(INFO) << "Selected pixel format: " << av_get_pix_fmt_name(ctx->pix_fmt);
-    }
-
-    // 7. 打开编码器（不变）
-    int ret = avcodec_open2(ctx, codec, nullptr);
-    if (ret < 0) {
-        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
-        av_strerror(ret, errbuf, sizeof(errbuf));
-        CMVR_LOG(ERROR) << "Failed to open " << codec_name << " encoder: " << errbuf;
-        return false;
-    }
-
-    // 8. 分配AVFrame（不变）
-    encoder->frame = av_frame_alloc();
-    if (!encoder->frame) {
-        CMVR_LOG(ERROR) << "Failed to allocate AVFrame!";
-        return false;
-    }
-    encoder->frame->format = ctx->pix_fmt;
-    encoder->frame->width = ctx->width;
-    encoder->frame->height = ctx->height;
-    if (av_frame_get_buffer(encoder->frame, 0) < 0) {
-        CMVR_LOG(ERROR) << "Failed to allocate AVFrame buffer!";
-        av_frame_free(&encoder->frame);
-        return false;
-    }
-
-    // 9. 分配AVPacket（不变）
-    encoder->packet = av_packet_alloc();
-    if (!encoder->packet) {
-        CMVR_LOG(ERROR) << "Failed to allocate AVPacket!";
-        return false;
-    }
-
-    // 添加调试信息，确认设置生效
-    CMVR_LOG(INFO) << "Encoder settings:";
-    CMVR_LOG(INFO) << "  GOP size: " << ctx->gop_size;
-    if (codec->id == AV_CODEC_ID_HEVC) {
-        char* params = nullptr;
-        if (av_opt_get(ctx->priv_data, "x265-params", 0, (uint8_t**)&params) >= 0) {
-            CMVR_LOG(INFO) << "  x265-params: " << params;
-            av_free(params);
-        }
-    }
-
-    CMVR_LOG(INFO) << "Successfully initialized " << codec_name << " encoder ( "
-              << width << "x" << height << "@" << fps << "fps )";
-    return true;
-}
-
-// 获取当前时间并格式化为字符串
-std::string getCurrentTimeString() {
-    // 获取当前时间（精确到毫秒）
-    auto now = std::chrono::system_clock::now();
-
-    // 转换为秒级时间
-    auto now_sec = std::chrono::time_point_cast<std::chrono::seconds>(now);
-    std::time_t now_time = std::chrono::system_clock::to_time_t(now_sec);
-    std::tm* tm_ptr = std::localtime(&now_time);
-
-    // 计算毫秒部分
-    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
-        now - now_sec
-    ).count();
-
-    // 格式化时间字符串
-    std::stringstream ss;
-    ss << std::put_time(tm_ptr, "%Y-%m-%d %H:%M:%S")
-       << "." << std::setw(3) << std::setfill('0') << ms;
-
-    return ss.str();
-}
-
-// 在cv::Mat右上角绘制时间戳
-void drawTimeStamp(cv::Mat& image) {
-    if (image.empty()) return;
-
-    std::string time_str = getCurrentTimeString();
-    cv::Point text_pos;
-    cv::Scalar text_color(255, 255, 255); // 白色文字
-    int font_face = cv::FONT_HERSHEY_SIMPLEX;
-    double font_scale = 0.8;
-    int thickness = 2;
-
-    // 计算文本尺寸，用于确定右上角位置
-    int baseline = 0;
-    cv::Size text_size = cv::getTextSize(time_str, font_face, font_scale, thickness, &baseline);
-
-    // 设置右上角坐标（留出10像素边距）
-    text_pos.x = image.cols - text_size.width - 10;
-    text_pos.y = text_size.height + 10;
-
-    // 绘制文字（先画黑色背景增加可读性）
-    cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(0, 0, 0), thickness + 2);
-    cv::putText(image, time_str, text_pos, font_face, font_scale, text_color, thickness);
-}
-
-bool RealsenseCamera::encodeFrameWithEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder, const cv::Mat& frame,std::vector<uint8_t>& encoded_frame,bool& is_key) {
-
-    if (!encoder || !encoder->codec_context || !encoder->frame || !encoder->packet) {
-        return false;
-    }
-
-    // 确保输入帧尺寸匹配（不变）
-    if (frame.cols != encoder->width || frame.rows != encoder->height) {
-        CMVR_LOG(ERROR) << "Frame size does not match encoder dimensions";
-        return false;
-    }
-
-    cv::Mat dateImage;
-    frame.copyTo(dateImage);
-    drawTimeStamp(dateImage);
-
-
-    // 【修复3：设置递增的PTS，确保编码器正确处理I帧请求】
-    encoder->frame->pts = encoder->frame_pts++; // 分配唯一PTS
-
-    // 根据cv::Mat的类型设置源格式（不变）
-    AVPixelFormat src_pix_fmt;
-    if (dateImage.channels() == 3) {
-        src_pix_fmt = AV_PIX_FMT_BGR24; // OpenCV默认BGR
-    } else if (dateImage.channels() == 4) {
-        src_pix_fmt = AV_PIX_FMT_BGRA;   // 4通道为BGRA
-    } else if (dateImage.channels() == 1) {
-        src_pix_fmt = AV_PIX_FMT_GRAY8;  // 单通道灰度图
-    } else {
-        CMVR_LOG(ERROR) << "Unsupported number of channels: " << dateImage.channels();
-        return false;
-    }
-
-    // 【修复4：动态创建sws_context（匹配当前输入格式）】
-    if (encoder->sws_context) {
-        sws_freeContext(encoder->sws_context); // 释放旧上下文
-    }
-    encoder->sws_context = sws_getContext(
-        dateImage.cols, dateImage.rows, src_pix_fmt,  // 输入格式由当前frame决定
-        encoder->codec_context->width, encoder->codec_context->height, encoder->codec_context->pix_fmt,
-        SWS_BILINEAR, nullptr, nullptr, nullptr
-    );
-    if (!encoder->sws_context) {
-        CMVR_LOG(ERROR) << "Failed to create SwsContext";
-        return false;
-    }
-
-    // 转换输入帧格式为编码器所需格式（不变）
-    const uint8_t* src_data[AV_NUM_DATA_POINTERS] = {dateImage.data};
-    int src_linesize[AV_NUM_DATA_POINTERS] = {static_cast<int>(dateImage.step)};
-
-    int ret = sws_scale(encoder->sws_context, src_data, src_linesize, 0, dateImage.rows,
-                        encoder->frame->data, encoder->frame->linesize);
-    if (ret < 0) {
-        CMVR_LOG(ERROR) << "Error scaling frame";
-        return false;
-    }
-    // 发送帧到编码器（不变）
-    ret = avcodec_send_frame(encoder->codec_context, encoder->frame);
-    if (ret < 0) {
-        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
-        av_strerror(ret, errbuf, sizeof(errbuf));
-        CMVR_LOG(ERROR) << "Error sending frame to encoder: " << errbuf;
-        return false;
-    }
-    while (true)
-    {
-        // 接收编码后的数据（不变）
-        ret = avcodec_receive_packet(encoder->codec_context, encoder->packet);
-        if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
-            break;
-        } else if (ret < 0) {
-            char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
-            av_strerror(ret, errbuf, sizeof(errbuf));
-            CMVR_LOG(ERROR) << "Error receiving packet from encoder: " << errbuf;
-            break;
-        }
-
-        // 调试：打印帧类型（I帧/P帧）
-        if (encoder->packet->flags & AV_PKT_FLAG_KEY) {
-            is_key = true;
-            //CMVR_LOG(INFO) << "Encoded I frame (size: " << encoder->packet->size << " bytes)";
-        } else {
-            is_key = false;
-            //CMVR_LOG(INFO) << "Encoded P frame (size: " << encoder->packet->size << " bytes)";
-        }
-
-        // 预留足够空间，避免多次内存分配
-        encoded_frame.reserve(encoded_frame.size() + encoder->packet->size);
-        // 复制数据包内容到输出向量
-        encoded_frame.insert(encoded_frame.end(),
-                             encoder->packet->data,
-                             encoder->packet->data + encoder->packet->size);
-        av_packet_unref(encoder->packet);
-    }
-
-
-    // 验证帧有效性（NALU起始码、元数据）
-    if (!encoded_frame.empty()) {
-        // 检查NALU起始码
-        bool has_start_code = false;
-        if ((encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 1) ||
-                (encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 0 && encoded_frame[3] == 1)) {
-            has_start_code = true;
-        }
-        if (!has_start_code) {
-            CMVR_LOG(ERROR) << "Invalid frame: no NALU start code!";
-            return false;
-        }
-    } else {
-        //CMVR_LOG(ERROR) << "Encoded frame is empty!";
-        return false;
-    }
-    return true;
-}
-
 void RealsenseCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) {
     // 环形队列取数据的index由接口传入
     auto frame = stream_frame_buffer_->pop(index);

cmvr-es/devices/camera/realsense_camera/src/realsense_camera_test.cpp

@@ -70,14 +70,20 @@ cmvr::config::RealSenseCameraConfig makeRsConfig() {
     cmvr::config::RealSenseCameraConfig cfg;
     cfg.set_id("realsense_health_check");
     cfg.set_serialnumber(kRsSerialForHealthCheck);
-    cfg.set_width(kRsWidth);
-    cfg.set_height(kRsHeight);
-    cfg.set_fps(kRsFps);
-    cfg.set_codec("H265");
     cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO);
-    cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
+    auto* capture = cfg.mutable_capture();
+    capture->set_width(kRsWidth);
+    capture->set_height(kRsHeight);
+    capture->set_fps(kRsFps);
+    capture->set_stream_mode(cmvr::config::STREAM_MODE_RGBD);
+    auto* encoder = cfg.mutable_encoder();
+    encoder->set_width(kRsWidth);
+    encoder->set_height(kRsHeight);
+    encoder->set_fps(kRsFps);
+    encoder->set_codec("H265");
+    encoder->set_enable_stream_timestamp(true);
+    encoder->set_buffer_size(30);
     cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
-    cfg.set_buffer_size(30);
     cfg.set_sync(true);
     return cfg;
 }

cmvr-es/devices/camera/uvc_camera/CMakeLists.txt

@@ -2,7 +2,7 @@ add_library(uvc_camera SHARED src/uvc_camera.cpp)
 
 target_include_directories(uvc_camera PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
-target_link_libraries(uvc_camera PUBLIC glog opencv_core opencv_imgproc cmvr_es::proto)
+target_link_libraries(uvc_camera PUBLIC glog opencv_core opencv_imgproc cmvr_es::proto cmvr_es::device::camera_stream_encoder)
 
 add_library(cmvr_es::device::uvc_camera ALIAS uvc_camera)
 install(TARGETS uvc_camera LIBRARY DESTINATION lib)

cmvr-es/devices/camera/uvc_camera/include/uvc_camera.h

@@ -7,17 +7,9 @@
 
 #include "common/base/ring_buffer.h"
 #include "camera/abstract_camera.h"
-
+#include "devices/camera/common/include/camera_stream_encoder.h"
-//使用ffmpeg来编码保存视频文件
-#define USE_FFMPEG_ENCODER 1
-
-#if USE_FFMPEG_ENCODER
-#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
-#endif
 
 namespace cmvr::device {
-    enum CameraMode {PHOTO_MODE, VIDEO_MODE};
-
 
     struct ImageData
     {
@@ -27,45 +19,6 @@ namespace cmvr::device {
         cv::Mat depthImage;
     };
 
-    struct FfmpegEncoderInfo {
-        std::string codec_name;                 // 编码器名称（如"h264"、"hevc"）
-        int width = 0;                          // 图像宽度
-        int height = 0;                         // 图像高度
-        int fps = 0;                            // 帧率
-        int64_t frame_pts = 0;
-        bool bRunning = false; // 是否进行编码
-        AVCodecContext* codec_context = nullptr; // 编码器上下文
-        AVFrame* frame = nullptr; // 输入帧
-        AVPacket* packet = nullptr; // 输出包
-        SwsContext* sws_context = nullptr; // 图像转换上下文（如果需要格式转换）
-
-        // 析构函数：释放FFmpeg资源（核心！避免内存泄漏）
-        ~FfmpegEncoderInfo() {
-            // 释放编码帧
-            if (frame) {
-                av_frame_free(&frame);
-                frame = nullptr;
-            }
-            // 释放编码包
-            if (packet) {
-                av_packet_free(&packet);
-                packet = nullptr;
-            }
-            // 释放编码器上下文
-            if (codec_context) {
-                avcodec_close(codec_context);    // 关闭编码器
-                avcodec_free_context(&codec_context);  // 释放上下文
-                codec_context = nullptr;
-            }
-            // 释放格式转换上下文
-            if (sws_context) {
-                sws_freeContext(sws_context);
-                sws_context = nullptr;
-            }
-            //std::cout << "FfmpegEncoderInfo resources released." << std::endl;
-        }
-    };
-
     //USB摄像机
     class UVCCamera final : public AbstractCamera {
     public:
@@ -83,11 +36,6 @@ namespace cmvr::device {
         void stopRecording() override;
         void pauseRecording() override;
         void resumeRecording() override;
-        // 初始化单个编码器的通用函数（复用逻辑，避免重复代码）
-        static bool initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,
-                              const std::string& codec_name,
-                              int width, int height, int fps);
-        static bool encodeFrameWithEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,const cv::Mat& frame,std::vector<uint8_t>& encoded_frame,bool& is_key);
         void getEncodedFrame(StreamFrameData& frame_data, size_t& index) override;
         bool getLatestEncodedFrame(StreamFrameData& frame_data, size_t& next_index) override;
 
@@ -106,6 +54,7 @@ namespace cmvr::device {
         cv::VideoCapture cap_;
         size_t buffer_size_;
         std::string codec_;
+        bool enable_stream_timestamp_{false};
         CameraMode mode_;
 
         // std::shared_ptr<cv::Mat> current_image_;

cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp

@@ -20,12 +20,15 @@ UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
         state_.error_message = "empty device serial number";
         return;
     }
-    fps_ = camera_.fps();
+    const auto& capture = camera_.capture();
-    width_ = camera_.width();
+    const auto& encoder = camera_.encoder();
-    height_ = camera_.height();
+    fps_ = capture.fps();
-    encode_width_ = camera_.encode_width() > 0 ? camera_.encode_width() : width_;
+    width_ = capture.width();
-    encode_height_ = camera_.encode_height() > 0 ? camera_.encode_height() : height_;
+    height_ = capture.height();
-    buffer_size_ = camera_.buffer_size();
+    encode_width_ = encoder.width() > 0 ? encoder.width() : width_;
+    encode_height_ = encoder.height() > 0 ? encoder.height() : height_;
+    buffer_size_ = encoder.buffer_size() > 0 ? encoder.buffer_size() : 30;
+    enable_stream_timestamp_ = encoder.enable_stream_timestamp();
 
     state_.fps = fps_;
     state_.width = width_;
@@ -44,7 +47,7 @@ UVCCamera::UVCCamera(const config::UVCCameraConfig& camera):camera_(camera)
         return;
     }
 
-    codec_ = camera_.codec();
+    codec_ = encoder.codec().empty() ? "H264" : encoder.codec();
 }
 UVCCamera::~UVCCamera() {
     stop();
@@ -100,7 +103,7 @@ bool UVCCamera::init() {
     }
     //初始化编码器
     // 初始化RGB编码器（示例参数：640x480，30fps，H.264）
-    if (!initSingleEncoder(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
+    if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) {
         CMVR_LOG(ERROR) << "[UVCCamera] (start): Failed to init RGB encoder!";
         state_.is_error = true;
         state_.error_message = "Failed to init RGB encoder!";
@@ -499,7 +502,13 @@ void UVCCamera::streaming_worker_() {
                            0.0,
                            cv::INTER_LINEAR);
             }
-            success = encodeFrameWithEncoder(rgbEncoder_, rgb_to_encode, frame_data.rgbFrame, frame_data.bKey);
+            CameraStreamEncodeOptions encode_options;
+            encode_options.draw_timestamp = enable_stream_timestamp_;
+            success = CameraStreamEncoder::encode(rgbEncoder_,
+                                                  rgb_to_encode,
+                                                  frame_data.rgbFrame,
+                                                  frame_data.bKey,
+                                                  encode_options);
             if (success) {
                 frame_data.fps = fps_;
                 frame_data.width = encode_width_;
@@ -621,216 +630,6 @@ void UVCCamera::recording_worker_() {
     state_.is_recording = false;
 }
 
-// 初始化单个编码器的通用函数
-bool UVCCamera::initSingleEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder,
-                          const std::string& codec_name,
-                          int width, int height, int fps) {
-    // 1. 创建编码器实例（不变）
-    encoder = std::make_shared<FfmpegEncoderInfo>();
-    encoder->codec_name = codec_name;
-    encoder->width = width;
-    encoder->height = height;
-    encoder->fps = fps;
-
-    // 2. 查找编码器（不变）
-    const AVCodec* codec = nullptr;
-    if (codec_name == "h264" || codec_name == "H264") {
-        codec = avcodec_find_encoder_by_name("libx264");
-        if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_H264);
-    } else if (codec_name == "h265" || codec_name == "HEVC" || codec_name == "H265") {
-        codec = avcodec_find_encoder_by_name("libx265");
-        if (!codec) codec = avcodec_find_encoder(AV_CODEC_ID_HEVC);
-    }
-    if (!codec) {
-        CMVR_LOG(ERROR) << "Failed to find " << codec_name << " encoder!";
-        return false;
-    }
-
-    // 3. 初始化编码器上下文（不变）
-    encoder->codec_context = avcodec_alloc_context3(codec);
-    if (!encoder->codec_context) {
-        CMVR_LOG(ERROR) << "Failed to allocate codec context!";
-        return false;
-    }
-
-    // 4. 设置编码器参数（不变）
-    AVCodecContext* ctx = encoder->codec_context;
-    ctx->codec_type = AVMEDIA_TYPE_VIDEO;
-    ctx->width = (width + 1) & ~1;  // 确保宽度为偶数
-    ctx->height = (height + 1) & ~1;// 确保高度为偶数
-    ctx->time_base = {1, fps};      // 时间基（1/fps）
-    ctx->framerate = {fps, 1};      // 帧率
-    ctx->max_b_frames = 0;          // 禁用B帧（降低延迟）
-    ctx->gop_size = 10;//I帧间隔1，每一帧都是I帧
-    // 5. 设置编码器私有参数
-    if (codec->id == AV_CODEC_ID_H264) {
-        av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
-        av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
-        av_opt_set(ctx->priv_data, "profile", "baseline", 0);
-        av_opt_set(ctx->priv_data, "repeat-headers", "1", 0);
-        av_opt_set(ctx->priv_data, "annexb", "1", 0);
-    } else if (codec->id == AV_CODEC_ID_HEVC) {
-        // 直接使用默认参数，不自定义
-        // av_opt_set(ctx->priv_data, "preset", "ultrafast", 0);
-        // av_opt_set(ctx->priv_data, "tune", "zerolatency", 0);
-    }
-
-    // 6. 设置像素格式（不变）
-    const enum AVPixelFormat* pix_fmts = codec->pix_fmts;
-    if (!pix_fmts) {
-        CMVR_LOG(INFO) << "Using default pixel format: YUV420P";
-        ctx->pix_fmt = AV_PIX_FMT_YUV420P;
-    } else {
-        ctx->pix_fmt = pix_fmts[0];
-        CMVR_LOG(INFO) << "Selected pixel format: " << av_get_pix_fmt_name(ctx->pix_fmt);
-    }
-
-    // 7. 打开编码器（不变）
-    int ret = avcodec_open2(ctx, codec, nullptr);
-    if (ret < 0) {
-        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
-        av_strerror(ret, errbuf, sizeof(errbuf));
-        CMVR_LOG(ERROR) << "Failed to open " << codec_name << " encoder: " << errbuf;
-        return false;
-    }
-
-    // 8. 分配AVFrame（不变）
-    encoder->frame = av_frame_alloc();
-    if (!encoder->frame) {
-        CMVR_LOG(ERROR) << "Failed to allocate AVFrame!";
-        return false;
-    }
-    encoder->frame->format = ctx->pix_fmt;
-    encoder->frame->width = ctx->width;
-    encoder->frame->height = ctx->height;
-    if (av_frame_get_buffer(encoder->frame, 0) < 0) {
-        CMVR_LOG(ERROR) << "Failed to allocate AVFrame buffer!";
-        av_frame_free(&encoder->frame);
-        return false;
-    }
-
-    // 9. 分配AVPacket（不变）
-    encoder->packet = av_packet_alloc();
-    if (!encoder->packet) {
-        CMVR_LOG(ERROR) << "Failed to allocate AVPacket!";
-        return false;
-    }
-
-    CMVR_LOG(INFO) << "Successfully initialized " << codec_name << " encoder ( "
-              << width << "x" << height << "@" << fps << "fps )";
-    return true;
-}
-
-bool UVCCamera::encodeFrameWithEncoder(std::shared_ptr<FfmpegEncoderInfo>& encoder, const cv::Mat& frame,std::vector<uint8_t>& encoded_frame,bool& is_key) {
-
-    if (!encoder || !encoder->codec_context || !encoder->frame || !encoder->packet) {
-        return false;
-    }
-
-    // 确保输入帧尺寸匹配（不变）
-    if (frame.cols != encoder->width || frame.rows != encoder->height) {
-        CMVR_LOG(ERROR) << "Frame size does not match encoder dimensions";
-        return false;
-    }
-
-    // 【修复3：设置递增的PTS，确保编码器正确处理I帧请求】
-    encoder->frame->pts = encoder->frame_pts++; // 分配唯一PTS
-
-    // 根据cv::Mat的类型设置源格式（不变）
-    AVPixelFormat src_pix_fmt;
-    if (frame.channels() == 3) {
-        src_pix_fmt = AV_PIX_FMT_BGR24; // OpenCV默认BGR
-    } else if (frame.channels() == 4) {
-        src_pix_fmt = AV_PIX_FMT_BGRA;   // 4通道为BGRA
-    } else if (frame.channels() == 1) {
-        src_pix_fmt = AV_PIX_FMT_GRAY8;  // 单通道灰度图
-    } else {
-        CMVR_LOG(ERROR) << "Unsupported number of channels: " << frame.channels();
-        return false;
-    }
-    // 【修复4：动态创建sws_context（匹配当前输入格式）】
-    if (encoder->sws_context) {
-        sws_freeContext(encoder->sws_context); // 释放旧上下文
-    }
-    encoder->sws_context = sws_getContext(
-        frame.cols, frame.rows, src_pix_fmt,  // 输入格式由当前frame决定
-        encoder->codec_context->width, encoder->codec_context->height, encoder->codec_context->pix_fmt,
-        SWS_BILINEAR, nullptr, nullptr, nullptr
-    );
-    if (!encoder->sws_context) {
-        CMVR_LOG(ERROR) << "Failed to create SwsContext";
-        return false;
-    }
-
-    // 转换输入帧格式为编码器所需格式（不变）
-    const uint8_t* src_data[AV_NUM_DATA_POINTERS] = {frame.data};
-    int src_linesize[AV_NUM_DATA_POINTERS] = {static_cast<int>(frame.step)};
-
-    int ret = sws_scale(encoder->sws_context, src_data, src_linesize, 0, frame.rows,
-                        encoder->frame->data, encoder->frame->linesize);
-    if (ret < 0) {
-        CMVR_LOG(ERROR) << "Error scaling frame";
-        return false;
-    }
-    // 发送帧到编码器（不变）
-    ret = avcodec_send_frame(encoder->codec_context, encoder->frame);
-    if (ret < 0) {
-        char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
-        av_strerror(ret, errbuf, sizeof(errbuf));
-        CMVR_LOG(ERROR) << "Error sending frame to encoder: " << errbuf;
-        return false;
-    }
-    while (true)
-    {
-        // 接收编码后的数据（不变）
-        ret = avcodec_receive_packet(encoder->codec_context, encoder->packet);
-        if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
-            break;
-        } else if (ret < 0) {
-            char errbuf[AV_ERROR_MAX_STRING_SIZE] = {0};
-            av_strerror(ret, errbuf, sizeof(errbuf));
-            CMVR_LOG(ERROR) << "Error receiving packet from encoder: " << errbuf;
-            break;
-        }
-
-        // 调试：打印帧类型（I帧/P帧）
-        if (encoder->packet->flags & AV_PKT_FLAG_KEY) {
-            is_key = true;
-            //CMVR_LOG(INFO) << "Encoded I frame (size: " << encoder->packet->size << " bytes)";
-        } else {
-            is_key = false;
-            //CMVR_LOG(INFO) << "Encoded P frame (size: " << encoder->packet->size << " bytes)";
-        }
-
-        // 预留足够空间，避免多次内存分配
-        encoded_frame.reserve(encoded_frame.size() + encoder->packet->size);
-        // 复制数据包内容到输出向量
-        encoded_frame.insert(encoded_frame.end(),
-                             encoder->packet->data,
-                             encoder->packet->data + encoder->packet->size);
-        av_packet_unref(encoder->packet);
-    }
-
-
-    // 验证帧有效性（NALU起始码、元数据）
-    if (!encoded_frame.empty()) {
-        // 检查NALU起始码
-        bool has_start_code = false;
-        if ((encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 1) ||
-                (encoded_frame[0] == 0 && encoded_frame[1] == 0 && encoded_frame[2] == 0 && encoded_frame[3] == 1)) {
-            has_start_code = true;
-        }
-        if (!has_start_code) {
-            CMVR_LOG(ERROR) << "Invalid frame: no NALU start code!";
-            return false;
-        }
-    } else {
-        //CMVR_LOG(ERROR) << "Encoded frame is empty!";
-        return false;
-    }
-    return true;
-}
-
 void UVCCamera::getEncodedFrame(StreamFrameData& frame_data, size_t& index) {
     // 环形队列取数据的index由接口传入
     auto frame = stream_frame_buffer_->pop(index);

cmvr-es/devices/device_types.h

@@ -18,6 +18,8 @@ namespace cmvr::device {
         Microphone,
         Motor,
         MotorSystem,
+        MujocoViewer,
+        MujocoWorld,
         Robot,
         Speaker,
     };
@@ -46,6 +48,10 @@ namespace cmvr::device {
                 return "Motor";
             case DeviceKind::MotorSystem:
                 return "MotorSystem";
+            case DeviceKind::MujocoViewer:
+                return "MujocoViewer";
+            case DeviceKind::MujocoWorld:
+                return "MujocoWorld";
             case DeviceKind::Robot:
                 return "Robot";
             case DeviceKind::Speaker:

cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt

@@ -12,17 +12,3 @@ target_link_libraries(px_6ax_gen3
 )
 
 install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib)
-
-add_executable(px_6ax_gen3_test
-        src/px_6ax_gen3_test.cpp
-)
-
-target_link_libraries(px_6ax_gen3_test PRIVATE
-        cmvr_es::device::px_6ax_gen3
-        cmvr_es::common
-        cmvr_es::proto
-        glog
-        gtest
-        gtest_main
-        pthread
-)

cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp

@@ -1,163 +0,0 @@
-#include "gtest/gtest.h"
-
-#include "../include/px_6ax_gen3.h"
-#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
-#include "common/config/config_files.h"
-
-#include <chrono>
-#include <cstdint>
-#include <iostream>
-#include <memory>
-#include <stdexcept>
-#include <string>
-#include <thread>
-#include <vector>
-
-namespace {
-
-using DexHand = cmvr::device::AbstractDexHand;
-using PX6AXGen3 = cmvr::device::PX6AXGen3;
-
-DexHand::FingerType parseFingerType(const std::string& value) {
-    if (value == "PINKY") {
-        return DexHand::FingerType::PINKY;
-    }
-    if (value == "RING") {
-        return DexHand::FingerType::RING;
-    }
-    if (value == "MIDDLE" || value == "MIDDLE_FINGER") {
-        return DexHand::FingerType::MIDDLE;
-    }
-    if (value == "THUMB") {
-        return DexHand::FingerType::THUMB;
-    }
-    if (value == "PALM") {
-        return DexHand::FingerType::PALM;
-    }
-    return DexHand::FingerType::INDEX;
-}
-
-DexHand::TactileRegion parseTactileRegion(const std::string& value) {
-    if (value == "FINGER") {
-        return DexHand::TactileRegion::FINGER;
-    }
-    if (value == "PAD") {
-        return DexHand::TactileRegion::PAD;
-    }
-    if (value == "THUMB_MIDDLE") {
-        return DexHand::TactileRegion::THUMB_MIDDLE;
-    }
-    if (value == "PALM_PAD") {
-        return DexHand::TactileRegion::PALM_PAD;
-    }
-    return DexHand::TactileRegion::TIP;
-}
-
-struct StopGuard {
-    std::shared_ptr<PX6AXGen3> hand;
-
-    ~StopGuard() {
-        if (!hand) {
-            return;
-        }
-        try {
-            hand->stop();
-        } catch (...) {
-        }
-    }
-};
-
-DexHand::ResultantForce readFirstValidResultantForce(PX6AXGen3& hand,
-                                                     const DexHand::FingerType finger,
-                                                     const DexHand::TactileRegion region,
-                                                     const int max_attempts,
-                                                     const std::chrono::milliseconds retry_interval) {
-    std::string last_error;
-
-    for (int attempt = 0; attempt < max_attempts; ++attempt) {
-        try {
-            return hand.getResultantForce(finger, region);
-        } catch (const std::exception& ex) {
-            last_error = ex.what();
-        }
-
-        if (attempt + 1 < max_attempts) {
-            std::this_thread::sleep_for(retry_interval);
-        }
-    }
-
-    throw std::runtime_error("Failed to read PX6AXGen3 resultant force: " + last_error);
-}
-
-DexHand::ResultantForce readStateResultantForce(PX6AXGen3& hand) {
-    cmvr::device::DexHandState state;
-    hand.getState(state);
-    return DexHand::ResultantForce{0, 0, state.hands[0].force};
-}
-
-} // namespace
-
-TEST(PX6AXGen3Test, PrintResultantForceOnly) {
-    cmvr::config::DexHandRootConfig root_config;
-    ASSERT_TRUE(cmvr::ConfigHelper::loadConfigFile("devices/dexhand/dexhand.pb.txt", root_config));
-    const cmvr::config::DexHandDeviceConfig* device_config_ptr = nullptr;
-    for (const auto& cfg : root_config.dexhand().dexhands()) {
-        if (cfg.id() == "paxini_tip_1" && cfg.has_px_6ax_gen3()) {
-            device_config_ptr = &cfg;
-            break;
-        }
-    }
-    ASSERT_NE(device_config_ptr, nullptr);
-    auto test_config = device_config_ptr->px_6ax_gen3();
-    test_config.set_id(device_config_ptr->id());
-    ASSERT_FALSE(test_config.serial_port().empty());
-
-    auto hand = std::make_shared<PX6AXGen3>(test_config);
-    ASSERT_NO_THROW(hand->init());
-    ASSERT_NO_THROW(hand->start());
-    StopGuard stop_guard{hand};
-
-    EXPECT_EQ(hand->state(), DexHand::Status::STREAMING);
-
-    const DexHand::FingerType finger = parseFingerType(test_config.tactile_finger());
-    const DexHand::TactileRegion region = parseTactileRegion(test_config.tactile_region());
-    const int warmup_ms = test_config.poll_interval_ms() > 0
-                              ? test_config.poll_interval_ms() * 5
-                              : 200;
-    const int iterations = 1000000;
-    const int read_interval_ms = test_config.poll_interval_ms() > 0
-                                     ? test_config.poll_interval_ms()
-                                     : 10;
-    if (warmup_ms > 0) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms));
-    }
-
-    bool read_once = false;
-    for (int iteration = 0; iteration < iterations; ++iteration) {
-        DexHand::ResultantForce resultant_force{};
-        ASSERT_NO_THROW(resultant_force = readFirstValidResultantForce(
-                            *hand,
-                            finger,
-                            region,
-                            10,
-                            std::chrono::milliseconds(100)));
-
-        DexHand::ResultantForce state_force{};
-        ASSERT_NO_THROW(state_force = readStateResultantForce(*hand));
-
-        std::cout << "[PX6AXGen3Test] iter=" << (iteration + 1)
-                  << " resultant_fx=" << resultant_force.fx
-                  << " resultant_fy=" << resultant_force.fy
-                  << " resultant_fz=" << resultant_force.fz
-                  << " state_resultant_fz=" << state_force.fz
-                  << std::endl;
-
-        read_once = true;
-
-        if (read_interval_ms > 0 && iteration + 1 < iterations) {
-            std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms));
-        }
-    }
-
-    EXPECT_TRUE(read_once);
-}

cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt

@@ -7,17 +7,3 @@ add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand)
 target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware cmvr_es::proto -lmodbus)
 
 install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib)
-
-add_executable(rh56dftp_dexhand_test
-        src/rh56dftp_dexhand_test.cpp
-)
-
-target_link_libraries(rh56dftp_dexhand_test PRIVATE
-        cmvr_es::device::rh56dftp_dexhand
-        cmvr_es::common
-        cmvr_es::proto
-        glog
-        gtest
-        gtest_main
-        pthread
-)

cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp

@@ -1,135 +0,0 @@
-#include "gtest/gtest.h"
-
-#include "../include/rh56dftp_dexhand.h"
-#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
-#include "common/config/config_files.h"
-
-#include <chrono>
-#include <iomanip>
-#include <iostream>
-#include <stdexcept>
-#include <string>
-#include <thread>
-#include <vector>
-
-namespace {
-
-using DexHand = cmvr::device::AbstractDexHand;
-using RH56DexHand = cmvr::device::RH56DFTPDexhand;
-
-const char* tactileRegionToString(const DexHand::TactileRegion region) {
-    switch (region) {
-        case DexHand::TactileRegion::TIP: return "TIP";
-        case DexHand::TactileRegion::FINGER: return "FINGER";
-        case DexHand::TactileRegion::PAD: return "PAD";
-        case DexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE";
-        case DexHand::TactileRegion::PALM_PAD: return "PALM_PAD";
-    }
-    return "UNKNOWN";
-}
-
-struct StopGuard {
-    std::shared_ptr<RH56DexHand> hand;
-
-    ~StopGuard() {
-        if (!hand) {
-            return;
-        }
-        try {
-            hand->stop();
-        } catch (...) {
-        }
-    }
-};
-
-} // namespace
-
-TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) {
-    cmvr::config::DexHandRootConfig root_config;
-    ASSERT_TRUE(cmvr::ConfigHelper::loadConfigFile("devices/dexhand/dexhand.pb.txt", root_config));
-    const cmvr::config::DexHandDeviceConfig* device_config_ptr = nullptr;
-    for (const auto& cfg : root_config.dexhand().dexhands()) {
-        if (cfg.id() == "hand2" && cfg.has_rh56dftp()) {
-            device_config_ptr = &cfg;
-            break;
-        }
-    }
-    ASSERT_NE(device_config_ptr, nullptr);
-    auto hand_config = device_config_ptr->rh56dftp();
-    hand_config.set_id(device_config_ptr->id());
-    ASSERT_FALSE(hand_config.ip().empty());
-
-    const auto finger = DexHand::FingerType::RING;
-    const auto region = DexHand::TactileRegion::TIP;
-    const int iterations = 20000;
-    const int warmup_ms = 200;
-    const int read_interval_ms = 10;
-
-    auto hand = std::make_shared<RH56DexHand>(hand_config);
-    ASSERT_NO_THROW(hand->init());
-    ASSERT_NO_THROW(hand->start());
-    StopGuard stop_guard{hand};
-
-    EXPECT_EQ(hand->state(), DexHand::Status::STREAMING);
-    ASSERT_NO_THROW(hand->setTactilePollingRegion(finger, region));
-
-    if (warmup_ms > 0) {
-        std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms));
-    }
-
-    double last_pressure_sum = 0.0;
-    double last_pressure_peak = 0.0;
-    int point_count = 0;
-    std::string sensor_name;
-
-    for (int i = 0; i < iterations; ++i) {
-        const auto region_data = hand->getSensorData(finger, region);
-
-        ASSERT_TRUE(region_data.valid());
-        EXPECT_EQ(region_data.finger, finger);
-        EXPECT_EQ(region_data.region, region);
-
-        if (point_count == 0) {
-            point_count = region_data.view.pointCount();
-            sensor_name = region_data.name == nullptr ? "" : region_data.name;
-        }
-
-        last_pressure_sum = 0.0;
-        last_pressure_peak = 0.0;
-        for (int index = 0; index < region_data.view.pointCount(); ++index) {
-            const double pressure = static_cast<double>(region_data.view.data[index].fz);
-            last_pressure_sum += pressure;
-            if (pressure > last_pressure_peak) {
-                last_pressure_peak = pressure;
-            }
-        }
-
-        std::cout << std::fixed << std::setprecision(3)
-                  << "[RH56DFTPDexhandLatencyTest] iter=" << (i + 1)
-                  << "/" << iterations
-                  << " sensor=" << sensor_name
-                  << " pressure_sum=" << last_pressure_sum
-                  << " pressure_peak=" << last_pressure_peak
-                  << std::endl;
-
-        if (read_interval_ms > 0) {
-            std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms));
-        }
-    }
-
-    std::cout << std::fixed << std::setprecision(3)
-              << "[RH56DFTPDexhandLatencyTest] id=" << hand_config.id()
-              << " ip=" << hand_config.ip()
-              << " port=" << hand_config.port()
-              << " finger=INDEX"
-              << " region=" << tactileRegionToString(region)
-              << " sensor=" << sensor_name
-              << " iterations=" << iterations
-              << " points=" << point_count
-              << " last_pressure_sum=" << last_pressure_sum
-              << " last_pressure_peak=" << last_pressure_peak
-              << " read_interval_ms=" << read_interval_ms
-              << "\n";
-
-    EXPECT_GT(point_count, 0);
-}

cmvr-es/devices/motor/CMakeLists.txt

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

cmvr-es/devices/motor/abstract_motor.h

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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