refactor(motion): consolidate IK and motion planners

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

@@ -24,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)>;

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;
 }
 
@@ -104,9 +107,7 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
         std::lock_guard<std::mutex> lock(mutex_);
         target_twist_ = {};
         target_frame_ = FrameType::Base;
-        if (acceleration.has_value()) {
+        target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
-            target_acceleration_ = *acceleration;
-        }
         command_active_ = true;
         ++command_version_;
     }
@@ -191,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,
@@ -153,6 +157,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 +209,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;
-            }
         }
     }
 
@@ -117,8 +110,9 @@ bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfi
 
 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;
@@ -463,6 +447,7 @@ bool PinocchioDlsIKSolver::solveVelocityBase(const Eigen::MatrixXd& jacobian_bas
     }
 
     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_);

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.q_lb()) || !std::isfinite(limit.q_ub()) ||
-                !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
-                limit.qd() <= 0.0) {
-                CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] invalid custom joint limit for "
-                                << limit.joint_name();
-                return false;
-            }
-            joint_lower_limits_[i] = limit.q_lb();
-            joint_upper_limits_[i] = limit.q_ub();
-            joint_velocity_limits_[i] = std::abs(limit.qd());
-        }
-        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/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
-          }
-        }
-      }
-    }
-  }
-}

protos/cmvr/config/cartesian_motion_validation_config.proto

@@ -0,0 +1,46 @@
+syntax = "proto3";
+
+package cmvr.config;
+
+message CartesianLineDeviationCheckConfig {
+  bool enable = 1;
+  double line_deviation_warn_m = 2;
+  double line_deviation_stop_m = 3;
+  double line_direction_warn_deg = 4;
+  double line_direction_stop_deg = 5;
+  double line_direction_reset_deg = 6;
+  double line_check_min_distance_m = 7;
+}
+
+message JointContinuityCheckConfig {
+  bool enable = 1;
+  double max_joint_delta_rad = 2;
+  double max_joint_velocity_rad_s = 3;
+  double max_joint_acceleration_rad_s2 = 4;
+}
+
+message JointVelocityCheckConfig {
+  bool enable = 1;
+  double max_joint_velocity_rad_s = 2;
+  double max_joint_acceleration_rad_s2 = 3;
+}
+
+message CartesianStepFeasibilityCheckConfig {
+  bool enable = 1;
+  double min_linear_speed_ratio = 2;
+  double max_linear_direction_deviation_deg = 3;
+  double min_angular_speed_ratio = 4;
+  double max_angular_direction_deviation_deg = 5;
+  double min_desired_linear_speed = 6;
+  double min_desired_angular_speed = 7;
+}
+
+message CartesianVelocityFeasibilityCheckConfig {
+  bool enable = 1;
+  double min_linear_speed_ratio = 2;
+  double max_linear_direction_deviation_deg = 3;
+  double min_angular_speed_ratio = 4;
+  double max_angular_direction_deviation_deg = 5;
+  double min_desired_linear_speed = 6;
+  double min_desired_angular_speed = 7;
+}

protos/cmvr/config/pinocchio_dls_ik_config.proto

@@ -14,13 +14,5 @@ message PinocchioDlsIKConfig {
   double pos_eps = 6;
   double rot_eps = 7;
   double damping = 8;
-  JointLimitAvoidanceConfig joint_limit_avoidance = 9;
+  JointLimitPolicyConfig joint_limit_policy = 9;
-  JointLimitsConfig joint_limits = 10;
-}
-
-message JointLimitAvoidanceConfig {
-  bool enable = 1;
-  double gain = 2;
-  double margin_ratio = 3;
-  double max_push = 4;
 }

protos/cmvr/config/pinocchio_qp_ik_config.proto

@@ -2,6 +2,8 @@ syntax = "proto3";
 
 package cmvr.config;
 
+import "cmvr/config/joint_limits_config.proto";
+
 // 只管 QP 这个 IK 的参数
 message PinocchioQpIKConfig {
   string urdf_path         = 1;
@@ -14,4 +16,5 @@ message PinocchioQpIKConfig {
   int32  max_iters         = 7;
   double tol               = 8;
   double qp_time_limit     = 9;
+  JointLimitPolicyConfig joint_limit_policy = 10;
 }