CMakeLists.txt

@@ -106,29 +106,23 @@ 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::algorithms::controller
+        cmvr_es::controller
+        cmvr_es::data_center
         cmvr_es::ik_solver
-        cmvr_es::base_motion
+        cmvr_es::planner
+        cmvr_es::device::humanoid_robot
         cmvr_es::common
-        cmvr_es::task
-        cmvr_es::task_manager
-        ccd
-        fcl
+        cmvr_es::applications
 )
 
 install(TARGETS cmvr_es RUNTIME 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)
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/cmvr-es/common/config DESTINATION bin)

cmvr-es/CMakeLists.txt

@@ -1,12 +1,16 @@
-add_subdirectory(common/base/logging)
-link_libraries(cmvr_es::logging)
-
-add_subdirectory(common)
+add_subdirectory(utils)
 add_subdirectory(hardware)
-add_subdirectory(algorithms)
 add_subdirectory(devices)
-add_subdirectory(manager/device_manager)
-add_subdirectory(task)
-add_subdirectory(manager/task_manager)
+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)

cmvr-es/algorithms/CMakeLists.txt

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

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

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

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

@@ -1,282 +0,0 @@
-#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/kinematics/ik_solver/ik_solver_factory.h

@@ -1,44 +0,0 @@
-#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/pinocchio/include/pinocchio_dls_ik_solver.h

@@ -1,95 +0,0 @@
-#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/src/pinocchio_dls_ik_solver.cpp

@@ -1,473 +0,0 @@
-// 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

@@ -1,330 +0,0 @@
-// 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/srs/include/srs_ik_solver.h

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

@@ -1,339 +0,0 @@
-#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/motion_planner/CMakeLists.txt

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,65 +0,0 @@
-
-
-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/applications/CMakeLists.txt

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

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

@@ -0,0 +1,105 @@
+#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,12 +1,19 @@
 #find_package(protobuf REQUIRED)
 
-add_library(common SHARED
-        ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/camera_capture.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/realsense_capture.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/video_frame_encoder.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/media/ffmpeg/video_writer.cpp
+
+file(GLOB SRC
+        ${CMAKE_CURRENT_SOURCE_DIR}/utils/config_helper/src/config_setting.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/CameraCapture.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/utils/ffmpeg/src/RealSenseCapture.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
@@ -45,3 +52,17 @@ 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/logging/CMakeLists.txt

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,151 @@
+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/curve/src/s_curve.cpp

@@ -8,7 +8,7 @@
  * @brief S 曲线轨迹生成器实现
  */
 
-#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h"
+#include "common/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）。
+  // 短距离：无巡航段（t4 = 0），与 moveL_SCurveLocal 中的 SCurveProfile1D 保持一致。
   const double j = profile.j_max;
   const double a = profile.a_max;
   const double v = profile.v_max;

cmvr-es/common/math/cartesian_motion_math.h

@@ -1,58 +0,0 @@
-#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/include/qp_solver.h

@@ -11,7 +11,7 @@
 #include <memory>
 #include <optional>
 #include <unordered_map>
-#include "common/base/logging/logger.h"
+#include <iostream>
 
 #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()) {
-        CMVR_LOG(ERROR) << "OSQP Solver Add cost function failed: Size issue Ax = b (A.rows(): "
-                  << A.rows() << ", b.rows(): " << b.rows() << ")";
+        std::cerr << "OSQP Solver Add cost function failed: Size issue Ax = b (A.rows(): "
+                  << A.rows() << ", b.rows(): " << b.rows() << ")" << std::endl;
         return;
     }
 
     if (b.cols() != 1) {
-        CMVR_LOG(ERROR) << "OSQP Solver Add cost function failed: Size issue: b (Nx1)";
+        std::cerr << "OSQP Solver Add cost function failed: Size issue: b (Nx1)" << std::endl;
         return;
     }
 

cmvr-es/common/math/include/support_functions.h

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

cmvr-es/common/math/joint_limits.h

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

@@ -1,115 +0,0 @@
-#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/transform_math.h

@@ -1,94 +0,0 @@
-#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/types/arm/arm_types.h

@@ -1,223 +0,0 @@
-#ifndef CMVR_ES_ARM_TYPES_H
-#define CMVR_ES_ARM_TYPES_H
-
-#include <cstdint>
-#include <string>
-#include <vector>
-
-namespace cmvr::device {
-
-enum class ArmErrorCode {
-    OK = 0,
-    NotConnected,
-    AlreadyConnected,
-    ConnectionFailed,
-    Timeout,
-    InvalidArgument,
-    InvalidDof,
-    OutOfJointLimit,
-    OutOfVelocityLimit,
-    OutOfAccelerationLimit,
-    OutOfWorkspace,
-    RobotNotReady,
-    RobotNotPowered,
-    RobotInFault,
-    RobotInProtectiveStop,
-    RobotInEmergencyStop,
-    CommandRejected,
-    CommandFailed,
-    UnsupportedCommand,
-    UnknownError
-};
-
-struct Result {
-    ArmErrorCode code{ArmErrorCode::OK};
-    std::string message{"OK"};
-
-    bool ok() const { return code == ArmErrorCode::OK; }
-    static Result success() { return {ArmErrorCode::OK, "OK"}; }
-    static Result failure(ArmErrorCode c, const std::string& msg) { return {c, msg}; }
-};
-
-enum class FrameType {
-    Base,
-    Tool,
-    World,
-    User
-};
-
-struct CartesianPose {
-    double x{0.0};
-    double y{0.0};
-    double z{0.0};
-    double rx{0.0};
-    double ry{0.0};
-    double rz{0.0};
-};
-
-struct CartesianVelocity {
-    double vx{0.0};
-    double vy{0.0};
-    double vz{0.0};
-    double wx{0.0};
-    double wy{0.0};
-    double wz{0.0};
-};
-
-struct CartesianWrench {
-    double fx{0.0};
-    double fy{0.0};
-    double fz{0.0};
-    double tx{0.0};
-    double ty{0.0};
-    double tz{0.0};
-};
-
-struct JointLimit {
-    double lower{0.0};
-    double upper{0.0};
-    double max_velocity{0.0};
-    double max_acceleration{0.0};
-    double max_torque{0.0};
-};
-
-struct RobotModel {
-    std::string name;
-    std::string manufacturer;
-    std::string serial_number;
-    std::size_t dof{0};
-    std::vector<std::string> joint_names;
-    std::vector<JointLimit> joint_limits;
-    double max_tcp_speed{0.0};
-    double max_tcp_acceleration{0.0};
-    double max_payload{0.0};
-
-    bool valid() const
-    {
-        return dof > 0 && joint_names.size() == dof &&
-               (joint_limits.empty() || joint_limits.size() == dof);
-    }
-};
-
-struct JointGroupState {
-    std::vector<double> position;
-    std::vector<double> velocity;
-    std::vector<double> effort;
-
-    bool validForModel(const RobotModel& model) const
-    {
-        return position.size() == model.dof &&
-               velocity.size() == model.dof &&
-               (effort.empty() || effort.size() == model.dof);
-    }
-};
-
-struct JointPositionCommand {
-    std::vector<double> position;
-
-    bool validForModel(const RobotModel& model) const
-    {
-        return position.size() == model.dof;
-    }
-};
-
-struct JointVelocityCommand {
-    std::vector<double> velocity;
-
-    bool validForModel(const RobotModel& model) const
-    {
-        return velocity.size() == model.dof;
-    }
-};
-
-struct JointTorqueCommand {
-    std::vector<double> torque;
-
-    bool validForModel(const RobotModel& model) const
-    {
-        return torque.size() == model.dof;
-    }
-};
-
-struct ToolConfig {
-    CartesianPose tcp_offset;
-};
-
-struct PayloadConfig {
-    double mass{0.0};
-    double cog_x{0.0};
-    double cog_y{0.0};
-    double cog_z{0.0};
-};
-
-struct MotionOptions {
-    double velocity{0.0};
-    double acceleration{0.0};
-    double blend_radius{0.0};
-    double jerk{5.0};
-    std::vector<double> joint_velocity_limits;
-    bool asynchronous{false};
-};
-
-struct ServoOptions {
-    double period{0.008};
-    double lookahead_time{0.1};
-    double gain{300.0};
-};
-
-enum class RobotMode {
-    Unknown = 0,
-    Disconnected,
-    PowerOff,
-    Idle,
-    Running,
-    Paused,
-    Stopped,
-    Fault
-};
-
-enum class SafetyMode {
-    Unknown = 0,
-    Normal,
-    Reduced,
-    ProtectiveStop,
-    EmergencyStop,
-    SafeguardStop,
-    SystemEmergencyStop,
-    Fault
-};
-
-enum class ControlMode {
-    None = 0,
-    Manual,
-    Position,
-    Velocity,
-    Torque,
-    Servo,
-    Freedrive
-};
-
-struct ArmState {
-    double timestamp{0.0};
-    RobotMode robot_mode{RobotMode::Unknown};
-    SafetyMode safety_mode{SafetyMode::Unknown};
-    ControlMode control_mode{ControlMode::None};
-    bool connected{false};
-    bool powered_on{false};
-    bool brake_released{false};
-    bool moving{false};
-    bool program_running{false};
-    bool protective_stopped{false};
-    bool emergency_stopped{false};
-    bool fault{false};
-    double speed_scaling{1.0};
-    JointGroupState actual_joint_state;
-    JointGroupState target_joint_state;
-    CartesianPose actual_tcp_pose;
-    CartesianVelocity actual_tcp_velocity;
-    CartesianWrench actual_tcp_wrench;
-};
-
-} // namespace cmvr::device
-
-#endif // CMVR_ES_ARM_TYPES_H

cmvr-es/common/utils/config_helper/include/config_helper.h

@@ -0,0 +1,163 @@
+//
+// Created by cmvr on 2026/1/16.
+//
+#pragma once
+#include "common/utils/config_helper/include/config_setting.h"
+#include "common/utils/io/proto_message_utils.h"
+#include "cmvr/config/pinocchio_qp_ik_solver_config.pb.h"
+#include "cmvr/config/camera_config/camera_config.pb.h"
+#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
+#include "cmvr/config/microphone_config/microphone_config.pb.h"
+#include "cmvr/config/speaker_config/speaker_conifg.pb.h"
+#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h"
+
+#define GET_CONFIG(file, para)                                             \
+    ([&]() -> bool {                                                       \
+        LOG(INFO) << "FLAGS_" #file " = " << FLAGS_##file;                 \
+        return getConfig(FLAGS_##file, (para));                            \
+    }())
+
+#define GET_CONFIG_BIN(file, para)                                         \
+    ([&]() -> bool {                                                       \
+        LOG(INFO) << "FLAGS_" #file " = " << FLAGS_##file;                 \
+        return getConfigBin(FLAGS_##file, (para));                         \
+    }())
+
+#define SET_CONFIG_BIN(para, file)                                         \
+    ([&]() -> bool {                                                       \
+        LOG(INFO) << "FLAGS_" #file " = " << FLAGS_##file;                 \
+        return setConfigBin((para), FLAGS_##file);                          \
+    }())
+
+#define SET_CONFIG(para, file)                                             \
+    ([&]() -> bool {                                                       \
+        LOG(INFO) << "FLAGS_" #file " = " << FLAGS_##file;                 \
+        return setConfig((para), FLAGS_##file);                            \
+    }())
+
+namespace cmvr
+{
+    class ConfigHelper
+    {
+    public:
+
+        static bool getPinocchioQpIkSolverConfig(config::PinocchioQpIKConfig& config)
+        {
+
+            return GET_CONFIG(pinocchio_qp_ik_solver_config_file, config);
+        }
+
+        static bool setPinocchioQpIkSolverConfig(const config::PinocchioQpIKConfig& config)
+        {
+            return SET_CONFIG(config, pinocchio_qp_ik_solver_config_file);
+        }
+
+        static bool getCamerasConfig(config::CameraConfig& config)
+        {
+            return GET_CONFIG(camera_config_file, config);
+        }
+
+        static bool getDexHandsConfig(config::DexHandConfig& config)
+        {
+            return GET_CONFIG(dexhand_config_file, config);
+        }
+
+        static bool getMicroPhonesConfig(config::MicroPhoneConfig& config)
+        {
+            return GET_CONFIG(microphone_config_file, config);
+        }
+
+        static bool getSpeakersConfig(config::SpeakerConfig& config)
+        {
+            return GET_CONFIG(speaker_config_file, config);
+        }
+
+        static bool getTouchScreenAppConfig(config::TouchScreenAppConfig& config)
+        {
+            return GET_CONFIG(touch_screen_app_config_file, config);
+        }
+
+        static bool setTouchScreenAppConfig(const config::TouchScreenAppConfig& config)
+        {
+            return SET_CONFIG(config, touch_screen_app_config_file);
+        }
+
+    private:
+        // Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx)
+        template <class T>
+        static bool getConfigBin(const std::string& file_name, T& message)
+        {
+            LOG(INFO) << "file_name = " << file_name;
+            if (file_name.empty())
+            {
+                LOG(ERROR) << "Empty file name.";
+                return false;
+            }
+
+            bool ok = ProtoMessageIo::getProtoFromBinaryFile(file_name, &message);
+            if (!ok)
+            {
+                LOG(ERROR) << "Failed to load binary proto config from: " << file_name;
+                return false;
+            }
+            return true;
+        }
+
+        template <class T>
+        static bool setConfig(const T& message, const std::string& file_name)
+        {
+            LOG(INFO) << "file_name = " << file_name;
+            if (file_name.empty())
+            {
+                LOG(ERROR) << "Empty file name.";
+                return false;
+            }
+
+            bool ok = ProtoMessageIo::setProtoToAsciiFile(message, file_name);
+            if (!ok)
+            {
+                LOG(ERROR) << "Failed to write ASCII proto config to: " << file_name;
+                return false;
+            }
+            return true;
+        }
+
+        template <class T>
+        static bool setConfigBin(const T& message, const std::string& file_name)
+        {
+            LOG(INFO) << "file_name = " << file_name;
+            if (file_name.empty())
+            {
+                LOG(ERROR) << "Empty file name.";
+                return false;
+            }
+
+            bool ok = ProtoMessageIo::setProtoToBinaryFile(message, file_name);
+            if (!ok)
+            {
+                LOG(ERROR) << "Failed to write binary proto config to: " << file_name;
+                return false;
+            }
+            return true;
+        }
+
+        template <class T>
+        static bool getConfig(const std::string& file_name, T& message)
+        {
+            // LOG(INFO) << "file_name = " << file_name;
+            if (file_name.empty())
+            {
+                LOG(ERROR) << "Empty file name.";
+                return false;
+            }
+
+            bool ok = ProtoMessageIo::getProtoFromAsciiFile(file_name, &message);
+            if (!ok)
+            {
+                LOG(ERROR) << "Failed to load ASCII proto config from: " << file_name;
+                return false;
+            }
+            return true;
+        }
+    };
+} // namespace cmvr

cmvr-es/common/utils/config_helper/include/config_setting.h

@@ -0,0 +1,8 @@
+#pragma once
+#include "gflags/gflags.h"
+DECLARE_string(pinocchio_qp_ik_solver_config_file);
+DECLARE_string(camera_config_file);
+DECLARE_string(dexhand_config_file);
+DECLARE_string(microphone_config_file);
+DECLARE_string(speaker_config_file);
+DECLARE_string(touch_screen_app_config_file);

cmvr-es/common/utils/config_helper/src/config_setting.cpp

@@ -0,0 +1,90 @@
+#include "common/utils/config_helper/include/config_setting.h"
+
+#include <array>
+#include <filesystem>
+#include <limits.h>
+#include <unistd.h>
+
+namespace {
+
+std::string normalizeDirectory(std::filesystem::path path) {
+    auto text = path.lexically_normal().string();
+    if (!text.empty() && text.back() != '/') {
+        text.push_back('/');
+    }
+    return text;
+}
+
+std::string probeConfigDirectory(std::filesystem::path start) {
+    static const std::array<std::filesystem::path, 4> candidates = {
+        std::filesystem::path("output/bin/config"),
+        std::filesystem::path("cmvr-es/common/config"),
+        std::filesystem::path("config"),
+        std::filesystem::path("common/config")
+    };
+
+    start = start.lexically_normal();
+    while (!start.empty()) {
+        for (const auto& candidate : candidates) {
+            const auto path = start / candidate;
+            if (std::filesystem::exists(path) && std::filesystem::is_directory(path)) {
+                return normalizeDirectory(path);
+            }
+        }
+
+        const auto parent = start.parent_path();
+        if (parent == start) {
+            break;
+        }
+        start = parent;
+    }
+    return {};
+}
+
+std::filesystem::path executableDirectory() {
+    char exe_path[PATH_MAX] = {0};
+    const auto count = ::readlink("/proc/self/exe", exe_path, sizeof(exe_path) - 1);
+    if (count <= 0) {
+        return {};
+    }
+    exe_path[count] = '\0';
+    return std::filesystem::path(exe_path).parent_path();
+}
+
+} // namespace
+
+static std::string basePath() {
+    if (const auto resolved = probeConfigDirectory(executableDirectory()); !resolved.empty()) {
+        return resolved;
+    }
+
+    if (const auto resolved = probeConfigDirectory(std::filesystem::current_path()); !resolved.empty()) {
+        return resolved;
+    }
+
+    return "config/";
+}
+
+DEFINE_string(pinocchio_qp_ik_solver_config_file,
+             basePath() + "ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt",
+              "The configuration file for pinocchio qp ik solver");
+
+DEFINE_string(camera_config_file,
+             basePath() + "camera_config/camera_config.pb.txt",
+              "The configuration file for cameras");
+
+DEFINE_string(dexhand_config_file,
+             basePath() + "dexhand_config/dexhand_config.pb.txt",
+              "The configuration file for dexhands");
+
+DEFINE_string(microphone_config_file,
+             basePath() + "microphone_config/microphone_config.pb.txt",
+              "The configuration file for microphone");
+
+DEFINE_string(speaker_config_file,
+             basePath() + "speaker_config/speaker_config.pb.txt",
+              "The configuration file for speaker");
+
+DEFINE_string(touch_screen_app_config_file,
+             basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt",
+              "The configuration file for TouchScreenApp");

cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp

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

cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp

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

cmvr-es/common/utils/ffmpeg/src/VideoFrameEncoder.cpp

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

cmvr-es/common/utils/ffmpeg/src/VideoWriter.cpp

@@ -1,6 +1,6 @@
 // VideoWriter.cpp
-#include "common/media/ffmpeg/video_writer.h"
-#include "common/base/logging/logger.h"
+#include "../include/VideoWriter.h"
+#include <iostream>
 
 namespace ffmpeg {
 
@@ -15,28 +15,28 @@ int VideoWriter::initialize(const Config& config, AVCodecContext* codec_ctx) {
     codec_ctx_ref_ = codec_ctx;
 
     if (!codec_ctx) {
-        CMVR_LOG(ERROR) << "编码器上下文为空";
+        std::cerr << "编码器上下文为空" << std::endl;
         return -1;
     }
 
     // 分配输出格式上下文
     int ret = avformat_alloc_output_context2(&fmt_ctx_, nullptr, nullptr, config_.output_file.c_str());
     if (ret < 0 || !fmt_ctx_) {
-        CMVR_LOG(ERROR) << "分配输出格式上下文失败";
+        std::cerr << "分配输出格式上下文失败" << std::endl;
         return ret;
     }
 
     // 创建输出流
     video_stream_ = avformat_new_stream(fmt_ctx_, nullptr);
     if (!video_stream_) {
-        CMVR_LOG(ERROR) << "创建输出流失败";
+        std::cerr << "创建输出流失败" << std::endl;
         return -1;
     }
 
     // 复制编码器参数到输出流
     ret = avcodec_parameters_from_context(video_stream_->codecpar, codec_ctx);
     if (ret < 0) {
-        CMVR_LOG(ERROR) << "复制编码器参数失败";
+        std::cerr << "复制编码器参数失败" << std::endl;
         return ret;
     }
 
@@ -52,7 +52,7 @@ int VideoWriter::initialize(const Config& config, AVCodecContext* codec_ctx) {
         if (ret < 0) {
             char err_buf[1024];
             av_strerror(ret, err_buf, sizeof(err_buf));
-            CMVR_LOG(ERROR) << "打开输出文件失败: " << err_buf;
+            std::cerr << "打开输出文件失败: " << err_buf << std::endl;
             return ret;
         }
     }
@@ -60,26 +60,26 @@ int VideoWriter::initialize(const Config& config, AVCodecContext* codec_ctx) {
     // 写入文件头
     ret = avformat_write_header(fmt_ctx_, nullptr);
     if (ret < 0) {
-        CMVR_LOG(ERROR) << "写入文件头失败";
+        std::cerr << "写入文件头失败" << std::endl;
         return ret;
     }
 
-    CMVR_LOG(INFO) << "视频写入器初始化完成，输出文件: " << config_.output_file
+    std::cout << "视频写入器初始化完成，输出文件: " << config_.output_file
               << " 时间基: " << video_stream_->time_base.num << "/" << video_stream_->time_base.den
-             ;
+              << std::endl;
     return 0;
 }
 
 int VideoWriter::write_packet(AVPacket* packet, int64_t pts, int64_t dts) {
     if (!fmt_ctx_ || !video_stream_) {
-        CMVR_LOG(ERROR) << "写入器未初始化";
+        std::cerr << "写入器未初始化" << std::endl;
         return -1;
     }
 
     // 克隆数据包，避免修改原始数据
     AVPacket* cloned_packet = av_packet_clone(packet);
     if (!cloned_packet) {
-        CMVR_LOG(ERROR) << "克隆数据包失败";
+        std::cerr << "克隆数据包失败" << std::endl;
         return -1;
     }
 
@@ -112,7 +112,7 @@ int VideoWriter::write_packet(AVPacket* packet, int64_t pts, int64_t dts) {
     if (ret < 0) {
         char err_buf[1024];
         av_strerror(ret, err_buf, sizeof(err_buf));
-        CMVR_LOG(ERROR) << "写入数据包失败: " << err_buf;
+        std::cerr << "写入数据包失败: " << err_buf << std::endl;
     } else {
         frame_count_++;
     }
@@ -129,9 +129,9 @@ int VideoWriter::finish() {
     // 写入文件尾
     int ret = av_write_trailer(fmt_ctx_);
     if (ret < 0) {
-        CMVR_LOG(ERROR) << "写入文件尾失败";
+        std::cerr << "写入文件尾失败" << std::endl;
     } else {
-        CMVR_LOG(INFO) << "视频写入完成，总帧数: " << frame_count_;
+        std::cout << "视频写入完成，总帧数: " << frame_count_ << std::endl;
     }
 
     cleanup();

cmvr-es/common/utils/io/proto_message_utils.h

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

cmvr-es/config/cmvr_es.pb.txt

@@ -1,5 +0,0 @@
-cmvr_es {
-  logger_config_file: "logger/logger.pb.txt"
-  device_manager_config_file: "manager/device_manager.pb.txt"
-  task_manager_config_file: "manager/task_manager.pb.txt"
-}

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

@@ -1,9 +0,0 @@
-agv {
-  agvs {
-    id: "agv_1"
-    my_agv {
-      ip: "127.0.0.1"
-      port: 8080
-    }
-  }
-}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -1,22 +0,0 @@
-arm {
-  robot_arms {
-    id: "aubo_arm"
-
-    vendor {
-      brand: VENDOR_ROBOT_ARM_BRAND_AUBO_ARM
-      ip: "192.168.1.100"
-      port: 30004
-      dof: 6
-      joint_names: "joint_1"
-      joint_names: "joint_2"
-      joint_names: "joint_3"
-      joint_names: "joint_4"
-      joint_names: "joint_5"
-      joint_names: "joint_6"
-      base_frame: "base"
-      tool_frame: "tool0"
-      username: "aubo"
-      password: "123456"
-    }
-  }
-}

cmvr-es/config/devices/biohead/bio_head.pb.txt

@@ -1,86 +0,0 @@
-bio_head {
-  id: "bio_head"
-  serial_port: "/dev/ttyUSB0"
-  baud_rate: 115200
-
-  servo_groups {
-    name: "eyebrow"
-    address: 64
-    channel_start: 0
-    channel_end: 3
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    min_angles: 20
-    min_angles: 77
-    min_angles: 90
-    min_angles: 20
-    max_angles: 90
-    max_angles: 170
-    max_angles: 155
-    max_angles: 110
-  }
-
-  servo_groups {
-    name: "eye"
-    address: 64
-    channel_start: 4
-    channel_end: 9
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    min_angles: 20
-    min_angles: 90
-    min_angles: 90
-    min_angles: 25
-    min_angles: 70
-    min_angles: 75
-    max_angles: 90
-    max_angles: 150
-    max_angles: 165
-    max_angles: 90
-    max_angles: 120
-    max_angles: 115
-  }
-
-  servo_groups {
-    name: "mouth"
-    address: 65
-    channel_start: 0
-    channel_end: 9
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    offset: 90
-    min_angles: 70
-    min_angles: 30
-    min_angles: 80
-    min_angles: 80
-    min_angles: 65
-    min_angles: 55
-    min_angles: 45
-    min_angles: 80
-    min_angles: 85
-    min_angles: 90
-    max_angles: 150
-    max_angles: 110
-    max_angles: 130
-    max_angles: 140
-    max_angles: 100
-    max_angles: 105
-    max_angles: 110
-    max_angles: 125
-    max_angles: 90
-    max_angles: 95
-  }
-}

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

@@ -1,81 +0,0 @@
-camera {
-  cameras {
-    id: "cam1"
-    realsense {
-      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
-    }
-  }
-
-  cameras {
-    id: "right_hand_cam"
-    realsense {
-      serialNumber: "243122072252"
-      width: 1280
-      height: 720
-      encode_width: 640
-      encode_height: 360
-      fps: 30
-      codec: "H264"
-      camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGB
-      align_mode: ALIGN_MODE_COLOR
-      buffer_size: 30
-      sync: false
-    }
-  }
-
-  cameras {
-    id: "cam3"
-    realsense {
-      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
-    }
-  }
-
-  cameras {
-    id: "left_eye_cam"
-    uvc {
-      usb: "/dev/uvc_left_camera"
-      width: 640
-      height: 480
-      fps: 30
-      codec: "H265"
-      camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGB
-      buffer_size: 30
-    }
-  }
-
-  cameras {
-    id: "cam5"
-    realsense {
-      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
-    }
-  }
-}

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

@@ -1,41 +0,0 @@
-dexhand {
-  dexhands {
-    id: "hand1"
-    rh56dftp {
-      ip: "192.168.1.213"
-      port: 6000
-      poll_interval_ms: 10
-    }
-  }
-
-  dexhands {
-    id: "hand2"
-    rh56dftp {
-      ip: "192.168.1.224"
-      port: 6000
-      poll_interval_ms: 10
-    }
-  }
-
-  dexhands {
-    id: "paxini_tip_1"
-    px_6ax_gen3 {
-      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
-    }
-  }
-}

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

@@ -1,11 +0,0 @@
-microphone {
-  microphones {
-    id: "mic1"
-    ffmpeg {
-      channels: 2
-      sampleRate: 44100
-      volume: 100
-      input_device: "default"
-    }
-  }
-}

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

@@ -1,17 +0,0 @@
-motor {
-  id: "mujoco_motors"
-
-  motor_groups {
-    id: "right_arm_mujoco"
-    bus_type: MOTOR_BUS_MUJOCO
-    tool_frame: "R_FINGER_TIP"
-
-    motors { id: 1 joint_name: "R_SHOULDER_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
-    motors { id: 2 joint_name: "R_SHOULDER_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
-    motors { id: 3 joint_name: "R_SHOULDER_Y" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
-    motors { id: 4 joint_name: "R_ELBOW_R" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
-    motors { id: 5 joint_name: "R_WRIST_P" limit_q_lb: -3.14 limit_q_ub: 3.14 limit_qd: 3.0 }
-    motors { id: 6 joint_name: "R_WRIST_Y" limit_q_lb: -1.102 limit_q_ub: 1.02 limit_qd: 3.0 }
-    motors { id: 7 joint_name: "R_WRIST_R" limit_q_lb: -0.293 limit_q_ub: 1.57079 limit_qd: 3.0 }
-  }
-}

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

@@ -1,81 +0,0 @@
-motor {
-  id: "ti5_motors"
-
-  motor_groups {
-    id: "left_arm_can"
-    bus_type: MOTOR_BUS_CAN
-    tool_frame: "L_FINGER_TIP"
-    can {
-      channel_id: 0
-    }
-    joint_limits {
-      source: JOINT_LIMIT_SOURCE_URDF
-    }
-    joint_limits_urdf_path: "model/xiaoyan_description/dual_arm.urdf"
-    motors { id: 23 joint_name: "L_SHOULDER_P" }
-    motors { id: 24 joint_name: "L_SHOULDER_R" }
-    motors { id: 25 joint_name: "L_SHOULDER_Y" }
-    motors { id: 26 joint_name: "L_ELBOW_R" }
-    motors { id: 27 joint_name: "L_WRIST_P" }
-    motors { id: 28 joint_name: "L_WRIST_Y" }
-    motors { id: 29 joint_name: "L_WRIST_R" }
-  }
-
-  motor_groups {
-    id: "right_arm_can"
-    bus_type: MOTOR_BUS_CAN
-    tool_frame: "R_FINGER_TIP"
-    can {
-      channel_id: 1
-    }
-    joint_limits {
-      source: JOINT_LIMIT_SOURCE_CUSTOM
-      joints { joint_name: "R_SHOULDER_P" lower: -3.14 upper: 3.14 velocity: 5.0 }
-      joints { joint_name: "R_SHOULDER_R" lower: -0.78 upper: 1.57 velocity: 5.0 }
-      joints { joint_name: "R_SHOULDER_Y" lower: -3.14 upper: 3.14 velocity: 5.0 }
-      joints { joint_name: "R_ELBOW_R" lower: 0 upper: 2.05 velocity: 5.0 }
-      joints { joint_name: "R_WRIST_P" lower: -3.14 upper: 3.14 velocity: 5.0 }
-      joints { joint_name: "R_WRIST_Y" lower: -0.78 upper: 0.78 velocity: 5.0 }
-      joints { joint_name: "R_WRIST_R" lower: -0.57 upper: 1.57 velocity: 5.0 }
-    }
-    motors { id: 16 joint_name: "R_SHOULDER_P" }
-    motors { id: 17 joint_name: "R_SHOULDER_R" }
-    motors { id: 18 joint_name: "R_SHOULDER_Y" }
-    motors { id: 19 joint_name: "R_ELBOW_R" }
-    motors { id: 20 joint_name: "R_WRIST_P" }
-    motors { id: 21 joint_name: "R_WRIST_Y" }
-    motors { id: 22 joint_name: "R_WRIST_R" }
-  }
-
-  motor_groups {
-    id: "head_can"
-    bus_type: MOTOR_BUS_CAN
-    can {
-      channel_id: 2
-    }
-    joint_limits {
-      source: JOINT_LIMIT_SOURCE_CUSTOM
-      joints { joint_name: "HEAD_Y" lower: -3.14 upper: 3.14 velocity: 3.0 }
-      joints { joint_name: "HEAD_P" lower: -3.14 upper: 3.14 velocity: 3.0 }
-      joints { joint_name: "HEAD_R" lower: -3.14 upper: 3.14 velocity: 3.0 }
-    }
-    motors { id: 32 joint_name: "HEAD_Y" }
-    motors { id: 30 joint_name: "HEAD_P" }
-    motors { id: 31 joint_name: "HEAD_R" }
-  }
-
-  motor_groups {
-    id: "waist_can"
-    bus_type: MOTOR_BUS_CAN
-    can {
-      channel_id: 3
-    }
-    joint_limits {
-      source: JOINT_LIMIT_SOURCE_CUSTOM
-      joints { joint_name: "WAIST_Y" lower: -3.14 upper: 3.14 velocity: 3.0 }
-      joints { joint_name: "WAIST_P" lower: -3.14 upper: 3.14 velocity: 3.0 }
-    }
-    motors { id: 4 joint_name: "WAIST_Y" }
-    motors { id: 15 joint_name: "WAIST_P" }
-  }
-}

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

@@ -1,7 +0,0 @@
-speaker {
-  speakers {
-    id: "spk1"
-    ffmpeg {
-    }
-  }
-}

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

@@ -1,38 +0,0 @@
-logger {
-  minimum_level: LOG_LEVEL_DEBUG
-
-  routes {
-    level: LOG_LEVEL_DEBUG
-    file: true
-    terminal: true
-  }
-  routes {
-    level: LOG_LEVEL_INFO
-    terminal: true
-  }
-  routes {
-    level: LOG_LEVEL_WARNING
-    terminal: true
-    file: true
-  }
-  routes {
-    level: LOG_LEVEL_ERROR
-    terminal: true
-    file: true
-  }
-  routes {
-    level: LOG_LEVEL_FATAL
-    terminal: true
-    file: true
-  }
-
-  directory: "../log"
-  max_file_size_mb: 100
-  flush_interval_seconds: 1
-  format {
-    show_time: false
-    show_level: true
-    show_thread_id: false
-    show_source_location: true
-  }
-}

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

@@ -1,61 +0,0 @@
-device_manager {
-  name: "cmvr_es"
-  version: "0.1"
-  description: "cmvr edge system version 0.1"
-
-  devices {
-    id: "right_hand_cam"
-    type: DEVICE_TYPE_CAMERA
-    config_file: "devices/camera/camera.pb.txt"
-    enable: true
-  }
-
-  devices {
-    id: "hand2"
-    type: DEVICE_TYPE_DEXHAND
-    config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: true
-  }
-
-  devices {
-    id: "paxini_tip_1"
-    type: DEVICE_TYPE_DEXHAND
-    config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: false
-  }
-
-  devices {
-    id: "ti5_motors"
-    type: DEVICE_TYPE_MOTOR_SYSTEM
-    config_file: "devices/motor/ti5_motors.pb.txt"
-    enable: true
-  }
-
-  devices {
-    id: "right_arm"
-    type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/arm.pb.txt"
-    enable: true
-  }
-
-  devices {
-    id: "aubo_arm"
-    type: DEVICE_TYPE_ROBOT_ARM
-    config_file: "devices/arm/aubo_arm.pb.txt"
-    enable: false
-  }
-
-  devices {
-    id: "bio_head"
-    type: DEVICE_TYPE_BIO_HEAD_ROBOT
-    config_file: "devices/biohead/bio_head.pb.txt"
-    enable: false
-  }
-
-  devices {
-    id: "agv_1"
-    type: DEVICE_TYPE_AGV
-    config_file: "devices/agv/agv.pb.txt"
-    enable: false
-  }
-}

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

@@ -1,17 +0,0 @@
-task_manager {
-  tasks {
-    id: "touch_screen"
-    type: TASK_TYPE_TOUCH_SCREEN
-    run_mode: TASK_RUN_MODE_PERIODIC_STEP
-    control_period_s: 0.001
-    config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
-    enable: true
-  }
-  tasks {
-    id: "grpc_server"
-    type: TASK_TYPE_GRPC_SERVER
-    run_mode: TASK_RUN_MODE_BLOCKING_SERVICE
-    config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt"
-    enable: true
-  }
-}

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

@@ -1,6 +0,0 @@
-grpc_server {
-  id: "grpc_server"
-  host: "0.0.0.0"
-  port: "50052"
-  enable_reflection: true
-}

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

@@ -1,101 +0,0 @@
-touch_screen_task {
-  id: "touch_screen"
-
-  devices {
-    arm_id: "right_arm"
-    dexhand_id: "paxini_tip_1"
-    camera_id: "right_hand_cam"
-  }
-
-  initialization {
-    before_start: true
-    after_finish: true
-    joint_positions { joint_name: "R_SHOULDER_P" rad: -0.3678 }
-    joint_positions { joint_name: "R_SHOULDER_R" rad: 1.1127 }
-    joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.6084 }
-    joint_positions { joint_name: "R_ELBOW_R" rad: 1.61 }
-    joint_positions { joint_name: "R_WRIST_P" rad: -2.5718 }
-    joint_positions { joint_name: "R_WRIST_Y" rad: 0.1276 }
-    joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 }
-    velocity: 1.0
-    acceleration: 2.0
-  }
-
-  perception {
-    apriltag {
-      tag_size_m: 0.012
-      depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
-      target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
-    }
-  }
-
-  alignment {
-    ibvs {
-      camera_link: "R_CAM"
-      lambda: 0.4
-      mu: 0.1
-      qdot_max: 1.0
-      vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
-      amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
-      twist_filter_alpha: 1.0
-      enable_joint_limit_avoidance: true
-      joint_limit_avoidance_gain: 0.2
-      joint_limit_avoidance_margin_ratio: 0.15
-      joint_limit_avoidance_max_push: 0.25
-      r_camera_to_visp {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: 1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: 1.0
-      }
-      r_camera_to_urdf {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: 1.0 m12: 0.0
-        m20: 0.0 m21: 0.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"
-    }
-    target {
-      position_in_camera { x: -0.001 y: 0.08 z: 0.15 }
-      rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
-      mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
-    }
-    error_threshold {
-      x: 0.005
-      y: 0.005
-      z: 0.01
-      rx: 0.1026646259971647
-      ry: 0.1026646259971647
-      rz: 0.1026646259971647
-    }
-    stable_frames: 2
-    timeout_s: 20.0
-    pause_when_reached: false
-  }
-
-  touch {
-    speed_l {
-      twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-      acceleration: 6.0
-      max_distance_m: 0.035
-    }
-    tactile {
-      finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
-      region: TOUCH_SCREEN_TACTILE_REGION_TIP
-      criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
-      force_threshold: 1.0
-    }
-    dwell_time_s: 0.0
-  }
-
-  retract {
-    twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 8.0
-    duration_s: 0.45
-  }
-}

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

@@ -1,101 +0,0 @@
-touch_screen_task {
-  id: "touch_screen"
-
-  devices {
-    arm_id: "right_arm_mujoco"
-    dexhand_id: "mujoco_zero_touch_dexhand"
-    camera_id: "hand_cam"
-  }
-
-  initialization {
-    before_start: true
-    after_finish: true
-    joint_positions { joint_name: "R_SHOULDER_P" rad: -0.2423 }
-    joint_positions { joint_name: "R_SHOULDER_R" rad: 1.2929 }
-    joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.61 }
-    joint_positions { joint_name: "R_ELBOW_R" rad: 1.58 }
-    joint_positions { joint_name: "R_WRIST_P" rad: -2.8792 }
-    joint_positions { joint_name: "R_WRIST_Y" rad: 0.1150 }
-    joint_positions { joint_name: "R_WRIST_R" rad: -0.08 }
-    velocity: 2.8
-    acceleration: 20.0
-  }
-
-  perception {
-    apriltag {
-      tag_size_m: 0.12
-      depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
-      target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
-    }
-  }
-
-  alignment {
-    ibvs {
-      camera_link: "R_CAM"
-      lambda: 0.4
-      mu: 0.1
-      qdot_max: 0.8
-      vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
-      amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
-      twist_filter_alpha: 1.0
-      enable_joint_limit_avoidance: true
-      joint_limit_avoidance_gain: 0.2
-      joint_limit_avoidance_margin_ratio: 0.15
-      joint_limit_avoidance_max_push: 0.25
-      r_camera_to_visp {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: -1.0 m12: 0.0
-        m20: 0.0 m21: 0.0 m22: -1.0
-      }
-      r_camera_to_urdf {
-        m00: 1.0 m01: 0.0 m02: 0.0
-        m10: 0.0 m11: -1.0 m12: 0.0
-        m20: 0.0 m21: 0.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"
-    }
-    target {
-      position_in_camera { x: 0.0 y: 0.0 z: 0.30 }
-      rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
-      mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
-    }
-    error_threshold {
-      x: 0.005
-      y: 0.005
-      z: 0.010
-      rx: 0.08726646259971647
-      ry: 0.08726646259971647
-      rz: 0.08726646259971647
-    }
-    stable_frames: 5
-    timeout_s: 20.0
-    pause_when_reached: false
-  }
-
-  touch {
-    speed_l {
-      twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-      acceleration: 6.0
-      max_distance_m: 0.12
-    }
-    tactile {
-      finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
-      region: TOUCH_SCREEN_TACTILE_REGION_TIP
-      criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ
-      force_threshold: 1.0
-    }
-    dwell_time_s: 0.0
-  }
-
-  retract {
-    twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 8.0
-    duration_s: 5.0
-  }
-}

cmvr-es/controller/CMakeLists.txt

@@ -1,8 +1,7 @@
 
-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
@@ -11,8 +10,8 @@ add_subdirectory(arm_control)
 #           ❌ 不要把 src/controller_test.cpp 放进来
 #)  其他动态库类似
 file(GLOB SRC
-        ${CMAKE_CURRENT_SOURCE_DIR}/pid/src/pid_controller.cpp
-        ${CMAKE_CURRENT_SOURCE_DIR}/ibvs/src/ibvs_controller.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/pid_controller.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/ibvs_controller.cpp
 
 )
 
@@ -25,6 +24,7 @@ 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::algorithms::controller ALIAS controller)
+add_library(cmvr_es::controller ALIAS controller)
 
 install(TARGETS controller LIBRARY DESTINATION lib)
 
@@ -65,17 +65,18 @@ install(TARGETS controller LIBRARY DESTINATION lib)
 # --------------------------------------------------------
 find_package(realsense2 REQUIRED)
 add_executable(controller_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/controller_test.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/controller_test.cpp
 )
 
 target_link_libraries(controller_test
         PRIVATE
+        cmvr_es::utils
         cmvr_es::perception
         cmvr_es::ik_solver
-        cmvr_es::base_motion
+        cmvr_es::planner
         cmvr_es::proto
         cmvr_es::mujoco_viewer
-        cmvr_es::algorithms::controller
+        cmvr_es::controller
         cmvr_es::device::mujoco_camera
         gtest
         gtest_main

cmvr-es/controller/include/ibvs_controller.h

@@ -14,8 +14,8 @@
 #include <visp3/visual_features/vpFeaturePoint.h>
 #include <visp3/vs/vpServo.h>
 
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
-#include "algorithms/perception/apriltag/include/apriltag_perception.h"
+#include "ik_solver/include/pinocchio_dls_ik_solver.h"
+#include "perception/include/apriltag_perception.h"
 
 namespace cmvr {
 
@@ -66,12 +66,16 @@ public:
     IbvsController();
 
     /**
-     * @brief 注入机械臂当前配置创建的 Pinocchio kinematics solver。
-     * @param solver 来自 RobotArm 的 Pinocchio IK solver。
+     * @brief 初始化 DLS 速度 IK 求解器（不再需要相机）。
+     * @param urdf_path URDF 文件路径。
+     * @param base_link IK 链基座 link 名称。
+     * @param flange_link IK 链末端法兰 link 名称。
      * @param camera_link URDF 中相机 link 名称，对应坐标系 `u`。
      * @return 初始化成功返回 `true`。
      */
-    bool init(std::shared_ptr<cmvr::PinocchioIKBase> solver,
+    bool init(const std::string& urdf_path,
+              const std::string& base_link,
+              const std::string& flange_link,
               const std::string& camera_link);
 
     /**
@@ -286,8 +290,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};
@@ -332,6 +336,7 @@ 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/controller/include/touch_controller.h

@@ -0,0 +1,114 @@
+//
+// Created by lgv on 2025/8/24.
+//
+
+#pragma once
+
+#include <utility>
+
+#include "../../devices/camera/abstract_camera.h"
+#include "../../devices/dexhand/abstract_dexhand.h"
+#include "../../devices/robot/abstract_robot.h"
+#include "cmvr/msgs/geometry.pb.h"
+#include "librealsense2/rs.h"
+#include "librealsense2/h/rs_frame.h"
+namespace cmvr {
+    namespace ctrl {
+
+        // 简易版 PID 控制器，带死区、积分限幅、输出限幅和输出斜率限制
+        class PID {
+        public:
+            PID(double kp, double ki, double kd,
+                double i_max,
+                double output_max_pos, double output_max_neg,
+                double delta_max = 0.0)   // 输出变化最大值，0 表示不限制
+                : kp_(kp), ki_(ki), kd_(kd),
+                  i_max_(i_max),
+                  output_max_pos_(output_max_pos),
+                  output_max_neg_(output_max_neg),
+                  delta_max_(delta_max),
+                  prev_error_(0), integral_(0), prev_output_(0) {}
+
+            double compute(double target, double current, double dt, double deadband = 0.0) {
+                double error = target - current;
+
+                // 死区处理
+                if (fabs(error) <= deadband) {
+                    error = 0.0;
+                }
+
+                // 积分累加限幅
+                integral_ += error * dt;
+                if (integral_ > i_max_) integral_ = i_max_;
+                if (integral_ < -i_max_) integral_ = -i_max_;
+
+                // 微分
+                double derivative = (error - prev_error_) / dt;
+                prev_error_ = error;
+
+                // PID 输出
+                double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
+
+                // 输出限幅
+                if (output > output_max_pos_) output = output_max_pos_;
+                if (output < -output_max_neg_) output = -output_max_neg_;
+
+                // 输出斜率限制
+                if (delta_max_ > 0.0) {
+                    double delta = output - prev_output_;
+                    if (delta > delta_max_) output = prev_output_ + delta_max_;
+                    else if (delta < -delta_max_) output = prev_output_ - delta_max_;
+                }
+
+                prev_output_ = output;
+                return output;
+            }
+
+        private:
+            double kp_, ki_, kd_;
+            double prev_error_;
+            double integral_;
+            double i_max_;             // 积分限幅
+            double output_max_pos_;    // 向下按的最大输出
+            double output_max_neg_;    // 向上抬的最大输出
+            double delta_max_;         // 输出斜率限制
+            double prev_output_;
+        };
+
+
+
+
+        class TouchController {
+        public:
+            TouchController() {};
+            TouchController(std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand,std::shared_ptr<device::AbstractCamera> cam)
+            :robot_(std::move(robot)),hand_(std::move(hand)),cam_(std::move(cam)),
+            pid_(std::make_shared<PID>(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){}
+            ~TouchController()=default;
+
+
+            bool isArrive(double max_force);
+
+            void touch(int u,int v ,double max_force);
+
+            void touch(std::shared_ptr<device::AbstractRobot> robot,const msgs::Pose3d pose,const msgs::Pose3d offset);
+            void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force);
+
+        private:
+            std::shared_ptr<device::AbstractRobot> robot_{nullptr};
+            std::shared_ptr<device::AbstractDexHand> hand_{nullptr};
+            std::shared_ptr<device::AbstractCamera> cam_{nullptr};
+
+            std::shared_ptr<PID> pid_{nullptr};
+
+            const double touch_threshold_ = 5.0;  // 触控判定阈值
+
+
+            // 从压阻矩阵提取触控点与压力
+            bool extractTouch(const std::vector<std::vector<uint16_t>>& matrix,double& force, int& x, int& y);
+
+
+
+        };
+    }
+}

cmvr-es/controller/src/controller_test.cpp

@@ -16,9 +16,9 @@
 #include <vector>
 
 
-#include "algorithms/controllers/ibvs/include/ibvs_controller.h"
+#include "controller/include/ibvs_controller.h"
 #include "devices/camera/mujoco_camera/include/mujoco_camera.h"
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
+#include "ik_solver/include/pinocchio_dls_ik_solver.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 using namespace cmvr;
 
@@ -551,23 +551,7 @@ protected:
     ibvs_controller_->setAlignCameraToVisp(R_align);
     ibvs_controller_->setAlignCameraToUrdf(R_align);
 
-    config::PinocchioDlsIKConfig dls_cfg;
-    dls_cfg.set_urdf_path(urdf_path_for_check_);
-    dls_cfg.set_base_frame_name("PELVIS_S");
-    dls_cfg.set_flange_frame_name("R_WRIST_R_S");
-    dls_cfg.set_tcp_frame_name(camera_frame_name_for_check_);
-    dls_cfg.set_max_iters(100);
-    dls_cfg.set_pos_eps(1e-6);
-    dls_cfg.set_rot_eps(1e-6);
-    dls_cfg.set_damping(mu_);
-    auto solver = std::make_shared<PinocchioDlsIKSolver>(dls_cfg);
-    if (!solver->init()) {
-      std::cout << "[IBVS] PinocchioDlsIKSolver init failed" << std::endl;
-      ready_ = false;
-      return;
-    }
-
-    if (!ibvs_controller_->init(solver, camera_frame_name_for_check_)) {
+    if (!ibvs_controller_->init(urdf_path_for_check_, "PELVIS_S", "R_WRIST_R_S", camera_frame_name_for_check_)) {
       std::cout << "[IBVS] IbvsController init failed" << std::endl;
       ready_ = false;
       return;

cmvr-es/controller/src/ibvs_controller.cpp

@@ -1,8 +1,7 @@
-#include "algorithms/controllers/ibvs/include/ibvs_controller.h"
+#include "controller/include/ibvs_controller.h"
 
-#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h"
-#include "common/vision/image_projection.h"
-#include "common/math/support_functions.h"
+#include "common/utils/image/image_process.h"
+#include "common/math/include/support_functions.h"
 
 #include <algorithm>
 #include <cmath>
@@ -108,20 +107,24 @@ IbvsController::IbvsController() {
     initTask();
 }
 
-bool IbvsController::init(std::shared_ptr<cmvr::PinocchioIKBase> solver,
+bool IbvsController::init(const std::string& urdf_path,
+                          const std::string& base_link,
+                          const std::string& flange_link,
                           const std::string& camera_link) {
-    solver_ = std::move(solver);
+    base_frame_name_ = base_link;
     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_) {
-        if (auto dls_solver = std::dynamic_pointer_cast<PinocchioDlsIKSolver>(solver_)) {
-            dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_,
-                                               limit_avoidance_gain_,
-                                               limit_avoidance_margin_ratio_,
-                                               limit_avoidance_max_push_);
-        }
+        dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_,
+                                            limit_avoidance_gain_,
+                                            limit_avoidance_margin_ratio_,
+                                            limit_avoidance_max_push_);
         has_joint_position_limits_ =
-            solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_);
+            dls_solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_);
     } else {
         has_joint_position_limits_ = false;
         q_lower_limits_.resize(0);
@@ -206,11 +209,11 @@ bool IbvsController::computeQdot(const std::vector<double>& joints_angle,
 }
 
 bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) const {
-    if (!solver_) {
+    if (!dls_solver_) {
         joint_names.clear();
         return false;
     }
-    return solver_->getChainJointNames(joint_names);
+    return dls_solver_->getChainJointNames(joint_names);
 }
 
 bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
@@ -222,7 +225,7 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
     last_tag_pos_visp_.setZero();
     last_v_camera_visp_.setZero();
 
-    if (!initialized_ || !solver_) {
+    if (!initialized_ || !dls_solver_) {
         last_compute_status_ = ComputeStatus::NOT_READY;
         return false;
     }
@@ -393,56 +396,28 @@ 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>();
 
-    Eigen::MatrixXd jacobian_base;
-    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>();
+    // IK
+    dls_solver_->update_joints_state(joints_angle);
 
     std::vector<double> qdot;
-    const bool ok = solver_->solveVelocityBase(jacobian_base,
-                                               twist_base,
-                                               joints_angle,
-                                               qdot,
-                                               std::numeric_limits<double>::infinity());
+    const bool ok = dls_solver_->ik(
+        base_frame_name_, camera_frame_name_, twist_urdf,
+        qdot, mu_, 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[i], -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);
-        }
+        qdot_out[i] = SupportFunctions::clamp(qdot_soft_limited[static_cast<Eigen::Index>(i)],
+                                              -qdot_max_,
+                                              qdot_max_);
     }
 
     last_compute_status_ = ComputeStatus::OK;
@@ -564,11 +539,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 (auto dls_solver = std::dynamic_pointer_cast<PinocchioDlsIKSolver>(solver_)) {
-        dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_,
-                                           limit_avoidance_gain_,
-                                           limit_avoidance_margin_ratio_,
-                                           limit_avoidance_max_push_);
+    if (dls_solver_) {
+        dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_,
+                                            limit_avoidance_gain_,
+                                            limit_avoidance_margin_ratio_,
+                                            limit_avoidance_max_push_);
     }
 }
 

cmvr-es/controller/src/pid_controller.cpp

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