Merge branch 'lgv_refator_code' into dev

CMakeLists.txt

@@ -106,23 +106,29 @@ add_executable(cmvr_es cmvr-es/main.cpp)
 target_include_directories(cmvr_es PRIVATE ${GLOG_INCLUDE_DIRS})
 target_link_libraries(cmvr_es PRIVATE
         cmvr_es::proto
+        cmvr_es::logging
         service
         ${GLOG_LIBRARIES}
         jsoncpp
-        cmvr_es::utils
         cmvr_es::service
-        cmvr_es::monitor_manager
         cmvr_es::hardware
         cmvr_es::device::canbus
         cmvr_es::device::ti5motor
-        cmvr_es::controller
+        cmvr_es::algorithms::controller
-        cmvr_es::data_center
         cmvr_es::ik_solver
-        cmvr_es::planner
+        cmvr_es::base_motion
-        cmvr_es::device::humanoid_robot
         cmvr_es::common
-        cmvr_es::applications
+        cmvr_es::task
+        cmvr_es::task_manager
+        ccd
+        fcl
 )
 
 install(TARGETS cmvr_es RUNTIME DESTINATION bin)
-install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/cmvr-es/common/config DESTINATION bin)
+install(CODE [[
+    file(REMOVE_RECURSE
+        "${CMAKE_INSTALL_PREFIX}/bin/config"
+        "${CMAKE_INSTALL_PREFIX}/bin/model")
+]])
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/cmvr-es/config DESTINATION bin)
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/model DESTINATION bin)

cmvr-es/CMakeLists.txt

@@ -1,16 +1,12 @@
-add_subdirectory(utils)
+add_subdirectory(common/base/logging)
-add_subdirectory(hardware)
+link_libraries(cmvr_es::logging)
-add_subdirectory(devices)
-add_subdirectory(device_manager)
-add_subdirectory(monitor)
-add_subdirectory(monitor_manager)
-add_subdirectory(service)
-add_subdirectory(perception)
-add_subdirectory(controller)
-add_subdirectory(planner)
 
-add_subdirectory(ik_solver)
-add_subdirectory(data_center)
-add_subdirectory(applications)
-add_subdirectory(simulate)
 add_subdirectory(common)
+add_subdirectory(hardware)
+add_subdirectory(algorithms)
+add_subdirectory(devices)
+add_subdirectory(manager/device_manager)
+add_subdirectory(task)
+add_subdirectory(manager/task_manager)
+add_subdirectory(service)
+add_subdirectory(simulate)

cmvr-es/algorithms/CMakeLists.txt

@@ -0,0 +1,4 @@
+add_subdirectory(motion_planner)
+add_subdirectory(kinematics/ik_solver)
+add_subdirectory(perception)
+add_subdirectory(controllers)

cmvr-es/algorithms/controllers/CMakeLists.txt

@@ -1,7 +1,8 @@
 
+add_subdirectory(arm_control)
+
 #find_package(VISP REQUIRED)
 
-
 #  如果报 relocation ... can not be used when making a shared object; recompile with -fPIC  ，说明SRC 中包含了test文件 ，test
 #  中链接 -lgtest -lgtest_main   ，这是静态库,导致 libcontroller.so 被迫依赖 gtest。
 #  所以 add_library(controller SHARED
@@ -10,8 +11,8 @@
 #           ❌ 不要把 src/controller_test.cpp 放进来
 #)  其他动态库类似
 file(GLOB SRC
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/pid_controller.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/pid/src/pid_controller.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/ibvs_controller.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/ibvs/src/ibvs_controller.cpp
 
 )
 
@@ -24,7 +25,6 @@ target_link_libraries(controller PUBLIC
         protobuf
         cmvr_es::perception
         cmvr_es::ik_solver
-        cmvr_es::device::humanoid_robot
         gtest
         gtest_main
         pthread
@@ -55,7 +55,7 @@ target_link_libraries(controller PUBLIC
         pinocchio_parsers
 )
 
-add_library(cmvr_es::controller ALIAS controller)
+add_library(cmvr_es::algorithms::controller ALIAS controller)
 
 install(TARGETS controller LIBRARY DESTINATION lib)
 
@@ -65,18 +65,17 @@ install(TARGETS controller LIBRARY DESTINATION lib)
 # --------------------------------------------------------
 find_package(realsense2 REQUIRED)
 add_executable(controller_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/controller_test.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/controller_test.cpp
 )
 
 target_link_libraries(controller_test
         PRIVATE
-        cmvr_es::utils
         cmvr_es::perception
         cmvr_es::ik_solver
-        cmvr_es::planner
+        cmvr_es::base_motion
         cmvr_es::proto
         cmvr_es::mujoco_viewer
-        cmvr_es::controller
+        cmvr_es::algorithms::controller
         cmvr_es::device::mujoco_camera
         gtest
         gtest_main

cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt

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

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

@@ -0,0 +1,80 @@
+#ifndef CMVR_ES_CARTESIAN_VELOCITY_CONTROLLER_H
+#define CMVR_ES_CARTESIAN_VELOCITY_CONTROLLER_H
+
+#include <atomic>
+#include <condition_variable>
+#include <cstddef>
+#include <cstdint>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <vector>
+
+#include "algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h"
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::device {
+
+class CartesianVelocityController {
+public:
+    struct Config {
+        double control_period_s{0.001};
+        double stop_twist_norm{1e-9};
+        double stop_command_velocity_norm{1e-3};
+        double stop_measured_velocity_norm{1e-2};
+    };
+
+    using ReadStateCallback = std::function<bool(std::vector<double>& q, std::vector<double>& qd)>;
+    using SendVelocityCallback = std::function<Result(const JointVelocityCommand& velocity, double acceleration)>;
+
+    CartesianVelocityController(Config config,
+                                std::shared_ptr<CartesianMotionPlanner> planner,
+                                std::size_t dof,
+                                ReadStateCallback read_state,
+                                SendVelocityCallback send_velocity);
+    ~CartesianVelocityController();
+
+    CartesianVelocityController(const CartesianVelocityController&) = delete;
+    CartesianVelocityController& operator=(const CartesianVelocityController&) = delete;
+
+    Result speedL(const CartesianVelocity& velocity,
+                  double acceleration,
+                  double duration,
+                  FrameType frame);
+    Result stop(double acceleration);
+    void shutdown();
+
+    bool busy() const { return busy_.load(); }
+    CartesianVelocity getCommandTwistBase() const;
+
+private:
+    void ensureWorkerStarted_();
+    void workerLoop_();
+    void sendZero_();
+
+    static double velocityNorm_(const std::vector<double>& velocity);
+    static double twistNorm_(const CartesianVelocity& velocity);
+
+private:
+    Config config_;
+    std::shared_ptr<CartesianMotionPlanner> planner_;
+    std::size_t dof_{0};
+    ReadStateCallback read_state_;
+    SendVelocityCallback send_velocity_;
+
+    std::unique_ptr<std::thread> worker_;
+    mutable std::mutex mutex_;
+    std::condition_variable cv_;
+    std::atomic<bool> stop_requested_{false};
+    bool command_active_{false};
+    CartesianVelocity target_twist_{};
+    FrameType target_frame_{FrameType::Base};
+    double target_acceleration_{0.25};
+    std::uint64_t command_version_{0};
+    std::atomic<bool> busy_{false};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CARTESIAN_VELOCITY_CONTROLLER_H

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

@@ -0,0 +1,282 @@
+#include "algorithms/controllers/arm_control/include/cartesian_velocity_controller.h"
+
+#include <chrono>
+#include <cmath>
+#include <thread>
+#include <utility>
+
+#include "common/base/logging/logger.h"
+
+namespace cmvr::device {
+
+namespace {
+
+CartesianVelocityController::Config normalizeConfig(CartesianVelocityController::Config config)
+{
+    const CartesianVelocityController::Config defaults;
+    if (config.control_period_s <= 0.0) {
+        config.control_period_s = defaults.control_period_s;
+    }
+    if (config.stop_twist_norm <= 0.0) {
+        config.stop_twist_norm = defaults.stop_twist_norm;
+    }
+    if (config.stop_command_velocity_norm <= 0.0) {
+        config.stop_command_velocity_norm = defaults.stop_command_velocity_norm;
+    }
+    if (config.stop_measured_velocity_norm <= 0.0) {
+        config.stop_measured_velocity_norm = defaults.stop_measured_velocity_norm;
+    }
+    return config;
+}
+
+} // namespace
+
+CartesianVelocityController::CartesianVelocityController(
+    Config config,
+    std::shared_ptr<CartesianMotionPlanner> planner,
+    const std::size_t dof,
+    ReadStateCallback read_state,
+    SendVelocityCallback send_velocity)
+    : config_(normalizeConfig(config)),
+      planner_(std::move(planner)),
+      dof_(dof),
+      read_state_(std::move(read_state)),
+      send_velocity_(std::move(send_velocity))
+{
+}
+
+CartesianVelocityController::~CartesianVelocityController()
+{
+    shutdown();
+}
+
+Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
+                                           const double acceleration,
+                                           const double duration,
+                                           const FrameType frame)
+{
+    if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
+        return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
+    }
+    if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
+        return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
+    }
+
+    ensureWorkerStarted_();
+
+    std::uint64_t command_version = 0;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_twist_ = velocity;
+        target_acceleration_ = acceleration;
+        target_frame_ = frame;
+        command_active_ = true;
+        command_version = ++command_version_;
+    }
+    cv_.notify_all();
+
+    if (duration > 0.0) {
+        std::this_thread::sleep_for(std::chrono::duration<double>(duration));
+        bool should_stop = false;
+        {
+            std::lock_guard<std::mutex> lock(mutex_);
+            if (command_version_ == command_version) {
+                target_twist_ = {};
+                target_frame_ = FrameType::Base;
+                command_active_ = true;
+                ++command_version_;
+                should_stop = true;
+            }
+        }
+        if (should_stop) {
+            cv_.notify_all();
+        }
+    }
+    return Result::success();
+}
+
+Result CartesianVelocityController::stop(const double acceleration)
+{
+    (void)acceleration;
+    if (!worker_ || !worker_->joinable()) {
+        return Result::success();
+    }
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+        command_active_ = true;
+        ++command_version_;
+    }
+    cv_.notify_all();
+    return Result::success();
+}
+
+void CartesianVelocityController::shutdown()
+{
+    if (!worker_ || !worker_->joinable()) {
+        busy_.store(false);
+        return;
+    }
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        stop_requested_.store(true);
+        command_active_ = false;
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+    }
+    cv_.notify_all();
+    worker_->join();
+    worker_.reset();
+    stop_requested_.store(false);
+    busy_.store(false);
+}
+
+CartesianVelocity CartesianVelocityController::getCommandTwistBase() const
+{
+    if (!planner_) {
+        return {};
+    }
+    return planner_->getSpeedLCommandTwistBase();
+}
+
+void CartesianVelocityController::ensureWorkerStarted_()
+{
+    if (worker_ && worker_->joinable()) {
+        return;
+    }
+    stop_requested_.store(false);
+    worker_ = std::make_unique<std::thread>(&CartesianVelocityController::workerLoop_, this);
+}
+
+void CartesianVelocityController::workerLoop_()
+{
+    const double dt = config_.control_period_s;
+    auto next_tick = std::chrono::steady_clock::now();
+
+    while (true) {
+        CartesianVelocity target_twist;
+        double acceleration = 0.25;
+        FrameType target_frame = FrameType::Base;
+        {
+            std::unique_lock<std::mutex> lock(mutex_);
+            cv_.wait(lock, [&]() {
+                return stop_requested_.load() || command_active_;
+            });
+            if (stop_requested_.load()) {
+                break;
+            }
+            target_twist = target_twist_;
+            acceleration = target_acceleration_;
+            target_frame = target_frame_;
+        }
+
+        next_tick = std::chrono::steady_clock::now();
+        while (true) {
+            {
+                std::lock_guard<std::mutex> lock(mutex_);
+                if (stop_requested_.load()) {
+                    sendZero_();
+                    busy_.store(false);
+                    return;
+                }
+                if (!command_active_) {
+                    break;
+                }
+                target_twist = target_twist_;
+                acceleration = target_acceleration_;
+                target_frame = target_frame_;
+            }
+
+            if (!planner_->updateSpeedLAcceleration(acceleration)) {
+                CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
+                                << acceleration;
+                sendZero_();
+                busy_.store(false);
+                return;
+            }
+
+            std::vector<double> q_now;
+            std::vector<double> qd_now;
+            if (!read_state_(q_now, qd_now)) {
+                CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
+                sendZero_();
+                busy_.store(false);
+                return;
+            }
+
+            std::vector<double> qd_cmd;
+            if (!planner_->speedLStep(target_twist, dt, q_now, qd_now, qd_cmd, target_frame)) {
+                CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] speedLStep failed, target_twist=["
+                                << target_twist.vx << ", " << target_twist.vy << ", "
+                                << target_twist.vz << ", " << target_twist.wx << ", "
+                                << target_twist.wy << ", " << target_twist.wz
+                                << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
+                sendZero_();
+                busy_.store(false);
+                return;
+            }
+
+            JointVelocityCommand velocity_command;
+            velocity_command.velocity = qd_cmd;
+            const auto send_result = send_velocity_(velocity_command, acceleration);
+            if (!send_result.ok()) {
+                CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
+                                << send_result.message;
+                sendZero_();
+                busy_.store(false);
+                return;
+            }
+
+            if (twistNorm_(target_twist) < config_.stop_twist_norm &&
+                velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm &&
+                velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) {
+                {
+                    std::lock_guard<std::mutex> lock(mutex_);
+                    command_active_ = false;
+                }
+                sendZero_();
+                busy_.store(false);
+                break;
+            }
+
+            next_tick += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
+                std::chrono::duration<double>(dt));
+            std::this_thread::sleep_until(next_tick);
+        }
+    }
+
+    sendZero_();
+    busy_.store(false);
+}
+
+void CartesianVelocityController::sendZero_()
+{
+    if (!send_velocity_) {
+        return;
+    }
+    JointVelocityCommand zero;
+    zero.velocity.assign(dof_, 0.0);
+    (void)send_velocity_(zero, 0.0);
+}
+
+double CartesianVelocityController::velocityNorm_(const std::vector<double>& velocity)
+{
+    double value = 0.0;
+    for (const double item : velocity) {
+        value += item * item;
+    }
+    return std::sqrt(value);
+}
+
+double CartesianVelocityController::twistNorm_(const CartesianVelocity& velocity)
+{
+    return std::sqrt(velocity.vx * velocity.vx +
+                     velocity.vy * velocity.vy +
+                     velocity.vz * velocity.vz +
+                     velocity.wx * velocity.wx +
+                     velocity.wy * velocity.wy +
+                     velocity.wz * velocity.wz);
+}
+
+} // namespace cmvr::device

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

@@ -14,8 +14,8 @@
 #include <visp3/visual_features/vpFeaturePoint.h>
 #include <visp3/vs/vpServo.h>
 
-#include "ik_solver/include/pinocchio_dls_ik_solver.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
-#include "perception/include/apriltag_perception.h"
+#include "algorithms/perception/apriltag/include/apriltag_perception.h"
 
 namespace cmvr {
 
@@ -66,16 +66,12 @@ public:
     IbvsController();
 
     /**
-     * @brief 初始化 DLS 速度 IK 求解器（不再需要相机）。
+     * @brief 注入机械臂当前配置创建的 Pinocchio kinematics solver。
-     * @param urdf_path URDF 文件路径。
+     * @param solver 来自 RobotArm 的 Pinocchio IK solver。
-     * @param base_link IK 链基座 link 名称。
-     * @param flange_link IK 链末端法兰 link 名称。
      * @param camera_link URDF 中相机 link 名称，对应坐标系 `u`。
      * @return 初始化成功返回 `true`。
      */
-    bool init(const std::string& urdf_path,
+    bool init(std::shared_ptr<cmvr::PinocchioIKBase> solver,
-              const std::string& base_link,
-              const std::string& flange_link,
               const std::string& camera_link);
 
     /**
@@ -290,8 +286,8 @@ private:
 
 private:
     bool initialized_{false};
-    std::string base_frame_name_;
     std::string camera_frame_name_;
+    std::shared_ptr<cmvr::PinocchioIKBase> solver_{nullptr};
     std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
 
     double lambda_{0.7};
@@ -336,7 +332,6 @@ private:
     vpFeaturePoint s_star_[4];
 
     int tracked_tag_id_{-1};
-    std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
 
     bool has_joint_position_limits_{false};
     Eigen::VectorXd q_lower_limits_;

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

@@ -1,7 +1,8 @@
-#include "controller/include/ibvs_controller.h"
+#include "algorithms/controllers/ibvs/include/ibvs_controller.h"
 
-#include "common/utils/image/image_process.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
-#include "common/math/include/support_functions.h"
+#include "common/vision/image_projection.h"
+#include "common/math/support_functions.h"
 
 #include <algorithm>
 #include <cmath>
@@ -107,24 +108,20 @@ IbvsController::IbvsController() {
     initTask();
 }
 
-bool IbvsController::init(const std::string& urdf_path,
+bool IbvsController::init(std::shared_ptr<cmvr::PinocchioIKBase> solver,
-                          const std::string& base_link,
-                          const std::string& flange_link,
                           const std::string& camera_link) {
-    base_frame_name_ = base_link;
+    solver_ = std::move(solver);
     camera_frame_name_ = camera_link;
-
+    initialized_ = solver_ != nullptr && !camera_frame_name_.empty();
-    dls_solver_ = std::make_unique<PinocchioDlsIKSolver>(
-        urdf_path, base_link, flange_link, camera_frame_name_, 100, 1e-6, 1e-6, mu_);
-
-    initialized_ = dls_solver_->init();
     if (initialized_) {
-        dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_,
+        if (auto dls_solver = std::dynamic_pointer_cast<PinocchioDlsIKSolver>(solver_)) {
-                                            limit_avoidance_gain_,
+            dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_,
-                                            limit_avoidance_margin_ratio_,
+                                               limit_avoidance_gain_,
-                                            limit_avoidance_max_push_);
+                                               limit_avoidance_margin_ratio_,
+                                               limit_avoidance_max_push_);
+        }
         has_joint_position_limits_ =
-            dls_solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_);
+            solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_);
     } else {
         has_joint_position_limits_ = false;
         q_lower_limits_.resize(0);
@@ -209,11 +206,11 @@ bool IbvsController::computeQdot(const std::vector<double>& joints_angle,
 }
 
 bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) const {
-    if (!dls_solver_) {
+    if (!solver_) {
         joint_names.clear();
         return false;
     }
-    return dls_solver_->getChainJointNames(joint_names);
+    return solver_->getChainJointNames(joint_names);
 }
 
 bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
@@ -225,7 +222,7 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
     last_tag_pos_visp_.setZero();
     last_v_camera_visp_.setZero();
 
-    if (!initialized_ || !dls_solver_) {
+    if (!initialized_ || !solver_) {
         last_compute_status_ = ComputeStatus::NOT_READY;
         return false;
     }
@@ -396,28 +393,56 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
     twist_urdf.head<3>() = R_camera_urdf_ * twist_cam.head<3>();
     twist_urdf.tail<3>() = R_camera_urdf_ * twist_cam.tail<3>();
 
-    // IK
+    Eigen::MatrixXd jacobian_base;
-    dls_solver_->update_joints_state(joints_angle);
+    Eigen::Matrix3d base_R_camera = Eigen::Matrix3d::Identity();
+    if (!solver_->computeJacobianBaseAtQ(joints_angle,
+                                         camera_frame_name_,
+                                         jacobian_base,
+                                         base_R_camera)) {
+        last_compute_status_ = ComputeStatus::IK_FAILED;
+        return false;
+    }
+
+    Eigen::Matrix<double, 6, 1> twist_base;
+    twist_base.head<3>() = base_R_camera * twist_urdf.head<3>();
+    twist_base.tail<3>() = base_R_camera * twist_urdf.tail<3>();
 
     std::vector<double> qdot;
-    const bool ok = dls_solver_->ik(
+    const bool ok = solver_->solveVelocityBase(jacobian_base,
-        base_frame_name_, camera_frame_name_, twist_urdf,
+                                               twist_base,
-        qdot, mu_, std::numeric_limits<double>::infinity());
+                                               joints_angle,
+                                               qdot,
+                                               std::numeric_limits<double>::infinity());
 
     if (!ok || qdot.size() != joints_angle.size()) {
         last_compute_status_ = ComputeStatus::IK_FAILED;
         return false;
     }
 
-    Eigen::Map<const Eigen::VectorXd> q_chain(joints_angle.data(), static_cast<Eigen::Index>(joints_angle.size()));
-    Eigen::Map<const Eigen::VectorXd> qdot_vec(qdot.data(), static_cast<Eigen::Index>(qdot.size()));
-    const Eigen::VectorXd qdot_soft_limited = dls_solver_->applyJointSoftLimitVelocity(q_chain, qdot_vec);
-
     qdot_out.resize(qdot.size());
     for (size_t i = 0; i < qdot.size(); ++i) {
-        qdot_out[i] = SupportFunctions::clamp(qdot_soft_limited[static_cast<Eigen::Index>(i)],
+        qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_);
-                                              -qdot_max_,
+
-                                              qdot_max_);
+        if (!has_joint_position_limits_ ||
+            i >= static_cast<size_t>(q_lower_limits_.size()) ||
+            i >= static_cast<size_t>(q_upper_limits_.size())) {
+            continue;
+        }
+
+        const double lower = q_lower_limits_[static_cast<Eigen::Index>(i)];
+        const double upper = q_upper_limits_[static_cast<Eigen::Index>(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);
+        const double q = joints_angle[i];
+        if (qdot_out[i] < 0.0 && q < lower + margin) {
+            qdot_out[i] *= SupportFunctions::clamp((q - lower) / margin, 0.0, 1.0);
+        } else if (qdot_out[i] > 0.0 && q > upper - margin) {
+            qdot_out[i] *= SupportFunctions::clamp((upper - q) / margin, 0.0, 1.0);
+        }
     }
 
     last_compute_status_ = ComputeStatus::OK;
@@ -539,11 +564,11 @@ void IbvsController::setJointLimitAvoidance(bool enable,
     limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49);
     limit_avoidance_max_push_ = max_push;
 
-    if (dls_solver_) {
+    if (auto dls_solver = std::dynamic_pointer_cast<PinocchioDlsIKSolver>(solver_)) {
-        dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_,
+        dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_,
-                                            limit_avoidance_gain_,
+                                           limit_avoidance_gain_,
-                                            limit_avoidance_margin_ratio_,
+                                           limit_avoidance_margin_ratio_,
-                                            limit_avoidance_max_push_);
+                                           limit_avoidance_max_push_);
     }
 }
 

cmvr-es/algorithms/controllers/pid/src/pid_controller.cpp

@@ -2,7 +2,7 @@
 // Created by lgv on 11/27/25.
 //
 
-#include "controller/include/pid_controller.h"
+#include "algorithms/controllers/pid/include/pid_controller.h"
 #include <cmath>
 
 namespace cmvr {

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

@@ -16,9 +16,9 @@
 #include <vector>
 
 
-#include "controller/include/ibvs_controller.h"
+#include "algorithms/controllers/ibvs/include/ibvs_controller.h"
 #include "devices/camera/mujoco_camera/include/mujoco_camera.h"
-#include "ik_solver/include/pinocchio_dls_ik_solver.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;
 
@@ -551,7 +551,23 @@ protected:
     ibvs_controller_->setAlignCameraToVisp(R_align);
     ibvs_controller_->setAlignCameraToUrdf(R_align);
 
-    if (!ibvs_controller_->init(urdf_path_for_check_, "PELVIS_S", "R_WRIST_R_S", camera_frame_name_for_check_)) {
+    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;

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

@@ -1,15 +1,14 @@
 
 add_library(ik_solver SHARED
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/ik_solver.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/common/src/ik_solver.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/ik_solver_creator.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/common/src/urdf_parser.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/urdf_parser.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/pinocchio/src/pinocchio_ik_base.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_ik_base.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/pinocchio/src/pinocchio_dls_ik_solver.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_dls_ik_solver.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/pinocchio/src/pinocchio_qp_ik_solver.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_qp_ik_solver.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/srs/src/srs_ik_solver.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/bias_srs_ik_slover.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/lawba/src/joints_limit_analyzer.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/joints_limit_analyzer.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/lawba/src/lawba_ik_solver.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/lawba_ik_solver.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/lawba/src/opt_psi_selector.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/opt_psi_selector.cpp
 )
 
 target_include_directories(ik_solver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -20,7 +19,7 @@ target_link_libraries(ik_solver PUBLIC
 	pinocchio_default
 	pinocchio_parsers
 	pinocchio_collision
-		cmvr_es::planner
+		cmvr_es::base_motion
 		cmvr_es::common
 )
 
@@ -32,15 +31,15 @@ install(TARGETS ik_solver LIBRARY DESTINATION lib)
 # --------------------------------------------------------
 
 add_executable(srs_ik_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/srs_ik_test.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/srs_ik_test.cpp
 )
 
 
 target_link_libraries(srs_ik_test
         PRIVATE
-        cmvr_es::utils
         cmvr_es::ik_solver
-        cmvr_es::planner
+        cmvr_es::base_motion
+        cmvr_es::arm_motion
 		cmvr_es::proto
         cmvr_es::mujoco_viewer
 		gtest
@@ -54,7 +53,7 @@ target_link_libraries(srs_ik_test
 
 
 add_executable(ik_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/ik_test.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/ik_test.cpp
 )
 
 target_link_libraries(ik_test

cmvr-es/algorithms/kinematics/ik_solver/common/include/ik_solver.h

@@ -3,8 +3,7 @@
 //
 
 #pragma once
-#include <common/consts/constant.h>
+#include <common/base/constants.h>
-#include "ik_solver.h"
 #include <vector>
 #include <string>
 #include <memory>

cmvr-es/algorithms/kinematics/ik_solver/common/src/ik_solver.cpp

@@ -2,10 +2,9 @@
 // Created by Codex on 2026/3/2.
 //
 
-#include "ik_solver/include/ik_solver.h"
+#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h"
-#include "ik_solver/include/urdf_parser.h"
+#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
-
+#include "common/base/logging/logger.h"
-#include <iostream>
 
 namespace cmvr {
 
@@ -31,7 +30,7 @@ bool IKSolver::initUrdfChain(const std::string& urdf_path,
     auto parser = std::make_shared<UrdfParser>();
     std::string err;
     if (!parser->loadModel(urdf_path, &err)) {
-        std::cerr << "[IKSolver] Failed to load URDF: " << err << "\n";
+        CMVR_LOG(ERROR) << "[IKSolver] Failed to load URDF: " << err;
         return false;
     }
     return initUrdfChain(parser, base_frame_name, tip_frame_name);
@@ -41,18 +40,18 @@ bool IKSolver::initUrdfChain(const std::shared_ptr<const UrdfParser>& parser,
                              const std::string& base_frame_name,
                              const std::string& tip_frame_name) {
     if (!parser) {
-        std::cerr << "[IKSolver] initUrdfChain failed: parser is null\n";
+        CMVR_LOG(ERROR) << "[IKSolver] initUrdfChain failed: parser is null";
         return false;
     }
     if (!parser->loaded()) {
-        std::cerr << "[IKSolver] initUrdfChain failed: parser model not loaded\n";
+        CMVR_LOG(ERROR) << "[IKSolver] initUrdfChain failed: parser model not loaded";
         return false;
     }
 
     UrdfParser::ChainInfo chain_info;
     std::string err;
     if (!parser->extractChain(base_frame_name, tip_frame_name, chain_info, &err)) {
-        std::cerr << "[IKSolver] Failed to extract chain: " << err << "\n";
+        CMVR_LOG(ERROR) << "[IKSolver] Failed to extract chain: " << err;
         return false;
     }
 

cmvr-es/algorithms/kinematics/ik_solver/common/src/urdf_parser.cpp

@@ -1,6 +1,6 @@
 // Created by Codex on 2026/3/2.
 
-#include "ik_solver/include/urdf_parser.h"
+#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
 
 #include <algorithm>
 

cmvr-es/algorithms/kinematics/ik_solver/ik_solver_factory.h

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <memory>
+#include <stdexcept>
+
+#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.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/srs/include/srs_ik_solver.h"
+#include "cmvr/config/arm_config/arm_config.pb.h"
+#include "common/base/logging/logger.h"
+#include "common/config/config_files.h"
+
+namespace cmvr {
+
+class IKSolverFactory {
+public:
+    static std::shared_ptr<IKSolver> create(const config::ArmKinematicsConfig& cfg)
+    {
+        switch (cfg.algorithm_case()) {
+        case config::ArmKinematicsConfig::kPinocchioDlsIkSolver: {
+            auto solver_cfg = cfg.pinocchio_dls_ik_solver();
+            solver_cfg.set_urdf_path(ConfigHelper::resolveResourceFile(solver_cfg.urdf_path()));
+            return std::make_shared<PinocchioDlsIKSolver>(solver_cfg);
+        }
+        case config::ArmKinematicsConfig::kPinocchioQpIkSolver: {
+            auto solver_cfg = cfg.pinocchio_qp_ik_solver();
+            solver_cfg.set_urdf_path(ConfigHelper::resolveResourceFile(solver_cfg.urdf_path()));
+            return std::make_shared<PinocchioQpIKSolver>(solver_cfg);
+        }
+        case config::ArmKinematicsConfig::kSrsIkSolver:
+            return std::make_shared<SrsIKSolver>(cfg.srs_ik_solver());
+        case config::ArmKinematicsConfig::kLawbaIkSolver:
+            return std::make_shared<LawbaIKSolver>(cfg.lawba_ik_solver());
+        case config::ArmKinematicsConfig::ALGORITHM_NOT_SET:
+        default:
+            CMVR_LOG(ERROR) << "[IKSolverFactory] missing arm kinematics algorithm config";
+            return nullptr;
+        }
+    }
+};
+
+} // namespace cmvr

cmvr-es/algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h

@@ -7,7 +7,7 @@
 
 #include <vector>
 #include <Eigen/Core>
-#include "common/math/include/support_functions.h"
+#include "common/math/support_functions.h"
 
 namespace cmvr {
     class JointsLimitAnalyzer {

cmvr-es/algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h

@@ -1,8 +1,8 @@
 //
 // Created by lgv on 11/7/25.
 //
-#ifndef CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
+#ifndef CMVR_ES_LAWBA_IK_SOLVER_H
-#define CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
+#define CMVR_ES_LAWBA_IK_SOLVER_H
 
 #include <memory>
 #include <vector>
@@ -10,17 +10,17 @@
 
 #include <Eigen/Dense>
 
-#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
+#include "cmvr/config/lawba_ik_config.pb.h"
-#include "ik_solver/include/bias_srs_ik_slover.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h"
-#include "ik_solver/include/joints_limit_analyzer.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h"
-#include "ik_solver/include/opt_psi_selector.h"
+#include "algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h"
-#include "ik_solver/include/ik_solver.h"
+#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h"
 
 namespace cmvr {
 
 class LawbaIKSolver : public IKSolver {
 public:
-    LawbaIKSolver();
+    explicit LawbaIKSolver(const config::LawbaIKConfig& cfg);
     ~LawbaIKSolver() override = default;
 
     bool init() override;
@@ -42,6 +42,8 @@ public:
     }
 
 private:
+    config::LawbaIKConfig config_;
+
     // 机械臂 URDF 中末端工具坐标系相对于 MDH 中末端法兰的变换矩阵。
     Eigen::Matrix4d T_tool_flange_{Eigen::Matrix4d::Identity()};
 
@@ -51,7 +53,7 @@ private:
     // 机械臂 URDF 中法兰姿态相对于 MDH 中法兰姿态的变换矩阵。
     Eigen::Matrix4d T_flange_urdf_mdh_{Eigen::Matrix4d::Identity()};
 
-    std::shared_ptr<BiasSRSIkSolver>     bias_srs_ik_solver_{nullptr};
+    std::shared_ptr<SrsIKSolver>         srs_ik_solver_{nullptr};
     std::shared_ptr<JointsLimitAnalyzer> joints_limit_analyzer_{nullptr};
     std::shared_ptr<OptPsiSelector>      opt_psi_selector_{nullptr};
 
@@ -88,14 +90,14 @@ private:
                          double &cost_out);
 
     // ConfigDirection ↔ 符号
-    static inline int sign_from_dir(BiasSRSIkSolver::ConfigDirection d) {
+    static inline int sign_from_dir(SrsIKSolver::ConfigDirection d) {
-        return (d == BiasSRSIkSolver::ConfigDirection::OUTWARD) ? +1 : -1;
+        return (d == SrsIKSolver::ConfigDirection::OUTWARD) ? +1 : -1;
     }
 
-    static inline BiasSRSIkSolver::ConfigDirection dir_from_sign(int s) {
+    static inline SrsIKSolver::ConfigDirection dir_from_sign(int s) {
         return (s >= 0)
-               ? BiasSRSIkSolver::ConfigDirection::OUTWARD
+               ? SrsIKSolver::ConfigDirection::OUTWARD
-               : BiasSRSIkSolver::ConfigDirection::INWARD;
+               : SrsIKSolver::ConfigDirection::INWARD;
     }
 
     // 符号 (-1/+1) → 索引 (1/0)
@@ -115,4 +117,4 @@ private:
 
 } // namespace cmvr
 
-#endif // CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
+#endif // CMVR_ES_LAWBA_IK_SOLVER_H

cmvr-es/algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h

@@ -7,7 +7,7 @@
 
 #include <cmath>
 #include <vector>
-#include "common/math/include/support_functions.h"
+#include "common/math/support_functions.h"
 
 namespace cmvr {
     class OptPsiSelector {

cmvr-es/algorithms/kinematics/ik_solver/lawba/src/joints_limit_analyzer.cpp

@@ -2,7 +2,7 @@
 // Created by lgv on 2025/11/3.
 //
 
-#include "ik_solver/include/joints_limit_analyzer.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h"
 #include <algorithm>
 #include <limits>
 

cmvr-es/algorithms/kinematics/ik_solver/lawba/src/lawba_ik_solver.cpp

@@ -1,14 +1,17 @@
 //
 // Created by lgv on 11/7/25.
 //
-#include "ik_solver/include/lawba_ik_solver.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h"
 
-#include "common/math/include/support_functions.h"
+#include "common/math/support_functions.h"
 
 using namespace cmvr;
 
-LawbaIKSolver::LawbaIKSolver() : IKSolver("", "", "") {
+LawbaIKSolver::LawbaIKSolver(const config::LawbaIKConfig& cfg)
-    bias_srs_ik_solver_ = std::make_shared<BiasSRSIkSolver>();
+    : IKSolver("", "", "")
+    , config_(cfg)
+{
+    srs_ik_solver_ = std::make_shared<SrsIKSolver>(config_.srs_config());
     joints_limit_analyzer_ = std::make_shared<JointsLimitAnalyzer>();
     opt_psi_selector_ = std::make_shared<OptPsiSelector>();
 
@@ -16,6 +19,10 @@ LawbaIKSolver::LawbaIKSolver() : IKSolver("", "", "") {
 }
 
 bool LawbaIKSolver::init() {
+    if (!srs_ik_solver_ || !srs_ik_solver_->init()) {
+        return false;
+    }
+
     Eigen::Matrix4d T_tool_flange, T_arm_robot;
 
     T_tool_flange << 0, 1, 0, -0.284077,
@@ -36,14 +43,23 @@ bool LawbaIKSolver::init() {
     T_tool_flange_ = T_tool_flange;
     T_arm_robot_ = T_arm_robot;
 
-    // 臂角更新参数
+    const double opt_psi_update_alpha =
-    opt_psi_selector_->set_update_params(0.6, 5.0, -1, 1e-4);
+        config_.opt_psi_update_alpha() > 0.0 ? config_.opt_psi_update_alpha() : 0.6;
+    const double opt_psi_max_delta =
+        config_.opt_psi_max_delta() > 0.0 ? config_.opt_psi_max_delta() : 5.0;
+    const double opt_psi_min =
+        config_.opt_psi_min() != 0.0 ? config_.opt_psi_min() : -1.0;
+    const double opt_psi_eps =
+        config_.opt_psi_eps() > 0.0 ? config_.opt_psi_eps() : 1e-4;
+    opt_psi_selector_->set_update_params(opt_psi_update_alpha,
+                                         opt_psi_max_delta,
+                                         opt_psi_min,
+                                         opt_psi_eps);
 
-    // 代价参数（可之后再通过 set_cost_params 调整）
+    set_cost_params(config_.lambda_q() > 0.0 ? config_.lambda_q() : 5.5,
-    set_cost_params(
+                    config_.cur_branch_cost_threshold() > 0.0
-        5.5, // lambda_q_
+                        ? config_.cur_branch_cost_threshold()
-        1e-3 // cur_branch_cost_threshold_
+                        : 1e-3);
-    );
 
     return true;
 }
@@ -51,7 +67,7 @@ bool LawbaIKSolver::init() {
 bool LawbaIKSolver::fk(const std::vector<double> &joints_angle,
                                Eigen::Matrix4d &cur_pose,
                                bool is_tcp) {
-    cur_pose = T_arm_robot_ * bias_srs_ik_solver_->calc_total_transform(joints_angle);
+    cur_pose = T_arm_robot_ * srs_ik_solver_->calc_total_transform(joints_angle);
     if (is_tcp) {
         cur_pose = cur_pose * T_tool_flange_;
     } else {
@@ -67,10 +83,10 @@ bool LawbaIKSolver::estimate_state_from_current_joints() {
 
     // 1) 当前位姿 & 系数矩阵
     Eigen::Matrix4d cur_pose =
-            bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_);
+            srs_ik_solver_->calc_total_transform(cur_joints_angle_);
 
     Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
-    bias_srs_ik_solver_->cal_coefficient_matrix(cur_pose, s_mat, w_mat);
+    srs_ik_solver_->cal_coefficient_matrix(cur_pose, s_mat, w_mat);
 
     // 2) 清空所有分支的缓存
     for (int i = 0; i < 2; ++i)
@@ -83,14 +99,14 @@ bool LawbaIKSolver::estimate_state_from_current_joints() {
     double best_score = std::numeric_limits<double>::infinity();
 
     // 3) 枚举所有 (s,e,w) 分支组合
-    BiasSRSIkSolver::ConfigDirection dirs[2] = {
+    SrsIKSolver::ConfigDirection dirs[2] = {
-        BiasSRSIkSolver::ConfigDirection::OUTWARD,
+        SrsIKSolver::ConfigDirection::OUTWARD,
-        BiasSRSIkSolver::ConfigDirection::INWARD
+        SrsIKSolver::ConfigDirection::INWARD
     };
 
-    for (BiasSRSIkSolver::ConfigDirection s_dir: dirs) {
+    for (SrsIKSolver::ConfigDirection s_dir: dirs) {
-        for (BiasSRSIkSolver::ConfigDirection e_dir: dirs) {
+        for (SrsIKSolver::ConfigDirection e_dir: dirs) {
-            for (BiasSRSIkSolver::ConfigDirection w_dir: dirs) {
+            for (SrsIKSolver::ConfigDirection w_dir: dirs) {
                 int s = sign_from_dir(s_dir);
                 int e = sign_from_dir(e_dir);
                 int w = sign_from_dir(w_dir);
@@ -131,10 +147,9 @@ bool LawbaIKSolver::estimate_state_from_current_joints() {
         return false;
     }
 
-    // 4) 把求得的“最佳分支”应用到 bias_srs_ik_solver_，保持一致
+    srs_ik_solver_->set_shoulder_config(dir_from_sign(best.s_conf));
-    bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(best.s_conf));
+    srs_ik_solver_->set_elbow_config(dir_from_sign(best.e_conf));
-    bias_srs_ik_solver_->set_elbow_config(dir_from_sign(best.e_conf));
+    srs_ik_solver_->set_wrist_config(dir_from_sign(best.w_conf));
-    bias_srs_ik_solver_->set_wrist_config(dir_from_sign(best.w_conf));
 
     branch_state_ = best;
     return true;
@@ -147,7 +162,7 @@ bool LawbaIKSolver::solve_on_branch(const Eigen::Matrix4d &target_cal_pose,
                                             std::vector<double> &q_out,
                                             double &psi_out,
                                             double &cost_out) {
-    auto joints_limits = bias_srs_ik_solver_->get_joints_limits();
+    auto joints_limits = srs_ik_solver_->get_joints_limits();
 
     // 1) 这一分支下的 ψ 可行区间
     auto limits = joints_limit_analyzer_->calc_arm_angle_limits(
@@ -167,14 +182,13 @@ bool LawbaIKSolver::solve_on_branch(const Eigen::Matrix4d &target_cal_pose,
         return false;
     }
 
-    // 3) 把 bias_srs_ik_solver_ 的分支设置为当前 branch
+    srs_ik_solver_->set_shoulder_config(dir_from_sign(branch.s_conf));
-    bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch.s_conf));
+    srs_ik_solver_->set_elbow_config(dir_from_sign(branch.e_conf));
-    bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch.e_conf));
+    srs_ik_solver_->set_wrist_config(dir_from_sign(branch.w_conf));
-    bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch.w_conf));
 
     // 4) 解析 IK
     std::vector<double> q;
-    if (!bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, q, best_psi)) {
+    if (!srs_ik_solver_->ikWithPsi(target_cal_pose, q, best_psi)) {
         return false;
     }
 
@@ -217,17 +231,17 @@ bool LawbaIKSolver::ik(const Eigen::Matrix4d &target_pose,
     // --- 1) 校验当前缓存的 branch_state_ 是否仍然对应 cur_joints_angle_ ---
     if (branch_state_.valid) {
         Eigen::Matrix4d cur_pose =
-                bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_);
+                srs_ik_solver_->calc_total_transform(cur_joints_angle_);
 
         // 确保 solver 内部分支与 branch_state_ 一致
-        bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf));
+        srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf));
-        bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf));
+        srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf));
-        bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf));
+        srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf));
 
         std::vector<double> cur_joints_angle;
 
-        if (!bias_srs_ik_solver_->inverse_kinematics(cur_pose, cur_joints_angle,
+        if (!srs_ik_solver_->ikWithPsi(cur_pose, cur_joints_angle,
-                                                     branch_state_.psi)) {
+                                       branch_state_.psi)) {
             branch_state_.valid = false;
         } else {
             for (int i = 0; i < static_cast<int>(cur_joints_angle_.size()); ++i) {
@@ -248,7 +262,7 @@ bool LawbaIKSolver::ik(const Eigen::Matrix4d &target_pose,
 
     // --- 3) 只算一次 target 位姿的系数矩阵 ---
     Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
-    bias_srs_ik_solver_->cal_coefficient_matrix(target_cal_pose, s_mat, w_mat);
+    srs_ik_solver_->cal_coefficient_matrix(target_cal_pose, s_mat, w_mat);
 
     // --- 4) 先在“当前分支”上试一次 ---
     std::vector<double> q_cur;
@@ -284,14 +298,14 @@ bool LawbaIKSolver::ik(const Eigen::Matrix4d &target_pose,
         best_state.valid = true;
     }
 
-    BiasSRSIkSolver::ConfigDirection dirs[2] = {
+    SrsIKSolver::ConfigDirection dirs[2] = {
-        BiasSRSIkSolver::ConfigDirection::OUTWARD,
+        SrsIKSolver::ConfigDirection::OUTWARD,
-        BiasSRSIkSolver::ConfigDirection::INWARD
+        SrsIKSolver::ConfigDirection::INWARD
     };
 
-    for (BiasSRSIkSolver::ConfigDirection s_dir: dirs) {
+    for (SrsIKSolver::ConfigDirection s_dir: dirs) {
-        for (BiasSRSIkSolver::ConfigDirection e_dir: dirs) {
+        for (SrsIKSolver::ConfigDirection e_dir: dirs) {
-            for (BiasSRSIkSolver::ConfigDirection w_dir: dirs) {
+            for (SrsIKSolver::ConfigDirection w_dir: dirs) {
                 int s = sign_from_dir(s_dir);
                 int e = sign_from_dir(e_dir);
                 int w = sign_from_dir(w_dir);
@@ -353,9 +367,9 @@ bool LawbaIKSolver::ik(const Eigen::Matrix4d &target_pose,
     // --- 6) 用最佳分支 + ψ 更新状态并返回 ---
     branch_state_ = best_state;
 
-    bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf));
+    srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf));
-    bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf));
+    srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf));
-    bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf));
+    srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf));
 
     joints_angle = best_q;
     cur_joints_angle_ = joints_angle; // 作为下一次的“当前姿态”

cmvr-es/algorithms/kinematics/ik_solver/lawba/src/opt_psi_selector.cpp

@@ -2,7 +2,7 @@
 // Created by lgv on 11/7/25.
 //
 
-#include "ik_solver/include/opt_psi_selector.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h"
 
 using namespace cmvr;
 

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

@@ -0,0 +1,95 @@
+#pragma once
+
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
+
+
+#include <Eigen/Core>
+#include <limits>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "cmvr/config/pinocchio_dls_ik_config.pb.h"
+
+namespace cmvr {
+
+/**
+ * @brief 基于 Pinocchio 的 DLS 逆运动学求解器。
+ *
+ * 提供以下能力：
+ * - 位姿 IK（`ik`）
+ * - 正运动学 FK（`fk`，继承自基类）
+ * - 末端 twist 到关节速度的微分 IK（`ik` 重载）
+ *
+ * 求解链路由 URDF 中 `base_frame_name -> flange_frame_name` 自动提取，
+ * 并支持可选 TCP frame。
+ */
+class PinocchioDlsIKSolver : public PinocchioIKBase {
+public:
+    explicit PinocchioDlsIKSolver(const config::PinocchioDlsIKConfig& cfg);
+
+    ~PinocchioDlsIKSolver() override = default;
+
+    bool init() override;
+
+    bool ik(const Eigen::Matrix4d &target_pose,
+            std::vector<double> &joints_angle,
+            bool is_tcp = true) override;
+
+    bool ik(const std::string& base_link,
+            const std::string& ee_link,
+            const Eigen::Matrix4d& target_pose,
+            std::vector<double>& joints_angle);
+
+    bool ik(const std::string& base_link,
+            const std::string& ee_link,
+            const Eigen::Matrix<double,6,1>& target_vel,
+            std::vector<double>& joints_vel,
+            double damping = -1.0,
+            double qdot_abs_max = std::numeric_limits<double>::infinity());
+
+    bool solveVelocityBase(const Eigen::MatrixXd& jacobian_base,
+                           const Eigen::Matrix<double,6,1>& target_twist_base,
+                           const std::vector<double>& q_chain,
+                           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; }
+
+    double damping() const { return damping_; }
+
+private:
+    Eigen::MatrixXd dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda);
+
+    bool refreshJointLimits_(const config::PinocchioDlsIKConfig& cfg);
+
+    Eigen::VectorXd computeJointLimitAvoidanceVelocity(const Eigen::VectorXd& q_chain) const;
+
+    Eigen::VectorXd projectToNullspace(const Eigen::MatrixXd& J_pinv,
+                                       const Eigen::MatrixXd& J,
+                                       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_;
+    double pos_eps_;
+    double rot_eps_;
+    double damping_;
+    config::PinocchioDlsIKConfig config_;
+
+};
+
+} // namespace cmvr

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

@@ -1,8 +1,8 @@
 // Created by Codex on 2026/3/3.
 #pragma once
 
-#include "ik_solver/include/ik_solver.h"
+#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h"
-#include "ik_solver/include/urdf_parser.h"
+#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
 
 #include <pinocchio/multibody/model.hpp>
 #include <pinocchio/multibody/data.hpp>
@@ -11,6 +11,7 @@
 #include <Eigen/Core>
 
 #include <memory>
+#include <limits>
 #include <string>
 
 namespace cmvr {
@@ -70,6 +71,39 @@ public:
             const std::vector<double>& joints_angle,
             Eigen::Matrix4d& cur_pose);
 
+    int chainDof() const { return chain_q_dof_; }
+    int chainVelocityDof() const { return chain_v_dof_; }
+
+    bool computeJacobianBaseAtQ(const std::vector<double>& q_chain,
+                                bool is_tcp,
+                                Eigen::MatrixXd& jacobian_base,
+                                Eigen::Matrix3d& base_R_ee);
+
+    bool computeJacobianBaseAtQ(const std::vector<double>& q_chain,
+                                const std::string& ee_frame_name,
+                                Eigen::MatrixXd& jacobian_base,
+                                Eigen::Matrix3d& base_R_ee);
+
+    bool computeTwistBaseAtQ(const std::vector<double>& q_chain,
+                             const std::vector<double>& qdot_chain,
+                             bool is_tcp,
+                             Eigen::Matrix<double, 6, 1>& twist_base,
+                             Eigen::MatrixXd* jacobian_base_out = nullptr,
+                             Eigen::Matrix3d* base_R_ee_out = nullptr);
+
+    bool computeTwistBaseAtQ(const std::vector<double>& q_chain,
+                             const std::vector<double>& qdot_chain,
+                             const std::string& ee_frame_name,
+                             Eigen::Matrix<double, 6, 1>& twist_base,
+                             Eigen::MatrixXd* jacobian_base_out = nullptr,
+                             Eigen::Matrix3d* base_R_ee_out = nullptr);
+
+    virtual bool solveVelocityBase(const Eigen::MatrixXd& jacobian_base,
+                                   const Eigen::Matrix<double, 6, 1>& target_twist_base,
+                                   const std::vector<double>& q_chain,
+                                   std::vector<double>& qdot_out,
+                                   double qdot_abs_max = std::numeric_limits<double>::infinity()) const;
+
 protected:
     /**
      * @brief 从父类缓存的 URDF 链信息初始化 Pinocchio 运行时对象与 frame 索引。
@@ -96,6 +130,29 @@ protected:
                              Eigen::VectorXd& q_full,
                              const char* context) const;
 
+    bool buildFullQFromChain(const Eigen::VectorXd& q_chain,
+                             Eigen::VectorXd& q_full,
+                             const char* context) const;
+
+    Eigen::MatrixXd extractChainJacobian(
+        const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian_full) const;
+
+    const pinocchio::SE3& getBasePoseWorld() const;
+
+    bool buildJacobianBaseAtQ(const std::vector<double>& q_chain,
+                              pinocchio::FrameIndex ee_id,
+                              Eigen::MatrixXd& jacobian_base,
+                              Eigen::Matrix3d& base_R_ee,
+                              Eigen::VectorXd* q_full_out = nullptr);
+
+    bool computeMeasuredTwistBase(const std::vector<double>& q_chain,
+                                  const std::vector<double>& qdot_chain,
+                                  pinocchio::FrameIndex ee_id,
+                                  Eigen::Matrix<double, 6, 1>& twist_base,
+                                  Eigen::MatrixXd* jacobian_base_out = nullptr,
+                                  Eigen::Matrix3d* base_R_ee_out = nullptr,
+                                  Eigen::VectorXd* q_full_out = nullptr);
+
     /**
      * @brief 在当前 `model_` 上更新 FK 与 frame placement。
      */
@@ -129,6 +186,10 @@ protected:
     /** @brief 是否存在有效 TCP frame。 */
     bool has_tcp_{false};
 
+    /** @brief 配置链路 base 在 world 下的中性位姿缓存。 */
+    bool base_pose_cached_{false};
+    pinocchio::SE3 oM_base_cached_;
+
     /** @brief 当前链在 full-model `q` 中的起始索引。 */
     int chain_q_start_{0};
     /** @brief 当前链关节位置自由度数量。 */

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

@@ -7,41 +7,22 @@
 
 #pragma once
 
-#include "ik_solver/include/pinocchio_ik_base.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
-#include "common/math/include/qp_solver.h"
+#include "common/math/qp_solver.h"
 
 #include <Eigen/Core>
+#include <limits>
 #include <memory>
 #include <string>
 #include <vector>
 
-#include "cmvr/config/pinocchio_qp_ik_solver_config.pb.h"
+#include "cmvr/config/pinocchio_qp_ik_config.pb.h"
 
 namespace cmvr {
 
 class PinocchioQpIKSolver : public PinocchioIKBase {
 public:
-    /// urdf_path          : URDF 路径（可以是单臂，也可以是双臂整机）
+    explicit PinocchioQpIKSolver(const config::PinocchioQpIKConfig& config);
-    /// base_frame_name    : 作为 IK 基坐标系的 frame 名（例：PELVIS_S）
-    /// flange_frame_name  : 末端法兰 frame 名（例：L_FLANGE）
-    /// tcp_frame_name     : TCP frame 名（可为空；为空则 is_tcp=true 时也用法兰）
-    /// lambda             : 阻尼（cost 中的 √lambda * I）
-    /// w_posrot           : 位置 / 姿态误差权重（0~1，越大越重姿态）
-    /// max_iters          : 外层迭代上限
-    /// tol                : 误差收敛阈值（||任务误差||）
-    /// qp_time_limit      : 每次 QP 的时间限制（秒）
-    PinocchioQpIKSolver(const std::string &urdf_path,
-                        const std::string &base_frame_name,
-                        const std::string &flange_frame_name,
-                        const std::string &tcp_frame_name = std::string(),
-                        double lambda   = 1e-4,
-                        double w_posrot = 0.5,
-                        int    max_iters = 100,
-                        double tol       = 1e-6,
-                        double qp_time_limit = 1e-2);
-
-    PinocchioQpIKSolver();
-    PinocchioQpIKSolver(const config::PinocchioQpIKConfig &config);
 
     ~PinocchioQpIKSolver() override = default;
 
@@ -57,6 +38,12 @@ public:
             std::vector<double> &joints_angle,
             bool is_tcp = true) override;
 
+    bool solveVelocityBase(const Eigen::MatrixXd& jacobian_base,
+                           const Eigen::Matrix<double, 6, 1>& target_twist_base,
+                           const std::vector<double>& q_chain,
+                           std::vector<double>& qdot_out,
+                           double qdot_abs_max = std::numeric_limits<double>::infinity()) const override;
+
     /// 如你有更严格的速度 / 加速度限位，可以覆盖默认值
     void setVelocityLimits(const Eigen::VectorXd &qd_max);
     void setAccelerationLimits(const Eigen::VectorXd &qdd_max);

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

@@ -0,0 +1,473 @@
+// Created by lgv on 11/28/25.
+
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
+#include "common/base/logging/logger.h"
+#include "common/math/joint_limits.h"
+#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
+#include "common/config/config_files.h"
+
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/algorithm/jacobian.hpp>
+#include <pinocchio/spatial/explog.hpp>
+
+#include <Eigen/SVD>
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <unordered_map>
+
+namespace cmvr {
+
+using cmvr::common::config::positiveOr;
+
+Eigen::MatrixXd PinocchioDlsIKSolver::dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda) {
+    const int m = J.rows();
+    const int n = J.cols();
+    const double l2 = lambda * lambda;
+
+    if (m <= n) {
+        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m, m);
+        Eigen::MatrixXd JJt = J * J.transpose() + l2 * I;
+        return J.transpose() * JJt.inverse();
+    } else {
+        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n, n);
+        Eigen::MatrixXd JtJ = J.transpose() * J + l2 * I;
+        return JtJ.inverse() * J.transpose();
+    }
+}
+
+PinocchioDlsIKSolver::PinocchioDlsIKSolver(const config::PinocchioDlsIKConfig& cfg)
+    : PinocchioIKBase(cfg.urdf_path(),
+                      cfg.base_frame_name(),
+                      cfg.flange_frame_name(),
+                      cfg.tcp_frame_name())
+    , max_iters_(cfg.max_iters() > 0 ? cfg.max_iters() : 100)
+    , pos_eps_(cfg.pos_eps() > 0.0 ? cfg.pos_eps() : 1e-6)
+    , rot_eps_(cfg.rot_eps() > 0.0 ? cfg.rot_eps() : 1e-6)
+    , damping_(cfg.damping() > 0.0 ? cfg.damping() : 1e-4)
+    , config_(cfg)
+{
+    if (cfg.has_joint_limit_avoidance()) {
+        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) {
+    const auto source = cfg.has_joint_limits()
+                            ? cfg.joint_limits().source()
+                            : config::JOINT_LIMIT_SOURCE_URDF;
+    if (source == config::JOINT_LIMIT_SOURCE_UNKNOWN ||
+        source == config::JOINT_LIMIT_SOURCE_URDF) {
+        return true;
+    }
+
+    if (source != config::JOINT_LIMIT_SOURCE_CUSTOM) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] unsupported joint limit source";
+        return false;
+    }
+
+    std::vector<std::string> joint_names;
+    if (!getChainJointNames(joint_names) || joint_names.empty()) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] failed to get chain joint names for custom limits";
+        return false;
+    }
+
+    std::unordered_map<std::string, config::JointLimitConfig> custom_limits;
+    if (cfg.has_joint_limits()) {
+        custom_limits.reserve(static_cast<std::size_t>(cfg.joint_limits().joints_size()));
+        for (const auto& item : cfg.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_pos_lower_limits_.resize(dof);
+    joint_pos_upper_limits_.resize(dof);
+    joint_vel_limits_.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) << "[PinocchioDlsIKSolver] missing custom joint limit for " << joint_name;
+            return false;
+        }
+        const auto& limit = it->second;
+        if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
+            !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
+            limit.velocity() <= 0.0) {
+            CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] invalid custom joint limit for "
+                            << limit.joint_name();
+            return false;
+        }
+        joint_pos_lower_limits_[i] = limit.lower();
+        joint_pos_upper_limits_[i] = limit.upper();
+        joint_vel_limits_[i] = std::abs(limit.velocity());
+    }
+    return true;
+}
+
+Eigen::VectorXd PinocchioDlsIKSolver::computeJointLimitAvoidanceVelocity(
+    const Eigen::VectorXd& q_chain) const {
+    if (!limit_avoidance_enabled_ ||
+        limit_avoidance_gain_ <= 0.0 ||
+        chain_v_dof_ != chain_q_dof_ ||
+        q_chain.size() != chain_q_dof_ ||
+        joint_pos_lower_limits_.size() != chain_q_dof_ ||
+        joint_pos_upper_limits_.size() != chain_q_dof_) {
+        return Eigen::VectorXd::Zero(chain_v_dof_);
+    }
+
+    return cmvr::kinematics::computeJointLimitAvoidanceVelocity(
+        q_chain,
+        joint_pos_lower_limits_,
+        joint_pos_upper_limits_,
+        limit_avoidance_enabled_,
+        limit_avoidance_gain_,
+        limit_avoidance_margin_ratio_,
+        limit_avoidance_max_push_);
+}
+
+Eigen::VectorXd PinocchioDlsIKSolver::projectToNullspace(const Eigen::MatrixXd& J_pinv,
+                                                         const Eigen::MatrixXd& J,
+                                                         const Eigen::VectorXd& secondary) const {
+    if (secondary.size() != J.cols()) {
+        return Eigen::VectorXd::Zero(J.cols());
+    }
+    const Eigen::MatrixXd N =
+        Eigen::MatrixXd::Identity(J.cols(), J.cols()) - J_pinv * J;
+    return N * secondary;
+}
+
+bool PinocchioDlsIKSolver::init() {
+    UrdfParser::ChainInfo chain_info;
+    std::string err;
+    if (!initPinocchioFromUrdfChain(&chain_info, &err)) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] Failed to init pinocchio base: " << err;
+        return false;
+    }
+    if (!refreshJointLimits_(config_)) {
+        return false;
+    }
+
+    {
+        Eigen::VectorXd q0 = pinocchio::neutral(model_);
+        updateKinematics(q0);
+        oM_base_cached_ = data_->oMf[base_frame_id_];
+        base_pose_cached_ = true;
+    }
+
+    cur_joints_angle_.assign(chain_q_dof_, 0.0);
+
+    initialized_ = true;
+    CMVR_LOG(INFO) << "[PinocchioDlsIKSolver] Chain '" << chain_base_frame_name_ << "' -> '" << chain_tip_frame_name_
+              << "': q_start=" << chain_q_start_ << " q_dof=" << chain_q_dof_
+              << ", v_start=" << chain_v_start_ << " v_dof=" << chain_v_dof_
+              << ", nq=" << model_.nq << " nv=" << model_.nv;
+    CMVR_LOG(INFO) << "[PinocchioDlsIKSolver] Chain joint position limits (rad):";
+    for (const auto& seg : chain_info.joints) {
+        const int q_idx = seg.q_index;
+        const int nq = seg.nq;
+        const std::string& jname = seg.name;
+        if (nq <= 0) continue;
+
+        for (int k = 0; k < nq; ++k) {
+            const int qi = q_idx - chain_q_start_ + k;
+            if (qi < 0 || qi >= chain_q_dof_) continue;
+            if (nq == 1) {
+                CMVR_LOG(INFO) << "  - " << jname
+                          << ": [" << joint_pos_lower_limits_[qi] << ", " << joint_pos_upper_limits_[qi] << "]";
+            } else {
+                CMVR_LOG(INFO) << "  - " << jname << "[" << k << "]"
+                          << ": [" << joint_pos_lower_limits_[qi] << ", " << joint_pos_upper_limits_[qi] << "]";
+            }
+        }
+    }
+    CMVR_LOG(INFO) << "[PinocchioDlsIKSolver] base pose cached (constant)";
+    return true;
+}
+
+bool PinocchioDlsIKSolver::ik(const Eigen::Matrix4d &target_pose_base,
+                              std::vector<double> &joints_angle,
+                              bool is_tcp)
+{
+    const std::string& ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : chain_tip_frame_name_;
+    return ik(chain_base_frame_name_, ee_link, target_pose_base, joints_angle);
+}
+
+bool PinocchioDlsIKSolver::ik(const std::string& base_link,
+                              const std::string& ee_link,
+                              const Eigen::Matrix4d& target_pose,
+                              std::vector<double>& joints_angle) {
+    if (!initialized_) return false;
+    if ((int)cur_joints_angle_.size() != chain_q_dof_) return false;
+    if (!model_.existFrame(base_link)) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] base frame not found: " << base_link;
+        return false;
+    }
+    if (!model_.existFrame(ee_link)) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] ee frame not found: " << ee_link;
+        return false;
+    }
+    const pinocchio::FrameIndex base_frame_id = model_.getFrameId(base_link);
+    const pinocchio::FrameIndex ee_frame_id = model_.getFrameId(ee_link);
+
+    const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent;
+    const pinocchio::JointIndex base_joint = model_.frames[base_frame_id].parent;
+    const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent;
+    const pinocchio::JointIndex ee_joint = model_.frames[ee_frame_id].parent;
+
+    auto jointOnParentPath = [this](pinocchio::JointIndex from,
+                                    pinocchio::JointIndex target) {
+        if (target == 0) return true;
+        pinocchio::JointIndex j = from;
+        while (j != 0) {
+            if (j == target) return true;
+            j = model_.parents[j];
+        }
+        return false;
+    };
+
+    auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) {
+        return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint);
+    };
+
+    const bool base_on_branch = jointOnConfiguredBranch(base_joint);
+    const bool ee_on_branch = jointOnConfiguredBranch(ee_joint);
+    const bool base_is_ancestor_of_ee =
+        jointOnParentPath(ee_joint, base_joint);
+    if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] base/ee must be on configured single chain and base must be ancestor of ee";
+        return false;
+    }
+
+    const pinocchio::SE3 base_M_target = matrix4ToSE3(target_pose);
+
+    Eigen::VectorXd q_chain = Eigen::Map<Eigen::VectorXd>(cur_joints_angle_.data(), chain_q_dof_);
+
+    bool success = false;
+    for (int iter = 0; iter < max_iters_; ++iter) {
+        Eigen::VectorXd q_full;
+        if (!buildFullQFromChain(q_chain, q_full, "ik(base,ee)")) {
+            return false;
+        }
+        updateKinematics(q_full);
+
+        const pinocchio::SE3& oM_base =
+            (base_frame_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_frame_id];
+        const pinocchio::SE3 oM_target = oM_base * base_M_target;
+        const pinocchio::SE3 &oM_cur = data_->oMf[ee_frame_id];
+        pinocchio::SE3 dM = oM_cur.inverse() * oM_target;
+
+        Eigen::Matrix<double,6,1> err = pinocchio::log6(dM).toVector();
+        if (err.head<3>().norm() < pos_eps_ && err.tail<3>().norm() < rot_eps_) {
+            success = true;
+            break;
+        }
+
+        Eigen::Matrix<double,6,Eigen::Dynamic> J_full(6, model_.nv);
+        pinocchio::computeFrameJacobian(model_, *data_, q_full,
+                                        ee_frame_id,
+                                        pinocchio::ReferenceFrame::LOCAL,
+                                        J_full);
+        Eigen::MatrixXd J = extractChainJacobian(J_full);
+        Eigen::MatrixXd J_pinv = dampedPseudoInverse(J, damping_);
+
+        Eigen::VectorXd dq = J_pinv * err;
+        q_chain += dq;
+        q_chain = cmvr::kinematics::clampToJointPositionLimits(
+            q_chain,
+            joint_pos_lower_limits_,
+            joint_pos_upper_limits_);
+    }
+
+    if (!success) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] IK solve failed";
+        return false;
+    }
+
+    cur_joints_angle_.assign(q_chain.data(), q_chain.data() + q_chain.size());
+    joints_angle = cur_joints_angle_;
+    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,
+                              std::vector<double>& joints_vel,
+                              double damping,
+                              double qdot_abs_max)
+{
+    if (!initialized_) return false;
+    if (!model_.existFrame(base_link)) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] base frame not found: " << base_link;
+        return false;
+    }
+    if (ee_link.empty()) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] ee_frame_name is empty";
+        return false;
+    }
+    if (!model_.existFrame(ee_link)) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] frame not found: " << ee_link;
+        return false;
+    }
+
+    if (chain_v_dof_ <= 0) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] invalid chain_v_dof";
+        return false;
+    }
+
+    if (static_cast<int>(cur_joints_angle_.size()) != chain_q_dof_) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] ik(velocity) current joint state not initialized";
+        return false;
+    }
+
+    const pinocchio::FrameIndex base_id = model_.getFrameId(base_link);
+    const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
+
+    const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent;
+    const pinocchio::JointIndex base_joint = model_.frames[base_id].parent;
+    const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent;
+    const pinocchio::JointIndex ee_joint = model_.frames[ee_id].parent;
+
+    auto jointOnParentPath = [this](pinocchio::JointIndex from,
+                                    pinocchio::JointIndex target) {
+        if (target == 0) return true;
+        pinocchio::JointIndex j = from;
+        while (j != 0) {
+            if (j == target) return true;
+            j = model_.parents[j];
+        }
+        return false;
+    };
+    auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) {
+        return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint);
+    };
+    const bool base_on_branch = jointOnConfiguredBranch(base_joint);
+    const bool ee_on_branch = jointOnConfiguredBranch(ee_joint);
+    const bool base_is_ancestor_of_ee = jointOnParentPath(ee_joint, base_joint);
+    if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] base/ee must be on configured single chain and base must be ancestor of ee";
+        return false;
+    }
+
+    const Eigen::Map<const Eigen::VectorXd> q_chain(cur_joints_angle_.data(), chain_q_dof_);
+
+    Eigen::VectorXd q_full;
+    if (!buildFullQFromChain(q_chain, q_full, "ik(velocity)")) {
+        return false;
+    }
+
+    updateKinematics(q_full);
+
+    const pinocchio::SE3 &oM_base =
+        (base_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_id];
+    const pinocchio::SE3 &oM_ee   = data_->oMf[ee_id];
+
+    const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
+    const Eigen::Matrix3d R_be = base_M_ee.rotation();
+
+    Eigen::Matrix<double,6,1> twist_base;
+    twist_base.head<3>() = R_be * target_vel.head<3>();
+    twist_base.tail<3>() = R_be * target_vel.tail<3>();
+
+    Eigen::Matrix<double,6,Eigen::Dynamic> J_world(6, model_.nv);
+    pinocchio::computeFrameJacobian(model_, *data_, q_full,
+                                    ee_id,
+                                    pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
+                                    J_world);
+    Eigen::MatrixXd J = extractChainJacobian(J_world);
+    const Eigen::Matrix3d R_bo = oM_base.rotation().transpose();
+    J.topRows(3) = R_bo * J.topRows(3);
+    J.bottomRows(3) = R_bo * J.bottomRows(3);
+
+    const double lambda = (damping > 0.0) ? damping : damping_;
+    Eigen::Matrix<double,6,6> A = J * J.transpose();
+    A.diagonal().array() += (lambda * lambda);
+    Eigen::LDLT<Eigen::Matrix<double,6,6>> ldlt(A);
+    if (ldlt.info() != Eigen::Success) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] ik(velocity) LDLT failed";
+        return false;
+    }
+    Eigen::VectorXd qdot = J.transpose() * ldlt.solve(twist_base);
+
+    const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain);
+    if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
+        const Eigen::Matrix<double,6,6> A_inv =
+            ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
+        const Eigen::MatrixXd J_pinv = J.transpose() * A_inv;
+        qdot += projectToNullspace(J_pinv, J, qdot_avoid);
+    }
+
+    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];
+    }
+
+    return true;
+}
+
+bool PinocchioDlsIKSolver::solveVelocityBase(const Eigen::MatrixXd& jacobian_base,
+                                             const Eigen::Matrix<double,6,1>& target_twist_base,
+                                             const std::vector<double>& q_chain_std,
+                                             std::vector<double>& qdot_out,
+                                             const double qdot_abs_max) const
+{
+    if (jacobian_base.rows() != 6 || jacobian_base.cols() != chain_v_dof_) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] solveVelocityBase failed: jacobian size mismatch";
+        return false;
+    }
+    if (static_cast<int>(q_chain_std.size()) != chain_q_dof_) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] solveVelocityBase failed: q size mismatch";
+        return false;
+    }
+
+    Eigen::Matrix<double,6,6> A = jacobian_base * jacobian_base.transpose();
+    A.diagonal().array() += (damping_ * damping_);
+
+    Eigen::LDLT<Eigen::Matrix<double,6,6>> ldlt(A);
+    if (ldlt.info() != Eigen::Success) {
+        CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] solveVelocityBase failed: LDLT failed";
+        return false;
+    }
+
+    Eigen::VectorXd qdot = jacobian_base.transpose() * ldlt.solve(target_twist_base);
+
+    const Eigen::Map<const Eigen::VectorXd> q_chain(q_chain_std.data(), chain_q_dof_);
+    const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain);
+    if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
+        const Eigen::Matrix<double,6,6> A_inv =
+            ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
+        const Eigen::MatrixXd J_pinv = jacobian_base.transpose() * A_inv;
+        qdot += projectToNullspace(J_pinv, jacobian_base, qdot_avoid);
+    }
+
+    qdot = cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max);
+    qdot_out.resize(chain_v_dof_);
+    for (int i = 0; i < chain_v_dof_; ++i) {
+        qdot_out[i] = qdot[i];
+    }
+    return true;
+}
+
+} // namespace cmvr

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

@@ -0,0 +1,330 @@
+// Created by Codex on 2026/3/3.
+
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
+#include "common/base/logging/logger.h"
+
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/jacobian.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+
+namespace cmvr {
+
+PinocchioIKBase::PinocchioIKBase(const std::string& urdf_path,
+                                 const std::string& base_frame_name,
+                                 const std::string& flange_frame_name,
+                                 const std::string& tcp_frame_name)
+    : IKSolver(urdf_path, base_frame_name, flange_frame_name)
+    , tcp_frame_name_(tcp_frame_name) {
+}
+
+PinocchioIKBase::PinocchioIKBase(std::shared_ptr<const UrdfParser> parser,
+                                 const std::string& base_frame_name,
+                                 const std::string& flange_frame_name,
+                                 const std::string& tcp_frame_name)
+    : IKSolver(parser, base_frame_name, flange_frame_name)
+    , tcp_frame_name_(tcp_frame_name) {
+}
+
+bool PinocchioIKBase::initPinocchioFromUrdfChain(UrdfParser::ChainInfo* chain_info_out,
+                                                  std::string* error) {
+    if (!urdf_parser_ || !urdf_parser_->loaded()) {
+        if (error) *error = "urdf parser not initialized";
+        return false;
+    }
+    if (chain_base_frame_name_.empty() || chain_tip_frame_name_.empty()) {
+        if (error) *error = "chain base/tip frame name is empty";
+        return false;
+    }
+    if (!urdf_chain_cached_) {
+        if (!initUrdfChain(urdf_parser_, chain_base_frame_name_, chain_tip_frame_name_)) {
+            if (error) *error = "initUrdfChain failed";
+            return false;
+        }
+    }
+
+    model_ = urdf_parser_->model();
+    data_ = std::make_unique<pinocchio::Data>(model_);
+
+    UrdfParser::ChainInfo chain_info;
+    std::string chain_err;
+    if (!urdf_parser_->extractChain(chain_base_frame_name_, chain_tip_frame_name_, chain_info, &chain_err)) {
+        if (error) *error = chain_err;
+        return false;
+    }
+
+    base_frame_id_ = chain_info.base_frame_id;
+    flange_frame_id_ = chain_info.tip_frame_id;
+    chain_q_start_ = chain_info.q_start;
+    chain_q_dof_ = chain_info.q_dof;
+    chain_v_start_ = chain_info.v_start;
+    chain_v_dof_ = chain_info.v_dof;
+
+    has_tcp_ = false;
+    tcp_frame_id_ = (pinocchio::FrameIndex)(-1);
+    if (!tcp_frame_name_.empty() && model_.existFrame(tcp_frame_name_)) {
+        tcp_frame_id_ = model_.getFrameId(tcp_frame_name_);
+        has_tcp_ = true;
+    }
+
+    if (chain_info_out != nullptr) {
+        *chain_info_out = chain_info;
+    }
+    return true;
+}
+
+bool PinocchioIKBase::buildFullQFromInput(const std::vector<double>& joints,
+                                          Eigen::VectorXd& q_full,
+                                          const char* context) const {
+    const int size = static_cast<int>(joints.size());
+    if (size != chain_q_dof_ && size != model_.nq) {
+        if (context != nullptr) {
+            CMVR_LOG(ERROR) << "[PinocchioIKBase] " << context << " joints size mismatch";
+        }
+        return false;
+    }
+
+    q_full = pinocchio::neutral(model_);
+    if (size == model_.nq) {
+        q_full = Eigen::Map<const Eigen::VectorXd>(joints.data(), model_.nq);
+    } else {
+        Eigen::Map<const Eigen::VectorXd> q_chain(joints.data(), chain_q_dof_);
+        q_full.segment(chain_q_start_, chain_q_dof_) = q_chain;
+    }
+    return true;
+}
+
+bool PinocchioIKBase::buildFullQFromChain(const Eigen::VectorXd& q_chain,
+                                          Eigen::VectorXd& q_full,
+                                          const char* context) const {
+    if (q_chain.size() != chain_q_dof_) {
+        if (context != nullptr) {
+            CMVR_LOG(ERROR) << "[PinocchioIKBase] " << context << " chain q size mismatch";
+        }
+        return false;
+    }
+    q_full = pinocchio::neutral(model_);
+    q_full.segment(chain_q_start_, chain_q_dof_) = q_chain;
+    return true;
+}
+
+Eigen::MatrixXd PinocchioIKBase::extractChainJacobian(
+    const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian_full) const {
+    return jacobian_full.middleCols(chain_v_start_, chain_v_dof_);
+}
+
+const pinocchio::SE3& PinocchioIKBase::getBasePoseWorld() const {
+    return base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_];
+}
+
+void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) {
+    pinocchio::forwardKinematics(model_, *data_, q_full);
+    pinocchio::updateFramePlacements(model_, *data_);
+}
+
+bool PinocchioIKBase::buildJacobianBaseAtQ(const std::vector<double>& q_chain_std,
+                                           const pinocchio::FrameIndex ee_id,
+                                           Eigen::MatrixXd& jacobian_base,
+                                           Eigen::Matrix3d& base_R_ee,
+                                           Eigen::VectorXd* q_full_out)
+{
+    if (static_cast<int>(q_chain_std.size()) != chain_q_dof_) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] buildJacobianBaseAtQ: q size mismatch";
+        return false;
+    }
+
+    const Eigen::Map<const Eigen::VectorXd> q_chain(q_chain_std.data(), chain_q_dof_);
+
+    Eigen::VectorXd q_full;
+    if (!buildFullQFromChain(q_chain, q_full, "buildJacobianBaseAtQ")) {
+        return false;
+    }
+
+    updateKinematics(q_full);
+
+    const pinocchio::SE3& oM_base = getBasePoseWorld();
+    const pinocchio::SE3& oM_ee = data_->oMf[ee_id];
+    const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
+    base_R_ee = base_M_ee.rotation();
+
+    Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_world(6, model_.nv);
+    pinocchio::computeFrameJacobian(model_, *data_, q_full,
+                                    ee_id,
+                                    pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
+                                    jacobian_world);
+
+    jacobian_base = extractChainJacobian(jacobian_world);
+
+    const Eigen::Matrix3d R_bo = oM_base.rotation().transpose();
+    jacobian_base.topRows(3) = R_bo * jacobian_base.topRows(3);
+    jacobian_base.bottomRows(3) = R_bo * jacobian_base.bottomRows(3);
+
+    if (q_full_out != nullptr) {
+        *q_full_out = q_full;
+    }
+
+    return true;
+}
+
+bool PinocchioIKBase::computeJacobianBaseAtQ(const std::vector<double>& q_chain,
+                                             const bool is_tcp,
+                                             Eigen::MatrixXd& jacobian_base,
+                                             Eigen::Matrix3d& base_R_ee)
+{
+    const pinocchio::FrameIndex ee_id = (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_;
+    return buildJacobianBaseAtQ(q_chain, ee_id, jacobian_base, base_R_ee, nullptr);
+}
+
+bool PinocchioIKBase::computeJacobianBaseAtQ(const std::vector<double>& q_chain,
+                                             const std::string& ee_frame_name,
+                                             Eigen::MatrixXd& jacobian_base,
+                                             Eigen::Matrix3d& base_R_ee)
+{
+    if (!model_.existFrame(ee_frame_name)) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] ee frame not found: " << ee_frame_name;
+        return false;
+    }
+    return buildJacobianBaseAtQ(q_chain,
+                                model_.getFrameId(ee_frame_name),
+                                jacobian_base,
+                                base_R_ee,
+                                nullptr);
+}
+
+bool PinocchioIKBase::computeMeasuredTwistBase(const std::vector<double>& q_chain,
+                                               const std::vector<double>& qdot_chain,
+                                               const pinocchio::FrameIndex ee_id,
+                                               Eigen::Matrix<double, 6, 1>& twist_base,
+                                               Eigen::MatrixXd* jacobian_base_out,
+                                               Eigen::Matrix3d* base_R_ee_out,
+                                               Eigen::VectorXd* q_full_out)
+{
+    if (static_cast<int>(qdot_chain.size()) != chain_v_dof_) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] computeMeasuredTwistBase: qdot size mismatch";
+        return false;
+    }
+
+    Eigen::MatrixXd jacobian_base;
+    Eigen::Matrix3d base_R_ee = Eigen::Matrix3d::Identity();
+    if (!buildJacobianBaseAtQ(q_chain, ee_id, jacobian_base, base_R_ee, q_full_out)) {
+        return false;
+    }
+
+    const Eigen::Map<const Eigen::VectorXd> qdot(qdot_chain.data(), chain_v_dof_);
+    twist_base = jacobian_base * qdot;
+
+    if (jacobian_base_out != nullptr) {
+        *jacobian_base_out = jacobian_base;
+    }
+    if (base_R_ee_out != nullptr) {
+        *base_R_ee_out = base_R_ee;
+    }
+
+    return true;
+}
+
+bool PinocchioIKBase::computeTwistBaseAtQ(const std::vector<double>& q_chain,
+                                          const std::vector<double>& qdot_chain,
+                                          const bool is_tcp,
+                                          Eigen::Matrix<double, 6, 1>& twist_base,
+                                          Eigen::MatrixXd* jacobian_base_out,
+                                          Eigen::Matrix3d* base_R_ee_out)
+{
+    const pinocchio::FrameIndex ee_id = (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_;
+    return computeMeasuredTwistBase(q_chain,
+                                    qdot_chain,
+                                    ee_id,
+                                    twist_base,
+                                    jacobian_base_out,
+                                    base_R_ee_out,
+                                    nullptr);
+}
+
+bool PinocchioIKBase::computeTwistBaseAtQ(const std::vector<double>& q_chain,
+                                          const std::vector<double>& qdot_chain,
+                                          const std::string& ee_frame_name,
+                                          Eigen::Matrix<double, 6, 1>& twist_base,
+                                          Eigen::MatrixXd* jacobian_base_out,
+                                          Eigen::Matrix3d* base_R_ee_out)
+{
+    if (!model_.existFrame(ee_frame_name)) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] ee frame not found: " << ee_frame_name;
+        return false;
+    }
+    return computeMeasuredTwistBase(q_chain,
+                                    qdot_chain,
+                                    model_.getFrameId(ee_frame_name),
+                                    twist_base,
+                                    jacobian_base_out,
+                                    base_R_ee_out,
+                                    nullptr);
+}
+
+bool PinocchioIKBase::solveVelocityBase(const Eigen::MatrixXd& jacobian_base,
+                                        const Eigen::Matrix<double, 6, 1>& target_twist_base,
+                                        const std::vector<double>& q_chain,
+                                        std::vector<double>& qdot_out,
+                                        const double qdot_abs_max) const
+{
+    (void)jacobian_base;
+    (void)target_twist_base;
+    (void)q_chain;
+    (void)qdot_out;
+    (void)qdot_abs_max;
+    CMVR_LOG(ERROR) << "[PinocchioIKBase] solveVelocityBase is not implemented by this solver";
+    return false;
+}
+
+bool PinocchioIKBase::fk(const std::vector<double>& joints_angle,
+                         Eigen::Matrix4d& cur_pose,
+                         bool is_tcp) {
+    const std::string& ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : chain_tip_frame_name_;
+    return fk(chain_base_frame_name_, ee_link, joints_angle, cur_pose);
+}
+
+bool PinocchioIKBase::fk(const std::string& base_link,
+                         const std::string& ee_link,
+                         const std::vector<double>& joints_angle,
+                         Eigen::Matrix4d& cur_pose) {
+    if (!data_) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] fk called before pinocchio init";
+        return false;
+    }
+    if (!model_.existFrame(base_link)) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] base frame not found: " << base_link;
+        return false;
+    }
+    if (!model_.existFrame(ee_link)) {
+        CMVR_LOG(ERROR) << "[PinocchioIKBase] ee frame not found: " << ee_link;
+        return false;
+    }
+
+    Eigen::VectorXd q_full;
+    if (!buildFullQFromInput(joints_angle, q_full, "fk")) {
+        return false;
+    }
+    updateKinematics(q_full);
+
+    const pinocchio::FrameIndex base_id = model_.getFrameId(base_link);
+    const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
+    const pinocchio::SE3& oM_base = data_->oMf[base_id];
+    const pinocchio::SE3& oM_ee = data_->oMf[ee_id];
+    const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
+    cur_pose = se3ToMatrix4(base_M_ee);
+    return true;
+}
+
+pinocchio::SE3 PinocchioIKBase::matrix4ToSE3(const Eigen::Matrix4d& T) {
+    pinocchio::SE3 M;
+    M.rotation() = T.block<3,3>(0,0);
+    M.translation() = T.block<3,1>(0,3);
+    return M;
+}
+
+Eigen::Matrix4d PinocchioIKBase::se3ToMatrix4(const pinocchio::SE3& M) {
+    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+    T.block<3,3>(0,0) = M.rotation();
+    T.block<3,1>(0,3) = M.translation();
+    return T;
+}
+
+} // namespace cmvr

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

@@ -2,79 +2,40 @@
 // Created by lgv on 12/2/25.
 //
 
-#include "ik_solver/include/pinocchio_qp_ik_solver.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h"
+#include "common/base/logging/logger.h"
+#include "common/math/joint_limits.h"
+#include "common/config/config_files.h"
 
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/algorithm/jacobian.hpp>
 #include <pinocchio/spatial/explog.hpp>
 
-#include <iostream>
 #include <algorithm>   // std::clamp, std::max, std::min
 #include <cmath>       // std::sqrt
-#include "common/utils/config_helper/include/config_helper.h"
 
 namespace cmvr {
     using Eigen::Matrix4d;
     using Eigen::VectorXd;
     using Eigen::MatrixXd;
-
+    using cmvr::common::config::positiveIntOr;
-    PinocchioQpIKSolver::PinocchioQpIKSolver(const std::string &urdf_path,
+    using cmvr::common::config::positiveOr;
-                                             const std::string &base_frame_name,
-                                             const std::string &flange_frame_name,
-                                             const std::string &tcp_frame_name,
-                                             double lambda,
-                                             double w_posrot,
-                                             int max_iters,
-                                             double tol,
-                                             double qp_time_limit)
-        : PinocchioIKBase(urdf_path, base_frame_name, flange_frame_name, tcp_frame_name)
-          , urdf_path_(urdf_path)
-          , lambda_(lambda)
-          , w_posrot_(w_posrot)
-          , max_iters_(max_iters)
-          , tol_(tol)
-          , qp_time_limit_(qp_time_limit)
-          , solver_() {
-    }
-
-    PinocchioQpIKSolver::PinocchioQpIKSolver()
-        : PinocchioIKBase("", "", "", "")
-        , solver_() {
-        config::PinocchioQpIKConfig config;
-        if (ConfigHelper::getPinocchioQpIkSolverConfig(config))
-        {
-            urdf_path_ = config.urdf_path();
-            chain_base_frame_name_ = config.base_frame_name();
-            chain_tip_frame_name_ = config.flange_frame_name();
-            tcp_frame_name_ = config.tcp_frame_name();
-            lambda_ = config.lambda();
-            w_posrot_ = config.w_posrot();
-            max_iters_ = config.max_iters();
-            tol_ = config.tol();
-            qp_time_limit_ = config.qp_time_limit();
-            if (!urdf_path_.empty() &&
-                !chain_base_frame_name_.empty() &&
-                !chain_tip_frame_name_.empty()) {
-                initUrdfChain(urdf_path_, chain_base_frame_name_, chain_tip_frame_name_);
-            }
-        }
-    }
 
     PinocchioQpIKSolver::PinocchioQpIKSolver(const config::PinocchioQpIKConfig& config)
         : PinocchioIKBase(config.urdf_path(),
                           config.base_frame_name(),
                           config.flange_frame_name(),
                           config.tcp_frame_name())
-        , config_(config)
+          , config_(config)
-        , solver_()
+          , urdf_path_(config.urdf_path())
+          , lambda_(positiveOr(config.lambda(), 1e-4))
+          , w_posrot_(positiveOr(config.w_posrot(), 0.5))
+          , max_iters_(positiveIntOr(config.max_iters(), 100))
+          , tol_(positiveOr(config.tol(), 1e-6))
+          , qp_time_limit_(positiveOr(config.qp_time_limit(), 1e-2))
+          , solver_()
     {
-        urdf_path_ = config.urdf_path();
-        lambda_ = config.lambda();
-        w_posrot_ = config.w_posrot();
-        max_iters_ = config.max_iters();
-        tol_ = config.tol();
-        qp_time_limit_ = config.qp_time_limit();
     }
 
     bool PinocchioQpIKSolver::init() {
@@ -82,11 +43,11 @@ namespace cmvr {
             if (urdf_path_.empty() ||
                 chain_base_frame_name_.empty() ||
                 chain_tip_frame_name_.empty()) {
-                std::cerr << "[PinocchioQpIKSolver] missing urdf/base/flange config before init().\n";
+                CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] missing urdf/base/flange config before init().";
                 return false;
             }
             if (!initUrdfChain(urdf_path_, chain_base_frame_name_, chain_tip_frame_name_)) {
-                std::cerr << "[PinocchioQpIKSolver] Failed to cache URDF chain metadata in IKSolver.\n";
+                CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] Failed to cache URDF chain metadata in IKSolver.";
                 return false;
             }
         }
@@ -94,18 +55,18 @@ namespace cmvr {
         UrdfParser::ChainInfo chain_info;
         std::string err;
         if (!initPinocchioFromUrdfChain(&chain_info, &err)) {
-            std::cerr << "[PinocchioQpIKSolver] Failed to init pinocchio base: " << err << std::endl;
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] Failed to init pinocchio base: " << err;
             return false;
         }
 
         if (chain_q_dof_ <= 0 || chain_v_dof_ <= 0) {
-            std::cerr << "[PinocchioQpIKSolver] Invalid chain dof: q=" << chain_q_dof_
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] Invalid chain dof: q=" << chain_q_dof_
-                      << " v=" << chain_v_dof_ << std::endl;
+                      << " v=" << chain_v_dof_;
             return false;
         }
         if (chain_q_dof_ != chain_v_dof_) {
-            std::cerr << "[PinocchioQpIKSolver] requires q_dof == v_dof, got q="
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] requires q_dof == v_dof, got q="
-                      << chain_q_dof_ << " v=" << chain_v_dof_ << std::endl;
+                      << chain_q_dof_ << " v=" << chain_v_dof_;
             return false;
         }
 
@@ -114,14 +75,14 @@ namespace cmvr {
         int expected_v = chain_v_start_;
         for (const auto& seg : chain_info.joints) {
             if (seg.nq != 1 || seg.nv != 1) {
-                std::cerr << "[PinocchioQpIKSolver] only supports nq=1,nv=1 joints. joint="
+                CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] only supports nq=1,nv=1 joints. joint="
-                          << seg.name << " nq=" << seg.nq << " nv=" << seg.nv << std::endl;
+                          << seg.name << " nq=" << seg.nq << " nv=" << seg.nv;
                 return false;
             }
             if (seg.q_index != expected_q || seg.v_index != expected_v) {
-                std::cerr << "[PinocchioQpIKSolver] chain q/v index must be contiguous. joint="
+                CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] chain q/v index must be contiguous. joint="
                           << seg.name << " q_index=" << seg.q_index << " expect_q=" << expected_q
-                          << " v_index=" << seg.v_index << " expect_v=" << expected_v << std::endl;
+                          << " v_index=" << seg.v_index << " expect_v=" << expected_v;
                 return false;
             }
             expected_q += seg.nq;
@@ -130,7 +91,7 @@ namespace cmvr {
 
         if (joint_pos_lower_limits_.size() != chain_q_dof_ ||
             joint_pos_upper_limits_.size() != chain_q_dof_) {
-            std::cerr << "[PinocchioQpIKSolver] position limits size mismatch with q_dof." << std::endl;
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] position limits size mismatch with q_dof.";
             return false;
         }
 
@@ -148,13 +109,13 @@ namespace cmvr {
         }
 
         initialized_ = true;
-        std::cout << "[PinocchioQpIKSolver] Init OK. full nq = " << model_.nq
+        CMVR_LOG(INFO) << "[PinocchioQpIKSolver] Init OK. full nq = " << model_.nq
                 << ", active dof = " << chain_q_dof_
                 << ", q_start=" << chain_q_start_ << ", v_start=" << chain_v_start_
                 << ", base frame = " << chain_base_frame_name_
                 << ", flange frame = " << chain_tip_frame_name_
                 << ", tcp frame = " << (has_tcp_ ? tcp_frame_name_ : "<none>")
-                << std::endl;
+               ;
         return true;
     }
 
@@ -162,8 +123,8 @@ namespace cmvr {
         if (qd_max.size() == chain_v_dof_) {
             joint_vel_limits_ = qd_max;
         } else {
-            std::cerr << "[PinocchioQpIKSolver] setVelocityLimits size mismatch. got "
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] setVelocityLimits size mismatch. got "
-                    << qd_max.size() << ", expect " << chain_v_dof_ << std::endl;
+                    << qd_max.size() << ", expect " << chain_v_dof_;
         }
     }
 
@@ -171,8 +132,8 @@ namespace cmvr {
         if (qdd_max.size() == chain_q_dof_) {
             qdd_max_global_ = qdd_max;
         } else {
-            std::cerr << "[PinocchioQpIKSolver] setAccelerationLimits size mismatch. got "
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] setAccelerationLimits size mismatch. got "
-                    << qdd_max.size() << ", expect " << chain_q_dof_ << std::endl;
+                    << qdd_max.size() << ", expect " << chain_q_dof_;
         }
     }
 
@@ -180,12 +141,12 @@ namespace cmvr {
                                  std::vector<double> &joints_angle,
                                  bool is_tcp) {
         if (!initialized_) {
-            std::cerr << "[PinocchioQpIKSolver] IK called before init()." << std::endl;
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] IK called before init().";
             return false;
         }
         if (cur_joints_angle_.size() != static_cast<std::size_t>(chain_q_dof_)) {
-            std::cerr << "[PinocchioQpIKSolver] cur_joints_angle_ size mismatch: "
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] cur_joints_angle_ size mismatch: "
-                    << cur_joints_angle_.size() << " vs dof = " << chain_q_dof_ << std::endl;
+                    << cur_joints_angle_.size() << " vs dof = " << chain_q_dof_;
             return false;
         }
 
@@ -195,9 +156,9 @@ namespace cmvr {
             target_frame_id = tcp_frame_id_;
         } else {
             if (is_tcp && !has_tcp_) {
-                std::cerr << "[PinocchioQpIKSolver] is_tcp=true but no TCP frame, "
+                CMVR_LOG(WARNING) << "[PinocchioQpIKSolver] is_tcp=true but no TCP frame, "
                         "fallback to flange."
-                        << std::endl;
+                       ;
             }
             target_frame_id = flange_frame_id_;
         }
@@ -301,19 +262,17 @@ namespace cmvr {
             try {
                 dq_local = solver_.Solve();
             } catch (const QPSolverException &e) {
-                std::cerr << "[PinocchioQpIKSolver] QP failed: " << e.what()
+                CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] QP failed: " << e.what()
-                        << " (code=" << e.code() << ")" << std::endl;
+                        << " (code=" << e.code() << ")";
                 return false;
             } catch (const std::exception &e) {
-                std::cerr << "[PinocchioQpIKSolver] QP failed: " << e.what() << std::endl;
+                CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] QP failed: " << e.what();
                 return false;
             }
 
             // 5) 更新 q_local 并写回 q_full
             q_local += dq_local;
-            for (int i = 0; i < chain_q_dof_; ++i) {
+            q_local = cmvr::kinematics::clampToJointPositionLimits(q_local, q_min, q_max);
-                q_local(i) = std::clamp(q_local(i), q_min(i), q_max(i));
-            }
             q_full.segment(chain_q_start_, chain_q_dof_) = q_local;
 
             // 6) 收敛检查
@@ -336,5 +295,96 @@ namespace cmvr {
         return false;
     }
 
+    bool PinocchioQpIKSolver::solveVelocityBase(
+        const MatrixXd& jacobian_base,
+        const Eigen::Matrix<double, 6, 1>& target_twist_base,
+        const std::vector<double>& q_chain_std,
+        std::vector<double>& qdot_out,
+        const double qdot_abs_max) const
+    {
+        if (!initialized_) {
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase called before init().";
+            return false;
+        }
+        if (jacobian_base.rows() != 6 || jacobian_base.cols() != chain_v_dof_) {
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase jacobian size mismatch.";
+            return false;
+        }
+        if (static_cast<int>(q_chain_std.size()) != chain_q_dof_ || chain_q_dof_ != chain_v_dof_) {
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase q size mismatch.";
+            return false;
+        }
+
+        const int dof = chain_v_dof_;
+        MatrixXd cost(6 + dof, dof);
+        VectorXd target(6 + dof);
+        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);
+
+        VectorXd lower(dof);
+        VectorXd upper(dof);
+        const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof);
+        for (int i = 0; i < dof; ++i) {
+            double limit = std::numeric_limits<double>::infinity();
+            if (joint_vel_limits_.size() == dof) {
+                const double joint_limit = std::abs(joint_vel_limits_[i]);
+                if (std::isfinite(joint_limit) && joint_limit > 0.0) {
+                    limit = std::min(limit, joint_limit);
+                }
+            }
+            if (std::isfinite(qdot_abs_max) && qdot_abs_max > 0.0) {
+                limit = std::min(limit, qdot_abs_max);
+            }
+            if (!std::isfinite(limit) || limit <= 0.0) {
+                limit = 1e6;
+            }
+
+            lower[i] = -limit;
+            upper[i] = limit;
+
+            if (joint_pos_lower_limits_.size() == dof &&
+                joint_pos_upper_limits_.size() == dof) {
+                const double q_min = joint_pos_lower_limits_[i];
+                const double q_max = joint_pos_upper_limits_[i];
+                if (std::isfinite(q_min) && std::isfinite(q_max) && q_max > q_min) {
+                    constexpr double kLimitEps = 1e-6;
+                    if (q_chain[i] <= q_min + kLimitEps) {
+                        lower[i] = std::max(0.0, lower[i]);
+                    }
+                    if (q_chain[i] >= q_max - kLimitEps) {
+                        upper[i] = std::min(0.0, upper[i]);
+                    }
+                }
+            }
+        }
+
+        QPSolver solver;
+        solver.Setup(dof, dof, qp_time_limit_);
+        solver.SetCostFunction(cost, target);
+        solver.SetConstraintsFunction(MatrixXd::Identity(dof, dof), lower, upper);
+
+        VectorXd qdot;
+        try {
+            qdot = solver.Solve();
+        } catch (const QPSolverException& e) {
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase QP failed: "
+                      << e.what() << " (code=" << e.code() << ")";
+            return false;
+        } catch (const std::exception& e) {
+            CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] solveVelocityBase QP failed: "
+                      << e.what();
+            return false;
+        }
+
+        if (qdot.size() != dof) {
+            return false;
+        }
+        qdot_out.assign(qdot.data(), qdot.data() + qdot.size());
+        return true;
+    }
+
 
 } // namespace cmvr

cmvr-es/algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include "cmvr/config/srs_ik_config.pb.h"
+#include "algorithms/kinematics/ik_solver/common/include/ik_solver.h"
+
+namespace cmvr {
+
+class SrsIKSolver : public IKSolver {
+public:
+    enum ConfigDirection {
+        OUTWARD = 1,
+        INWARD = -1
+    };
+
+    explicit SrsIKSolver(const config::SrsIKConfig& cfg);
+    ~SrsIKSolver() override = default;
+
+    bool init() override;
+
+    bool ik(const Eigen::Matrix4d& target_pose,
+            std::vector<double>& joints_angle,
+            bool is_tcp = true) override;
+
+    bool fk(const std::vector<double>& joints_angle,
+            Eigen::Matrix4d& cur_pose,
+            bool is_tcp = true) override;
+
+    bool ikWithPsi(const Eigen::Matrix4d& pose,
+                   std::vector<double>& joints,
+                   double psi);
+
+    Eigen::Matrix4d calc_total_transform(const std::vector<double>& joint_angles);
+
+    bool cal_coefficient_matrix(const Eigen::Matrix4d& pose,
+                                Eigen::MatrixXd& s_mat,
+                                Eigen::MatrixXd& w_mat);
+
+    void setPsi(double psi) { psi_ = psi; }
+    double psi() const { return psi_; }
+
+    void set_shoulder_config(ConfigDirection value) { shoulder_config_ = value; }
+    void set_elbow_config(ConfigDirection value) { elbow_config_ = value; }
+    void set_wrist_config(ConfigDirection value) { wrist_config_ = value; }
+
+    int get_shoulder_config() const { return static_cast<int>(shoulder_config_); }
+    int get_elbow_config() const { return static_cast<int>(elbow_config_); }
+    int get_wrist_config() const { return static_cast<int>(wrist_config_); }
+
+    std::vector<std::pair<double, double>> get_joints_limits() const { return joints_limits_; }
+
+private:
+    static ConfigDirection toConfigDirection_(int value);
+
+    Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S,
+                                    const Eigen::Vector3d& W);
+    Eigen::Matrix3d calc_rotation_matrix(const Eigen::Vector3d& rotation_axis,
+                                         double rotation_angle);
+    Eigen::Matrix4d calc_dh(double d, double alpha, double a, double theta);
+
+private:
+    config::SrsIKConfig cfg_;
+    double psi_{0.0};
+
+    ConfigDirection shoulder_config_{OUTWARD};
+    ConfigDirection elbow_config_{OUTWARD};
+    ConfigDirection wrist_config_{OUTWARD};
+
+    Eigen::VectorXd link_lengths_;
+    Eigen::MatrixXd dh_params_;
+
+    double d_bs_{0.0};
+    double d_se_{0.0};
+    double d_ew_{0.0};
+    double d_wt_{0.0};
+
+    std::vector<std::pair<double, double>> joints_limits_{};
+};
+
+} // namespace cmvr

cmvr-es/algorithms/kinematics/ik_solver/srs/src/srs_ik_solver.cpp

@@ -0,0 +1,339 @@
+#include "algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h"
+
+#include <algorithm>
+#include <cmath>
+#include "common/base/logging/logger.h"
+
+#include "common/base/constants.h"
+#include "common/math/support_functions.h"
+
+namespace cmvr {
+
+SrsIKSolver::SrsIKSolver(const config::SrsIKConfig& cfg)
+    : IKSolver("", "", "")
+    , cfg_(cfg)
+{
+}
+
+bool SrsIKSolver::init()
+{
+    psi_ = cfg_.initial_psi();
+    shoulder_config_ = toConfigDirection_(cfg_.shoulder_config());
+    elbow_config_ = toConfigDirection_(cfg_.elbow_config());
+    wrist_config_ = toConfigDirection_(cfg_.wrist_config());
+
+    link_lengths_ = Eigen::VectorXd(4);
+    if (cfg_.link_lengths_size() == 4) {
+        for (int i = 0; i < 4; ++i) {
+            link_lengths_[i] = cfg_.link_lengths(i);
+        }
+    } else {
+        link_lengths_ << 0.0945 + 0.0765, 0.1475 + 0.1025, 0.0965 + 0.1525, 0.03;
+    }
+
+    const double half_pi = M_PI / 2.0;
+    dh_params_ = Eigen::MatrixXd(7, 4);
+    dh_params_ << link_lengths_[0], -half_pi, 0.0, 0.0,
+            0.0, half_pi, 0.0, 0.0,
+            link_lengths_[1], -half_pi, 0.0, 0.0,
+            0.0, half_pi, 0.0, 0.0,
+            link_lengths_[2], -half_pi, 0.0, half_pi,
+            0.0, half_pi, 0.0, half_pi,
+            link_lengths_[3], 0.0, 0.0, 0.0;
+
+    d_bs_ = link_lengths_[0];
+    d_se_ = link_lengths_[1];
+    d_ew_ = link_lengths_[2];
+    d_wt_ = link_lengths_[3];
+
+    joints_limits_.clear();
+    if (cfg_.joint_lower_limits_size() == 7 && cfg_.joint_upper_limits_size() == 7) {
+        joints_limits_.reserve(7);
+        for (int i = 0; i < 7; ++i) {
+            joints_limits_.emplace_back(cfg_.joint_lower_limits(i), cfg_.joint_upper_limits(i));
+        }
+    } else {
+        joints_limits_ = {
+            {-M_PI, M_PI},
+            {-0.78, 1.57},
+            {-M_PI, M_PI},
+            {0.0, 2.05},
+            {-M_PI, M_PI},
+            {-0.78, 0.78},
+            {-0.26, 1.57},
+        };
+    }
+
+    return true;
+}
+
+bool SrsIKSolver::ik(const Eigen::Matrix4d& target_pose,
+                     std::vector<double>& joints_angle,
+                     bool is_tcp)
+{
+    (void)is_tcp;
+    return ikWithPsi(target_pose, joints_angle, psi_);
+}
+
+bool SrsIKSolver::fk(const std::vector<double>& joints_angle,
+                     Eigen::Matrix4d& cur_pose,
+                     bool is_tcp)
+{
+    (void)is_tcp;
+    if (joints_angle.size() < 7) {
+        return false;
+    }
+    cur_pose = calc_total_transform(joints_angle);
+    return true;
+}
+
+SrsIKSolver::ConfigDirection SrsIKSolver::toConfigDirection_(const int value)
+{
+    return value < 0 ? INWARD : OUTWARD;
+}
+
+Eigen::Matrix3d SrsIKSolver::reference_plane(const Eigen::Vector3d& S,
+                                             const Eigen::Vector3d& W)
+{
+    const double d_sw = (W - S).norm();
+    const Eigen::Vector3d v_sw = (W - S).normalized();
+
+    const double x = (d_sw * d_sw + d_se_ * d_se_ - d_ew_ * d_ew_) / (2.0 * d_sw);
+    const double r = std::sqrt(std::max(d_se_ * d_se_ - x * x, 0.0));
+    const Eigen::Vector3d F = S + x * v_sw;
+
+    Eigen::Vector3d FE;
+    if (v_sw.head<2>().cwiseAbs().maxCoeff() <= 1e-6) {
+        FE = Eigen::Vector3d(-1.0, 0.0, 0.0);
+    } else {
+        FE(0) = -v_sw(0) * v_sw(2) / (v_sw(0) * v_sw(0) + v_sw(1) * v_sw(1));
+        FE(1) = -v_sw(1) * v_sw(2) / (v_sw(0) * v_sw(0) + v_sw(1) * v_sw(1));
+        FE(2) = 1.0;
+    }
+
+    const Eigen::Vector3d E = F + elbow_config_ * r * FE.normalized();
+    const Eigen::Vector3d v_es = (S - E).normalized();
+    const Eigen::Vector3d v_ew = (W - E).normalized();
+
+    const Eigen::Vector3d R30_y = v_es;
+    Eigen::Vector3d R30_z = v_ew.cross(v_es);
+
+    if (elbow_config_ == INWARD) {
+        R30_z = -R30_z;
+    }
+
+    const double nz = R30_z.norm();
+    if (nz > 1e-12) {
+        R30_z /= nz;
+    } else {
+        R30_z = Eigen::Vector3d(-0.0, 1.0, 0.0);
+    }
+
+    const Eigen::Vector3d R30_x = R30_y.cross(R30_z);
+
+    Eigen::Matrix3d R30;
+    R30.col(0) = R30_x;
+    R30.col(1) = R30_y;
+    R30.col(2) = R30_z;
+
+    return R30;
+}
+
+bool SrsIKSolver::ikWithPsi(const Eigen::Matrix4d& pose,
+                            std::vector<double>& joints,
+                            double psi)
+{
+    joints.resize(7, 0.0);
+
+    const Eigen::Vector3d P_target = pose.block<3, 1>(0, 3);
+    const Eigen::Vector3d S(0.0, 0.0, d_bs_);
+    const Eigen::Vector3d P67(0.0, 0.0, d_wt_);
+    const Eigen::Vector3d W = P_target - pose.block<3, 3>(0, 0) * P67;
+
+    const double d_sw = (W - S).norm();
+    const double r_max = d_se_ + d_ew_;
+    const double r_min = std::abs(d_se_ - d_ew_);
+    const double diff_max = d_sw - r_max;
+    const double diff_min = r_min - d_sw;
+    if (diff_max > EPS || diff_min > EPS) {
+        CMVR_LOG(ERROR) << "[SrsIKSolver] pose outside reachable workspace, IK solve failed";
+        return false;
+    }
+
+    double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2.0 * d_se_ * d_ew_);
+    cos_elbow = std::clamp(cos_elbow, -1.0, 1.0);
+    joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow));
+
+    const Eigen::Matrix3d R30 = reference_plane(S, W);
+    const Eigen::Matrix3d R_axis = calc_rotation_matrix((W - S).normalized(), psi);
+    const Eigen::Matrix3d R3 = R_axis * R30;
+
+    double k = shoulder_config_;
+    const double c2 = std::clamp(-R3(2, 1), -1.0, 1.0);
+    constexpr double eps = 1e-8;
+
+    if (std::fabs(c2 - 1.0) < eps) {
+        joints[1] = 0.0 * k;
+        joints[0] = 0.0;
+        joints[2] = std::atan2(k * R3(1, 0), k * R3(0, 0));
+    } else if (std::fabs(c2 + 1.0) < eps) {
+        joints[1] = k * M_PI;
+        joints[2] = std::atan2(k * R3(1, 0), k * R3(1, 2));
+        joints[0] = 0.0;
+    } else {
+        joints[0] = std::atan2(-k * R3(1, 1), -k * R3(0, 1));
+        joints[1] = k * std::acos(c2);
+        joints[2] = std::atan2(k * R3(2, 2), -k * R3(2, 0));
+    }
+
+    Eigen::Matrix3d R04 = Eigen::Matrix3d::Identity();
+    for (int i = 0; i < 4; ++i) {
+        const Eigen::Vector4d dh = dh_params_.row(i);
+        R04 = R04 * calc_dh(dh[0], dh[1], dh[2], dh[3] + joints[i]).block<3, 3>(0, 0);
+    }
+
+    const Eigen::Matrix3d R47 = R04.transpose() * pose.block<3, 3>(0, 0);
+    k = wrist_config_;
+
+    const double c = std::clamp(R47(2, 2), -1.0, 1.0);
+    double theta_y = k * std::acos(c);
+    double phi_z = std::atan2(k * R47(1, 2), k * R47(0, 2));
+    double psi_z = std::atan2(k * R47(2, 1), -k * R47(2, 0));
+
+    if (std::fabs(c - 1.0) < eps) {
+        theta_y = 0.0;
+        phi_z = std::atan2(k * R47(1, 0), k * R47(0, 0));
+        psi_z = 0.0;
+    } else if (std::fabs(c + 1.0) < eps) {
+        theta_y = k * M_PI;
+        phi_z = std::atan2(-k * R47(1, 0), -k * R47(0, 0));
+        psi_z = 0.0;
+    }
+
+    joints[4] = SupportFunctions::normalize_angle(phi_z - M_PI / 2.0);
+    joints[5] = SupportFunctions::normalize_angle(theta_y - M_PI / 2.0);
+    joints[6] = SupportFunctions::normalize_angle(psi_z);
+
+    psi_ = psi;
+    return true;
+}
+
+Eigen::Matrix3d SrsIKSolver::calc_rotation_matrix(const Eigen::Vector3d& rotation_axis,
+                                                  double rotation_angle)
+{
+    const Eigen::Vector3d normalized_axis = rotation_axis.normalized();
+    const double ux = normalized_axis[0];
+    const double uy = normalized_axis[1];
+    const double uz = normalized_axis[2];
+
+    Eigen::Matrix3d u_hat;
+    u_hat << 0.0, -uz, uy,
+            uz, 0.0, -ux,
+            -uy, ux, 0.0;
+
+    return Eigen::Matrix3d::Identity()
+           + std::sin(rotation_angle) * u_hat
+           + (1.0 - std::cos(rotation_angle)) * (u_hat * u_hat);
+}
+
+Eigen::Matrix4d SrsIKSolver::calc_dh(double d, double alpha, double a, double theta)
+{
+    const double ca = std::cos(alpha);
+    const double sa = std::sin(alpha);
+    const double ct = std::cos(theta);
+    const double st = std::sin(theta);
+
+    Eigen::Matrix4d T;
+    T << ct, -st * ca, st * sa, a * ct,
+            st, ct * ca, -ct * sa, a * st,
+            0.0, sa, ca, d,
+            0.0, 0.0, 0.0, 1.0;
+
+    return T;
+}
+
+Eigen::Matrix4d SrsIKSolver::calc_total_transform(const std::vector<double>& joint_angles)
+{
+    Eigen::Matrix4d T_total = Eigen::Matrix4d::Identity();
+    if (joint_angles.size() < static_cast<std::size_t>(dh_params_.rows())) {
+        return T_total;
+    }
+
+    for (int i = 0; i < dh_params_.rows(); ++i) {
+        const double d = dh_params_(i, 0);
+        const double alpha = dh_params_(i, 1);
+        const double a = dh_params_(i, 2);
+        const double theta0 = dh_params_(i, 3);
+        T_total = T_total * calc_dh(d, alpha, a, theta0 + joint_angles[i]);
+    }
+
+    return T_total;
+}
+
+bool SrsIKSolver::cal_coefficient_matrix(const Eigen::Matrix4d& pose,
+                                         Eigen::MatrixXd& s_mat,
+                                         Eigen::MatrixXd& w_mat)
+{
+    if (s_mat.rows() != 3 || s_mat.cols() != 9) {
+        s_mat.setZero(3, 9);
+    }
+    if (w_mat.rows() != 3 || w_mat.cols() != 9) {
+        w_mat.setZero(3, 9);
+    }
+
+    std::vector<double> joints(7, 0.0);
+    const Eigen::Vector3d P_target = pose.block<3, 1>(0, 3);
+    const Eigen::Vector3d S(0.0, 0.0, d_bs_);
+    const Eigen::Vector3d P67(0.0, 0.0, d_wt_);
+    const Eigen::Vector3d W = P_target - pose.block<3, 3>(0, 0) * P67;
+
+    const double d_sw = (W - S).norm();
+    const double r_max = d_se_ + d_ew_;
+    const double r_min = std::abs(d_se_ - d_ew_);
+    const double diff_max = d_sw - r_max;
+    const double diff_min = r_min - d_sw;
+    if (diff_max > EPS || diff_min > EPS) {
+        CMVR_LOG(ERROR) << "[SrsIKSolver] pose outside reachable workspace, IK solve failed";
+        return false;
+    }
+
+    double cos_elbow = (d_se_ * d_se_ + d_ew_ * d_ew_ - d_sw * d_sw) / (2.0 * d_se_ * d_ew_);
+    cos_elbow = std::clamp(cos_elbow, -1.0, 1.0);
+    joints[3] = elbow_config_ * (M_PI - std::acos(cos_elbow));
+
+    const Eigen::Matrix3d R30 = reference_plane(S, W);
+    const Eigen::Vector3d normalized_axis = (W - S).normalized();
+
+    const double ux = normalized_axis[0];
+    const double uy = normalized_axis[1];
+    const double uz = normalized_axis[2];
+    Eigen::Matrix3d u_hat;
+    u_hat << 0.0, -uz, uy,
+            uz, 0.0, -ux,
+            -uy, ux, 0.0;
+
+    const Eigen::MatrixXd A_s = u_hat * R30;
+    const Eigen::MatrixXd B_s = -u_hat * u_hat * R30;
+    const Eigen::MatrixXd C_s = (Eigen::MatrixXd::Identity(3, 3) + u_hat * u_hat) * R30;
+
+    const Eigen::MatrixXd T34 = calc_dh(dh_params_(3, 0),
+                                        dh_params_(3, 1),
+                                        dh_params_(3, 2),
+                                        dh_params_(3, 3) + joints[3]);
+    const Eigen::MatrixXd R34 = T34.block(0, 0, 3, 3);
+    const Eigen::MatrixXd A_w = R34.transpose() * A_s.transpose() * pose.block(0, 0, 3, 3);
+    const Eigen::MatrixXd B_w = R34.transpose() * B_s.transpose() * pose.block(0, 0, 3, 3);
+    const Eigen::MatrixXd C_w = R34.transpose() * C_s.transpose() * pose.block(0, 0, 3, 3);
+
+    s_mat.block<3, 3>(0, 0) = A_s;
+    s_mat.block<3, 3>(0, 3) = B_s;
+    s_mat.block<3, 3>(0, 6) = C_s;
+
+    w_mat.block<3, 3>(0, 0) = A_w;
+    w_mat.block<3, 3>(0, 3) = B_w;
+    w_mat.block<3, 3>(0, 6) = C_w;
+
+    return true;
+}
+
+} // namespace cmvr

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

@@ -13,12 +13,51 @@
 #include "pinocchio/multibody/sample-models.hpp"
 #include <Eigen/Dense>
 #include "gtest/gtest.h"
-#include "ik_solver/include/pinocchio_dls_ik_solver.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
-#include "ik_solver/include/pinocchio_qp_ik_solver.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h"
-#include "ik_solver/include/lawba_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:
@@ -185,28 +224,18 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
         {-0.26,   1.57},
     }};
 
-    constexpr int N_SAMPLES = 10;  // 样本数：1000 组随机关节角
+    constexpr int N_SAMPLES = 10;
 
     // ========== 2. 创建三个求解器实例 ==========
 
     // 数值优化类 QP IK
-    PinocchioQpIKSolver qp_solver(
+    PinocchioQpIKSolver qp_solver(makeQpConfig());
-        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
-        "PELVIS_S",
-        "R_WRIST_R_S",        // 法兰 frame
-        "R_FINGER_TIP_FIXED"  // TCP frame
-    );
 
     // 基于广义逆雅可比矩阵的数值增量 IK
-    PinocchioDlsIKSolver pinv_solver(
+    PinocchioDlsIKSolver pinv_solver(makeDlsConfig());
-        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
-        "PELVIS_S",
-        "R_WRIST_R_S",        // 法兰 frame
-        "R_FINGER_TIP_FIXED"  // TCP frame
-    );
 
     // 你的专利方法：臂角 ψ + 可行域 + 势场
-    LawbaIKSolver psi_solver;
+    LawbaIKSolver psi_solver(config::LawbaIKConfig{});
 
     // 统一容器，方便 for 循环
     std::vector<IKSolver*> solvers = {
@@ -236,6 +265,8 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
     // ========== 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) {
@@ -308,7 +339,7 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
             }
 
             viewer.moveJ(q_sol);
-            std::this_thread::sleep_for(std::chrono::duration<double>(1));
+            std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
             double dt_ms = std::chrono::duration_cast<
                 std::chrono::microseconds>(t1 - t0).count() / 1000.0;
@@ -395,34 +426,24 @@ void benchmarkIkSolversRandomJoints()
 
     }};
 
-    constexpr int N_SAMPLES = 100;  // 样本数：1000 组随机关节角
+    constexpr int N_SAMPLES = 10;
 
     // ========== 2. 创建三个求解器实例 ==========
 
     // 数值优化类 QP IK
-    PinocchioQpIKSolver qp_solver(
+    PinocchioQpIKSolver qp_solver(makeQpConfig());
-        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
-        "PELVIS_S",
-        "R_WRIST_R_S",        // 法兰 frame
-        "R_FINGER_TIP_FIXED"  // TCP frame
-    );
 
     // 基于广义逆雅可比矩阵的数值增量 IK
-    PinocchioDlsIKSolver pinv_solver(
+    PinocchioDlsIKSolver pinv_solver(makeDlsConfig());
-        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
-        "PELVIS_S",
-        "R_WRIST_R_S",        // 法兰 frame
-        "R_FINGER_TIP_FIXED"  // TCP frame
-    );
 
     // 你的专利方法：臂角 ψ + 可行域 + 势场
-    LawbaIKSolver psi_solver;
+    LawbaIKSolver psi_solver(config::LawbaIKConfig{});
 
     // 统一容器，方便 for 循环
     std::vector<IKSolver*> solvers = {
-        // &psi_solver,
+        &psi_solver,
         &pinv_solver,
-        // &qp_solver
+        &qp_solver
     };
 
 
@@ -446,6 +467,8 @@ void benchmarkIkSolversRandomJoints()
     // ========== 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) {
@@ -586,30 +609,23 @@ void benchmarkIkSolversRandomJoints()
 }
 
 TEST(ik_test,pinocchio_lib_test) {
-    // PinocchioQpIKSolver solver(
+    PinocchioQpIKSolver solver(makeQpConfig());
-    //         "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
+    // PinocchioDlsIKSolver solver(makeDlsConfig());
-    //         "PELVIS_S",
+    ASSERT_TRUE(solver.init());
-    //         "R_WRIST_R_S",   // 法兰 frame
-    //        "R_FINGER_TIP_FIXED"      // TCP frame
-    //     );
-
-    LawbaIKSolver solver;
-    solver.init();
 
 
     // 真实关节角（比如从控制器读回来）
     std::vector<double> q_cur(7, 0.0);
-    solver.update_joints_state(q_cur);
-
     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 cur_tcp_pose = Eigen::Matrix4d::Identity();
-    solver.fk(q_cur, cur_tcp_pose, false);  // is_tcp = true
+    ASSERT_TRUE(solver.fk(q_cur, cur_tcp_pose, false));
     std::cout <<  cur_tcp_pose<< std::endl;
 
 
@@ -617,45 +633,29 @@ TEST(ik_test,pinocchio_lib_test) {
 
 
     std::vector<double> q_target;
-    bool ok = solver.ik(target_tcp_pose, q_target, false);  // is_tcp = true
+    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 cur_flange_pose = Eigen::Matrix4d::Identity();
-    solver.fk(q_target, cur_flange_pose, false);              // is_tcp = false
+    ASSERT_TRUE(solver.fk(q_target, cur_flange_pose, false));
     std::cout <<  cur_flange_pose<< std::endl;
 }
 
 TEST(ik_test,ik_compare) {
-    // benchmarkIkSolversRandomJoints();
+    constexpr const char* model_path =
-
+        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml";
-
-
-    const char *model_path =
-           "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml";
 
     DualArmViewer viewer(model_path);
-    std::this_thread::sleep_for(std::chrono::seconds(3));
+    std::thread benchmark_thread([&viewer]() {
-
+        // viewer.run() 在主线程创建仿真和渲染资源，稍后再开始发送关节目标。
-    // int viewer = 0;
+        std::this_thread::sleep_for(std::chrono::seconds(1));
-    // 把所有 IK 运算 + moveJ 循环放到控制线程里
-    std::thread ctrl_thread([&viewer]() {
         benchmarkIkSolversRandomJoints(viewer);
-
+        std::cout << "[ik_compare] benchmark finished; close the viewer to exit." << std::endl;
-        //
-        // // 可视化：右臂关节位置控制
-        // viewer.moveJ(q); // 更新目标角
-        // std::this_thread::sleep_for(std::chrono::duration<double>(0.01));
     });
 
-
+    viewer.run();
-    viewer.run(); // 阻塞，直到你关掉窗口
+    benchmark_thread.join();
-    ctrl_thread.join(); // 控制线程结束
 }
-
-
-
-
-

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

@@ -2,11 +2,11 @@
 // Created by lgv on 2025/11/3.
 //
 
-#include "ik_solver/include/pinocchio_dls_ik_solver.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
 #include "gtest/gtest.h"
-#include "ik_solver/include/bias_srs_ik_slover.h"
+#include "algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h"
-#include "ik_solver/include/joints_limit_analyzer.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h"
-#include "ik_solver/include/opt_psi_selector.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h"
 #include <algorithm>
 #include <array>
 #include <chrono>
@@ -16,19 +16,60 @@
 #include <iomanip>
 #include <iostream>
 #include <limits>
+#include <memory>
 #include <mutex>
 #include <thread>
 #include <vector>
 
-#include "ik_solver/include/lawba_ik_solver.h"
+#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h"
+#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h"
+#include "common/math/transform_math.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
-#include "planner/joint_space_planner/include/toppra_bspline.h"
+#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
-#include "planner/joint_space_planner/include/joint_space_planner_creator.h"
+#include "common/math/support_functions.h"
-#include "common/math/include/support_functions.h"
+#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h"
-#include "ik_solver/include/pinocchio_qp_ik_solver.h"
 #include <matplot/matplot.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
+
 
 struct IkSample {
     double psi;
@@ -347,11 +388,11 @@ private:
 //     int viewer = 0;
 //     // 把所有 IK 运算 + moveJ 循环放到控制线程里
 //     std::thread ctrl_thread([&viewer]() {
-//         BiasSRSIkSolver slover;
+//         SrsIKSolver slover(config::SrsIKConfig{});
 //         std::vector<IkSample> samples;
 //         samples.reserve(4096);
 //
-//         slover.set_shoulder_config(BiasSRSIkSolver::INWARD);
+//         slover.set_shoulder_config(SrsIKSolver::INWARD);
 //
 //         std::vector<double> joint_angles(7, 0);
 //         // joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56};
@@ -431,7 +472,7 @@ private:
 //
 //             double psi = psi_vals[idx];
 //
-//             auto q = slover.inverse_kinematics(target_pose, psi);
+//             auto q = slover.ikWithPsi(target_pose, psi);
 //                if (q.size() != 7 || std::any_of(q.begin(), q.end(),
 //                                                 [](double v) { return !std::isfinite(v); })) {
 //                    ++bad;
@@ -522,7 +563,7 @@ private:
 //
 //     // std::cout << std::fixed << std::setprecision(7);
 //
-//     BiasSRSIkSolver slover;
+//     SrsIKSolver slover(config::SrsIKConfig{});
 //     std::vector<IkSample> samples;
 //     samples.reserve(4096);
 //
@@ -572,7 +613,7 @@ private:
 //     };
 //
 //     // IK 解
-//     auto q = slover.inverse_kinematics(target_pose, best_psi);
+//     auto q = slover.ikWithPsi(target_pose, best_psi);
 //
 //     for (double q1: q) {
 //         std::cout << q1 << " , ";
@@ -836,7 +877,7 @@ TEST(SRS_IK_TEST, MOVE_L_PLANNER_TEST) {
         // 等 MuJoCo / OpenGL 初始化好
         std::this_thread::sleep_for(3s);
 
-        LawbaIKSolver solver;
+        LawbaIKSolver solver(config::LawbaIKConfig{});
 
         // =============== 1) 设置初始关节状态 ===============
         std::vector<double> joint_angles(7, 0.0);
@@ -937,8 +978,8 @@ TEST(SRS_IK_TEST, MOVE_L_PLANNER_TEST) {
             return;
         }
 
-        // =============== 5) 使用 JointSpacePlanner 对 IK 路点做时间参数化 ===============
+        // =============== 5) 使用 JointTrajectoryPlanner 对 IK 路点做时间参数化 ===============
-        auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE);
+        auto planner = std::make_shared<ToppraJointTrajectoryPlanner>();
         planner->setPathType(PathType::Natural);
 
         // 按自己实际的关节约束改
@@ -993,7 +1034,7 @@ TEST(SRS_IK_TEST, TR_TEST) {
     using std::endl;
 
 
-    PinocchioQpIKSolver solver;
+    PinocchioDlsIKSolver solver(makeDlsConfig());
     solver.init();
     Eigen::Matrix4d T;
     T <<  9.99998311e-01, -1.78940420e-03,  4.20611000e-04,  3.83367005e-02,
@@ -1235,15 +1276,8 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) {
         using namespace std::chrono_literals;
         std::this_thread::sleep_for(3s);
 
-        cmvr::PinocchioDlsIKSolver solver(
+        auto solver = std::make_shared<cmvr::PinocchioDlsIKSolver>(makeDlsConfig());
-            "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
+        if (!solver->init()) return;
-            "PELVIS_S",
-            "R_WRIST_R_S",
-            "R_FINGER_TIP_FIXED"
-        );
-
-        LawbaIKSolver solver_1;
-        if (!solver.init()) return;
 
         // 起点关节
         // std::vector<double> q_start = {-0.424743, 0.759386, 1.80129, 2.03728, -1.34668, 0.0560845, -0.26};
@@ -1254,14 +1288,14 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) {
 
         // 起点位姿（base下）
         Eigen::Matrix4d T0;
-        if (!solver_1.fk(q_start, T0, true)) return;
+        if (!solver->fk(q_start, T0, true)) return;
 
         std::cout << T0 << std::endl;
 
         // 目标位姿：base X 方向走 0.25m，姿态保持起点
         Eigen::Matrix4d Tg = T0;
         // Tg(0,3) += 0.2;
-        Tg(2,3) += 0.2;
+        Tg(1,3) -= 0.2;
         Eigen::Vector3d dp_check = Tg.block<3,1>(0,3) - T0.block<3,1>(0,3);
         std::cerr << "dp(base)=" << dp_check.transpose() << "\n";
 
@@ -1272,15 +1306,29 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) {
         const double j_tcp  = 100.00;
         std::vector<double> qd_max(7, 3.0);
 
-        std::vector<std::vector<double>> q_traj;
+        cmvr::device::PinocchioDlsCartesianMotionPlanner planner(solver);
-        std::vector<double> t_traj;
+        cmvr::config::MoveLPlannerConfig move_l_config;
-
+        move_l_config.set_sample_period_s(dt_gen);
-
+        move_l_config.set_position_gain(4.0);
-        if (!solver.moveL_SCurveLocal(Tg, q_start, q_traj, t_traj,
+        move_l_config.set_rotation_gain(4.0);
-                                      dt_gen, v_tcp, a_tcp, j_tcp, qd_max, true)) {
+        if (!planner.configureMoveL(move_l_config)) {
-            std::cerr << "moveL_SCurveLocal failed\n";
+            std::cerr << "configureMoveL failed\n";
             return;
         }
+        cmvr::device::CartesianJointTrajectory trajectory;
+        if (!planner.planMoveL(cmvr::common::math::matrixToPose(Tg),
+                               q_start,
+                               qd_max,
+                               v_tcp,
+                               a_tcp,
+                               j_tcp,
+                               cmvr::device::FrameType::Base,
+                               trajectory)) {
+            std::cerr << "planMoveL failed\n";
+            return;
+        }
+        const auto& q_traj = trajectory.position;
+        const auto& t_traj = trajectory.time;
 
         std::cerr << "traj gen samples=" << q_traj.size()
                   << "  T=" << (t_traj.empty()?0.0:t_traj.back()) << "s\n";
@@ -1288,7 +1336,7 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) {
         write_qtraj_csv(csv_path, t_traj, q_traj);
 
     std::vector<Eigen::Vector3d> p, v, a;
-    if (build_tcp_pva(solver, t_traj, q_traj, p, v, a, true)) {
+    if (build_tcp_pva(*solver, t_traj, q_traj, p, v, a, true)) {
         write_tcp_pva_csv(tcp_csv_path, t_traj, p, v, a);
         std::cerr << "saved tcp csv: " << tcp_csv_path << "\n";
     } else {
@@ -1297,7 +1345,7 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) {
         std::cerr << "saved csv: " << csv_path << "\n";
 
         // 正确的 TCP 速度统计（用 t_traj）
-        print_tcp_speed_stats(solver, t_traj, q_traj, true);
+        print_tcp_speed_stats(*solver, t_traj, q_traj, true);
 
         // 直接按采样时刻逐点下发位置目标，不在 viewer 内做轨迹缓存和插值。
         const auto t0 = std::chrono::steady_clock::now();
@@ -1425,13 +1473,10 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) {
             std::cerr << msg << "\n";
         };
 
-        cmvr::PinocchioDlsIKSolver solver(
+        auto dls_cfg = makeDlsConfig();
-            urdf_path,
+        dls_cfg.set_urdf_path(urdf_path);
-            "PELVIS_S",
+        auto solver = std::make_shared<cmvr::PinocchioDlsIKSolver>(dls_cfg);
-            "R_WRIST_R_S",
+        if (!solver->init()) {
-            "R_FINGER_TIP_FIXED"
-        );
-        if (!solver.init()) {
             fail("speedL test: solver.init() failed");
             return;
         }
@@ -1449,23 +1494,25 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) {
         std::this_thread::sleep_for(1s);
 
         const std::vector<double> q_init = viewer.getQ();
-        solver.update_joints_state(q_init);
+        solver->update_joints_state(q_init);
         std::vector<double> q_ref = q_init;
 
-        cmvr::PinocchioDlsIKSolver::SpeedLConfig speedl_config;
+        cmvr::config::SpeedLPlannerConfig speedl_config;
-        speedl_config.linear_velocity_max = 0.55;
+        speedl_config.set_linear_velocity_max(0.55);
-        speedl_config.linear_acceleration_max = 0.80;
+        speedl_config.set_linear_acceleration_max(0.80);
-        speedl_config.linear_jerk_max = 3.30;
+        speedl_config.set_linear_jerk_max(3.30);
-        speedl_config.angular_velocity_max = 1.00;
+        speedl_config.set_angular_velocity_max(1.00);
-        speedl_config.angular_acceleration_max = 3.00;
+        speedl_config.set_angular_acceleration_max(3.00);
-        speedl_config.angular_jerk_max = 12.0;
+        speedl_config.set_angular_jerk_max(12.0);
-        speedl_config.joint_acceleration_max = std::vector<double>(7, 8.0);
+        for (int i = 0; i < 7; ++i) {
-        speedl_config.linear_target_replan_threshold = 1e-4;
+            speedl_config.add_joint_acceleration_max(8.0);
-        speedl_config.angular_target_replan_threshold = 1e-4;
+        }
-        speedl_config.linear_reverse_cos_threshold = -0.8660254037844386;
+        speedl_config.set_linear_target_replan_threshold(1e-4);
-        speedl_config.linear_reverse_switch_speed_threshold = 1e-3;
+        speedl_config.set_angular_target_replan_threshold(1e-4);
-        // speedl_config.qdot_measurement_lowpass_alpha = 0.5;
+        speedl_config.set_linear_reverse_cos_threshold(-0.8660254037844386);
-        if (!solver.configureSpeedL(speedl_config)) {
+        speedl_config.set_linear_reverse_switch_speed_threshold(1e-3);
+        cmvr::device::PinocchioDlsCartesianMotionPlanner planner(solver);
+        if (!planner.configureSpeedL(speedl_config, q_init.size())) {
             fail("speedL test: configureSpeedL() failed");
             return;
         }
@@ -1487,11 +1534,17 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) {
         tcp_omega_traj.reserve(reserve_count);
 
         auto record_sample = [&](double t_sample,
-                                 const std::vector<double>& q_sample) -> bool {
+                                 const std::vector<double>& q_sample,
+                                 const std::vector<double>& qd_sample) -> bool {
             q_traj.push_back(q_sample);
             t_traj.push_back(t_sample);
-            twist_cmd_traj.push_back(solver.getSpeedLCommandTwistBase());
+            twist_cmd_traj.push_back(
-            const auto twist_exec = solver.getSpeedLExecutedTwistBase();
+                cmvr::common::math::velocityToVector(planner.getSpeedLCommandTwistBase()));
+            Eigen::Matrix<double, 6, 1> twist_exec = Eigen::Matrix<double, 6, 1>::Zero();
+            if (!solver->computeTwistBaseAtQ(q_sample, qd_sample, true, twist_exec)) {
+                fail("speedL test: failed to compute executed TCP twist");
+                return false;
+            }
 
             Eigen::Vector3d tcp_pos = Eigen::Vector3d::Zero();
             Eigen::Vector3d tcp_vel = Eigen::Vector3d::Zero();
@@ -1535,21 +1588,21 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) {
                 Eigen::Matrix<double,6,1>::Zero();
             if (t < segment_time) {
                 target_twist[1] = linear_speed_cmd;
-                target_twist[2] = 0.1;
+                target_twist[2] = 0;
             } else if (t < 2.0 * segment_time) {
                 target_twist[1] = -linear_speed_cmd;
-                target_twist[2] = -0.1;
+                target_twist[2] = 0;
             } else {
                 target_twist.setZero();
             }
 
             std::vector<double> qd_cmd;
-            if (!solver.speedLStep(target_twist,
+            if (!planner.speedLStep(cmvr::common::math::vectorToVelocity(target_twist),
-                                   dt,
+                                    dt,
-                                   q_meas,
+                                    q_meas,
-                                   qd_cmd,
+                                    qd_meas,
-                                   cmvr::CartesianFrame::Base,
+                                    qd_cmd,
-                                   true)) {
+                                    cmvr::device::FrameType::Base)) {
                 viewer.moveJ(q_meas);
                 fail("speedL test: speedLStep() failed");
                 return;
@@ -1564,7 +1617,7 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) {
                                     std::min(q_ref[j], q_meas[j] + max_q_ref_tracking_error));
             }
             viewer.moveJ(q_ref);
-            if (!record_sample(t, q_meas)) {
+            if (!record_sample(t, q_meas, qd_meas)) {
                 return;
             }
         }
@@ -1583,12 +1636,12 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) {
                 Eigen::Matrix<double,6,1>::Zero();
 
             std::vector<double> qd_cmd;
-            if (!solver.speedLStep(target_twist,
+            if (!planner.speedLStep(cmvr::common::math::vectorToVelocity(target_twist),
-                                   dt,
+                                    dt,
-                                   q_meas,
+                                    q_meas,
-                                   qd_cmd,
+                                    qd_meas,
-                                   cmvr::CartesianFrame::Base,
+                                    qd_cmd,
-                                   true)) {
+                                    cmvr::device::FrameType::Base)) {
                 viewer.moveJ(q_meas);
                 fail("speedL test: speedLStep() failed during stop phase");
                 return;
@@ -1603,7 +1656,7 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) {
                                     std::min(q_ref[j], q_meas[j] + max_q_ref_tracking_error));
             }
             viewer.moveJ(q_ref);
-            if (!record_sample(t, q_meas)) {
+            if (!record_sample(t, q_meas, qd_meas)) {
                 return;
             }
         }

cmvr-es/algorithms/motion_planner/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_subdirectory(base_motion)
+add_subdirectory(arm_motion)

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

@@ -0,0 +1,17 @@
+add_library(arm_motion SHARED
+        cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp
+        cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp
+        joint_motion/toppra/src/toppra_joint_motion_planner.cpp
+)
+
+target_include_directories(arm_motion PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(arm_motion
+        PUBLIC
+        cmvr_es::ik_solver
+        cmvr_es::base_motion
+)
+
+add_library(cmvr_es::arm_motion ALIAS arm_motion)
+add_library(cmvr_es::algorithms::arm_motion ALIAS arm_motion)
+install(TARGETS arm_motion LIBRARY DESTINATION lib)

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

@@ -0,0 +1,47 @@
+#ifndef CMVR_ES_CARTESIAN_MOTION_PLANNER_H
+#define CMVR_ES_CARTESIAN_MOTION_PLANNER_H
+
+#include <vector>
+
+#include "cmvr/config/arm_config/arm_config.pb.h"
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::device {
+
+struct CartesianJointTrajectory {
+    std::vector<std::vector<double>> position;
+    std::vector<std::vector<double>> velocity;
+    std::vector<double> time;
+};
+
+class CartesianMotionPlanner {
+public:
+    virtual ~CartesianMotionPlanner() = default;
+
+    virtual bool configureSpeedL(const config::SpeedLPlannerConfig& config,
+                                 std::size_t dof) = 0;
+    virtual bool configureMoveL(const config::MoveLPlannerConfig& config) = 0;
+
+    virtual 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) = 0;
+
+    virtual 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) = 0;
+
+    virtual bool updateSpeedLAcceleration(double acceleration) = 0;
+    virtual CartesianVelocity getSpeedLCommandTwistBase() const = 0;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CARTESIAN_MOTION_PLANNER_H

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

@@ -0,0 +1,78 @@
+#ifndef CMVR_ES_CARTESIAN_MOTION_PLANNER_FACTORY_H
+#define CMVR_ES_CARTESIAN_MOTION_PLANNER_FACTORY_H
+
+#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_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"
+
+namespace cmvr::device {
+
+class CartesianMotionPlannerFactory {
+public:
+    static std::shared_ptr<CartesianMotionPlanner> create(
+        const config::MoveLConfig& move_l,
+        const config::SpeedLConfig& speed_l,
+        const std::shared_ptr<cmvr::PinocchioIKBase>& solver)
+    {
+        if (!solver) {
+            return nullptr;
+        }
+        switch (move_l.algorithm_case()) {
+            case config::MoveLConfig::kPinocchioQpCartesianMotionPlanner:
+                if (speed_l.algorithm_case() !=
+                    config::SpeedLConfig::kPinocchioQpCartesianMotionPlanner) {
+                    return nullptr;
+                }
+                return std::make_shared<PinocchioQpCartesianMotionPlanner>(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;
+        }
+    }
+
+    static const config::SpeedLPlannerConfig* speedLConfig(
+        const config::SpeedLConfig& cfg)
+    {
+        switch (cfg.algorithm_case()) {
+        case config::SpeedLConfig::kPinocchioQpCartesianMotionPlanner:
+            return &cfg.pinocchio_qp_cartesian_motion_planner();
+        case config::SpeedLConfig::kPinocchioDlsCartesianMotionPlanner:
+            return &cfg.pinocchio_dls_cartesian_motion_planner();
+        case config::SpeedLConfig::ALGORITHM_NOT_SET:
+        default:
+            return nullptr;
+        }
+    }
+
+    static const config::MoveLPlannerConfig* moveLConfig(
+        const config::MoveLConfig& cfg)
+    {
+        switch (cfg.algorithm_case()) {
+        case config::MoveLConfig::kPinocchioQpCartesianMotionPlanner:
+            return &cfg.pinocchio_qp_cartesian_motion_planner();
+        case config::MoveLConfig::kPinocchioDlsCartesianMotionPlanner:
+            return &cfg.pinocchio_dls_cartesian_motion_planner();
+        case config::MoveLConfig::ALGORITHM_NOT_SET:
+        default:
+            return nullptr;
+        }
+    }
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_CARTESIAN_MOTION_PLANNER_FACTORY_H

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

@@ -0,0 +1,66 @@
+#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

@@ -0,0 +1,408 @@
+#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

@@ -0,0 +1,76 @@
+#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

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

cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h

@@ -0,0 +1,51 @@
+#ifndef CMVR_ES_TWIST_LIMITER_CONFIG_H
+#define CMVR_ES_TWIST_LIMITER_CONFIG_H
+
+#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
+#include "cmvr/config/arm_config/arm_config.pb.h"
+#include "common/config/config_files.h"
+
+namespace cmvr::device::cartesian_motion {
+
+inline void configureTwistLimiterFromSpeedLConfig(
+    cmvr::CartesianTwistLimiter& limiter,
+    const config::SpeedLPlannerConfig& config)
+{
+    using cmvr::common::config::positiveOr;
+
+    limiter.setLinearConstraints(positiveOr(config.linear_velocity_max(), 0.55),
+                                 positiveOr(config.linear_acceleration_max(), 5.0),
+                                 positiveOr(config.linear_jerk_max(), 10.0));
+    limiter.setAngularConstraints(positiveOr(config.angular_velocity_max(), 1.0),
+                                  positiveOr(config.angular_acceleration_max(), 5.0),
+                                  positiveOr(config.angular_jerk_max(), 12.0));
+    limiter.setLinearTargetReplanThreshold(
+        positiveOr(config.linear_target_replan_threshold(), 1e-4));
+    limiter.setAngularTargetReplanThreshold(
+        positiveOr(config.angular_target_replan_threshold(), 1e-4));
+    limiter.setLinearReverseSwitchPolicy(
+        config.linear_reverse_cos_threshold() != 0.0
+            ? config.linear_reverse_cos_threshold()
+            : -0.8660254037844386,
+        positiveOr(config.linear_reverse_switch_speed_threshold(), 1e-3));
+    limiter.initialize(Eigen::Matrix<double, 6, 1>::Zero());
+}
+
+inline void updateTwistLimiterAcceleration(
+    cmvr::CartesianTwistLimiter& limiter,
+    const config::SpeedLPlannerConfig& config,
+    const double acceleration)
+{
+    using cmvr::common::config::positiveOr;
+
+    limiter.setLinearConstraints(positiveOr(config.linear_velocity_max(), 0.55),
+                                 acceleration,
+                                 positiveOr(config.linear_jerk_max(), 10.0));
+    limiter.setAngularConstraints(positiveOr(config.angular_velocity_max(), 1.0),
+                                  acceleration,
+                                  positiveOr(config.angular_jerk_max(), 12.0));
+}
+
+} // namespace cmvr::device::cartesian_motion
+
+#endif // CMVR_ES_TWIST_LIMITER_CONFIG_H

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h

@@ -0,0 +1,31 @@
+#ifndef CMVR_ES_JOINT_MOTION_PLANNER_H
+#define CMVR_ES_JOINT_MOTION_PLANNER_H
+
+#include <vector>
+
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::device {
+
+struct JointTrajectorySample {
+    double t{0.0};
+    std::vector<double> position;
+    std::vector<double> velocity;
+};
+
+class JointMotionPlanner {
+public:
+    virtual ~JointMotionPlanner() = default;
+
+    virtual bool init() = 0;
+
+    virtual bool planMoveJ(const std::vector<double>& start,
+                           const JointPositionCommand& target,
+                           const MotionOptions& options,
+                           double speed_scaling,
+                           std::vector<JointTrajectorySample>& samples) = 0;
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_JOINT_MOTION_PLANNER_H

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h

@@ -0,0 +1,55 @@
+#ifndef CMVR_ES_JOINT_MOTION_PLANNER_FACTORY_H
+#define CMVR_ES_JOINT_MOTION_PLANNER_FACTORY_H
+
+#include <memory>
+
+#include "algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h"
+#include "algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h"
+#include "cmvr/config/arm_config/arm_config.pb.h"
+
+namespace cmvr::device {
+
+class JointMotionPlannerFactory {
+public:
+    static std::shared_ptr<JointMotionPlanner> create(
+        const config::MoveJConfig& cfg)
+    {
+        if (!cfg.has_toppra_joint_motion_planner()) {
+            return nullptr;
+        }
+
+        cmvr::PathType path_type;
+        switch (cfg.toppra_joint_motion_planner().path_type()) {
+        case config::TOPPRA_PATH_TYPE_LINEAR:
+            path_type = cmvr::PathType::Linear;
+            break;
+        case config::TOPPRA_PATH_TYPE_CUBIC_HERMITE:
+            path_type = cmvr::PathType::CubicHermite;
+            break;
+        case config::TOPPRA_PATH_TYPE_QUINTIC:
+            path_type = cmvr::PathType::Quintic;
+            break;
+        case config::TOPPRA_PATH_TYPE_NATURAL:
+            path_type = cmvr::PathType::Natural;
+            break;
+        case config::TOPPRA_PATH_TYPE_UNKNOWN:
+        default:
+            return nullptr;
+        }
+
+        const auto& toppra = cfg.toppra_joint_motion_planner();
+        auto planner = std::make_shared<ToppraJointMotionPlanner>(
+            path_type,
+            toppra.sample_period_s(),
+            toppra.grid_size(),
+            toppra.high_grid_size());
+        if (!planner->init()) {
+            return nullptr;
+        }
+        return planner;
+    }
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_JOINT_MOTION_PLANNER_FACTORY_H

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h

@@ -0,0 +1,36 @@
+#ifndef CMVR_ES_TOPPRA_JOINT_MOTION_PLANNER_H
+#define CMVR_ES_TOPPRA_JOINT_MOTION_PLANNER_H
+
+#include <memory>
+
+#include "../../joint_motion_planner.h"
+#include "../../../../base_motion/joint_trajectory/joint_trajectory_planner.h"
+
+namespace cmvr::device {
+
+class ToppraJointMotionPlanner final : public JointMotionPlanner {
+public:
+    ToppraJointMotionPlanner(cmvr::PathType path_type,
+                             double sample_period_s,
+                             int grid_size,
+                             int high_grid_size);
+
+    bool init() override;
+
+    bool planMoveJ(const std::vector<double>& start,
+                   const JointPositionCommand& target,
+                   const MotionOptions& options,
+                   double speed_scaling,
+                   std::vector<JointTrajectorySample>& samples) override;
+
+private:
+    std::shared_ptr<cmvr::JointTrajectoryPlanner> planner_;
+    cmvr::PathType path_type_{cmvr::PathType::Quintic};
+    double sample_period_s_{0.001};
+    int grid_size_{150};
+    int high_grid_size_{300};
+};
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_TOPPRA_JOINT_MOTION_PLANNER_H

cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp

@@ -0,0 +1,76 @@
+#include "algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h"
+
+#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
+
+namespace cmvr::device {
+
+namespace {
+
+std::vector<double> toStdVector(const Eigen::VectorXd& value)
+{
+    std::vector<double> result;
+    result.reserve(static_cast<std::size_t>(value.size()));
+    for (int i = 0; i < value.size(); ++i) {
+        result.push_back(value[i]);
+    }
+    return result;
+}
+
+} // namespace
+
+ToppraJointMotionPlanner::ToppraJointMotionPlanner(const cmvr::PathType path_type,
+                                                   const double sample_period_s,
+                                                   const int grid_size,
+                                                   const int high_grid_size)
+    : path_type_(path_type)
+    , sample_period_s_(sample_period_s)
+    , grid_size_(grid_size)
+    , high_grid_size_(high_grid_size)
+{
+}
+
+bool ToppraJointMotionPlanner::init()
+{
+    if (sample_period_s_ <= 0.0 || grid_size_ <= 0 || high_grid_size_ < grid_size_) {
+        return false;
+    }
+    planner_ = std::make_shared<cmvr::ToppraJointTrajectoryPlanner>(path_type_);
+    planner_->setGridSizes(grid_size_, high_grid_size_);
+    return true;
+}
+
+bool ToppraJointMotionPlanner::planMoveJ(const std::vector<double>& start,
+                                         const JointPositionCommand& target,
+                                         const MotionOptions& options,
+                                         const double speed_scaling,
+                                         std::vector<JointTrajectorySample>& samples)
+{
+    samples.clear();
+    if (!planner_ || start.empty() || start.size() != target.position.size() ||
+        options.velocity <= 0.0 || options.acceleration <= 0.0) {
+        return false;
+    }
+
+    cmvr::TrajPtr trajectory;
+    planner_->setPathType(path_type_);
+    planner_->setGridSizes(grid_size_, high_grid_size_);
+    planner_->setSymmetricLimits(
+        std::vector<double>(start.size(), options.velocity * speed_scaling),
+        std::vector<double>(start.size(), options.acceleration));
+    if (!planner_->plan(start, target.position, trajectory)) {
+        return false;
+    }
+
+    const auto raw_samples = planner_->sampleTrajectory(trajectory, sample_period_s_);
+    samples.reserve(raw_samples.size());
+    for (const auto& sample : raw_samples) {
+        JointTrajectorySample dst;
+        dst.t = sample.t;
+        dst.position = toStdVector(sample.q);
+        dst.velocity = toStdVector(sample.qd);
+        samples.push_back(std::move(dst));
+    }
+    return true;
+}
+
+} // namespace cmvr::device

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

@@ -0,0 +1,65 @@
+
+
+add_library(base_motion SHARED
+    joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp
+
+    motion_profile/s_curve/src/s_curve.cpp
+    motion_profile/s_curve/src/s_curve_position_planner.cpp
+    motion_profile/s_curve/src/s_curve_velocity_planner.cpp
+    cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp
+)
+
+target_include_directories(base_motion PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(base_motion PUBLIC
+        OsqpEigen
+		tinyxml2
+        fcl
+        toppra
+		cmvr_es::common
+)
+
+add_library(cmvr_es::base_motion ALIAS base_motion)
+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/include/cartesian_twist_limiter.h

@@ -2,7 +2,7 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
-#include "planner/s_curve_planner/include/s_curve_velocity_planner.h"
+#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h"
 namespace cmvr
 {
 

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

@@ -7,7 +7,7 @@
 #include <Eigen/Core>
 #include <matplot/matplot.h>
 
-#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h"
+#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
 
 namespace cmvr
 {

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/joint_trajectory_planner.h

@@ -4,7 +4,11 @@
 
 #pragma once
 
+#include <memory>
+#include <string>
 #include <vector>
+
+#include <Eigen/Dense>
 #include <toppra/geometric_path/piecewise_poly_path.hpp>
 #include <toppra/parametrizer/const_accel.hpp>
 #include <toppra/parametrizer/spline.hpp>
@@ -33,12 +37,12 @@ namespace cmvr {
             Quintic,
             // 三次B样条
             Natural };
-    class JointSpacePlanner {
+    class JointTrajectoryPlanner {
     public:
 
-        explicit JointSpacePlanner(PathType p):path_type_(p){};
+        explicit JointTrajectoryPlanner(PathType p):path_type_(p){};
-        JointSpacePlanner()=default;
+        JointTrajectoryPlanner()=default;
-        virtual ~JointSpacePlanner() = default;
+        virtual ~JointTrajectoryPlanner() = default;
 
         virtual bool plan(const std::vector<double>& start_joints,
                          const std::vector<double>& goal_joints,
@@ -62,14 +66,20 @@ namespace cmvr {
         }
 
         // 对称限：[-v_max, v_max]、[-a_max, a_max]
-        virtual void setSymmetricLimits(const std::vector<double> &v_max,
+        virtual void setSymmetricLimits(const std::vector<double> &v_max,const std::vector<double> &a_max){
-                                const std::vector<double> &a_max);
+            v_max_ = v_max;
+            a_max_ = a_max;
+        }
 
         // 可调网格密度（默认 150/300）
-        virtual void setGridSizes(int N, int N_high);
+        virtual void setGridSizes(int N, int N_high)
+        {
+            N_grid_ = N;
+            N_grid_high_ = N_high;
+        }
 
         // 切换几何路径类型
-        virtual void setPathType(PathType p);
+        virtual void setPathType(PathType p){path_type_ = p;}
     protected:
         PathType path_type_{PathType::Quintic};
         int N_grid_ = 150;

cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h

@@ -4,7 +4,7 @@
 #include <vector>
 #include <Eigen/Dense>
 
-#include "planner/joint_space_planner/include/joint_space_planner.h"
+#include "../../joint_trajectory_planner.h"
 #include <toppra/geometric_path/piecewise_poly_path.hpp>
 #include <toppra/parametrizer/const_accel.hpp>
 #include <toppra/parametrizer/spline.hpp>
@@ -47,9 +47,9 @@ namespace cmvr {
     };
 
     // 具体规划器：一次/三次/五次可切换；ConstAccel 校验失败自动回退 Spline
-    class ToppraBSpline : public JointSpacePlanner {
+    class ToppraJointTrajectoryPlanner : public JointTrajectoryPlanner {
     public:
-        explicit ToppraBSpline(PathType type = PathType::Quintic);
+        explicit ToppraJointTrajectoryPlanner(PathType type = PathType::Quintic);
 
 
         // 统一入口：两点/多点皆可

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

@@ -3,7 +3,7 @@
 #include <toppra/constraint/linear_joint_acceleration.hpp>
 #include <toppra/solver/seidel.hpp>
 #include <toppra/toppra.hpp>
-#include "planner/joint_space_planner/include/toppra_bspline.h"
+#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
 
 #include <fstream>
 #include <iomanip>
@@ -31,19 +31,19 @@ namespace cmvr {
     Eigen::VectorXd SplineTraj::qd(double t) const { return impl_.eval_single(t, 1); }
     Eigen::VectorXd SplineTraj::qdd(double t) const { return impl_.eval_single(t, 2); }
 
-    // ===== ToppraBSpline =====
+    // ===== ToppraJointTrajectoryPlanner =====
-    ToppraBSpline::ToppraBSpline(PathType type) :JointSpacePlanner(type) {
+    ToppraJointTrajectoryPlanner::ToppraJointTrajectoryPlanner(PathType type) :JointTrajectoryPlanner(type) {
     }
 
 
 
-    bool ToppraBSpline::ensureLimitsSized(std::size_t DoF) {
+    bool ToppraJointTrajectoryPlanner::ensureLimitsSized(std::size_t DoF) {
         if (v_max_.size() != DoF) v_max_.assign(DoF, 1.5);
         if (a_max_.size() != DoF) a_max_.assign(DoF, 3.0);
         return true;
     }
 
-    void ToppraBSpline::sanitizeVsq(toppra::Vector &v) {
+    void ToppraJointTrajectoryPlanner::sanitizeVsq(toppra::Vector &v) {
         for (int i = 0; i < v.size(); ++i) if (v[i] < 0 && v[i] > -1e-10) v[i] = 0;
         if (v.size() > 0) {
             v[0] = 0;
@@ -51,7 +51,7 @@ namespace cmvr {
         }
     }
     // ===== 统一入口：两点/多点 =====
-    bool ToppraBSpline::plan(const std::vector<std::vector<double>>& waypoints,
+    bool ToppraJointTrajectoryPlanner::plan(const std::vector<std::vector<double>>& waypoints,
                              TrajPtr& traj_out) {
         traj_out.reset();
         const size_t M = waypoints.size();
@@ -115,7 +115,7 @@ namespace cmvr {
     }
 
 
-    bool ToppraBSpline::plan(const std::vector<double>& start_joints,
+    bool ToppraJointTrajectoryPlanner::plan(const std::vector<double>& start_joints,
                              const std::vector<double>& goal_joints,
                              TrajPtr& traj_out) {
         if (start_joints.empty() || start_joints.size()!=goal_joints.size()) return false;
@@ -125,7 +125,7 @@ namespace cmvr {
 
 
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildPathUnified(const std::vector<Eigen::VectorXd>& q,
+    ToppraJointTrajectoryPlanner::buildPathUnified(const std::vector<Eigen::VectorXd>& q,
                                     const std::vector<toppra::value_type>& S) {
         const size_t M = q.size();
         if (M == 2) return buildTwoPointPath(q[0], q[1]);
@@ -140,7 +140,7 @@ namespace cmvr {
 
 
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildTwoPointPath(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
+    ToppraJointTrajectoryPlanner::buildTwoPointPath(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
         switch (path_type_) {
             case PathType::Linear:       return buildLinearTwo(q0, q1);
             case PathType::CubicHermite: return buildCubicHermiteTwo(q0, q1);
@@ -152,7 +152,7 @@ namespace cmvr {
 
     //  二点：Linear ——
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildLinearTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
+    ToppraJointTrajectoryPlanner::buildLinearTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
         const size_t DoF = static_cast<size_t>(q0.size());
         toppra::Matrix seg(2, DoF);
         seg.row(0) = (q1 - q0).transpose();
@@ -163,7 +163,7 @@ namespace cmvr {
 
     //  二点：Cubic Hermite（端点速度 0） ——
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildCubicHermiteTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
+    ToppraJointTrajectoryPlanner::buildCubicHermiteTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
         toppra::Vectors pos{q0, q1};
         toppra::Vectors vel{Eigen::VectorXd::Zero(q0.size()),
                             Eigen::VectorXd::Zero(q1.size())};
@@ -174,7 +174,7 @@ namespace cmvr {
 
     //  二点：Quintic rest-to-rest ——
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildQuinticRestToRestTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
+    ToppraJointTrajectoryPlanner::buildQuinticRestToRestTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
         const size_t DoF = static_cast<size_t>(q0.size());
         const Eigen::VectorXd dq = q1 - q0;
         toppra::Matrix seg(6, DoF); // x^5..x^0
@@ -188,7 +188,7 @@ namespace cmvr {
     }
 
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildNaturalTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
+    ToppraJointTrajectoryPlanner::buildNaturalTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
         using PWP = toppra::PiecewisePolyPath;
 
         // positions（两点）
@@ -217,7 +217,7 @@ namespace cmvr {
 
     // —— 多点：Linear ——
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildLinearMulti(const std::vector<Eigen::VectorXd>& q,
+    ToppraJointTrajectoryPlanner::buildLinearMulti(const std::vector<Eigen::VectorXd>& q,
                                     const std::vector<toppra::value_type>& S) {
         const size_t M = q.size(), DoF = q[0].size();
         toppra::Matrices segs; segs.reserve(M-1);
@@ -234,7 +234,7 @@ namespace cmvr {
 
     // —— 多点：Cubic Hermite ——
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildCubicHermiteMulti(const std::vector<Eigen::VectorXd>& q,
+    ToppraJointTrajectoryPlanner::buildCubicHermiteMulti(const std::vector<Eigen::VectorXd>& q,
                                           const std::vector<toppra::value_type>& S) {
         auto v = estimateVelsCatmull(q, S);
         clampNodeVels(v, q, /*k=*/1.0);
@@ -246,7 +246,7 @@ namespace cmvr {
 
 
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildNaturalMulti(const std::vector<Eigen::VectorXd>& q,
+    ToppraJointTrajectoryPlanner::buildNaturalMulti(const std::vector<Eigen::VectorXd>& q,
                                      const std::vector<toppra::value_type>& S) {
         using PWP = toppra::PiecewisePolyPath;
 
@@ -278,7 +278,7 @@ namespace cmvr {
 
     // —— 多点：Quintic C² ——
     std::shared_ptr<toppra::PiecewisePolyPath>
-    ToppraBSpline::buildQuinticC2Multi(const std::vector<Eigen::VectorXd>& q,
+    ToppraJointTrajectoryPlanner::buildQuinticC2Multi(const std::vector<Eigen::VectorXd>& q,
                                        const std::vector<toppra::value_type>& S) {
         const size_t M = q.size(), DoF = q[0].size();
         auto v = estimateVelsCatmull(q, S);
@@ -312,7 +312,7 @@ namespace cmvr {
         return std::make_shared<toppra::PiecewisePolyPath>(segs, std::vector<double>(S.begin(), S.end()));
     }
 
-    std::vector<TrajSample> ToppraBSpline::sampleTrajectory(const TrajPtr &traj, double dt) {
+    std::vector<TrajSample> ToppraJointTrajectoryPlanner::sampleTrajectory(const TrajPtr &traj, double dt) {
         std::vector<TrajSample> out;
         if (!traj) return out;
 
@@ -357,7 +357,7 @@ namespace cmvr {
     }
 
 
-    bool ToppraBSpline::writeTrajectoryCsv(const std::string &filename, const std::vector<TrajSample> &samples) {
+    bool ToppraJointTrajectoryPlanner::writeTrajectoryCsv(const std::string &filename, const std::vector<TrajSample> &samples) {
         if (samples.empty()) return false;
 
         const int dof = static_cast<int>(samples.front().q.size());

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

@@ -3,12 +3,14 @@
 //
 
 #include "gtest/gtest.h"
-#include "planner/joint_space_planner/include/toppra_bspline.h"
+#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h"
-#include "planner/joint_space_planner/include/joint_space_planner_creator.h"
-using namespace cmvr;
-TEST(JOINT_SPACE_PLANNER_TEST,TOPPRA_TEST) {
 
-    auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE);
+#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),
@@ -30,9 +32,9 @@ TEST(JOINT_SPACE_PLANNER_TEST,TOPPRA_TEST) {
 
 }
 
-TEST(JOINT_SPACE_PLANNER_TEST, TOPPRA_WAYPOINTS_TEST) {
+TEST(TOPPRA_JOINT_TRAJECTORY_PLANNER_TEST, TOPPRA_WAYPOINTS_TEST) {
 
-    auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE);
+    auto planner = std::make_shared<ToppraJointTrajectoryPlanner>();
     planner->setPathType(PathType::Quintic);
 
     // 7 自由度对称速度 / 加速度约束

cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve.cpp

@@ -8,7 +8,7 @@
  * @brief S 曲线轨迹生成器实现
  */
 
-#include "common/curve/include/s_curve.h"
+#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h"
 #include <cmath>
 #include <algorithm>
 #include <stdexcept>
@@ -187,7 +187,7 @@ SCurveProfile SCurve::calculateProfile(double start_position, double end_positio
 
 void SCurve::calculateShortProfile(SCurveProfile& profile) const
 {
-  // 短距离：无巡航段（t4 = 0），与 moveL_SCurveLocal 中的 SCurveProfile1D 保持一致。
+  // 短距离：无巡航段（t4 = 0）。
   const double j = profile.j_max;
   const double a = profile.a_max;
   const double v = profile.v_max;

cmvr-es/algorithms/perception/CMakeLists.txt

@@ -2,8 +2,8 @@
 find_package(OpenCV REQUIRED)
 
 add_library(perception SHARED
-        src/tag_relative_target_3d.cpp
+        apriltag/src/tag_relative_target_3d.cpp
-        src/apriltag_perception.cpp
+        apriltag/src/apriltag_perception.cpp
 )
 
 target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -33,7 +33,7 @@ add_library(cmvr_es::perception ALIAS perception)
 install(TARGETS perception LIBRARY DESTINATION lib)
 
 add_executable(tag_relative_target_3d_test
-        src/tag_relative_target_3d_test.cpp
+        apriltag/src/tag_relative_target_3d_test.cpp
 )
 
 target_link_libraries(tag_relative_target_3d_test

cmvr-es/algorithms/perception/apriltag/include/tag_relative_target_3d.h

@@ -12,7 +12,7 @@
 #include <Eigen/Dense>
 #include <opencv2/opencv.hpp>
 
-#include "perception/include/apriltag_perception.h"
+#include "algorithms/perception/apriltag/include/apriltag_perception.h"
 
 namespace cmvr::perception {
 

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

@@ -2,7 +2,7 @@
 // Created by lgv on 2026/3/5.
 //
 
-#include "perception/include/apriltag_perception.h"
+#include "algorithms/perception/apriltag/include/apriltag_perception.h"
 
 #include <algorithm>
 #include <cmath>

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

@@ -1,11 +1,11 @@
-#include "perception/include/tag_relative_target_3d.h"
+#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
 
 #include <algorithm>
 #include <cmath>
 #include <cstring>
 #include <limits>
 
-#include "common/utils/image/image_process.h"
+#include "common/vision/image_projection.h"
 
 namespace cmvr::perception {
 namespace {

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

@@ -2,7 +2,7 @@
 // Created by lgv on 2026/2/26.
 //
 
-#include "perception/include/tag_relative_target_3d.h"
+#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
 #include "devices/camera/realsense_camera/include/realsense_camera.h"
 
 #include <gtest/gtest.h>
@@ -301,7 +301,6 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
     cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
     cam_cfg.set_buffer_size(30);
     cam_cfg.set_sync(true);
-    cam_cfg.set_enable(true);
 
     auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
 
@@ -623,7 +622,6 @@ TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) {
 //     cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR);
 //     cam_cfg.set_buffer_size(30);
 //     cam_cfg.set_sync(true);
-//     cam_cfg.set_enable(true);
 //
 //     auto camera = std::make_shared<cmvr::device::RealsenseCamera>(cam_cfg);
 //     cmvr::perception::TagRelativeTarget3D tracker(camera);

cmvr-es/applications/CMakeLists.txt

@@ -1,31 +0,0 @@
-add_library(applications
-       src/touch_screen_app.cpp
-)
-
-target_include_directories(applications PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-target_link_libraries(applications
-        PUBLIC
-        cmvr_es::controller
-        cmvr_es::common
-        PRIVATE
-        cmvr_es::device_manager
-)
-
-add_library(cmvr_es::applications ALIAS applications)
-install(TARGETS applications LIBRARY DESTINATION lib)
-
-add_executable(touch_screen_app_test
-        src/touch_screen_app_test.cpp
-)
-
-target_link_libraries(touch_screen_app_test PRIVATE
-        cmvr_es::applications
-        cmvr_es::device_manager
-        cmvr_es::service
-        cmvr_es::monitor_manager
-        gtest
-        gtest_main
-        pthread
-        glog
-)

cmvr-es/applications/include/touch_screen_app.h

@@ -1,329 +0,0 @@
-#pragma once
-
-#ifndef CMVR_ES_TOUCH_SCREEN_APP_H
-#define CMVR_ES_TOUCH_SCREEN_APP_H
-
-#include <array>
-#include <chrono>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include <Eigen/Dense>
-
-#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
-#include "controller/include/ibvs_controller.h"
-#include "devices/camera/abstract_camera.h"
-#include "devices/dexhand/abstract_dexhand.h"
-#include "devices/robot/abstract_robot.h"
-#include "perception/include/apriltag_perception.h"
-#include "perception/include/tag_relative_target_3d.h"
-
-namespace cmvr::app {
-
-class TouchScreenApp {
-public:
-    enum class Phase {
-        IDLE = 0,       // 空闲，尚未开始任务。
-        ALIGNING,       // 视觉对准阶段：持续 IBVS 对齐目标点。
-        ALIGN_REACHED,  // 视觉对准已达到阈值，等待进入下一阶段。
-        TOUCHING,       // 前进触控阶段：沿设定方向向屏幕推进。
-        DWELLING,       // 已检测到接触，保持当前位置短暂停留。
-        RETRACTING,     // 回退阶段：沿设定回退方向离开屏幕。
-        DONE,           // 整个流程成功完成。
-        FAILED          // 流程失败并已停止。
-    };
-
-    enum class Status {
-        IDLE = 0,                 // 空闲状态。
-        NOT_INITIALIZED,          // 尚未调用 init() 完成初始化。
-        INVALID_CONFIG,           // 配置非法，无法启动或应用参数。
-        CONTROL_JOINT_MISMATCH,   // 控制关节顺序与 IK 链不一致。
-        ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。
-        ALIGN_WAITING_TRACK,      // 对准阶段等待目标点跟踪恢复成功。
-        ALIGN_TARGET_SETUP_FAILED,// 视觉目标设置失败，setTargetFromPointInTag 失败。
-        ALIGN_COMPUTE_FAILED,     // 对准阶段 IBVS 或 IK 计算失败。
-        ALIGN_TIMEOUT,            // 对准阶段超时仍未收敛。
-        ALIGNING,                 // 正在执行视觉对准。
-        ALIGN_REACHED,            // 视觉对准完成。
-        TOUCHING,                 // 正在向前触控。
-        TACTILE_UNAVAILABLE,      // 触觉数据不可用。
-        TOUCH_TRIGGERED,          // 已检测到接触触发。
-        TOUCH_FORWARD_TIMEOUT,    // 前进触控时间到，但未触发接触。
-        RETRACTING,               // 正在回退离开屏幕。
-        DONE,                     // 流程成功完成。
-        STOPPED,                  // 被外部 stop() 主动停止。
-        ROBOT_STATE_FAILED,       // 读取机器人状态失败。
-        ROBOT_COMMAND_FAILED      // 向机器人下发控制命令失败。
-    };
-
-    enum class AlignMode {
-        POSE_AND_POSITION = 0,  // 使用配置里的固定 rx/ry/rz 与位置一起对齐。
-        RX_RY_AND_POSITION,     // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。
-        POSITION_ONLY           // 保留锁定时看到的完整 tag 姿态，只按位置对齐。
-    };
-
-    enum class TactileCriterion {
-        FZ = 0,
-        MAGNITUDE
-    };
-
-    struct Config {
-        // 是否在触控流程开始前先回到指定初始关节位姿。
-        bool move_to_init_position_before_start{false};
-        // 是否在触控流程结束（DONE/FAILED）后回到指定初始关节位姿。
-        bool move_to_init_position{false};
-        // 初始关节位姿目标，在前置回位或结束后回位开启时使用。
-        std::vector<device::JointPoint> init_joint_positions{};
-        // 回到初始位姿时的 moveJ 主导速度，单位 rad/s。
-        double init_movej_vel{1.0};
-        // 回到初始位姿时的 moveJ 主导加速度，单位 rad/s^2。
-        double init_movej_acc{2.0};
-
-        // IBVS / IK 初始化参数。
-        // URDF 文件路径，用于初始化 IbvsController 内部 IK 求解器。
-        std::string urdf_path;
-        // IK 链基座 link 名称。
-        std::string base_link{"PELVIS_S"};
-        // IK 链末端法兰 link 名称。
-        std::string flange_link{"R_WRIST_R_S"};
-        // URDF 中相机 link 名称。
-        std::string camera_link;
-
-        // 视觉感知参数。
-        // AprilTag 实际边长，单位米。
-        double tag_size_m{0.12};
-        // 感知更新时如何使用深度图：不用 / 尽量用 / 必须用。
-        perception::AprilTagPerception::DepthPolicy depth_policy{
-            perception::AprilTagPerception::DepthPolicy::NONE};
-        // 从像素恢复目标点时采用 tag 平面求交，还是深度图反投影。
-        perception::TagRelativeTarget3D::TargetPointMethod target_point_method{
-            perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE};
-
-        // 视觉阶段目标：触控点在相机坐标系中的 hover 位置。
-        // 目标点在相机坐标系中的期望位置，单位米。
-        Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.40};
-        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
-        double target_rx{3.14159265358979323846};
-        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
-        double target_ry{0.0};
-        // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
-        double target_rz{0.0};
-        // 对齐模式：1) 固定姿态+位置；2) 固定 rx/ry + 锁定时 yaw + 位置；3) 仅位置。
-        AlignMode align_mode{AlignMode::POSE_AND_POSITION};
-
-        // IBVS 参数。
-        // 视觉伺服增益 lambda。
-        double ibvs_lambda{0.6};
-        // DLS IK 阻尼系数 mu。
-        double ibvs_mu{0.1};
-        // 单关节最大速度，单位 rad/s。
-        double ibvs_qdot_max{0.15};
-        // 相机 twist 六维限幅 `[vx, vy, vz, wx, wy, wz]`。
-        std::array<double, 6> ibvs_vmax6{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
-        // 相机 twist 六维加速度限幅 `[ax, ay, az, alphax, alphay, alphaz]`；
-        // 分量小于等于 0 表示该维度不启用加速度限幅。
-        std::array<double, 6> ibvs_amax6{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
-        // 相机 twist 一阶低通滤波系数；取值在 (0, 1) 时启用低通，默认 1.0 表示不过滤。
-        double ibvs_twist_filter_alpha{1.0};
-        // 是否启用关节限位回避。
-        bool enable_joint_limit_avoidance{true};
-        // 关节限位回避增益。
-        double joint_limit_avoidance_gain{0.2};
-        // 距离关节限位多近时开始回避，按关节范围比例计算。
-        double joint_limit_avoidance_margin_ratio{0.15};
-        // 单关节限位回避最大推回速度。
-        double joint_limit_avoidance_max_push{0.25};
-
-        // `AbstractCamera` 相机坐标系到 ViSP 相机坐标系的旋转矩阵。
-        Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()};
-        // `AbstractCamera` 相机坐标系到 URDF 相机坐标系的旋转矩阵。
-        Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()};
-
-        // 关节控制链，默认右臂 7 轴。
-        // 顺序必须与 IbvsController 内部 IK 链顺序一致。
-        std::vector<std::string> control_joint_names{
-            "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
-            "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
-
-        // 视觉对准收敛判据 `[x, y, z, rx, ry, rz]`。
-        // 其中位置误差单位米，旋转误差单位弧度；旋转部分使用目标姿态误差 rotvec 的三个分量分别比较。
-        std::array<double, 6> align_error_threshold6{{0.003, 0.003, 0.010,
-                                                       0.08726646259971647,
-                                                       0.08726646259971647,
-                                                       0.08726646259971647}};
-        // 连续多少帧都满足阈值，才认为对准完成。
-        int align_stable_frames{5};
-        // 对准阶段超时时间，单位秒。
-        double align_timeout_s{10.0};
-        // 为 true 时对准完成后暂停，不自动进入触控阶段。
-        bool pause_after_align_reached{false};
-
-        // 触控阶段前进方向。当前按末端 Tool 坐标系解释，字段名保留兼容。
-        // 为 true 时，TOUCHING 阶段使用 speedL；为 false 时使用 moveL。
-        // 当前 moveL 路径为同步前进，执行完成后直接结束流程，并按配置决定是否回初始位姿。
-        bool touch_use_speedl{true};
-        // 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`，单位 m/s 和 rad/s。
-        // 当前机器人上 `[0, -0.08, 0, 0, 0, 0]` 表示沿 Tool -Y 方向向前触屏。
-        // 当 TOUCHING 使用 moveL 时，会取其线速度方向并按 touch_forward_l 构造位移目标。
-        Eigen::Matrix<double, 6, 1> touch_twist_base{
-            (Eigen::Matrix<double, 6, 1>() << 0.0, -0.08, 0.0, 0.0, 0.0, 0.0).finished()};
-        // 触控阶段 speedL 的加速度参数。
-        double touch_speedl_acceleration{3.0};
-        // 触控阶段使用 moveL 时，沿 touch_twist_base 线速度方向前进的距离，单位米。
-        double touch_forward_l{0.08};
-        // 触控阶段使用 moveL 时的末端速度，单位 m/s。
-        double touch_movel_speed{0.25};
-        // 触控阶段使用 moveL 时的末端加速度，单位 m/s^2。
-        double touch_movel_acceleration{1.2};
-        // 触控阶段使用 moveL 时的末端 jerk，单位 m/s^3。
-        double touch_movel_jerk{5.0};
-        // 触控阶段使用 moveL 时的关节速度上限；为空时退回 robot->moveL 默认值。
-        std::vector<double> touch_movel_qd_max{2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5};
-        // 前进触控阶段的最大累计位移，单位米。
-        // 该距离仅在 TOUCHING 使用 speedL 时生效，由 TouchScreenApp 根据末端相对触控起点的累计位移判断。
-        // 大于 0 时，达到该距离后无论压力是否达阈值，都会立即进入回退阶段。
-        // 小于等于 0 时，表示不启用这条限制。
-        double touch_speedl_forward_l{0.08};
-
-        // 接触后停留与回退。当前按末端 Tool 坐标系解释。
-        // 检测到接触后在当前位置停留的时间，单位秒。
-        // 当该值小于 0 时，表示不做停留，直接把 speedL 切换为回退。
-        double dwell_time_s{0.05};
-        // 回退阶段的 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`。
-        // 当前机器人上 `[0, +0.08, 0, 0, 0, 0]` 表示沿 Tool +Y 方向向后离屏。
-        // 回退阶段统一使用 speedL。
-        Eigen::Matrix<double, 6, 1> retract_twist_base{
-            (Eigen::Matrix<double, 6, 1>() << 0.0, 0.08,0.0, 0.0, 0.0, 0.0).finished()};
-        // 回退阶段 speedL 的加速度参数。
-        double retract_acceleration{3.0};
-        // 回退阶段 speedL 持续时间，单位秒。
-        double retract_duration_s{0.8};
-
-        // 指尖触觉判据。
-        // 使用哪根手指的触觉阵列判断是否接触。
-        device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX};
-        // 使用该手指的哪个触觉区域。
-        device::AbstractDexHand::TactileRegion tactile_region{
-            device::AbstractDexHand::TactileRegion::TIP};
-        // 三维合力标量化方式：直接使用法向 fz，或使用三维力模长。
-        TactileCriterion tactile_criterion{TactileCriterion::FZ};
-        // 触觉区域三维合力按 tactile_criterion 标量化后的阈值；超过该值认为已经接触。
-        double tactile_pressure_sum_threshold{100.0};
-        // 保留兼容的旧字段；当前合力判定逻辑不再使用非零点数量阈值。
-        int tactile_nonzero_count_threshold{1};
-    };
-
-    TouchScreenApp();
-    ~TouchScreenApp() = default;
-
-    bool init();
-    bool init(const std::shared_ptr<device::AbstractRobot>& robot,
-              const std::shared_ptr<device::AbstractDexHand>& dexhand,
-              const std::shared_ptr<device::AbstractCamera>& camera);
-    bool init(const std::shared_ptr<device::AbstractRobot>& robot,
-              const std::shared_ptr<device::AbstractDexHand>& dexhand,
-              const std::shared_ptr<device::AbstractCamera>& camera,
-              const cmvr::config::TouchScreenAppConfig& config);
-
-    bool setConfigFromProto();
-    bool setConfigFromProto(const cmvr::config::TouchScreenAppConfig& config);
-    bool setTouchSpeedlForwardL(double forward_l);
-
-    bool startFromPixel(int u, int v);
-    bool step(double dt);
-    void stop();
-
-    Phase phase() const { return phase_; }
-    Status lastStatus() const { return last_status_; }
-    static const char* phaseToString(Phase phase);
-    static const char* statusToString(Status status);
-
-    bool isBusy() const { return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED ||
-                                 phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING ||
-                                 phase_ == Phase::RETRACTING; }
-    bool isFinished() const { return phase_ == Phase::DONE; }
-    bool isFailed() const { return phase_ == Phase::FAILED; }
-
-    int targetU() const { return target_u_; }
-    int targetV() const { return target_v_; }
-    double lastTouchPressureSum() const { return last_touch_pressure_sum_; }
-    int lastTouchNonzeroCount() const { return last_touch_nonzero_count_; }
-    int lastActiveTagId() const { return last_active_tag_id_; }
-    const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; }
-
-    const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
-    const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
-    const IbvsController& ibvs() const { return ibvs_; }
-
-private:
-    using Clock = std::chrono::steady_clock;
-
-    static bool configFromProto(const cmvr::config::TouchScreenAppConfig& proto_config,
-                                Config& config_out);
-    void setConfig(const Config& config);
-    bool applyConfig();
-    bool validateControlJointNames() const;
-    bool stepAligning(double dt);
-    bool stepTouching();
-    bool stepDwelling();
-    bool stepRetracting();
-
-    bool readControlledJointPositions(std::vector<double>& q_out) const;
-    bool sendJointVelocity(const std::vector<double>& qdot) const;
-    bool sendZeroJointVelocity() const;
-    void hardStopIbvsMotion();
-    bool holdCurrentControlledPosition() const;
-    bool moveToInitPositionIfEnabled() const;
-    bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
-    void logTouchingSpeedLState() const;
-
-    bool startTouchPhase();
-    bool handleTouchTriggered(bool stop_forward_motion);
-    bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract);
-    void enterFailed(Status status);
-
-    bool updateTouchPressure();
-
-private:
-    std::shared_ptr<device::AbstractRobot> robot_{nullptr};
-    std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
-    std::shared_ptr<device::AbstractCamera> camera_{nullptr};
-
-    std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
-    perception::TagRelativeTarget3D tracker_;
-    IbvsController ibvs_;
-    Config config_{};
-
-    Phase phase_{Phase::IDLE};
-    Phase phase_after_retract_{Phase::DONE};
-    Status last_status_{Status::NOT_INITIALIZED};
-
-    bool initialized_{false};
-    bool target_locked_{false};
-    bool ibvs_target_initialized_{false};
-    bool touch_command_started_{false};
-    bool retract_command_started_{false};
-
-    int target_u_{-1};
-    int target_v_{-1};
-    int align_stable_count_{0};
-    int last_active_tag_id_{-1};
-
-    double last_touch_pressure_sum_{0.0};
-    int last_touch_nonzero_count_{0};
-    Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
-    bool locked_target_rotation_valid_{false};
-    Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
-    bool touch_start_position_valid_{false};
-    Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
-
-    Clock::time_point phase_start_time_{};
-    Status final_status_after_retract_{Status::DONE};
-};
-
-using touch_screen_app = TouchScreenApp;
-
-} // namespace cmvr::app
-
-#endif // CMVR_ES_TOUCH_SCREEN_APP_H

cmvr-es/applications/src/touch_screen_app_test.cpp

@@ -1,105 +0,0 @@
-#include "gtest/gtest.h"
-
-#include <algorithm>
-#include <chrono>
-#include <iostream>
-#include <thread>
-
-#include "applications/include/touch_screen_app.h"
-
-#include "include/device_manager.h"
-#include "service/grpc/include/server_runner.h"
-namespace {
-
-constexpr const char* kConfigPath =
-    "/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml";
-constexpr int kTargetU = 1280 / 2.0;
-constexpr int kTargetV = 720 / 2.0;
-
-
-void run_touch_once(int u, int v) {
-
-    const XmlNode config(kConfigPath);
-    // auto& dm = cmvr::device::DeviceManager::getInstance();
-    cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager"));
-
-
-    // cmvr::service::ServerRunner runner;
-    // runner.start(config);
-
-    ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
-
-    cmvr::app::TouchScreenApp app;
-    ASSERT_TRUE(app.init())
-        << "TouchScreenApp init failed";
-
-    ASSERT_TRUE(app.startFromPixel(u, v)) << "startFromPixel failed";
-
-    bool align_reached = false;
-    bool touch_triggered = false;
-    auto last_step_time = std::chrono::steady_clock::now();
-    bool first_step = true;
-    while (app.isBusy()) {
-        const auto now = std::chrono::steady_clock::now();
-        double dt = 0.02;
-        if (!first_step) {
-            dt = std::chrono::duration<double>(now - last_step_time).count();
-            dt = std::clamp(dt, 0.005, 0.05);
-        }
-        last_step_time = now;
-        first_step = false;
-
-        const bool step_ok = app.step(dt);
-        const auto& p_c_target = app.tracker().lastTargetInCamera();
-        std::cout << "phase=" << cmvr::app::TouchScreenApp::phaseToString(app.phase())
-                  << ", status=" << cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
-                  << ", active_tag=" << app.lastActiveTagId()
-                  << ", target_c=[" << p_c_target.x() << ", "
-                  << p_c_target.y() << ", "
-                  << p_c_target.z() << "]"
-                  << ", nonzero_count=" << app.lastTouchNonzeroCount()
-                  << ", pressure_sum=" << app.lastTouchPressureSum()
-                  << ", err_c=[" << app.lastAlignErrorCamera().x() << ", "
-                  << app.lastAlignErrorCamera().y() << ", "
-                  << app.lastAlignErrorCamera().z() << "]\n";
-
-        if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::ALIGN_REACHED) {
-            std::cout << "align reached, target_c=[" << p_c_target.x() << ", "
-                      << p_c_target.y() << ", "
-                      << p_c_target.z() << "]\n";
-            align_reached = true;
-        }
-        if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) {
-            std::cout << "touch triggered, nonzero_count=" << app.lastTouchNonzeroCount()
-                      << ", pressure_sum=" << app.lastTouchPressureSum() << "\n";
-            touch_triggered = true;
-        }
-        if (!step_ok) {
-            const auto failed_status = app.lastStatus();
-            app.stop();
-            FAIL() << "touch flow failed, status="
-                   << cmvr::app::TouchScreenApp::statusToString(failed_status);
-        }
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-    }
-
-    const auto final_status = app.lastStatus();
-    ASSERT_TRUE(align_reached)
-        << "align was never reached, final status="
-        << cmvr::app::TouchScreenApp::statusToString(final_status);
-    ASSERT_TRUE(touch_triggered)
-        << "touch was never triggered, final status="
-        << cmvr::app::TouchScreenApp::statusToString(final_status);
-    ASSERT_TRUE(app.isFinished())
-        << "touch did not finish successfully, final status="
-        << cmvr::app::TouchScreenApp::statusToString(final_status);
-    std::cout << "touch done, final status="
-              << cmvr::app::TouchScreenApp::statusToString(final_status) << "\n";
-    app.stop();
-}
-
-} // namespace
-
-TEST(TouchScreenAppTest, RunTouchOnceOnRealRobot) {
-    run_touch_once(kTargetU, kTargetV);
-}

cmvr-es/common/CMakeLists.txt

@@ -1,19 +1,12 @@
 #find_package(protobuf REQUIRED)
 
-
+add_library(common SHARED
-file(GLOB SRC
+        ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/camera_capture.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/utils/config_helper/src/config_setting.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/realsense_capture.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/CameraCapture.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/video_frame_encoder.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/RealSenseCapture.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/video_writer.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoFrameEncoder.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/VideoWriter.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/curve/src/s_curve.cpp
 )
 
-
-add_library(common SHARED ${SRC})
-
-
 target_include_directories(common PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 target_link_libraries(common PUBLIC
@@ -52,17 +45,3 @@ install(TARGETS common LIBRARY DESTINATION lib)
 #        pthread
 #)
 #
-#add_executable(s_curve_test
-#        curve/src/s_curve_test.cpp
-#)
-#
-#target_link_libraries(s_curve_test
-#        PRIVATE
-#        cmvr_es::common
-#        cmvr_es::proto
-#        glog
-#        gtest
-#        gtest_main
-#        pthread
-#        Matplot++::matplot
-#)

cmvr-es/common/base/grpc_utils.h

@@ -5,6 +5,8 @@
 #ifndef GRPC_UTILS_H
 #define GRPC_UTILS_H
 
+#include <chrono>
+
 #include <grpcpp/grpcpp.h>
 
 inline void setCurrentTimestamp(google::protobuf::Timestamp* ts) {

cmvr-es/common/base/logging/CMakeLists.txt

@@ -0,0 +1,15 @@
+add_library(logging STATIC
+        logger.cpp
+)
+
+target_include_directories(logging PUBLIC
+        ${PROJECT_SOURCE_DIR}/cmvr-es
+)
+
+target_link_libraries(logging PUBLIC
+        cmvr_es::proto
+)
+
+add_library(cmvr_es::logging ALIAS logging)
+
+install(TARGETS logging ARCHIVE DESTINATION lib)

cmvr-es/common/base/logging/logger.cpp

@@ -0,0 +1,365 @@
+#include "common/base/logging/logger.h"
+
+#include <cstdlib>
+#include <ctime>
+#include <iomanip>
+#include <iostream>
+#include <system_error>
+#include <thread>
+
+namespace cmvr::logging {
+
+namespace {
+
+std::string formatTimestamp()
+{
+    const auto now = std::chrono::system_clock::now();
+    const auto time = std::chrono::system_clock::to_time_t(now);
+    const auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(
+        now.time_since_epoch()) % 1000;
+
+    std::tm local_time{};
+    localtime_r(&time, &local_time);
+
+    std::ostringstream output;
+    output << std::put_time(&local_time, "%Y-%m-%d %H:%M:%S")
+           << '.' << std::setfill('0') << std::setw(3) << milliseconds.count();
+    return output.str();
+}
+
+const char* baseName(const char* path)
+{
+    if (path == nullptr) {
+        return "unknown";
+    }
+    const char* result = path;
+    for (const char* current = path; *current != '\0'; ++current) {
+        if (*current == '/' || *current == '\\') {
+            result = current + 1;
+        }
+    }
+    return result;
+}
+
+const char* terminalColor(const Level level)
+{
+    switch (level) {
+    case Level::WARNING:
+        return "\033[33m";
+    case Level::ERROR:
+    case Level::FATAL:
+        return "\033[31m";
+    case Level::DEBUG:
+    case Level::INFO:
+    case Level::COUNT:
+        break;
+    }
+    return "";
+}
+
+} // namespace
+
+Logger& Logger::instance()
+{
+    static Logger logger;
+    return logger;
+}
+
+Logger::Logger()
+{
+    routes_[index(Level::INFO)].terminal = true;
+    routes_[index(Level::WARNING)].terminal = true;
+    routes_[index(Level::ERROR)].terminal = true;
+    routes_[index(Level::FATAL)].terminal = true;
+}
+
+bool Logger::initialize(const config::LoggerConfig& config,
+                        const std::string& application_name,
+                        const std::filesystem::path& executable_directory)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    std::array<Route, static_cast<std::size_t>(Level::COUNT)> new_routes{};
+    std::array<bool, static_cast<std::size_t>(Level::COUNT)> configured{};
+    bool any_file_route = false;
+    Level new_minimum_level;
+
+    if (!convertLevel(config.minimum_level(), new_minimum_level)) {
+        std::cerr << "Invalid minimum logging level" << std::endl;
+        return false;
+    }
+
+    for (const auto& route_config : config.routes()) {
+        Level level;
+        if (!convertLevel(route_config.level(), level)) {
+            std::cerr << "Invalid logging level in route" << std::endl;
+            return false;
+        }
+        const auto route_index = index(level);
+        if (configured[route_index]) {
+            std::cerr << "Duplicate logging route for " << levelName(level) << std::endl;
+            return false;
+        }
+        configured[route_index] = true;
+        new_routes[route_index] = {route_config.terminal(), route_config.file()};
+        any_file_route = any_file_route || route_config.file();
+    }
+
+    if (config.routes().empty()) {
+        std::cerr << "No logging routes configured" << std::endl;
+        return false;
+    }
+
+    std::ofstream new_log_file;
+    std::filesystem::path new_log_path;
+    if (any_file_route) {
+        std::filesystem::path directory = config.directory().empty()
+            ? std::filesystem::path("../log")
+            : std::filesystem::path(config.directory());
+        if (directory.is_relative()) {
+            directory = executable_directory / directory;
+        }
+        directory = directory.lexically_normal();
+
+        std::error_code error;
+        std::filesystem::create_directories(directory, error);
+        if (error) {
+            std::cerr << "Failed to create log directory: " << directory
+                      << ": " << error.message() << std::endl;
+            return false;
+        }
+
+        const std::string file_name = application_name.empty() ? "cmvr_es.log" : application_name + ".log";
+        new_log_path = directory / file_name;
+        new_log_file.open(new_log_path, std::ios::out | std::ios::app);
+        if (!new_log_file.is_open()) {
+            std::cerr << "Failed to open log file: " << new_log_path << std::endl;
+            return false;
+        }
+    }
+
+    if (log_file_.is_open()) {
+        log_file_.flush();
+        log_file_.close();
+    }
+
+    routes_ = new_routes;
+    format_ = parseFormat_(config);
+    minimum_level_ = new_minimum_level;
+    log_file_path_ = std::move(new_log_path);
+    log_file_ = std::move(new_log_file);
+    max_file_size_bytes_ = static_cast<std::uintmax_t>(
+        config.max_file_size_mb() > 0 ? config.max_file_size_mb() : 100) * 1024U * 1024U;
+    flush_interval_ = std::chrono::seconds(
+        config.flush_interval_seconds() > 0 ? config.flush_interval_seconds() : 1);
+    last_flush_ = std::chrono::steady_clock::now();
+    initialized_ = true;
+    return true;
+}
+
+void Logger::shutdown()
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (log_file_.is_open()) {
+        log_file_.flush();
+        log_file_.close();
+    }
+    initialized_ = false;
+}
+
+bool Logger::enabled(const Level level) const
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (level != Level::FATAL && level < minimum_level_) {
+        return false;
+    }
+    const auto& route = routes_[index(level)];
+    return route.terminal || route.file || level == Level::FATAL;
+}
+
+void Logger::write(const Level level,
+                   const char* source_file,
+                   const int source_line,
+                   const std::string& message)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (level != Level::FATAL && level < minimum_level_) {
+        return;
+    }
+    const Route route = routes_[index(level)];
+    if (!route.terminal && !route.file && level != Level::FATAL) {
+        return;
+    }
+
+    const std::string line = formatLine_(level, source_file, source_line, message);
+
+    if (route.terminal || level == Level::FATAL) {
+        writeTerminal_(level, line);
+    }
+    if (route.file && log_file_.is_open()) {
+        rotateIfNeeded_();
+        log_file_ << line << '\n';
+        const auto now = std::chrono::steady_clock::now();
+        if (level == Level::ERROR || level == Level::FATAL || now - last_flush_ >= flush_interval_) {
+            log_file_.flush();
+            last_flush_ = now;
+        }
+    }
+}
+
+std::size_t Logger::index(const Level level)
+{
+    return static_cast<std::size_t>(level);
+}
+
+const char* Logger::levelName(const Level level)
+{
+    switch (level) {
+    case Level::DEBUG: return "DEBUG";
+    case Level::INFO: return "INFO";
+    case Level::WARNING: return "WARNING";
+    case Level::ERROR: return "ERROR";
+    case Level::FATAL: return "FATAL";
+    case Level::COUNT: break;
+    }
+    return "UNKNOWN";
+}
+
+bool Logger::convertLevel(const config::LogLevel input, Level& output)
+{
+    switch (input) {
+    case config::LOG_LEVEL_DEBUG: output = Level::DEBUG; return true;
+    case config::LOG_LEVEL_INFO: output = Level::INFO; return true;
+    case config::LOG_LEVEL_WARNING: output = Level::WARNING; return true;
+    case config::LOG_LEVEL_ERROR: output = Level::ERROR; return true;
+    case config::LOG_LEVEL_FATAL: output = Level::FATAL; return true;
+    case config::LOG_LEVEL_UNSPECIFIED: break;
+    }
+    return false;
+}
+
+Format Logger::parseFormat_(const config::LoggerConfig& config)
+{
+    Format format;
+    if (!config.has_format()) {
+        return format;
+    }
+
+    const auto& format_config = config.format();
+    if (format_config.has_show_time()) {
+        format.show_time = format_config.show_time();
+    }
+    if (format_config.has_show_level()) {
+        format.show_level = format_config.show_level();
+    }
+    if (format_config.has_show_thread_id()) {
+        format.show_thread_id = format_config.show_thread_id();
+    }
+    if (format_config.has_show_source_location()) {
+        format.show_source_location = format_config.show_source_location();
+    }
+    return format;
+}
+
+std::string Logger::formatLine_(const Level level,
+                                const char* source_file,
+                                const int source_line,
+                                const std::string& message) const
+{
+    std::ostringstream formatted;
+    bool has_prefix = false;
+
+    const auto append_prefix_part = [&formatted, &has_prefix](const std::string& part) {
+        if (has_prefix) {
+            formatted << ' ';
+        }
+        formatted << part;
+        has_prefix = true;
+    };
+
+    if (format_.show_time) {
+        append_prefix_part(formatTimestamp());
+    }
+    if (format_.show_level) {
+        append_prefix_part(std::string("[") + levelName(level) + "]");
+    }
+    if (format_.show_thread_id) {
+        std::ostringstream thread_id;
+        thread_id << '[' << std::this_thread::get_id() << ']';
+        append_prefix_part(thread_id.str());
+    }
+    if (format_.show_source_location) {
+        std::ostringstream source_location;
+        source_location << '[' << baseName(source_file) << ':' << source_line << ']';
+        append_prefix_part(source_location.str());
+    }
+
+    if (has_prefix) {
+        formatted << ' ';
+    }
+    formatted << message;
+    return formatted.str();
+}
+
+void Logger::rotateIfNeeded_()
+{
+    if (log_file_path_.empty() || !log_file_.is_open()) {
+        return;
+    }
+
+    std::error_code error;
+    const auto size = std::filesystem::file_size(log_file_path_, error);
+    if (error || size < max_file_size_bytes_) {
+        return;
+    }
+
+    log_file_.flush();
+    log_file_.close();
+
+    const auto backup_path = log_file_path_.string() + ".1";
+    std::filesystem::remove(backup_path, error);
+    error.clear();
+    std::filesystem::rename(log_file_path_, backup_path, error);
+    if (error) {
+        std::cerr << "Failed to rotate log file: " << error.message() << std::endl;
+    }
+    log_file_.open(log_file_path_, std::ios::out | std::ios::trunc);
+}
+
+void Logger::writeTerminal_(const Level level, const std::string& line)
+{
+    const char* color = terminalColor(level);
+    if (color[0] == '\0') {
+        std::cout << line << std::endl;
+        return;
+    }
+    std::cout << color << line << "\033[0m" << std::endl;
+}
+
+LogMessage::LogMessage(const Level level, const char* source_file, const int source_line)
+    : level_(level), source_file_(source_file), source_line_(source_line)
+{
+}
+
+LogMessage::~LogMessage()
+{
+    Logger::instance().write(level_, source_file_, source_line_, stream_.str());
+    if (level_ == Level::FATAL) {
+        Logger::instance().shutdown();
+        std::abort();
+    }
+}
+
+bool initLogging(const config::LoggerConfig& config,
+                 const std::string& application_name,
+                 const std::filesystem::path& executable_directory)
+{
+    return Logger::instance().initialize(config, application_name, executable_directory);
+}
+
+void shutdownLogging()
+{
+    Logger::instance().shutdown();
+}
+
+} // namespace cmvr::logging

cmvr-es/common/base/logging/logger.h

@@ -0,0 +1,116 @@
+#pragma once
+
+#include <atomic>
+#include <array>
+#include <chrono>
+#include <filesystem>
+#include <fstream>
+#include <mutex>
+#include <ostream>
+#include <sstream>
+#include <string>
+
+#include "cmvr/config/logger_config/logger_config.pb.h"
+
+namespace cmvr::logging {
+
+enum class Level {
+    DEBUG = 0,
+    INFO,
+    WARNING,
+    ERROR,
+    FATAL,
+    COUNT
+};
+
+struct Route {
+    bool terminal{false};
+    bool file{false};
+};
+
+struct Format {
+    bool show_time{true};
+    bool show_level{true};
+    bool show_thread_id{true};
+    bool show_source_location{true};
+};
+
+class Logger {
+public:
+    static Logger& instance();
+
+    bool initialize(const config::LoggerConfig& config,
+                    const std::string& application_name,
+                    const std::filesystem::path& executable_directory);
+    void shutdown();
+    bool enabled(Level level) const;
+    void write(Level level, const char* source_file, int source_line, const std::string& message);
+
+private:
+    Logger();
+
+    static std::size_t index(Level level);
+    static const char* levelName(Level level);
+    static bool convertLevel(config::LogLevel input, Level& output);
+    static Format parseFormat_(const config::LoggerConfig& config);
+
+    std::string formatLine_(Level level, const char* source_file, int source_line, const std::string& message) const;
+    void rotateIfNeeded_();
+    void writeTerminal_(Level level, const std::string& line);
+
+    mutable std::mutex mutex_;
+    std::array<Route, static_cast<std::size_t>(Level::COUNT)> routes_{};
+    Format format_{};
+    Level minimum_level_{Level::INFO};
+    std::filesystem::path log_file_path_;
+    std::ofstream log_file_;
+    std::uintmax_t max_file_size_bytes_{100U * 1024U * 1024U};
+    std::chrono::seconds flush_interval_{1};
+    std::chrono::steady_clock::time_point last_flush_{};
+    bool initialized_{false};
+};
+
+class LogMessage {
+public:
+    LogMessage(Level level, const char* source_file, int source_line);
+    ~LogMessage();
+
+    std::ostream& stream() { return stream_; }
+
+private:
+    Level level_;
+    const char* source_file_;
+    int source_line_;
+    std::ostringstream stream_;
+};
+
+class LogMessageVoidify {
+public:
+    void operator&(std::ostream&) const {}
+};
+
+bool initLogging(const config::LoggerConfig& config,
+                 const std::string& application_name,
+                 const std::filesystem::path& executable_directory);
+void shutdownLogging();
+
+} // namespace cmvr::logging
+
+#define CMVR_LOG(level) \
+    !::cmvr::logging::Logger::instance().enabled(::cmvr::logging::Level::level) \
+        ? static_cast<void>(0) \
+        : ::cmvr::logging::LogMessageVoidify() & \
+              ::cmvr::logging::LogMessage( \
+                  ::cmvr::logging::Level::level, __FILE__, __LINE__).stream()
+
+#define CMVR_LOG_EVERY_N(level, n) \
+    if (![]() { \
+            static std::atomic<unsigned long> counter{0}; \
+            return counter.fetch_add(1, std::memory_order_relaxed) % (n) == 0; \
+        }()) {} else CMVR_LOG(level)
+
+#define CMVR_LOG_IF_EVERY_N(level, condition, n) \
+    if (!(condition) || ![]() { \
+            static std::atomic<unsigned long> counter{0}; \
+            return counter.fetch_add(1, std::memory_order_relaxed) % (n) == 0; \
+        }()) {} else CMVR_LOG(level)

cmvr-es/common/config/cabin_robot.xml

@@ -1,152 +0,0 @@
-<CMVR-ES>
-    <Constants rootDir="/home/xtkuang/projects/cmvr-es"/>
-    <Logger dir="../log" level="info" bufSize="5" logSize="1024"/>
-
-    <DeviceManager name="cmvr_es" ver="0.1" description="cmvr edge system version 0.1">
-        <Devices>
-            <AGV>
-            </AGV>
-
-            <Battery>
-            </Battery>
-
-            <Camera>
-<!--                <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
-<!--                <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
-<!--                <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
-<!--                <RealsenseCamera id="cam4" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
-<!--                <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
-<!--                <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
-            </Camera>
-
-            <DexHand>
-<!--                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
-<!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
-<!--                </RH56DFTP>-->
-<!--                <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
-<!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
-<!--                </RH56DFTP>-->
-            </DexHand>
-
-            <Robot>
-<!--                <LeftArm id="left_arm" devtype="ti5Robot"  />-->
-<!--                <RightArm />-->
-<!--                <Neck/>-->
-                <Humanoid id="hc01" dof="14"
-                          urdf="/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
-                          baseLink="PELVIS_S"
-                          jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
-                          linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
-                          bufferSize="50"
-                          verbose="false">
-                    <CanManger id="" devId="">
-                        <LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">
-                            <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                        </LeftArmCan>
-                        <RightArmCan id = " " devId = " " channelId ="1"  enable="true" toolFrame="R_FINGER_TIP">
-                            <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
-                            <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
-                        </RightArmCan>
-                        <HeadCan id = " " devId = " " channelId ="2" enable="false">
-                            <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                        </HeadCan>
-                        <WaistCan id = " " devId = " " channelId ="3" enable="false">
-                            <Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                            <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-                        </WaistCan>
-                    </CanManger>
-
-                </Humanoid>
-            </Robot>
-
-            <BioHead>
-                <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">
-
-                    <!-- 眉毛 -->
-                    <EyeBrow serial="64:0~3"
-                             offest="90 90 90 90"
-                             jLmtUp="90 170 155 110"
-                             jLmtLow="20 77 90 20"/>
-
-                    <!-- 眼睛 -->
-                    <Eye serial="64:4~9"
-                         offest="90 90 90 90 90 90"
-                         jLmtUp="90 150 165 90 120 115"
-                         jLmtLow="20 90 90 25 70 75"/>
-
-                    <!-- 嘴巴 -->
-                    <Mouth serial="65:0~9"
-                           offest="90 90 90 90 90 90 90 90 90 90"
-                           jLmtUp="150 110 130 140 100 105 110 125 90 95"
-                           jLmtLow="70  30  80  80  65  55  45  80  85  90"/>
-
-                </esp32>
-            </BioHead >
-
-            <Microphone>
-<!--                <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
-<!--                <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>-->
-            </Microphone>
-
-            <Speaker>
-                <ffmpegSpeaker id="spk1" serial="" alas="default" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="100"/>
-            </Speaker>
-
-            <Canbus>
-<!--                <rightArmCan id="can1"  brand="SOCKET_CAN_RAW"  type="USB_CARD" channel_id="CHANNEL_ID_ZERO" interface="NATIVE" baudrate="BCAN_BAUDRATE_500K"/>-->
-            </Canbus>
-
-        </Devices>
-
-        <HighLevelController>
-            <BioHeadExpre headId="bio_head" />
-            <CartesianWBC urdf="" />
-            <ScreenTouch robotID="" DexhandID="" />
-        </HighLevelController>
-    </DeviceManager>
-
-    <MonitorManager>
-        <DiskMonitor id="file_monitor" freq="1">
-<!--            <Folder fileDir="/home/share/assets/audio" maxVolume="1000"/>-->
-<!--            <Folder fileDir="/home/share/assets/image" maxVolume="1000"/>-->
-<!--            <Folder fileDir="/home/share/assets/video" maxVolume="1000"/>-->
-<!--            <Folder fileDir="../log" maxVolume="1000"/>-->
-        </DiskMonitor>
-
-        <JointMonitor id="robot_joint_monitor" freq="200">
-            <RobotJoint robotID="left_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="left_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="left_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="left_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="left_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="left_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="left_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="right_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="right_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="right_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="right_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="right_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="right_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
-            <RobotJoint robotID="right_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
-        </JointMonitor>
-
-    </MonitorManager>
-
-    <gRPCServer port="50052">
-
-    </gRPCServer>
-
-</CMVR-ES>

cmvr-es/common/config/camera_config/camera_config.pb.txt

@@ -1,74 +0,0 @@
-realsense_cameras {
-  id: "cam1"
-  serialNumber: "243122074587"
-  width: 640
-  height: 480
-  fps: 30
-  codec: "H265"
-  camera_mode: CAMERA_MODE_VIDEO
-  stream_mode: STREAM_MODE_RGBD
-  align_mode: ALIGN_MODE_COLOR
-  buffer_size: 30
-  sync: false
-  enable: false
-}
-
-realsense_cameras {
-  id: "right_hand_cam"
-  serialNumber: "243122072252"
-  width: 1280
-  height: 720
-  encode_width: 640
-  encode_height: 360
-  fps: 30
-  codec: "H264"
-  camera_mode: CAMERA_MODE_VIDEO
-  stream_mode: STREAM_MODE_RGB
-  align_mode: ALIGN_MODE_COLOR
-  buffer_size: 30
-  sync: false
-  enable: true
-}
-
-realsense_cameras {
-  id: "cam3"
-  serialNumber: "243122075614"
-  width: 640
-  height: 480
-  fps: 30
-  codec: "H265"
-  camera_mode: CAMERA_MODE_VIDEO
-  stream_mode: STREAM_MODE_RGBD
-  align_mode: ALIGN_MODE_COLOR
-  buffer_size: 30
-  sync: false
-  enable: false
-}
-
-uvc_cameras {
-  id: "left_eye_cam"
-  usb: "/dev/uvc_left_camera"
-  width: 640
-  height: 480
-  fps: 30
-  codec: "H265"
-  camera_mode: CAMERA_MODE_VIDEO
-  stream_mode: STREAM_MODE_RGB
-  buffer_size: 30
-  enable: false
-}
-
-realsense_cameras {
-  id: "cam5"
-  serialNumber: "243122075389"
-  width: 640
-  height: 480
-  fps: 30
-  codec: "H265"
-  camera_mode: CAMERA_MODE_VIDEO
-  stream_mode: STREAM_MODE_RGBD
-  align_mode: ALIGN_MODE_COLOR
-  buffer_size: 30
-  sync: false
-  enable: false
-}

cmvr-es/common/config/config_files.h

@@ -0,0 +1,152 @@
+#pragma once
+
+#include <cmath>
+#include <filesystem>
+#include <string>
+
+#include "common/base/logging/logger.h"
+#include "common/io/proto_file_io.h"
+
+namespace cmvr {
+
+class ConfigHelper
+{
+public:
+    static void setConfigRootFromFile(const std::string& file_name)
+    {
+        if (file_name.empty()) {
+            return;
+        }
+
+        const auto parent = std::filesystem::path(file_name).lexically_normal().parent_path();
+        if (!parent.empty()) {
+            configRoot() = parent;
+        }
+    }
+
+    static std::string resolveConfigFile(const std::string& file_name)
+    {
+        if (file_name.empty()) {
+            return {};
+        }
+
+        const std::filesystem::path path(file_name);
+        if (path.is_absolute()) {
+            return path.lexically_normal().string();
+        }
+
+        const auto& root = configRoot();
+        if (!root.empty()) {
+            return (root / path).lexically_normal().string();
+        }
+        return path.lexically_normal().string();
+    }
+
+    static std::string resolveResourceFile(const std::string& file_name)
+    {
+        if (file_name.empty()) {
+            return {};
+        }
+
+        const std::filesystem::path path(file_name);
+        if (path.is_absolute()) {
+            return path.lexically_normal().string();
+        }
+
+        const auto& root = configRoot();
+        if (root.empty()) {
+            return path.lexically_normal().string();
+        }
+
+        const std::filesystem::path candidates[] = {
+            root / path,
+            root.parent_path() / path,
+            root.parent_path().parent_path() / path,
+            root.parent_path().parent_path().parent_path() / path,
+        };
+        for (const auto& candidate : candidates) {
+            const auto normalized = candidate.lexically_normal();
+            if (std::filesystem::exists(normalized)) {
+                return normalized.string();
+            }
+        }
+        return (root / path).lexically_normal().string();
+    }
+
+    template <class T>
+    static bool loadConfigFile(const std::string& file_name, T& message)
+    {
+        return getConfig(resolveConfigFile(file_name), message, true);
+    }
+
+    template <class T>
+    static bool loadConfigFileSilent(const std::string& file_name, T& message)
+    {
+        return getConfig(resolveConfigFile(file_name), message, false);
+    }
+
+    template <class T>
+    static bool saveConfigFile(const std::string& file_name, const T& message)
+    {
+        return setConfig(message, resolveConfigFile(file_name));
+    }
+
+private:
+    static std::filesystem::path& configRoot()
+    {
+        static std::filesystem::path root;
+        return root;
+    }
+
+    template <class T>
+    static bool setConfig(const T& message, const std::string& file_name)
+    {
+        CMVR_LOG(INFO) << "file_name = " << file_name;
+        if (file_name.empty()) {
+            CMVR_LOG(ERROR) << "Empty file name.";
+            return false;
+        }
+
+        const bool ok = ProtoMessageIo::setProtoToAsciiFile(message, file_name);
+        if (!ok) {
+            CMVR_LOG(ERROR) << "Failed to write ASCII proto config to: " << file_name;
+            return false;
+        }
+        return true;
+    }
+
+    template <class T>
+    static bool getConfig(const std::string& file_name, T& message, const bool log_success)
+    {
+        if (file_name.empty()) {
+            CMVR_LOG(ERROR) << "Empty file name.";
+            return false;
+        }
+
+        if (log_success) {
+            CMVR_LOG(INFO) << "[ConfigHelper] Load config file: " << file_name;
+        }
+        const bool ok = ProtoMessageIo::getProtoFromAsciiFile(file_name, &message);
+        if (!ok) {
+            CMVR_LOG(ERROR) << "Failed to load ASCII proto config from: " << file_name;
+            return false;
+        }
+        return true;
+    }
+};
+
+namespace common::config {
+
+inline double positiveOr(const double value, const double fallback)
+{
+    return std::isfinite(value) && value > 0.0 ? value : fallback;
+}
+
+inline int positiveIntOr(const int value, const int fallback)
+{
+    return value > 0 ? value : fallback;
+}
+
+} // namespace common::config
+
+} // namespace cmvr

cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt

@@ -1,36 +0,0 @@
-rh56dftp_dexhands {
-    id: "hand1"
-    ip: "192.168.1.213"
-    port: 6000
-    poll_interval_ms: 10
-    enable: false
-}
-
-rh56dftp_dexhands {
-    id: "hand2"
-    ip: "192.168.1.224"
-    port: 6000
-    poll_interval_ms: 10
-    enable: true
-}
-
-px_6ax_gen3 {
-    id: "paxini_tip_1"
-    serial_port: "/dev/ttyACM1"
-    sensor_model: "S1813_core"
-    module_id: 2
-    baud_rate: 921600
-    distributed_length: 153
-    resultant_length: 3
-    poll_interval_ms: 5
-    response_timeout_ms: 200
-    response_header_bytes: 14
-    tactile_rows: 1
-    tactile_cols: 51
-    tactile_finger: "INDEX"
-    tactile_region: "TIP"
-    sensor_name: "Paxini Gen3末端压力"
-    polling_read_mode: PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE
-    auto_calibrate: false
-    enable: true
-}

cmvr-es/common/config/ik_solver_config/pinocchio_dls_ik_solver_config.pb.txt

@@ -1,9 +0,0 @@
-urdf_path:         "/home/lgv/cmvr/dual_arm.urdf"
-base_frame_name:   "PELVIS_S"
-flange_frame_name: "R_FLANGE"
-tcp_frame_name:    "R_TCP"
-
-max_iters: 100
-pos_eps:   1e-6
-rot_eps:   1e-6
-damping:   1e-6

cmvr-es/common/config/ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt

@@ -1,10 +0,0 @@
-urdf_path:         "/home/cmvr/Projects/cmvr-es/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:        0.0001
-w_posrot:      0.5
-max_iters:     80
-tol:           1e-6
-qp_time_limit: 0.005

cmvr-es/common/config/microphone_config/microphone_config.pb.txt

@@ -1,8 +0,0 @@
-ffmpeg_microphones {
-    id: "mic1"
-    channels: 2
-    sampleRate: 44100
-    volume: 100
-    enable: false
-    input_device: "default"
-}

cmvr-es/common/config/speaker_config/speaker_config.pb.txt

@@ -1,4 +0,0 @@
-ffmpeg_speakers {
-    id: "spk1"
-    enable: false
-}

cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt

@@ -1,151 +0,0 @@
-robot_id: "hc01"
-dexhand_id: "paxini_tip_1"
-camera_id: "right_hand_cam"
-move_to_init_position_before_start: true
-move_to_init_position: true
-init_joint_positions {
-  joint_name: "R_SHOULDER_P"
-  rad: -0.3678
-}
-init_joint_positions {
-  joint_name: "R_SHOULDER_R"
-  rad: 1.1127
-}
-init_joint_positions {
-  joint_name: "R_SHOULDER_Y"
-  rad: 1.6084
-}
-init_joint_positions {
-  joint_name: "R_ELBOW_R"
-  rad: 1.61
-}
-init_joint_positions {
-  joint_name: "R_WRIST_P"
-  rad: -2.5718
-}
-init_joint_positions {
-  joint_name: "R_WRIST_Y"
-  rad: 0.1276
-}
-init_joint_positions {
-  joint_name: "R_WRIST_R"
-  rad: 0.1297
-}
-init_movej_vel: 1.0
-init_movej_acc: 2.0
-
-urdf_path: "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
-base_link: "PELVIS_S"
-flange_link: "R_WRIST_R_S"
-camera_link: "R_CAM"
-
-tag_size_m: 0.012
-depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
-target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
-
-hover_target_in_camera {
-  x: -0.001
-  y: 0.08
-  z: 0.15
-}
-target_rx: 3.14159265358979323846
-target_ry: 0.0
-target_rz: 0.0
-align_mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
-
-ibvs_lambda: 0.4
-ibvs_mu: 0.1
-ibvs_qdot_max: 1.0
-ibvs_vmax6 {
-  vx: 1.0
-  vy: 1.0
-  vz: 1.0
-  wx: 0.6
-  wy: 0.6
-  wz: 0.6
-}
-ibvs_amax6 {
-  vx: 2.4
-  vy: 2.4
-  vz: 4.5
-  wx: 2.5
-  wy: 2.5
-  wz: 2.5
-}
-ibvs_twist_filter_alpha: 1.0
-enable_joint_limit_avoidance: true
-joint_limit_avoidance_gain: 0.2
-joint_limit_avoidance_margin_ratio: 0.15
-joint_limit_avoidance_max_push: 0.25
-
-r_camera_to_visp {
-  m00: 1.0
-  m11: 1.0
-  m22: 1.0
-}
-r_camera_to_urdf {
-  m00: 1.0
-  m11: 1.0
-  m22: 1.0
-}
-
-control_joint_names: "R_SHOULDER_P"
-control_joint_names: "R_SHOULDER_R"
-control_joint_names: "R_SHOULDER_Y"
-control_joint_names: "R_ELBOW_R"
-control_joint_names: "R_WRIST_P"
-control_joint_names: "R_WRIST_Y"
-control_joint_names: "R_WRIST_R"
-
-align_error_threshold6 {
-  x: 0.005
-  y: 0.005
-  z: 0.01
-  rx: 0.1026646259971647
-  ry: 0.1026646259971647
-  rz: 0.1026646259971647
-}
-align_stable_frames: 2
-align_timeout_s: 20.0
-pause_after_align_reached: false
-
-touch_twist_base {
-  vx: 0.0
-  vy: -0.04
-  vz: 0.0
-  wx: 0.0
-  wy: 0.0
-  wz: 0.0
-}
-touch_use_speedl: true
-touch_speedl_acceleration: 6.0
-touch_forward_l: 0.064
-touch_movel_speed: 0.1
-touch_movel_acceleration: 5.0
-touch_movel_jerk: 5.0
-touch_movel_qd_max: 2.5
-touch_movel_qd_max: 2.5
-touch_movel_qd_max: 2.5
-touch_movel_qd_max: 2.5
-touch_movel_qd_max: 2.5
-touch_movel_qd_max: 2.5
-touch_movel_qd_max: 2.5
-touch_speedl_forward_l: 0.035
-
-dwell_time_s: -1
-retract_twist_base {
-  vx: 0.0
-  vy: 0.08
-  vz: 0.0
-  wx: 0.0
-  wy: 0.0
-  wz: 0.0
-}
-retract_acceleration: 8.0
-retract_duration_s: 0.45
-
-tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
-tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP
-tactile_criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
-tactile_pressure_sum_threshold: 1.0
-tactile_nonzero_count_threshold: 1

cmvr-es/common/io/proto_file_io.h

@@ -1,10 +1,12 @@
 #pragma once
 
-#include <glog/logging.h>
+#include "common/base/logging/logger.h"
 
 #include "google/protobuf/io/zero_copy_stream_impl.h"
 #include "google/protobuf/text_format.h"
 
+#include <cerrno>
+#include <cstring>
 #include <fcntl.h>
 #include <fstream>
 #include <string>
@@ -21,17 +23,17 @@ public:
         std::ofstream output(fileName, std::ios::out | std::ios::trunc | std::ios::binary);
         if (!output.good())
         {
-            LOG(WARNING) << "Failed to open file for binary write: " << fileName;
+            CMVR_LOG(WARNING) << "Failed to open file for binary write: " << fileName;
             return false;
         }
 
         if (!message.SerializePartialToOstream(&output))
         {
-            LOG(WARNING) << "Failed to serialize proto to binary file: " << fileName;
+            CMVR_LOG(WARNING) << "Failed to serialize proto to binary file: " << fileName;
             return false;
         }
 
-        LOG(INFO) << "Successfully wrote binary proto file: " << fileName;
+        CMVR_LOG(INFO) << "Successfully wrote binary proto file: " << fileName;
         return true;
     }
 
@@ -40,20 +42,20 @@ public:
     {
         if (message == nullptr)
         {
-            LOG(ERROR) << "Null message pointer when reading binary proto file: " << fileName;
+            CMVR_LOG(ERROR) << "Null message pointer when reading binary proto file: " << fileName;
             return false;
         }
 
         std::ifstream input(fileName, std::ios::in | std::ios::binary);
         if (!input.good())
         {
-            LOG(WARNING) << "Failed to open file for binary read: " << fileName;
+            CMVR_LOG(WARNING) << "Failed to open file for binary read: " << fileName;
             return false;
         }
 
         if (!message->ParseFromIstream(&input))
         {
-            LOG(WARNING) << "Failed to parse binary proto file: " << fileName;
+            CMVR_LOG(WARNING) << "Failed to parse binary proto file: " << fileName;
             return false;
         }
 
@@ -68,15 +70,16 @@ public:
         const int fd = ::open(fileName.c_str(), O_WRONLY | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR);
         if (fd < 0)
         {
-            PLOG(WARNING) << "Failed to open file for ASCII write: " << fileName;
+            CMVR_LOG(WARNING) << "Failed to open file for ASCII write: " << fileName
+                              << ": " << std::strerror(errno);
             return false;
         }
 
         const bool ok = setProtoToAsciiFile(message, fd);
         if (ok)
-            LOG(INFO) << "Successfully wrote ASCII proto file: " << fileName;
+            CMVR_LOG(INFO) << "Successfully wrote ASCII proto file: " << fileName;
         else
-            LOG(WARNING) << "Failed to write ASCII proto file: " << fileName;
+            CMVR_LOG(WARNING) << "Failed to write ASCII proto file: " << fileName;
 
         return ok;
     }
@@ -89,7 +92,7 @@ public:
 
         if (fileDescriptor < 0)
         {
-            LOG(WARNING) << "Invalid file descriptor for ASCII write.";
+            CMVR_LOG(WARNING) << "Invalid file descriptor for ASCII write.";
             return false;
         }
 
@@ -110,17 +113,20 @@ public:
 
         if (message == nullptr)
         {
-            LOG(ERROR) << "Null message pointer when reading ASCII proto file: " << fileName;
+            CMVR_LOG(ERROR) << "Null message pointer when reading ASCII proto file: " << fileName;
             return false;
         }
 
         const int fd = ::open(fileName.c_str(), O_RDONLY);
         if (fd < 0)
         {
-            if (isOptional)
+            if (isOptional) {
-                PLOG(INFO) << "Optional ASCII proto file not found/openable: " << fileName;
+                CMVR_LOG(INFO) << "Optional ASCII proto file not found/openable: " << fileName
-            else
+                               << ": " << std::strerror(errno);
-                PLOG(ERROR) << "Failed to open ASCII proto file: " << fileName;
+            } else {
+                CMVR_LOG(ERROR) << "Failed to open ASCII proto file: " << fileName
+                                << ": " << std::strerror(errno);
+            }
             return false;
         }
 
@@ -131,9 +137,9 @@ public:
         if (!ok)
         {
             if (isOptional)
-                LOG(INFO) << "Failed to parse optional ASCII proto file: " << fileName;
+                CMVR_LOG(INFO) << "Failed to parse optional ASCII proto file: " << fileName;
             else
-                LOG(ERROR) << "Failed to parse ASCII proto file: " << fileName;
+                CMVR_LOG(ERROR) << "Failed to parse ASCII proto file: " << fileName;
         }
         return ok;
     }

cmvr-es/common/math/cartesian_motion_math.h

@@ -0,0 +1,58 @@
+#ifndef CMVR_ES_CARTESIAN_MOTION_MATH_H
+#define CMVR_ES_CARTESIAN_MOTION_MATH_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+namespace cmvr::device::cartesian_motion {
+
+inline double clamp(const double value, const double lo, const double hi)
+{
+    return std::max(lo, std::min(hi, value));
+}
+
+inline Eigen::VectorXd toEigenVector(const std::vector<double>& values)
+{
+    if (values.empty()) {
+        return {};
+    }
+    return Eigen::Map<const Eigen::VectorXd>(values.data(),
+                                             static_cast<Eigen::Index>(values.size()));
+}
+
+inline std::vector<double> toStdVector(const Eigen::VectorXd& values)
+{
+    return {values.data(), values.data() + values.size()};
+}
+
+inline double directionDeviationDeg(const Eigen::Vector3d& desired,
+                                    const Eigen::Vector3d& actual)
+{
+    const double desired_norm = desired.norm();
+    const double actual_norm = actual.norm();
+    if (desired_norm <= 1e-9 || actual_norm <= 1e-9) {
+        return 0.0;
+    }
+    const double direction_cos =
+        clamp(desired.dot(actual) / (desired_norm * actual_norm), -1.0, 1.0);
+    constexpr double rad_to_deg = 180.0 / 3.14159265358979323846;
+    return std::acos(direction_cos) * rad_to_deg;
+}
+
+inline Eigen::Vector3d rotationVector(const Eigen::Matrix3d& rotation)
+{
+    Eigen::AngleAxisd angle_axis(rotation);
+    const double angle = angle_axis.angle();
+    if (std::abs(angle) <= 1e-9) {
+        return Eigen::Vector3d::Zero();
+    }
+    return angle_axis.axis() * angle;
+}
+
+} // namespace cmvr::device::cartesian_motion
+
+#endif // CMVR_ES_CARTESIAN_MOTION_MATH_H

cmvr-es/common/math/joint_limits.h

@@ -0,0 +1,101 @@
+#ifndef CMVR_ES_JOINT_LIMITS_H
+#define CMVR_ES_JOINT_LIMITS_H
+
+#include <Eigen/Core>
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+namespace cmvr::kinematics {
+
+inline Eigen::VectorXd clampToJointPositionLimits(
+    const Eigen::VectorXd& q,
+    const Eigen::VectorXd& lower,
+    const Eigen::VectorXd& upper)
+{
+    if (lower.size() != q.size() || upper.size() != q.size()) {
+        return q;
+    }
+    return q.cwiseMax(lower).cwiseMin(upper);
+}
+
+inline double velocityLimitScale(const Eigen::VectorXd& qdot,
+                                 const Eigen::VectorXd& velocity_limits,
+                                 const double abs_max = std::numeric_limits<double>::infinity())
+{
+    double scale = 1.0;
+    for (Eigen::Index i = 0; i < qdot.size(); ++i) {
+        double limit = std::numeric_limits<double>::infinity();
+        if (std::isfinite(abs_max) && abs_max > 0.0) {
+            limit = std::min(limit, abs_max);
+        }
+        if (velocity_limits.size() == qdot.size()) {
+            const double joint_limit = std::abs(velocity_limits[i]);
+            if (std::isfinite(joint_limit) && joint_limit > 0.0) {
+                limit = std::min(limit, joint_limit);
+            }
+        }
+
+        const double value = std::abs(qdot[i]);
+        if (std::isfinite(limit) && limit > 0.0 && value > limit) {
+            scale = std::min(scale, limit / value);
+        }
+    }
+    return scale;
+}
+
+inline Eigen::VectorXd scaleToVelocityLimits(
+    const Eigen::VectorXd& qdot,
+    const Eigen::VectorXd& velocity_limits,
+    const double abs_max = std::numeric_limits<double>::infinity())
+{
+    return velocityLimitScale(qdot, velocity_limits, abs_max) * qdot;
+}
+
+inline Eigen::VectorXd computeJointLimitAvoidanceVelocity(
+    const Eigen::VectorXd& q,
+    const Eigen::VectorXd& lower,
+    const Eigen::VectorXd& upper,
+    const bool enable,
+    const double gain,
+    const double margin_ratio,
+    const double max_push)
+{
+    const Eigen::Index dof = q.size();
+    if (!enable || gain <= 0.0 || dof <= 0 ||
+        lower.size() != dof || upper.size() != dof) {
+        return Eigen::VectorXd::Zero(dof);
+    }
+
+    Eigen::VectorXd qdot_avoid = Eigen::VectorXd::Zero(dof);
+    for (Eigen::Index i = 0; i < dof; ++i) {
+        const double lo = lower[i];
+        const double hi = upper[i];
+        if (!std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) {
+            continue;
+        }
+
+        const double span = hi - lo;
+        const double margin = std::max(1e-4, margin_ratio * span);
+        double push = 0.0;
+        if (q[i] < lo + margin) {
+            const double s = (lo + margin - q[i]) / margin;
+            push += gain * s * s;
+        } else if (q[i] > hi - margin) {
+            const double s = (q[i] - (hi - margin)) / margin;
+            push -= gain * s * s;
+        }
+
+        if (max_push > 0.0) {
+            push = std::max(-max_push, std::min(max_push, push));
+        }
+        qdot_avoid[i] = push;
+    }
+
+    return qdot_avoid;
+}
+
+} // namespace cmvr::kinematics
+
+#endif // CMVR_ES_JOINT_LIMITS_H

cmvr-es/common/math/proto_geometry.h

@@ -0,0 +1,115 @@
+#ifndef CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H
+#define CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H
+
+#include <Eigen/Dense>
+
+#include "cmvr/common/geometry.pb.h"
+
+namespace cmvr::common::math {
+
+inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src,
+                                   Eigen::Vector3d defaults)
+{
+    if (src.has_x()) {
+        defaults.x() = src.x();
+    }
+    if (src.has_y()) {
+        defaults.y() = src.y();
+    }
+    if (src.has_z()) {
+        defaults.z() = src.z();
+    }
+    return defaults;
+}
+
+inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src)
+{
+    return toEigenVec3(src, Eigen::Vector3d::Zero());
+}
+
+inline Eigen::Matrix<double, 6, 1> toEigenVec6(
+    const cmvr::common::Vec6& src,
+    Eigen::Matrix<double, 6, 1> defaults)
+{
+    if (src.has_x()) {
+        defaults[0] = src.x();
+    }
+    if (src.has_y()) {
+        defaults[1] = src.y();
+    }
+    if (src.has_z()) {
+        defaults[2] = src.z();
+    }
+    if (src.has_rx()) {
+        defaults[3] = src.rx();
+    }
+    if (src.has_ry()) {
+        defaults[4] = src.ry();
+    }
+    if (src.has_rz()) {
+        defaults[5] = src.rz();
+    }
+    return defaults;
+}
+
+inline Eigen::Matrix<double, 6, 1> toEigenVec6(const cmvr::common::Vec6& src)
+{
+    return toEigenVec6(src, Eigen::Matrix<double, 6, 1>::Zero());
+}
+
+inline Eigen::Matrix3d toEigenMat3(const cmvr::common::Mat3& src,
+                                   Eigen::Matrix3d defaults)
+{
+    if (src.has_m00()) {
+        defaults(0, 0) = src.m00();
+    }
+    if (src.has_m01()) {
+        defaults(0, 1) = src.m01();
+    }
+    if (src.has_m02()) {
+        defaults(0, 2) = src.m02();
+    }
+    if (src.has_m10()) {
+        defaults(1, 0) = src.m10();
+    }
+    if (src.has_m11()) {
+        defaults(1, 1) = src.m11();
+    }
+    if (src.has_m12()) {
+        defaults(1, 2) = src.m12();
+    }
+    if (src.has_m20()) {
+        defaults(2, 0) = src.m20();
+    }
+    if (src.has_m21()) {
+        defaults(2, 1) = src.m21();
+    }
+    if (src.has_m22()) {
+        defaults(2, 2) = src.m22();
+    }
+    return defaults;
+}
+
+inline Eigen::Matrix3d toEigenMat3(const cmvr::common::Mat3& src)
+{
+    return toEigenMat3(src, Eigen::Matrix3d::Identity());
+}
+
+} // namespace cmvr::common::math
+
+
+inline bool hasVec3(const cmvr::common::Vec3& value) {
+    return value.has_x() && value.has_y() && value.has_z();
+}
+
+inline bool hasVec6(const cmvr::common::Vec6& value) {
+    return value.has_x() && value.has_y() && value.has_z() &&
+           value.has_rx() && value.has_ry() && value.has_rz();
+}
+
+inline bool hasMat3(const cmvr::common::Mat3& value) {
+    return value.has_m00() && value.has_m01() && value.has_m02() &&
+           value.has_m10() && value.has_m11() && value.has_m12() &&
+           value.has_m20() && value.has_m21() && value.has_m22();
+}
+#endif // CMVR_ES_COMMON_MATH_PROTO_GEOMETRY_H

cmvr-es/common/math/qp_solver.h

@@ -11,7 +11,7 @@
 #include <memory>
 #include <optional>
 #include <unordered_map>
-#include <iostream>
+#include "common/base/logging/logger.h"
 
 #include <Eigen/Core>
 #include <OsqpEigen/OsqpEigen.h>
@@ -171,13 +171,13 @@ inline void QPSolverImpl::InitFunctionImpl() {
 
 inline void QPSolverImpl::AddCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b) {
     if (A.rows() != b.rows()) {
-        std::cerr << "OSQP Solver Add cost function failed: Size issue Ax = b (A.rows(): "
+        CMVR_LOG(ERROR) << "OSQP Solver Add cost function failed: Size issue Ax = b (A.rows(): "
-                  << A.rows() << ", b.rows(): " << b.rows() << ")" << std::endl;
+                  << A.rows() << ", b.rows(): " << b.rows() << ")";
         return;
     }
 
     if (b.cols() != 1) {
-        std::cerr << "OSQP Solver Add cost function failed: Size issue: b (Nx1)" << std::endl;
+        CMVR_LOG(ERROR) << "OSQP Solver Add cost function failed: Size issue: b (Nx1)";
         return;
     }
 

cmvr-es/common/math/support_functions.h

@@ -6,7 +6,8 @@
 #include <cmath>
 #include <vector>
 #include <algorithm>
-#include <iostream>
+#include <sstream>
+#include "common/base/logging/logger.h"
 #include <Eigen/Core>
 
 class SupportFunctions {
@@ -124,10 +125,13 @@ public:
     }
 
     static void print_intervals(const std::vector<std::pair<double, double> > &intervals) {
+        std::ostringstream output;
         for (const auto &interval: intervals) {
-            std::cout << "[" << interval.first << ", " << interval.second << "] ";
+            output << "[" << interval.first << ", " << interval.second << "] ";
+        }
+        if (!output.str().empty()) {
+            CMVR_LOG(INFO) << output.str();
         }
-        std::cout << std::endl;
     }
 
 

cmvr-es/common/math/transform_math.h

@@ -0,0 +1,94 @@
+#ifndef CMVR_ES_COMMON_MATH_TRANSFORM_MATH_H
+#define CMVR_ES_COMMON_MATH_TRANSFORM_MATH_H
+
+#include <cmath>
+
+#include <Eigen/Dense>
+
+#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
+#include "common/types/arm/arm_types.h"
+
+namespace cmvr::common::math {
+
+inline Eigen::Matrix3d eulerZYXToRotationMatrix(const double rx,
+                                                const double ry,
+                                                const double rz)
+{
+    Eigen::Matrix3d r_x;
+    r_x << 1.0, 0.0, 0.0,
+           0.0, std::cos(rx), -std::sin(rx),
+           0.0, std::sin(rx), std::cos(rx);
+
+    Eigen::Matrix3d r_y;
+    r_y << std::cos(ry), 0.0, std::sin(ry),
+           0.0, 1.0, 0.0,
+           -std::sin(ry), 0.0, std::cos(ry);
+
+    Eigen::Matrix3d r_z;
+    r_z << std::cos(rz), -std::sin(rz), 0.0,
+           std::sin(rz), std::cos(rz), 0.0,
+           0.0, 0.0, 1.0;
+
+    return r_x * r_y * r_z;
+}
+
+inline Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d& rotation)
+{
+    const double ry = std::asin(rotation(0, 2));
+    const double cy = std::cos(ry);
+    double rx = 0.0;
+    double rz = 0.0;
+    if (std::abs(cy) > 1e-6) {
+        rx = std::atan2(-rotation(1, 2), rotation(2, 2));
+        rz = std::atan2(-rotation(0, 1), rotation(0, 0));
+    } else {
+        rz = ry > 0.0 ? std::atan2(rotation(1, 0), rotation(1, 1))
+                      : std::atan2(-rotation(1, 0), rotation(1, 1));
+    }
+    return {rx, ry, rz};
+}
+
+inline Eigen::Matrix4d poseToMatrix(const device::CartesianPose& pose)
+{
+    Eigen::Matrix4d transform = Eigen::Matrix4d::Identity();
+    transform.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.rx, pose.ry, pose.rz);
+    transform(0, 3) = pose.x;
+    transform(1, 3) = pose.y;
+    transform(2, 3) = pose.z;
+    return transform;
+}
+
+inline device::CartesianPose matrixToPose(const Eigen::Matrix4d& transform)
+{
+    device::CartesianPose pose;
+    pose.x = transform(0, 3);
+    pose.y = transform(1, 3);
+    pose.z = transform(2, 3);
+    const Eigen::Vector3d euler = rotationMatrixToEulerZYX(transform.block<3, 3>(0, 0));
+    pose.rx = euler(0);
+    pose.ry = euler(1);
+    pose.rz = euler(2);
+    return pose;
+}
+
+inline Eigen::Matrix<double, 6, 1> velocityToVector(const device::CartesianVelocity& velocity)
+{
+    Eigen::Matrix<double, 6, 1> value;
+    value << velocity.vx, velocity.vy, velocity.vz, velocity.wx, velocity.wy, velocity.wz;
+    return value;
+}
+
+inline device::CartesianVelocity vectorToVelocity(const Eigen::Matrix<double, 6, 1>& velocity)
+{
+    return {velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]};
+}
+
+inline cmvr::CartesianFrame toPlannerFrame(const device::FrameType frame)
+{
+    return frame == device::FrameType::Tool ? cmvr::CartesianFrame::Tool
+                                            : cmvr::CartesianFrame::Base;
+}
+
+} // namespace cmvr::common::math
+
+#endif // CMVR_ES_COMMON_MATH_TRANSFORM_MATH_H

cmvr-es/common/media/ffmpeg/camera_capture.cpp

@@ -1,6 +1,6 @@
 // CameraCapture.cpp
-#include "../include/CameraCapture.h"
+#include "common/media/ffmpeg/camera_capture.h"
-#include <iostream>
+#include "common/base/logging/logger.h"
 #include <thread>
 #include <chrono>
 
@@ -28,21 +28,21 @@ int CameraCapture::initialize(const Config& config) {
     // 初始化设备
     int ret = init_device();
     if (ret < 0) {
-        std::cerr << "初始化设备失败" << std::endl;
+        CMVR_LOG(ERROR) << "初始化设备失败";
         return ret;
     }
 
     // 初始化解码器
     ret = init_decoder();
     if (ret < 0) {
-        std::cerr << "初始化解码器失败" << std::endl;
+        CMVR_LOG(ERROR) << "初始化解码器失败";
         return ret;
     }
 
     // 初始化SWS上下文
     ret = init_sws_context();
     if (ret < 0) {
-        std::cerr << "初始化SWS上下文失败" << std::endl;
+        CMVR_LOG(ERROR) << "初始化SWS上下文失败";
         return ret;
     }
 
@@ -52,12 +52,12 @@ int CameraCapture::initialize(const Config& config) {
     rgb_frame_->format = AV_PIX_FMT_BGR24;  // OpenCV使用BGR格式
     ret = av_frame_get_buffer(rgb_frame_, 0);
     if (ret < 0) {
-        std::cerr << "分配RGB帧缓冲区失败" << std::endl;
+        CMVR_LOG(ERROR) << "分配RGB帧缓冲区失败";
         return ret;
     }
 
-    std::cout << "摄像头初始化成功: " << config_.width << "x" << config_.height
+    CMVR_LOG(INFO) << "摄像头初始化成功: " << config_.width << "x" << config_.height
-              << "@" << config_.fps << "fps" << std::endl;
+              << "@" << config_.fps << "fps";
     return 0;
 }
 
@@ -74,7 +74,7 @@ int CameraCapture::init_device() {
 #endif
 
     if (!input_fmt) {
-        std::cerr << "找不到输入格式" << std::endl;
+        CMVR_LOG(ERROR) << "找不到输入格式";
         return -1;
     }
 
@@ -90,21 +90,21 @@ int CameraCapture::init_device() {
     if (ret < 0) {
         char err_buf[1024];
         av_strerror(ret, err_buf, sizeof(err_buf));
-        std::cerr << "打开摄像头失败: " << err_buf << std::endl;
+        CMVR_LOG(ERROR) << "打开摄像头失败: " << err_buf;
         return ret;
     }
 
     // 查找流信息
     ret = avformat_find_stream_info(fmt_ctx_, nullptr);
     if (ret < 0) {
-        std::cerr << "查找流信息失败" << std::endl;
+        CMVR_LOG(ERROR) << "查找流信息失败";
         return ret;
     }
 
     // 查找视频流
     video_stream_index_ = av_find_best_stream(fmt_ctx_, AVMEDIA_TYPE_VIDEO, -1, -1, nullptr, 0);
     if (video_stream_index_ < 0) {
-        std::cerr << "找不到视频流" << std::endl;
+        CMVR_LOG(ERROR) << "找不到视频流";
         return video_stream_index_;
     }
 
@@ -116,27 +116,27 @@ int CameraCapture::init_decoder() {
     const AVCodec* decoder = avcodec_find_decoder(stream->codecpar->codec_id);
 
     if (!decoder) {
-        std::cerr << "找不到解码器" << std::endl;
+        CMVR_LOG(ERROR) << "找不到解码器";
         return -1;
     }
 
     decoder_ctx_ = avcodec_alloc_context3(decoder);
     if (!decoder_ctx_) {
-        std::cerr << "分配解码器上下文失败" << std::endl;
+        CMVR_LOG(ERROR) << "分配解码器上下文失败";
         return -1;
     }
 
     // 复制参数到解码器上下文
     int ret = avcodec_parameters_to_context(decoder_ctx_, stream->codecpar);
     if (ret < 0) {
-        std::cerr << "复制解码器参数失败" << std::endl;
+        CMVR_LOG(ERROR) << "复制解码器参数失败";
         return ret;
     }
 
     // 打开解码器
     ret = avcodec_open2(decoder_ctx_, decoder, nullptr);
     if (ret < 0) {
-        std::cerr << "打开解码器失败" << std::endl;
+        CMVR_LOG(ERROR) << "打开解码器失败";
         return ret;
     }
 
@@ -154,7 +154,7 @@ int CameraCapture::init_sws_context() {
     );
 
     if (!sws_ctx_) {
-        std::cerr << "创建SWS上下文失败" << std::endl;
+        CMVR_LOG(ERROR) << "创建SWS上下文失败";
         return -1;
     }
 
@@ -185,14 +185,14 @@ cv::Mat CameraCapture::avframe_to_cvmat(AVFrame* frame) {
 
 int CameraCapture::start_capture(FrameCallback callback) {
     if (!callback || !fmt_ctx_ || !decoder_ctx_) {
-        std::cerr << "参数无效或未初始化" << std::endl;
+        CMVR_LOG(ERROR) << "参数无效或未初始化";
         return -1;
     }
 
     is_capturing_ = true;
     frame_count_ = 0;
 
-    std::cout << "开始采集..." << std::endl;
+    CMVR_LOG(INFO) << "开始采集...";
 
     while (is_capturing_) {
         // 读取数据包
@@ -202,7 +202,7 @@ int CameraCapture::start_capture(FrameCallback callback) {
                 std::this_thread::sleep_for(std::chrono::milliseconds(1));
                 continue;
             }
-            std::cerr << "读取帧失败: " << ret << std::endl;
+            CMVR_LOG(ERROR) << "读取帧失败: " << ret;
             break;
         }
 
@@ -211,7 +211,7 @@ int CameraCapture::start_capture(FrameCallback callback) {
             // 发送数据包到解码器
             ret = avcodec_send_packet(decoder_ctx_, packet_);
             if (ret < 0 && ret != AVERROR(EAGAIN)) {
-                std::cerr << "发送数据包到解码器失败: " << ret << std::endl;
+                CMVR_LOG(ERROR) << "发送数据包到解码器失败: " << ret;
                 av_packet_unref(packet_);
                 continue;
             }
@@ -222,7 +222,7 @@ int CameraCapture::start_capture(FrameCallback callback) {
                 if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
                     break;
                 } else if (ret < 0) {
-                    std::cerr << "接收解码帧失败: " << ret << std::endl;
+                    CMVR_LOG(ERROR) << "接收解码帧失败: " << ret;
                     break;
                 }
 

cmvr-es/common/media/ffmpeg/realsense_capture.cpp

@@ -1,6 +1,6 @@
 // RealSenseCapture.cpp
-#include "../include/RealSenseCapture.h"
+#include "common/media/ffmpeg/realsense_capture.h"
-#include <iostream>
+#include "common/base/logging/logger.h"
 #include <chrono>
 
 namespace ffmpeg {
@@ -26,7 +26,7 @@ int RealSenseCapture::initialize(const Config& config) {
         // 初始化设备
         int ret = init_device();
         if (ret < 0) {
-            std::cerr << "初始化RealSense设备失败" << std::endl;
+            CMVR_LOG(ERROR) << "初始化RealSense设备失败";
             return ret;
         }
 
@@ -38,20 +38,20 @@ int RealSenseCapture::initialize(const Config& config) {
         rgb_frame_ = create_avframe(config_.width, config_.height, AV_PIX_FMT_BGR24);
 
         if (!frame_ || !rgb_frame_) {
-            std::cerr << "创建AVFrame失败" << std::endl;
+            CMVR_LOG(ERROR) << "创建AVFrame失败";
             return -1;
         }
 
-        std::cout << "RealSense摄像头初始化成功: "
+        CMVR_LOG(INFO) << "RealSense摄像头初始化成功: "
-                  << config_.width << "x" << config_.height << "@" << config_.fps << "fps" << std::endl;
+                  << config_.width << "x" << config_.height << "@" << config_.fps << "fps";
 
         return 0;
 
     } catch (const rs2::error& e) {
-        std::cerr << "RealSense错误: " << e.what() << std::endl;
+        CMVR_LOG(ERROR) << "RealSense错误: " << e.what();
         return -1;
     } catch (const std::exception& e) {
-        std::cerr << "常规错误: " << e.what() << std::endl;
+        CMVR_LOG(ERROR) << "常规错误: " << e.what();
         return -1;
     }
 }
@@ -63,11 +63,11 @@ int RealSenseCapture::init_device() {
         size_t device_count = devices.size();
 
         if (device_count == 0) {
-            std::cerr << "未检测到RealSense设备" << std::endl;
+            CMVR_LOG(ERROR) << "未检测到RealSense设备";
             return -1;
         }
 
-        std::cout << "检测到 " << device_count << " 个RealSense设备" << std::endl;
+        CMVR_LOG(INFO) << "检测到 " << device_count << " 个RealSense设备";
 
         // 如果指定了序列号，查找对应设备
         if (!config_.serial_number.empty()) {
@@ -77,7 +77,7 @@ int RealSenseCapture::init_device() {
                 std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
 
                 if (serial == config_.serial_number) {
-                    std::cout << "找到指定序列号的设备: " << serial << std::endl;
+                    CMVR_LOG(INFO) << "找到指定序列号的设备: " << serial;
                     found = true;
                     rs_cfg_.enable_device(serial);
                     break;
@@ -85,15 +85,15 @@ int RealSenseCapture::init_device() {
             }
 
             if (!found) {
-                std::cerr << "未找到序列号为 " << config_.serial_number << " 的设备" << std::endl;
+                CMVR_LOG(ERROR) << "未找到序列号为 " << config_.serial_number << " 的设备";
                 return -1;
             }
         } else {
             // 使用第一个设备
             rs2::device dev = devices[0];
             std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
-            std::cout << "使用第一个设备: " << dev.get_info(RS2_CAMERA_INFO_NAME)
+            CMVR_LOG(INFO) << "使用第一个设备: " << dev.get_info(RS2_CAMERA_INFO_NAME)
-                      << " (序列号: " << serial << ")" << std::endl;
+                      << " (序列号: " << serial << ")";
             rs_cfg_.enable_device(serial);
         }
 
@@ -107,7 +107,7 @@ int RealSenseCapture::init_device() {
         return 0;
 
     } catch (const rs2::error& e) {
-        std::cerr << "初始化设备失败: " << e.what() << std::endl;
+        CMVR_LOG(ERROR) << "初始化设备失败: " << e.what();
         return -1;
     }
 }
@@ -170,12 +170,12 @@ int RealSenseCapture::start_capture(FrameCallback callback) {
     std::lock_guard<std::mutex> lock(mutex_);
 
     if (!callback) {
-        std::cerr << "回调函数为空" << std::endl;
+        CMVR_LOG(ERROR) << "回调函数为空";
         return -1;
     }
 
     if (is_capturing_) {
-        std::cerr << "已经在捕获中" << std::endl;
+        CMVR_LOG(ERROR) << "已经在捕获中";
         return -1;
     }
 
@@ -190,7 +190,7 @@ int RealSenseCapture::start_capture(FrameCallback callback) {
         // 等待第一帧，确保设备正常工作
         rs2::frameset frames = pipe_.wait_for_frames(2000); // 2秒超时
         if (!frames.get_color_frame()) {
-            std::cerr << "无法获取第一帧" << std::endl;
+            CMVR_LOG(ERROR) << "无法获取第一帧";
             pipe_.stop();
             return -1;
         }
@@ -202,11 +202,11 @@ int RealSenseCapture::start_capture(FrameCallback callback) {
         capture_thread_ = std::make_unique<std::thread>(&RealSenseCapture::capture_thread_func,
                                                          this, callback);
 
-        std::cout << "RealSense开始采集" << std::endl;
+        CMVR_LOG(INFO) << "RealSense开始采集";
         return 0;
 
     } catch (const rs2::error& e) {
-        std::cerr << "启动采集失败: " << e.what() << std::endl;
+        CMVR_LOG(ERROR) << "启动采集失败: " << e.what();
         return -1;
     }
 }
@@ -238,7 +238,7 @@ void RealSenseCapture::capture_thread_func(FrameCallback callback) {
             callback(av_frame, rgb_image, frame_count_++);
 
         } catch (const rs2::error& e) {
-            std::cerr << "采集错误: " << e.what() << std::endl;
+            CMVR_LOG(ERROR) << "采集错误: " << e.what();
             if (!is_capturing_) break;
         }
 
@@ -270,10 +270,10 @@ void RealSenseCapture::stop_capture() {
     try {
         pipe_.stop();
     } catch (const std::exception& e) {
-        std::cerr << "停止pipeline时出错: " << e.what() << std::endl;
+        CMVR_LOG(ERROR) << "停止pipeline时出错: " << e.what();
     }
 
-    std::cout << "RealSense停止采集" << std::endl;
+    CMVR_LOG(INFO) << "RealSense停止采集";
 }
 
 std::string RealSenseCapture::get_device_info() const {

cmvr-es/common/media/ffmpeg/video_frame_encoder.cpp

@@ -1,6 +1,6 @@
 // VideoFrameEncoder.cpp
-#include "../include/VideoFrameEncoder.h"
+#include "common/media/ffmpeg/video_frame_encoder.h"
-#include <iostream>
+#include "common/base/logging/logger.h"
 #include <chrono>
 
 namespace ffmpeg {
@@ -30,14 +30,14 @@ int VideoFrameEncoder::initialize(const Config& config) {
     //const AVCodec* encoder = avcodec_find_encoder(AV_CODEC_ID_H264);
     const AVCodec* encoder = avcodec_find_encoder_by_name(config.codec.c_str());
     if (!encoder) {
-        std::cerr << "找不到H.264编码器" << std::endl;
+        CMVR_LOG(ERROR) << "找不到H.264编码器";
         return -1;
     }
 
     // 分配编码器上下文
     encoder_ctx_ = avcodec_alloc_context3(encoder);
     if (!encoder_ctx_) {
-        std::cerr << "分配编码器上下文失败" << std::endl;
+        CMVR_LOG(ERROR) << "分配编码器上下文失败";
         return -1;
     }
 
@@ -70,7 +70,7 @@ int VideoFrameEncoder::initialize(const Config& config) {
     if (ret < 0) {
         char err_buf[1024];
         av_strerror(ret, err_buf, sizeof(err_buf));
-        std::cerr << "打开编码器失败: " << err_buf << std::endl;
+        CMVR_LOG(ERROR) << "打开编码器失败: " << err_buf;
         return ret;
     }
 
@@ -81,7 +81,7 @@ int VideoFrameEncoder::initialize(const Config& config) {
 
     ret = av_frame_get_buffer(converted_frame_, 0);
     if (ret < 0) {
-        std::cerr << "分配帧缓冲区失败" << std::endl;
+        CMVR_LOG(ERROR) << "分配帧缓冲区失败";
         return ret;
     }
 
@@ -90,7 +90,7 @@ int VideoFrameEncoder::initialize(const Config& config) {
 
 int VideoFrameEncoder::init_sws_context(AVFrame* frame) {
     if (!frame) {
-        std::cerr << "输入帧为空" << std::endl;
+        CMVR_LOG(ERROR) << "输入帧为空";
         return -1;
     }
 
@@ -110,7 +110,7 @@ int VideoFrameEncoder::init_sws_context(AVFrame* frame) {
     );
 
     if (!sws_ctx_) {
-        std::cerr << "创建SWS上下文失败" << std::endl;
+        CMVR_LOG(ERROR) << "创建SWS上下文失败";
         return -1;
     }
 
@@ -119,7 +119,7 @@ int VideoFrameEncoder::init_sws_context(AVFrame* frame) {
 
 int VideoFrameEncoder::encode_frame(AVFrame* frame) {
     if (!encoder_ctx_ || !frame) {
-        std::cerr << "编码器未初始化或输入帧为空" << std::endl;
+        CMVR_LOG(ERROR) << "编码器未初始化或输入帧为空";
         return -1;
     }
 
@@ -151,7 +151,7 @@ int VideoFrameEncoder::encode_frame(AVFrame* frame) {
     if (ret < 0) {
         char err_buf[1024];
         av_strerror(ret, err_buf, sizeof(err_buf));
-        std::cerr << "发送帧到编码器失败: " << err_buf << std::endl;
+        CMVR_LOG(ERROR) << "发送帧到编码器失败: " << err_buf;
         return ret;
     }
 
@@ -162,7 +162,7 @@ int VideoFrameEncoder::encode_frame(AVFrame* frame) {
         if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
             break;
         } else if (ret < 0) {
-            std::cerr << "接收编码数据包失败" << std::endl;
+            CMVR_LOG(ERROR) << "接收编码数据包失败";
             av_packet_free(&packet);
             return ret;
         }
@@ -191,7 +191,7 @@ int VideoFrameEncoder::flush() {
     // 发送空帧刷新编码器
     int ret = avcodec_send_frame(encoder_ctx_, nullptr);
     if (ret < 0) {
-        std::cerr << "发送刷新帧失败" << std::endl;
+        CMVR_LOG(ERROR) << "发送刷新帧失败";
         return ret;
     }
 
@@ -202,7 +202,7 @@ int VideoFrameEncoder::flush() {
         if (ret == AVERROR_EOF) {
             break;
         } else if (ret < 0) {
-            std::cerr << "接收刷新数据包失败" << std::endl;
+            CMVR_LOG(ERROR) << "接收刷新数据包失败";
             av_packet_free(&packet);
             return ret;
         }