feat:add data_center

CMakeLists.txt

@@ -66,6 +66,8 @@ include_directories(
         ${PROJECT_SOURCE_DIR}/src/devices
         ${PROJECT_SOURCE_DIR}/src/utils
         ${PROJECT_SOURCE_DIR}/third_party
+        ${PROJECT_SOURCE_DIR}/src
+
 )
 
 ############################################################
@@ -96,4 +98,6 @@ target_link_libraries(cmvr_es PRIVATE
         cmvr_es::device::canbus
         cmvr_es::device::ti5motor
         cmvr_es::ctrl::controller
+        cmvr_es::data_center
+        cmvr_es::ik_solver
 )

assets/toppra

@@ -0,0 +1 @@
+Subproject commit fab1f402ef8677beac43d2226eb1cc6ce0d54735

data/plot_joint_traj.py

@@ -0,0 +1,59 @@
+# pip install toppra numpy scipy pandas matplotlib
+import numpy as np
+import pandas as pd
+import matplotlib.pyplot as plt
+import toppra as ta
+import toppra.algorithm as algo
+import toppra.constraint as constraint
+import toppra.interpolator as interp
+
+# === 1) 读取 CSV（只取 q1..q7）===
+CSV_PATH = "/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv"   #
+joint_cols = ["q1","q2","q3","q4","q5","q6","q7"]
+df = pd.read_csv(CSV_PATH)
+assert all(c in df.columns for c in joint_cols), "CSV 缺少关节列 q1..q7"
+waypoints = df[joint_cols].to_numpy()
+N, dof = waypoints.shape
+assert N >= 2, "至少需要两行关节路点"
+
+# === 2) 构造样条路径（路径参数 s∈[0,1]）===
+s_grid = np.linspace(0, 1, N)
+path = interp.SplineInterpolator(s_grid, waypoints)
+
+# === 3) 约束（示例值：请改成你的真实上限）===
+vmax = np.array([1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5])  # rad/s
+amax = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])  # rad/s^2
+vlim = np.vstack([-vmax, vmax]).T
+alim = np.vstack([-amax, amax]).T
+pc_vel = constraint.JointVelocityConstraint(vlim)
+pc_acc = constraint.JointAccelerationConstraint(alim)
+
+# === 4) toppra 时间最优轨迹（起止速度=0）===
+topp = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper="seidel")
+traj = topp.compute_trajectory(0.0, 0.0)
+T = traj.get_duration()
+print(f"Total duration: {T:.4f} s")
+
+# === 5) 采样并绘图 ===
+ts = np.linspace(0, T, 300)
+q   = traj.eval(ts)    # (300,7)
+qd  = traj.evald(ts)
+qdd = traj.evaldd(ts)
+
+# 位置
+plt.figure()
+for i in range(dof):
+    plt.plot(ts, q[:, i], label=f"q{i+1}")
+plt.xlabel("Time (s)"); plt.ylabel("Position (rad)"); plt.title("Joint Positions"); plt.legend(); plt.tight_layout(); plt.show()
+
+# 速度
+plt.figure()
+for i in range(dof):
+    plt.plot(ts, qd[:, i], label=f"qd{i+1}")
+plt.xlabel("Time (s)"); plt.ylabel("Velocity (rad/s)"); plt.title("Joint Velocities"); plt.legend(); plt.tight_layout(); plt.show()
+
+# 加速度
+plt.figure()
+for i in range(dof):
+    plt.plot(ts, qdd[:, i], label=f"qdd{i+1}")
+plt.xlabel("Time (s)"); plt.ylabel("Acceleration (rad/s²)"); plt.title("Joint Accelerations"); plt.legend(); plt.tight_layout(); plt.show()

include/device_manager/device_factory.h

@@ -35,6 +35,7 @@ namespace cmvr::device {
         std::shared_ptr<AbstractRobot> create_robot_(const XmlNode& cfg);
         std::shared_ptr<AbstractSpeaker> create_speaker_(const XmlNode& cfg);
         std::shared_ptr<AbstractBiohead> create_biohead_(const XmlNode& cfg);
+
     };
 }
 

src/CMakeLists.txt

@@ -5,4 +5,8 @@ add_subdirectory(monitor)
 add_subdirectory(device_manager)
 add_subdirectory(service)
 add_subdirectory(http)
-add_subdirectory(controller)
+add_subdirectory(controller)
+add_subdirectory(planner)
+
+add_subdirectory(ik_solver)
+add_subdirectory(data_center)

src/common/consts/constant.h

@@ -0,0 +1,66 @@
+#pragma once
+
+#define M2MM 0.001
+#define MM2M 0.001
+#define M2CM 100
+#define CM2M 0.01
+
+#define PI 3.141592653589793
+#define HALF_PI 1.570796326794897
+#define TWO_PI 6.283185307179586
+
+
+// 计数：最多 10 个
+#define _ArgCount(_0,_1,_2,_3,_4,_5,_6,_7,_8,_9, COUNT, ...) COUNT
+#define ArgCount(...) _ArgCount(__VA_ARGS__, 10,9,8,7,6,5,4,3,2,1,0)
+
+// 拼接
+#define _ConCAT(A,B) A##B
+#define ConCAT(A,B)  _ConCAT(A,B)
+
+// 主宏：根据参数个数路由
+#define UNUSED_VARIABLE(...) \
+ConCAT(UNUSED_VARIABLE, ArgCount(__VA_ARGS__))(__VA_ARGS__)
+
+// 0 个参数：啥也不做，保持语句形式
+#define UNUSED_VARIABLE0() do {} while (0)
+
+// 1..10 个参数：递归展开为 (void)(x);
+#define UNUSED_VARIABLE1(_0) \
+(void)(_0)
+
+#define UNUSED_VARIABLE2(_0,_1) \
+UNUSED_VARIABLE1(_0); \
+(void)(_1)
+
+#define UNUSED_VARIABLE3(_0,_1,_2) \
+UNUSED_VARIABLE2(_0,_1); \
+(void)(_2)
+
+#define UNUSED_VARIABLE4(_0,_1,_2,_3) \
+UNUSED_VARIABLE3(_0,_1,_2); \
+(void)(_3)
+
+#define UNUSED_VARIABLE5(_0,_1,_2,_3,_4) \
+UNUSED_VARIABLE4(_0,_1,_2,_3); \
+(void)(_4)
+
+#define UNUSED_VARIABLE6(_0,_1,_2,_3,_4,_5) \
+UNUSED_VARIABLE5(_0,_1,_2,_3,_4); \
+(void)(_5)
+
+#define UNUSED_VARIABLE7(_0,_1,_2,_3,_4,_5,_6) \
+UNUSED_VARIABLE6(_0,_1,_2,_3,_4,_5); \
+(void)(_6)
+
+#define UNUSED_VARIABLE8(_0,_1,_2,_3,_4,_5,_6,_7) \
+UNUSED_VARIABLE7(_0,_1,_2,_3,_4,_5,_6); \
+(void)(_7)
+
+#define UNUSED_VARIABLE9(_0,_1,_2,_3,_4,_5,_6,_7,_8) \
+UNUSED_VARIABLE8(_0,_1,_2,_3,_4,_5,_6,_7); \
+(void)(_8)
+
+#define UNUSED_VARIABLE10(_0,_1,_2,_3,_4,_5,_6,_7,_8,_9) \
+UNUSED_VARIABLE9(_0,_1,_2,_3,_4,_5,_6,_7,_8); \
+(void)(_9)

src/common/utils/math/support_functions.h

@@ -0,0 +1,141 @@
+//
+// Created by lgv on 11/10/25.
+//
+
+#pragma once
+#include <cmath>
+#include <vector>
+#include <algorithm>
+#include <iostream>
+#include <Eigen/Core>
+
+class SupportFunctions {
+private:
+    static constexpr double EPS = 1e-9;
+public:
+    static double normalize_angle(double angle) {
+        double a = std::fmod(angle, 2.0 * M_PI);
+        if (a < -M_PI) a += 2.0 * M_PI;
+        if (a > M_PI) a -= 2.0 * M_PI;
+
+        if (std::abs(a - M_PI) < EPS) return M_PI;
+        if (std::abs(a + M_PI) < EPS) return -M_PI;
+        return a;
+    }
+
+    static bool angle_in_wrap(double x, double L, double U) {
+        x = normalize_angle(x);
+        L = normalize_angle(L);
+        U = normalize_angle(U);
+        if (L <= U) return (x > L - EPS && x < U + EPS);
+        return (x > L - EPS || x < U + EPS);
+    }
+
+    static std::vector<std::pair<double, double> >
+    union_intervals(const std::vector<std::pair<double, double> > &in) {
+        if (in.empty()) return {};
+        std::vector<std::pair<double, double> > v = in;
+        std::sort(v.begin(), v.end(), [](auto &a, auto &b) {
+            return (a.first < b.first) || (a.first == b.first && a.second < b.second);
+        });
+        std::vector<std::pair<double, double> > out;
+        double L = v[0].first, R = v[0].second;
+        for (size_t i = 1; i < v.size(); ++i) {
+            if (v[i].first <= R + EPS) R = std::max(R, v[i].second);
+            else {
+                out.push_back({L, R});
+                L = v[i].first;
+                R = v[i].second;
+            }
+        }
+        out.push_back({L, R});
+        return out;
+    }
+
+
+    static std::vector<std::pair<double, double> > intersect_intervals(
+        const std::vector<std::pair<double, double> > &A, const std::vector<std::pair<double, double> > &B) {
+        if (A.empty() || B.empty()) return {};
+
+        // 先复制并排序（按起点）
+        auto SA = A, SB = B;
+        std::sort(SA.begin(), SA.end(),
+                  [](auto &x, auto &y) { return x.first < y.first; });
+        std::sort(SB.begin(), SB.end(),
+                  [](auto &x, auto &y) { return x.first < y.first; });
+
+        // 双指针求交
+        std::vector<std::pair<double, double> > out;
+        size_t i = 0, j = 0;
+        while (i < SA.size() && j < SB.size()) {
+            double L = std::max(SA[i].first, SB[j].first);
+            double R = std::min(SA[i].second, SB[j].second);
+            if (R > L) out.emplace_back(L, R);
+
+            // 谁先结束谁前进
+            if (SA[i].second < SB[j].second) ++i;
+            else ++j;
+        }
+        // 合并可能相邻/重叠的小段
+        if (out.empty()) return out;
+
+        std::vector<std::pair<double, double> > merged;
+        merged.reserve(out.size());
+        std::sort(out.begin(), out.end(),
+                  [](auto &x, auto &y) { return x.first < y.first; });
+
+        merged.push_back(out[0]);
+        for (size_t k = 1; k < out.size(); ++k) {
+            if (out[k].first <= merged.back().second + EPS) {
+                merged.back().second = std::max(merged.back().second, out[k].second);
+            } else {
+                merged.push_back(out[k]);
+            }
+        }
+        return merged;
+    }
+
+
+    static bool wraps(double L, double U) {
+        L = normalize_angle(L); // [-π, π]
+        U = normalize_angle(U); // [-π, π]
+        return (L > U); // 在 [-π, π] 规范下仍成立
+    }
+
+    static double deg2rad(double deg) { return deg * M_PI / 180.0; }
+    static double rad2deg(double rad) { return rad * 180.0 / M_PI; }
+
+    template<class T>
+    static constexpr int sign(T x, T eps) {
+        return (x > eps) - (x < -eps);
+    }
+
+    template<class T>
+    static constexpr int sign(T x) {
+        return sign(x, T(0));
+    }
+
+    template<typename T>
+    static T clamp(T v, T lo, T hi) {
+        return std::max(lo, std::min(hi, v));
+    }
+
+    static void print_intervals(const std::vector<std::pair<double, double> > &intervals) {
+        for (const auto &interval: intervals) {
+            std::cout << "[" << interval.first << ", " << interval.second << "] ";
+        }
+        std::cout << std::endl;
+    }
+
+
+    // 假设 T 是合法的刚体变换（旋转正交、det≈+1）
+    static  Eigen::Matrix4d invertHomogeneous(const Eigen::Matrix4d& T) {
+        Eigen::Matrix3d R = T.block<3,3>(0,0);
+        Eigen::Vector3d t = T.block<3,1>(0,3);
+        Eigen::Matrix4d Ti = Eigen::Matrix4d::Identity();
+        Eigen::Matrix3d Rt = R.transpose();
+        Ti.block<3,3>(0,0) = Rt;
+        Ti.block<3,1>(0,3) = -Rt * t;
+        return Ti;
+    }
+};

src/data_center/CMakeLists.txt

@@ -0,0 +1,17 @@
+find_package(protobuf REQUIRED)
+
+add_library(data_center STATIC
+    src/data_center.cpp
+        src/motors_info.cpp
+)
+
+target_include_directories(data_center PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+add_library(cmvr_es::data_center ALIAS data_center)
+
+target_link_libraries(data_center PRIVATE
+        cmvr_es::utils
+        cmvr_es::device::canbus
+        cmvr_es::device::ti5motor
+        protobuf::libprotobuf
+)

src/data_center/include/data_center.h

@@ -0,0 +1,31 @@
+//
+// Created by lgv on 11/10/25.
+//
+
+#pragma once
+#include "data_center/include/motors_info.h"
+
+namespace cmvr {
+    class DataCenter {
+    private:
+        DataCenter();
+        DataCenter(const DataCenter &) = delete;
+        DataCenter &operator=(const DataCenter &) = delete;
+
+    public:
+        static DataCenter *getInstance()
+        {
+            static DataCenter instance;
+            return &instance;
+        }
+        ~DataCenter() {}
+
+    public:
+        std::shared_ptr<MotorsInfo> getMotorsInfo() const {
+            return motors_info_;
+        }
+
+    private:
+        std::shared_ptr<MotorsInfo> motors_info_{nullptr};
+    };
+}

src/data_center/include/motors_info.h

@@ -0,0 +1,85 @@
+//
+// Created by lgv on 11/10/25.
+//
+
+#ifndef CMVR_ES_MOTORS_INFO_H
+#define CMVR_ES_MOTORS_INFO_H
+
+#include "devices/abstract_canbus.h"
+#include "canbus/can_comm/can_sender.h"
+#include "canbus/can_comm/can_receiver.h"
+#include "canbus/can_comm/message_manager.h"
+#include "cmvr/msgs/robot_detail.pb.h"
+#include "motor/motor_manager.h"
+
+namespace cmvr {
+    class MotorsInfo {
+    private:
+        MotorsInfo();
+
+        MotorsInfo(const MotorsInfo &) = delete;
+
+        MotorsInfo &operator=(const MotorsInfo &) = delete;
+
+    public:
+        ~MotorsInfo(){};
+
+        void init(const XmlNode &cfg);
+
+        static MotorsInfo *getInstance() {
+            static MotorsInfo instance;
+            return &instance;
+        }
+
+        void getJointsAngle(const std::vector<std::string> &joints_name,std::unordered_map<std::string, double> &joint_qs) const;
+
+        void getJointsAngle(const std::vector<std::string> &joints_name,std::vector<double> &joint_qs) const;
+
+        std::shared_ptr<device::MotorManager> getMotorManager() const {
+            return motor_manager_;
+        }
+
+
+
+
+
+    private:
+        std::vector<XmlNode>  l_motors_cfg_{};
+        std::vector<XmlNode>  r_motors_cfg_{};
+        std::vector<XmlNode>  waist_motors_cfg_{};
+        std::vector<XmlNode>  head_motors_cfg_{};
+
+
+        std::shared_ptr<device::AbstractCanbus> l_can_client_{nullptr};
+        std::shared_ptr<device::AbstractCanbus> r_can_client_{nullptr};
+        std::shared_ptr<device::AbstractCanbus> waist_can_client_{nullptr};
+        std::shared_ptr<device::AbstractCanbus> head_can_client_{nullptr};
+
+        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>>  l_can_receiver_{nullptr};
+        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>>  r_can_receiver_{nullptr};
+        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>>  waist_can_receiver_{nullptr};
+        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>>  head_can_receiver_{nullptr};
+
+        std::shared_ptr<device::CanSender<msgs::RobotDetail>>  l_can_sender_{nullptr};
+        std::shared_ptr<device::CanSender<msgs::RobotDetail>>  r_can_sender_{nullptr};
+        std::shared_ptr<device::CanSender<msgs::RobotDetail>>  waist_can_sender_{nullptr};
+        std::shared_ptr<device::CanSender<msgs::RobotDetail>>  head_can_sender_{nullptr};
+
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail>>  l_message_manager_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail>>  r_message_manager_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail>>  waist_message_manager_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail>>  head_message_manager_{nullptr};
+
+        bool waist_enabled_{false};
+        bool right_arm_enabled_{false};
+        bool left_arm_enabled_{false};
+        bool head_enabled_{false};
+
+        std::shared_ptr<device::MotorManager>  motor_manager_{nullptr};
+
+    };
+}
+
+
+
+#endif //CMVR_ES_MOTORS_INFO_H

src/data_center/src/data_center.cpp

@@ -0,0 +1,13 @@
+//
+// Created by lgv on 11/10/25.
+//
+
+#include "data_center/include/data_center.h"
+using namespace cmvr;
+
+
+DataCenter::DataCenter()
+{
+    motors_info_ = std::shared_ptr<MotorsInfo>(MotorsInfo::getInstance());
+
+}

src/data_center/src/motors_info.cpp

@@ -0,0 +1,177 @@
+//
+// Created by lgv on 11/10/25.
+//
+
+#include "data_center/include/motors_info.h"
+#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
+#include "motor/ti5_motor/ti5_motor.h"
+
+using namespace cmvr;
+using namespace cmvr::device;
+
+MotorsInfo::MotorsInfo() {
+}
+
+void MotorsInfo::init(const XmlNode &cfg) {
+    // 定义 lambda 函数，用于读取 XML 节点 enable 属性
+    auto readEnable = [](const XmlNode &node) -> bool {
+        std::string enable_str = node.getAttrString("enable"); // 默认 false
+        return (enable_str == "true" || enable_str == "1");
+    };
+    auto can_cfg = cfg.getChild("CanManger");
+
+    auto l_can_cfg = can_cfg.getChild("LeftArmCan");
+    left_arm_enabled_ = readEnable(l_can_cfg);
+
+    if (left_arm_enabled_) {
+        l_motors_cfg_ = l_can_cfg.getChildren("Motor");
+        l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
+        l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
+        l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
+        l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
+    }
+
+
+    auto r_can_cfg = can_cfg.getChild("RightArmCan");
+    right_arm_enabled_ = readEnable(r_can_cfg);
+    if (right_arm_enabled_) {
+        r_motors_cfg_ = r_can_cfg.getChildren("Motor");
+        r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
+        r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
+        r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
+        r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
+    }
+
+
+    auto waist_can_cfg = can_cfg.getChild("WaistCan");
+    waist_enabled_ = readEnable(waist_can_cfg);
+    if (waist_enabled_) {
+        waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
+        waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
+        waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
+        waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
+        waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
+    }
+
+    auto head_can_cfg = can_cfg.getChild("HeadCan");
+    head_enabled_ = readEnable(head_can_cfg);
+    if (head_enabled_) {
+        head_motors_cfg_ = head_can_cfg.getChildren("Motor");
+        head_can_client_ = std::make_shared<SocketCanClientRaw>(head_can_cfg);
+        head_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
+        head_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
+        head_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
+    }
+
+    // 开始初始化
+    struct Limb {
+        std::string name;
+        bool enabled;
+        std::shared_ptr<AbstractCanbus> client;
+        std::shared_ptr<CanSender<msgs::RobotDetail> > sender;
+        std::shared_ptr<CanReceiver<msgs::RobotDetail> > receiver;
+        std::shared_ptr<MessageManager<msgs::RobotDetail> > message_manager;
+        std::vector<XmlNode> motor_cfgs;
+    };
+
+    std::vector<Limb> limbs{
+        {
+            "WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_,
+            waist_motors_cfg_
+        },
+        {
+            "LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_,
+            l_motors_cfg_
+        },
+        {
+            "RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_,
+            r_motors_cfg_
+        },
+        {
+            "HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_,
+            head_motors_cfg_
+        }
+    };
+
+    // 创建 MotorManager
+    motor_manager_ = std::make_shared<MotorManager>();
+
+    std::vector<std::future<void> > tasks;
+
+    for (auto &limb: limbs) {
+        if (!limb.enabled) continue;
+
+
+        if (limb.client) limb.client->init();
+
+        // 2. 初始化 Sender / Receiver（如果有）
+        if (limb.sender && limb.receiver && limb.client) {
+            auto ret = limb.sender->Init(limb.client.get(), false);
+            if (ret != ErrorCode::OK)
+                LOG(ERROR) << "Failed to init " << limb.name << " CAN sender.";
+
+            ret = limb.receiver->Init(limb.client.get(), limb.message_manager.get(), false);
+            if (ret != ErrorCode::OK)
+                LOG(ERROR) << "Failed to init " << limb.name << " CAN receiver.";
+
+            limb.client->start();
+            ret = limb.sender->Start();
+            if (ret != ErrorCode::OK)
+                LOG(ERROR) << "Failed to start " << limb.name << " CAN sender.";
+
+            ret = limb.receiver->Start();
+            if (ret != ErrorCode::OK)
+                LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver.";
+        }
+
+        // 3. 创建协议（如果有CAN）
+        std::shared_ptr<Ti5MotorCanopenProtocol> protocol = nullptr;
+        if (limb.sender && limb.message_manager) {
+            protocol = std::make_shared<Ti5MotorCanopenProtocol>(limb.sender, limb.message_manager);
+        }
+
+        // 4. 并行初始化电机
+        if (!limb.motor_cfgs.empty()) {
+            tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] {
+                LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name <<
+                        " motors...";
+                for (const auto &cfg: limb.motor_cfgs) {
+                    auto motor = std::make_shared<Ti5Motor>(cfg);
+                    if (protocol) motor->setProtocol(protocol);
+                    motor->init();
+                    motor_manager_->addMotor(motor);
+                }
+            }));
+        }
+    }
+
+    // 等待所有任务完成
+    for (auto &task: tasks) task.get();
+
+    LOG(INFO) << "All enabled motors initialized successfully.";
+}
+
+void MotorsInfo::getJointsAngle(const std::vector<std::string> &joints_name,
+                                std::unordered_map<std::string, double> &joint_qs) const {
+    joint_qs.clear();
+    for (auto &name: joints_name) {
+        auto motor = motor_manager_->getMotor(name);
+        if (motor) {
+            joint_qs.emplace(name, motor->getQ());
+        } else {
+            joint_qs.emplace(name, std::numeric_limits<double>::quiet_NaN());
+        }
+    }
+}
+
+void MotorsInfo::getJointsAngle(const std::vector<std::string> &joints_name, std::vector<double> &joint_qs) const {
+    joint_qs.clear();
+    for (auto &name: joints_name) {
+        auto motor = motor_manager_->getMotor(name);
+        if (motor) {
+            joint_qs.emplace_back(motor->getQ());
+        } else {
+            joint_qs.emplace_back(std::numeric_limits<double>::quiet_NaN());
+        }
+    }
+}

src/device_manager/device_factory.cpp

@@ -12,6 +12,7 @@
 #include "dexhand/rh56dftp_dexhand/rh56dftp_dexhand.h"
 #include "robot/humanoid_robot/humanoid_robot.h"
 //#include "robot/ti5_robot/ti5_robot.h"
+#include "data_center/include/motors_info.h"
 using namespace std;
 using namespace cmvr::device;
 
@@ -120,6 +121,8 @@ std::shared_ptr<AbstractMicrophone> DeviceFactory::create_mic_(const XmlNode& cf
 std::shared_ptr<AbstractRobot> DeviceFactory::create_robot_(const XmlNode& cfg) {
     try {
         if (cfg.getNodeName() == "Humanoid") {
+            auto motos_info = MotorsInfo::getInstance();
+            motos_info->init(cfg);
             return std::make_shared<HumanoidRobot<14>>(cfg);
         }
         else {

src/devices/robot/humanoid_robot/CMakeLists.txt

@@ -8,9 +8,9 @@ add_library(cmvr_es::device::humanoid_robot ALIAS humanoid_robot)
 
 target_link_libraries(humanoid_robot PRIVATE
         cmvr_es::utils
-        cmvr_es::device::canbus
-        cmvr_es::device::ti5motor
+        cmvr_es::data_center
         protobuf::libprotobuf
+        cmvr_es::ik_solver
 )
 
 
@@ -38,8 +38,7 @@ add_executable(humanoid_robot_test
 
 target_link_libraries(humanoid_robot_test
         PRIVATE
-        cmvr_es::device::canbus
-        cmvr_es::device::ti5motor
+        cmvr_es::data_center
         cmvr_es::device::humanoid_robot
         gtest
         gtest_main
@@ -50,4 +49,5 @@ target_link_libraries(humanoid_robot_test
         fcl
         cmvr_es::device_manager
         ${OpenCV_LIBS}
+        cmvr_es::ik_solver
 )

src/devices/robot/humanoid_robot/humanoid_robot.cpp

@@ -6,7 +6,7 @@
 #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
 #include "motor/ti5_motor/ti5_motor.h"
 #include  "utils/base/abstract_interpolation.h"
-
+#include "data_center/include/motors_info.h"
 
 using namespace std;
 using namespace cmvr::device;
@@ -37,56 +37,6 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
         CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50));
 
 
-        // 定义 lambda 函数，用于读取 XML 节点 enable 属性
-        auto readEnable = [](const XmlNode &node) -> bool {
-            std::string enable_str = node.getAttrString("enable"); // 默认 false
-            return (enable_str == "true" || enable_str == "1");
-        };
-        auto can_cfg = cfg.getChild("CanManger");
-
-        auto l_can_cfg = can_cfg.getChild("LeftArmCan");
-        left_arm_enabled_ = readEnable(l_can_cfg);
-
-        if (left_arm_enabled_) {
-            l_motors_cfg_ = l_can_cfg.getChildren("Motor");
-            l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
-            l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
-            l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
-            l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
-        }
-
-
-        auto r_can_cfg = can_cfg.getChild("RightArmCan");
-        right_arm_enabled_ = readEnable(r_can_cfg);
-        if (right_arm_enabled_) {
-            r_motors_cfg_ = r_can_cfg.getChildren("Motor");
-            r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
-            r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
-            r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
-            r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
-        }
-
-
-        auto waist_can_cfg = can_cfg.getChild("WaistCan");
-        waist_enabled_ = readEnable(waist_can_cfg);
-        if (waist_enabled_) {
-            waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
-            waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
-            waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
-            waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
-            waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
-        }
-
-        auto head_can_cfg = can_cfg.getChild("HeadCan");
-        head_enabled_ = readEnable(head_can_cfg);
-        if (head_enabled_) {
-            head_motors_cfg_ = head_can_cfg.getChildren("Motor");
-            head_can_client_ = std::make_shared<SocketCanClientRaw>(head_can_cfg);
-            head_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
-            head_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
-            head_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
-        }
-
 
         upd_timer_ = make_shared<FDTimer>();
         upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
@@ -100,92 +50,8 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
 
 template<int DOF>
 void HumanoidRobot<DOF>::init() {
-    struct Limb {
-        std::string name;
-        bool enabled;
-        std::shared_ptr<AbstractCanbus> client;
-        std::shared_ptr<CanSender<msgs::RobotDetail> > sender;
-        std::shared_ptr<CanReceiver<msgs::RobotDetail> > receiver;
-        std::shared_ptr<MessageManager<msgs::RobotDetail> > message_manager;
-        std::vector<XmlNode> motor_cfgs;
-    };
-
-    std::vector<Limb> limbs{
-        {
-            "WAIST", waist_enabled_, waist_can_client_, waist_can_sender_, waist_can_receiver_, waist_message_manager_,
-            waist_motors_cfg_
-        },
-        {
-            "LEFT_ARM", left_arm_enabled_, l_can_client_, l_can_sender_, l_can_receiver_, l_message_manager_,
-            l_motors_cfg_
-        },
-        {
-            "RIGHT_ARM", right_arm_enabled_, r_can_client_, r_can_sender_, r_can_receiver_, r_message_manager_,
-            r_motors_cfg_
-        },
-        {
-            "HEAD", head_enabled_, head_can_client_, head_can_sender_, head_can_receiver_, head_message_manager_,
-            head_motors_cfg_
-        }
-    };
-
-    // 创建 MotorManager
-    motor_manager_ = std::make_shared<MotorManager>();
-
-    std::vector<std::future<void> > tasks;
-
-    for (auto &limb: limbs) {
-        if (!limb.enabled) continue;
-
-
-        if (limb.client) limb.client->init();
-
-        // 2. 初始化 Sender / Receiver（如果有）
-        if (limb.sender && limb.receiver && limb.client) {
-            auto ret = limb.sender->Init(limb.client.get(), false);
-            if (ret != ErrorCode::OK)
-                LOG(ERROR) << "Failed to init " << limb.name << " CAN sender.";
-
-            ret = limb.receiver->Init(limb.client.get(), limb.message_manager.get(), false);
-            if (ret != ErrorCode::OK)
-                LOG(ERROR) << "Failed to init " << limb.name << " CAN receiver.";
-
-            limb.client->start();
-            ret = limb.sender->Start();
-            if (ret != ErrorCode::OK)
-                LOG(ERROR) << "Failed to start " << limb.name << " CAN sender.";
-
-            ret = limb.receiver->Start();
-            if (ret != ErrorCode::OK)
-                LOG(ERROR) << "Failed to start " << limb.name << " CAN receiver.";
-        }
-
-        // 3. 创建协议（如果有CAN）
-        std::shared_ptr<Ti5MotorCanopenProtocol> protocol = nullptr;
-        if (limb.sender && limb.message_manager) {
-            protocol = std::make_shared<Ti5MotorCanopenProtocol>(limb.sender, limb.message_manager);
-        }
-
-        // 4. 并行初始化电机
-        if (!limb.motor_cfgs.empty()) {
-            tasks.push_back(std::async(std::launch::async, [this, protocol, &limb] {
-                LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing " << limb.name <<
-                        " motors...";
-                for (const auto &cfg: limb.motor_cfgs) {
-                    auto motor = std::make_shared<Ti5Motor>(cfg);
-                    if (protocol) motor->setProtocol(protocol);
-                    motor->init();
-                    motor_manager_->addMotor(motor);
-                }
-            }));
-        }
-    }
-
-    // 等待所有任务完成
-    for (auto &task: tasks) task.get();
-
+    motor_manager_ = MotorsInfo::getInstance()->getMotorManager();
     rsm_.store(ROBOT_ESTOP);
-    LOG(INFO) << "All enabled motors initialized successfully.";
 }
 
 template<int DOF>

src/devices/robot/humanoid_robot/humanoid_robot.h

@@ -187,36 +187,6 @@ namespace cmvr::device{
 
 
     private:
-        std::vector<XmlNode>  l_motors_cfg_{};
-        std::vector<XmlNode>  r_motors_cfg_{};
-        std::vector<XmlNode>  waist_motors_cfg_{};
-        std::vector<XmlNode>  head_motors_cfg_{};
-
-
-        std::shared_ptr<AbstractCanbus> l_can_client_{nullptr};
-        std::shared_ptr<AbstractCanbus> r_can_client_{nullptr};
-        std::shared_ptr<AbstractCanbus> waist_can_client_{nullptr};
-        std::shared_ptr<AbstractCanbus> head_can_client_{nullptr};
-
-        std::shared_ptr<CanReceiver<msgs::RobotDetail>>  l_can_receiver_{nullptr};
-        std::shared_ptr<CanReceiver<msgs::RobotDetail>>  r_can_receiver_{nullptr};
-        std::shared_ptr<CanReceiver<msgs::RobotDetail>>  waist_can_receiver_{nullptr};
-        std::shared_ptr<CanReceiver<msgs::RobotDetail>>  head_can_receiver_{nullptr};
-
-        std::shared_ptr<CanSender<msgs::RobotDetail>>  l_can_sender_{nullptr};
-        std::shared_ptr<CanSender<msgs::RobotDetail>>  r_can_sender_{nullptr};
-        std::shared_ptr<CanSender<msgs::RobotDetail>>  waist_can_sender_{nullptr};
-        std::shared_ptr<CanSender<msgs::RobotDetail>>  head_can_sender_{nullptr};
-
-        std::shared_ptr<MessageManager<msgs::RobotDetail>>  l_message_manager_{nullptr};
-        std::shared_ptr<MessageManager<msgs::RobotDetail>>  r_message_manager_{nullptr};
-        std::shared_ptr<MessageManager<msgs::RobotDetail>>  waist_message_manager_{nullptr};
-        std::shared_ptr<MessageManager<msgs::RobotDetail>>  head_message_manager_{nullptr};
-
-        bool waist_enabled_{false};
-        bool right_arm_enabled_{false};
-        bool left_arm_enabled_{false};
-        bool head_enabled_{false};
 
         std::shared_ptr<MotorManager>  motor_manager_{nullptr};
 

src/ik_solver/CMakeLists.txt

@@ -0,0 +1,76 @@
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+find_package(osqp REQUIRED)
+find_package(OsqpEigen REQUIRED)
+find_package(PkgConfig REQUIRED)
+find_package(fcl REQUIRED)
+pkg_check_modules(TINYXML2 REQUIRED tinyxml2)
+
+include_directories(${TINYXML2_INCLUDE_DIRS})
+
+add_library(ik_solver STATIC
+    ik_solver/src/ik_solver.cpp
+    ik_solver/src/ik_solver_creator.cpp
+        opt_psi_limit_bias_solver/src/bias_srs_ik_slover.cpp
+        opt_psi_limit_bias_solver/src/joints_limit_analyzer.cpp
+        opt_psi_limit_bias_solver/src/opt_psi_limit_bias_solver.cpp
+        opt_psi_limit_bias_solver/src/opt_psi_selector.cpp
+)
+
+target_include_directories(ik_solver PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(ik_solver PRIVATE
+        Eigen3::Eigen
+        OsqpEigen::OsqpEigen
+        ${TINYXML2_LIBRARIES}
+        fcl
+        cmvr_es::data_center
+)
+
+add_library(cmvr_es::ik_solver ALIAS ik_solver)
+
+
+# --------------------------------------------------------
+# Unit test
+# --------------------------------------------------------
+find_package(glog REQUIRED)
+find_package(PkgConfig REQUIRED)
+find_package(fcl REQUIRED)
+find_package(OpenCV REQUIRED)
+
+include_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
+)
+
+include_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/manif/0.0.5/include
+)
+
+
+link_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
+)
+
+
+
+
+add_executable(srs_ik_test
+        ${CMAKE_CURRENT_SOURCE_DIR}/opt_psi_limit_bias_solver/src/srs_ik_test.cpp
+)
+
+
+target_link_libraries(srs_ik_test
+        PRIVATE
+        gtest
+        gtest_main
+        pthread
+        glog::glog
+        proto-objects
+        ccd
+        fcl
+        ${OpenCV_LIBS}
+        Eigen3::Eigen
+        OsqpEigen::OsqpEigen
+        cmvr_es::utils
+        cmvr_es::data_center
+        cmvr_es::ik_solver
+)

src/ik_solver/ik_solver/include/ik_solver.h

@@ -0,0 +1,49 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+#pragma once
+
+#include "ik_solver.h"
+#include <vector>
+#include <Eigen/Core>
+#include <common/consts/constant.h>
+namespace cmvr {
+    class IKSolver {
+    public:
+        IKSolver()=default;
+        virtual ~IKSolver()=default;
+
+        virtual bool init() {
+            return false;
+        }
+
+        virtual bool ik(const Eigen::Matrix4d &target_pose,std::vector<double> &joints_angle) {
+            UNUSED_VARIABLE(target_pose,joints_angle);
+            return false;
+        }
+
+        virtual bool fk(const std::vector<double> &joints_angle,Eigen::Matrix4d &cur_pose,bool robot_base = false) {
+            UNUSED_VARIABLE(joints_angle,cur_pose,robot_base);
+            return false;
+        };
+
+        // 设置 机械臂末端工具坐标系相对于末端法兰的变换矩阵
+        void setTcpTransform(const Eigen::Matrix4d& T_tool_flange) {
+            T_tool_flange_ = T_tool_flange;
+        }
+
+
+
+        void setArmBaseTransform(const Eigen::Matrix4d& T_arm_robot) {
+            T_arm_robot_ = T_arm_robot;
+        }
+
+    protected:
+        //机械臂末端工具坐标系相对于末端法兰的变换矩阵
+        Eigen::Matrix4d T_tool_flange_{};
+
+        // 机械臂基座相对于机器人基座的变换矩阵
+        Eigen::Matrix4d T_arm_robot_{};
+    };
+}

src/ik_solver/ik_solver/include/ik_solver_creator.h

@@ -0,0 +1,34 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+#pragma once
+
+#include <memory>
+#include <unordered_map>
+#include "ik_solver.h"
+
+namespace cmvr
+{
+    enum IkSolverType
+    {
+        IK_SOLVER_UNKNOWN = 0,
+
+        // 最优臂角+角度限制+角度偏置的SRS的解析解
+        OPT_PSI_LIMIT_BIAS_SRS = 1,
+    };
+    class IkSolverCreator
+    {
+    protected:
+        static std::unordered_map<IkSolverType, std::shared_ptr<IKSolver>>
+            registers_;
+        static std::shared_ptr<IKSolver> createNew(const IkSolverType type);
+
+    public:
+        static void clear()
+        {
+            registers_.clear();
+        }
+        static std::shared_ptr<IKSolver> create(const IkSolverType type);
+    };
+}

src/ik_solver/ik_solver/src/ik_solver.cpp

@@ -0,0 +1,3 @@
+//
+// Created by lgv on 11/7/25.
+//

src/ik_solver/ik_solver/src/ik_solver_creator.cpp

@@ -0,0 +1,39 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+
+#include "ik_solver/include/ik_solver_creator.h"
+namespace cmvr
+{
+    std::unordered_map<IkSolverType, std::shared_ptr<IKSolver>>
+        IkSolverCreator::registers_;
+
+    std::shared_ptr<IKSolver> IkSolverCreator::create(const IkSolverType type)
+    {
+        static std::unordered_map<IkSolverType, std::shared_ptr<IKSolver>>
+            registered_planners;
+        auto planner_pair = registered_planners.find(type);
+        if (planner_pair == registered_planners.end())
+        {
+            auto res = createNew(type);
+            registered_planners.emplace(type, res);
+            return res;
+        }
+        return planner_pair->second;
+    }
+    std::shared_ptr<IKSolver>
+    IkSolverCreator::createNew(const IkSolverType type)
+    {
+        std::shared_ptr<IKSolver> result;
+        switch (type)
+        {
+        case IK_SOLVER_UNKNOWN:
+            return nullptr;
+        case OPT_PSI_LIMIT_BIAS_SRS:
+            // return std::make_shared<HybridAstarParkIn>();
+        default:
+            return nullptr;
+        }
+    }
+}

src/ik_solver/opt_psi_limit_bias_solver/include/bias_srs_ik_slover.h

@@ -7,11 +7,10 @@
 
 
 #include <Eigen/Dense>
-#include "srs_ik/ik_limit_analyzer.h"
-namespace cmvr {
-namespace utils {
 
-    class SRSIkSlover {
+namespace cmvr {
+
+    class BiasSRSIkSolver {
     public:
         enum ConfigDirection {
             OUTWARD = 1,  // 向外
@@ -19,15 +18,14 @@ namespace utils {
         };
 
     public:
-        SRSIkSlover();
-        ~SRSIkSlover(){};
+        BiasSRSIkSolver();
+        ~BiasSRSIkSolver(){};
 
         std::vector<double>  inverse_kinematics(const Eigen::MatrixXd& pose, double psi);
 
         Eigen::Matrix4d calc_total_transform(const std::vector<double>& joint_angles);
 
         bool cal_coefficient_matrix(const Eigen::MatrixXd& pose,Eigen::MatrixXd& s_mat , Eigen::MatrixXd& w_mat);
-        std::vector<std::pair<double, double>>  calc_arm_angle_limits(const Eigen::MatrixXd& s_mat ,const Eigen::MatrixXd& w_mat);
 
         void set_shoulder_config(ConfigDirection value) {
             shoulder_config_ = value;
@@ -77,9 +75,6 @@ namespace utils {
         // first : min   second : max
         std::vector<std::pair<double, double>>  joints_limits_{};
 
-        IkLimitAnalyzer  ik_limit_analyzer_;
-
-
 
         // 计算参考平面相对于基坐标系的旋转矩阵
         Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, const Eigen::Vector3d& W);
@@ -91,15 +86,8 @@ namespace utils {
         // 使用 DH 参数计算变换矩阵
          Eigen::Matrix4d calc_dh(double d, double alpha, double a, double theta);
 
-        // 将角度归一化到 [-π, π] 范围内
-        double normalize_angle(const double angle);
-
-
-
-
     };
 }
-}
 
 
 #endif //CMVR_ES_SRS_IK_SLOVER_H

src/ik_solver/opt_psi_limit_bias_solver/include/joints_limit_analyzer.h

@@ -0,0 +1,113 @@
+//
+// Created by lgv on 2025/11/3.
+//
+
+#ifndef CMVR_ES_IK_LIMIT_ANALYZER_H
+#define CMVR_ES_IK_LIMIT_ANALYZER_H
+
+#include <vector>
+#include <Eigen/Core>
+#include "common/utils/math/support_functions.h"
+
+namespace cmvr {
+    class JointsLimitAnalyzer {
+    public:
+        static std::vector<std::pair<double, double> > calc_arm_angle_limits(
+            const Eigen::MatrixXd &s_mat, const Eigen::MatrixXd &w_mat,
+            std::vector<std::pair<double, double> > joints_limits,
+            int s_conf, int e_conf, int w_conf);
+
+        // Tan 型关节：给定 (an,ad,bn,bd,cn,cd) 与关节极限
+        // 返回 psi 的允许区间边界： [L1,R1,L2,R2,...]
+        static std::vector<std::pair<double, double> >
+        calc_tan_limits(double an, double ad,
+                        double bn, double bd,
+                        double cn, double cd,
+                        double joint_l, double joint_u);
+
+        static std::vector<std::pair<double, double> >
+        calc_tan_limits(double an, double ad,
+                        double bn, double bd,
+                        double cn, double cd,
+                        double joint_l, double joint_u, double offset);
+
+        static std::vector<std::pair<double, double> >
+        calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u);
+
+        static std::vector<std::pair<double, double> >
+        calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u, double offset);
+
+
+
+        static void set_sing_avid(double value_deg) { psi_sing_avid_ = SupportFunctions::deg2rad(value_deg); }
+
+    public:
+        //  反算 ψ 的返回结果
+        struct PsiEstimateResult {
+            bool ok{false};
+            double psi{0.0}; // 估计出来的 ψ
+            double score{0.0}; // 总残差（越小越好）
+            std::vector<std::pair<double, double> > candidates{}; // 候选 ψ ，以及分数
+        };
+
+        // 用当前关节角反算“上一时刻/当前估计”的臂角 ψ
+        // s_conf/e_conf/w_conf = {+1, -1}；prefer_psi  NaN 表示无偏好
+        static PsiEstimateResult estimate_psi_from_joints(
+            const Eigen::MatrixXd &s_mat, // 3x9  [As|Bs|Cs]
+            const Eigen::MatrixXd &w_mat, // 3x9  [Aw|Bw|Cw]
+            const std::vector<double> &theta_c, // 当前 7 关节角
+            int s_conf, int e_conf, int w_conf,
+            double prefer_psi = std::numeric_limits<double>::quiet_NaN()
+        );
+
+    private:
+        // 打分时的前向预测（方程里的“φ”减去 offset 得 θ̂）
+        static inline double predict_theta_tan(double an, double ad, double bn, double bd, double cn, double cd,
+                                               double psi, double offset);
+
+        static inline double predict_theta_cos(double a, double b, double c, int conf, double psi, double offset);
+
+    private:
+
+        static constexpr double EPS = 1e-9;
+
+
+        inline static double psi_sing_avid_ = SupportFunctions::deg2rad(5);
+
+
+        // tan 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解，式（32）
+        static std::vector<double>
+        calc_tan_solution(double an, double ad,
+                          double bn, double bd,
+                          double cn, double cd,
+                          double theta);
+
+        // cos 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解
+        static std::vector<double>
+        calc_cos_solution(double a, double b, double c, double theta);
+
+
+        // 检核：tan 型
+        static bool check_tan_solution(double an, double ad,
+                                       double bn, double bd,
+                                       double cn, double cd,
+                                       double theta_target,
+                                       double psi);
+
+        // 检核：cos 型
+        static bool check_cos_solution(double a, double b, double c,
+                                       double theta_target,
+                                       double psi);
+
+        static std::vector<std::pair<double, double> >
+        bounds_to_pairs(const std::vector<double> &bounds);
+
+        // 根据DH 参数中的 关节角 θi (rad)  的偏移来计算实际的限位区间
+        static inline std::vector<std::pair<double, double> >
+        cal_offset_limits(double joint_l, double joint_u, double offset);
+
+    };
+}
+
+
+#endif //CMVR_ES_IK_LIMIT_ANALYZER_H

src/ik_solver/opt_psi_limit_bias_solver/include/opt_psi_limit_bias_solver.h

@@ -0,0 +1,45 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+#ifndef CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
+#define CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H
+#include "joints_limit_analyzer.h"
+#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
+#include "ik_solver/opt_psi_limit_bias_solver/include/bias_srs_ik_slover.h"
+#include "ik_solver/opt_psi_limit_bias_solver/include/joints_limit_analyzer.h"
+#include "ik_solver/opt_psi_limit_bias_solver/include/opt_psi_selector.h"
+#include "ik_solver/ik_solver/include/ik_solver.h"
+
+namespace cmvr{
+    class OptPsiLimitBiasSolver : public IKSolver{
+    public:
+        OptPsiLimitBiasSolver();
+        ~OptPsiLimitBiasSolver()=default;
+
+        bool init() override;
+        bool ik(const Eigen::Matrix4d &target_pose, std::vector<double> &joints_angle) override;
+
+        bool fk(const std::vector<double> &joints_angle, Eigen::Matrix4d &cur_pose, bool robot_base = false) override;
+
+        // 机械臂运动完成后一定要调用一下此接口
+        void update_joints_state(std::vector<double> cur_joints_angle) {
+            cur_joints_angle_ = std::move(cur_joints_angle);
+        }
+    private:
+        std::shared_ptr<BiasSRSIkSolver> bias_srs_ik_solver_{nullptr};
+        std::shared_ptr<JointsLimitAnalyzer> joints_limit_analyzer_{nullptr};
+        std::shared_ptr<OptPsiSelector> opt_psi_selector_{nullptr};
+        // 先前的最优臂角
+        struct  {
+            double value{};
+            bool valid{false};
+        }prev_opt_psi_;
+
+
+        // todo :: 后续添加到 data_center
+        std::vector<double> cur_joints_angle_{};
+    };
+}
+
+#endif //CMVR_ES_OPT_PSI_LIMIT_BIAS_SLOVER_H

src/ik_solver/opt_psi_limit_bias_solver/include/opt_psi_selector.h

@@ -0,0 +1,41 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+
+#pragma once
+
+#include <cmath>
+#include <vector>
+#include "common/utils/math/support_functions.h"
+
+namespace cmvr {
+    class OptPsiSelector {
+    public:
+
+        // 根据全局可行区间 Ψ_all（并集），按论文公式从 ψ_{t-1} 计算 ψ_{t}
+        bool update_psi(
+            double &psi_best,
+            double psi_prev,
+            const std::vector<std::pair<double, double> > &psi_all // merged ∪-intervals, each L<R in [-π,π]
+        );
+
+        void set_update_params(double k, double alpha,double step_cap, double edge_margin) {
+            k_ = k;
+            alpha_ = alpha;
+            step_cap_ = step_cap;
+            edge_margin_ = edge_margin;
+        }
+
+
+
+    private:
+        // 论文 Fig.10 的“Calculate New ψ”一步更新所需参数
+        double k_ = 0.6; // [0,1] 排斥强度
+        double alpha_ = 5.0; // >0  开始排斥的“敏感度”
+        double step_cap_ = 0.0; // psi 与上一次psi 的单步最大变化（rad），<=0 表示不限制
+        double edge_margin_ = 1e-4; // 落到区间边界时的内缩量
+
+        static constexpr double EPS = 1e-9;
+    };
+}

src/ik_solver/opt_psi_limit_bias_solver/src/bias_srs_ik_slover.cpp

@@ -2,16 +2,15 @@
 // Created by lgv on 2025/11/3.
 //
 
-#include "srs_ik_slover.h"
-
+#include "opt_psi_limit_bias_solver/include/bias_srs_ik_slover.h"
 #include <iostream>
-
-using namespace cmvr::utils;
+#include "common/utils/math/support_functions.h"
+using namespace cmvr;
 using namespace Eigen;
 
-SRSIkSlover::SRSIkSlover() {
+BiasSRSIkSolver::BiasSRSIkSolver() {
     link_lengths_ = Eigen::VectorXd(4);
-    link_lengths_ << 0.0945 + 0.0765, 0.1475 + 0.1025, 0.0965 + 0.1525, 3.222;
+    link_lengths_ << 0.0945 + 0.0765, 0.1475 + 0.1025, 0.0965 + 0.1525, 0.03;
 
     // DH 参数 [d_i, alpha_i, a_i(=0), theta_offset_i]
     dh_params_ = Eigen::MatrixXd(7, 4);
@@ -40,7 +39,7 @@ SRSIkSlover::SRSIkSlover() {
     };
 }
 
-Eigen::Matrix3d SRSIkSlover::reference_plane(const Eigen::Vector3d &S, const Eigen::Vector3d &W) {
+Eigen::Matrix3d BiasSRSIkSolver::reference_plane(const Eigen::Vector3d &S, const Eigen::Vector3d &W) {
     double d_sw = (W - S).norm();
     Eigen::Vector3d v_sw = (W - S).normalized();
 
@@ -65,7 +64,7 @@ Eigen::Matrix3d SRSIkSlover::reference_plane(const Eigen::Vector3d &S, const Eig
     Vector3d v_ew = (W - E).normalized();
 
 
-    Vector3d R30_y = v_es;
+    const Vector3d R30_y = v_es;
     Vector3d R30_z = v_ew.cross(v_es);
 
 
@@ -92,7 +91,7 @@ Eigen::Matrix3d SRSIkSlover::reference_plane(const Eigen::Vector3d &S, const Eig
     return R30;
 }
 
-std::vector<double> SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose, double psi) {
+std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &pose, double psi) {
     try {
         // Eigen::VectorXd joints(7);
         std::vector<double> joints(7, 0);
@@ -166,11 +165,9 @@ std::vector<double> SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose,
         }
 
         // 调整角度
-        joints[4] = normalize_angle(phi_z - M_PI / 2);
-        joints[5] = normalize_angle(theta_y - M_PI / 2);
-        joints[6] = normalize_angle(psi_z);
-
-
+        joints[4] = SupportFunctions::normalize_angle(phi_z - M_PI / 2);
+        joints[5] = SupportFunctions::normalize_angle(theta_y - M_PI / 2);
+        joints[6] = SupportFunctions::normalize_angle(psi_z);
 
         return joints;
     } catch (const std::exception &e) {
@@ -178,7 +175,7 @@ std::vector<double> SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose,
     }
 }
 
-Eigen::Matrix3d SRSIkSlover::calc_rotation_matrix(const Eigen::Vector3d &rotation_axis, double rotation_angle) {
+Eigen::Matrix3d BiasSRSIkSolver::calc_rotation_matrix(const Eigen::Vector3d &rotation_axis, double rotation_angle) {
     // 归一化旋转轴
     Eigen::Vector3d normalized_axis = rotation_axis.normalized();
 
@@ -196,7 +193,7 @@ Eigen::Matrix3d SRSIkSlover::calc_rotation_matrix(const Eigen::Vector3d &rotatio
     return rotation_matrix;
 }
 
-Eigen::Matrix4d SRSIkSlover::calc_dh(double d, double alpha, double a, double theta) {
+Eigen::Matrix4d BiasSRSIkSolver::calc_dh(double d, double alpha, double a, double theta) {
     double ca = std::cos(alpha);
     double sa = std::sin(alpha);
     double ct = std::cos(theta);
@@ -212,18 +209,7 @@ Eigen::Matrix4d SRSIkSlover::calc_dh(double d, double alpha, double a, double th
     return T;
 }
 
-double SRSIkSlover::normalize_angle(const double angle) {
-    double EPS = 1E-9;
-    double a = std::fmod(angle, 2.0 * M_PI);
-    if (a < -M_PI) a += 2.0 * M_PI;
-    if (a > M_PI) a -= 2.0 * M_PI;
-
-    if (std::abs(a - M_PI) < EPS) return M_PI;
-    if (std::abs(a + M_PI) < EPS) return -M_PI;
-    return a;
-}
-
-Eigen::Matrix4d SRSIkSlover::calc_total_transform(const std::vector<double> &joint_angles) {
+Eigen::Matrix4d BiasSRSIkSolver::calc_total_transform(const std::vector<double> &joint_angles) {
     Eigen::Matrix4d T_total = Eigen::Matrix4d::Identity(); // 初始化为单位矩阵
 
     // 遍历每一组 DH 参数
@@ -247,7 +233,7 @@ Eigen::Matrix4d SRSIkSlover::calc_total_transform(const std::vector<double> &joi
     return T_total;
 }
 
-bool SRSIkSlover::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen::MatrixXd &s_mat, Eigen::MatrixXd &w_mat) {
+bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &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);
 
@@ -317,63 +303,4 @@ bool SRSIkSlover::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen::Mat
         throw std::runtime_error(e.what());
         return false;
     }
-}
-
-
-std::vector<std::pair<double, double> > SRSIkSlover::calc_arm_angle_limits(
-    const Eigen::MatrixXd &s_mat, const Eigen::MatrixXd &w_mat) {
-    // 期望 s_mat / w_mat 都是 3x9： [A | B | C]
-    if (s_mat.rows() != 3 || s_mat.cols() != 9 ||
-        w_mat.rows() != 3 || w_mat.cols() != 9) {
-        // 尺寸不对，直接返回空
-        return {};
-    }
-
-
-    const Eigen::Matrix3d As = s_mat.block<3, 3>(0, 0);
-    const Eigen::Matrix3d Bs = s_mat.block<3, 3>(0, 3);
-    const Eigen::Matrix3d Cs = s_mat.block<3, 3>(0, 6);
-
-    const Eigen::Matrix3d Aw = w_mat.block<3, 3>(0, 0);
-    const Eigen::Matrix3d Bw = w_mat.block<3, 3>(0, 3);
-    const Eigen::Matrix3d Cw = w_mat.block<3, 3>(0, 6);
-
-    auto s = static_cast<int>(shoulder_config_);
-    auto w = static_cast<int>(wrist_config_);
-
-    auto limit_1 = ik_limit_analyzer_.calc_tan_limits(-s * As(1, 1), -s * As(0, 1), -s * Bs(1, 1), -s * Bs(0, 1),
-                                                      -s * Cs(1, 1), -s * Cs(0, 1),
-                                                      joints_limits_[0].first, joints_limits_[0].second);
-
-    auto limit_2 = ik_limit_analyzer_.calc_cos_limits(-As(2, 1), -Bs(2, 1), -Cs(2, 1), s,
-                                                      joints_limits_[1].first, joints_limits_[1].second);
-
-    auto limit_3 = ik_limit_analyzer_.calc_tan_limits(s * As(2, 2),
-                                                      -s * As(2, 0), s * Bs(2, 2), -s * Bs(2, 0),
-                                                      s * Cs(2, 2), -s * Cs(2, 0),
-                                                      joints_limits_[2].first, joints_limits_[2].second);
-
-    auto limit_5 = ik_limit_analyzer_.calc_tan_limits(w * Aw(1, 2),
-                                                      w * Aw(0, 2), w * Bw(1, 2), w * Bw(0, 2),
-                                                      w * Cw(1, 2), w * Cw(0, 2),
-                                                      joints_limits_[4].first, joints_limits_[4].second,M_PI / 2.0);
-
-    auto limit_6 = ik_limit_analyzer_.calc_cos_limits(Aw(2, 2), Bw(2, 2), Cw(2, 2), w,
-                                                      joints_limits_[5].first, joints_limits_[5].second,M_PI / 2.0);
-    auto limit_7 = ik_limit_analyzer_.calc_tan_limits(w * Aw(2, 1),
-                                                      -w * Aw(2, 0), w * Bw(2, 1), -w * Bw(2, 0),
-                                                      w * Cw(2, 1), -w * Cw(2, 0),
-                                                      joints_limits_[6].first, joints_limits_[6].second);
-
-
-    auto limits = ik_limit_analyzer_.intersect(limit_1,limit_2);
-    limits = ik_limit_analyzer_.intersect(limits,limit_3);
-    limits = ik_limit_analyzer_.intersect(limits,limit_5);
-    limits = ik_limit_analyzer_.intersect(limits,limit_6);
-    limits = ik_limit_analyzer_.intersect(limits,limit_7);
-
-    ik_limit_analyzer_.print_intervals(limits);
-
-
-    return limits;
-}
+}

src/ik_solver/opt_psi_limit_bias_solver/src/joints_limit_analyzer.cpp

@@ -0,0 +1,622 @@
+//
+// Created by lgv on 2025/11/3.
+//
+
+#include "opt_psi_limit_bias_solver/include/joints_limit_analyzer.h"
+#include <algorithm>
+#include <limits>
+
+using namespace cmvr;
+
+
+
+bool JointsLimitAnalyzer::check_tan_solution(double an, double ad, double bn, double bd, double cn, double cd,
+                                             double theta_target, double psi) {
+    // 由 ψ 求 θ：θ = atan2( N, D )
+    // N = an*sinψ + bn*cosψ + cn
+    // D = ad*sinψ + bd*cosψ + cd
+    double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
+    double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
+    double theta = std::atan2(N, D);
+    double err = SupportFunctions::normalize_angle(theta - theta_target);
+    return std::abs(err) < 1e-6;
+}
+
+
+std::vector<double> JointsLimitAnalyzer::calc_tan_solution(
+    double an, double ad, double bn, double bd, double cn, double cd, double theta) {
+    std::vector<double> out;
+
+    // 用 sin/cos(θ) 形成方程： (D sinθ - N cosθ) = 0
+    const double s = std::sin(theta);
+    const double c = std::cos(theta);
+
+    // 二次式 A t^2 + B t + C = 0,  t = tan(ψ/2)
+    double A = s * (cd - bd) - c * (cn - bn);
+    double B = 2.0 * (s * ad - c * an);
+    double C = s * (bd + cd) - c * (bn + cn);
+
+    // 退化线性兜底
+    if (std::abs(A) < EPS) {
+        if (std::abs(B) < EPS) {
+            // A≈0 且 B≈0：按无解处理。
+            if (std::abs(C) < 1e-12) {
+                // 恒等：任意 ψ 都满足——按需要返回空或[-π,π]，这里返回空让上层判定。
+            }
+            return {};
+        }
+        // 线性：B t + C = 0
+        double psi = 2.0 * std::atan2(-C, B);
+        psi =  SupportFunctions::normalize_angle(psi);
+        if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi))
+            return {psi};
+        return {};
+    }
+
+    // 判别式（带稳健夹零）
+    double D = B * B - 4.0 * A * C;
+    if (D < -1e-14 * (A * A + B * B + C * C)) return {}; // 为负，无解
+    D = std::max(0.0, D);
+    double sqrtD = std::sqrt(D);
+
+    // t = (-B ± sqrtD)/(2A)  → ψ = 2*atan(t)
+    double psi1 = 2.0 * std::atan2(-(B - sqrtD), 2.0 * A);
+    double psi2 = 2.0 * std::atan2(-(B + sqrtD), 2.0 * A);
+
+    psi1 =  SupportFunctions::normalize_angle(psi1);
+    psi2 =  SupportFunctions::normalize_angle(psi2);
+
+    if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi1))
+        out.push_back(psi1);
+    if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi2) &&
+        std::abs(psi2 - psi1) > EPS)
+        out.push_back(psi2);
+
+    std::sort(out.begin(), out.end());
+    return out;
+}
+
+
+std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_tan_limits(double an, double ad, double bn, double bd,
+                                                                             double cn, double cd, double joint_l,
+                                                                             double joint_u) {
+    // 1) 微分系数
+    double at = bd * cn - bn * cd;
+    double bt = an * cd - ad * cn;
+    double ct = an * bd - ad * bn;
+
+    // 2) 奇异点（式 31）：at^2 + bt^2 - ct^2 = 0
+    double dt = at * at + bt * bt - ct * ct;
+    std::vector<std::pair<double, double> > singular_allow; // 允许区间列表
+    if (std::abs(at * at + bt * bt - ct * ct) < 1e-6) {
+        double psi_sing =  SupportFunctions::normalize_angle(2.0 * std::atan2(at, (bt - ct)));
+        double safe = psi_sing_avid_;
+
+        double L =  SupportFunctions::normalize_angle(psi_sing - safe);
+        double R =  SupportFunctions::normalize_angle(psi_sing + safe);
+
+        // 允许集 = [-π, L] ∪ [R, π]
+        if (L <= R) {
+            singular_allow = {{-M_PI, L}, {R, M_PI}};
+        } else {
+            singular_allow = {{-M_PI, R}, {L, M_PI}};
+        }
+    }
+
+    // 3) 将上下限 ±jl 映射为 psi（tan 型）
+    std::vector<double> pt1 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_u);
+    std::vector<double> pt2 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_l);
+
+    std::vector<double> ptlim;
+    ptlim.reserve(pt1.size() + pt2.size());
+    ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
+    ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
+
+
+    // 去重
+    std::sort(ptlim.begin(), ptlim.end());
+    ptlim.erase(std::unique(ptlim.begin(), ptlim.end(),
+                            [](double a, double b) { return std::abs(a - b) < EPS; }),
+                ptlim.end());
+
+    std::vector<std::pair<double, double> > allow_pairs; // 最终允许区间
+
+    auto theta_of = [&](double psi) {
+        double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
+        double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
+        return std::atan2(N, D);
+    };
+
+    if (!ptlim.empty()) {
+        // === 采样 + 交替
+        // 构造边界：[-π, cuts..., π]
+        std::vector<double> bounds;
+        bounds.reserve(ptlim.size() + 2);
+        bounds.push_back(-M_PI);
+        bounds.insert(bounds.end(), ptlim.begin(), ptlim.end());
+        bounds.push_back(M_PI);
+
+        // 左侧偏移采样，判断首段是否“允许”
+        for (size_t i = 0; i + 1 < bounds.size(); ++i) {
+            double L = bounds[i];
+            double R = bounds[i + 1];
+            if (R <= L) continue;
+
+            double probe = (L + R) * 0.5; // 取中点
+
+            if ( SupportFunctions::angle_in_wrap(theta_of(probe), joint_l, joint_u)) {
+                allow_pairs.emplace_back(L, R);
+            }
+        }
+    } else {
+        // 4) 无交点：整圈全允许或全禁止（用环形比较）
+        double tlim = theta_of(0.0);
+        if ( SupportFunctions::angle_in_wrap(tlim, joint_l, joint_u)) {
+            allow_pairs = {{-M_PI, M_PI}};
+        } else {
+            allow_pairs.clear();
+        }
+    }
+
+    // 5) 去掉奇异点
+    if (!singular_allow.empty() && !allow_pairs.empty()) {
+        allow_pairs = SupportFunctions::intersect_intervals(allow_pairs, singular_allow);
+    }
+    // 合并小段
+    allow_pairs =  SupportFunctions::union_intervals(allow_pairs);
+    return allow_pairs;
+}
+
+
+
+
+std::vector<std::pair<double, double> > JointsLimitAnalyzer::bounds_to_pairs(const std::vector<double> &bounds) {
+    std::vector<std::pair<double, double> > segs;
+    if (bounds.empty()) return segs;
+
+    // 要求：bounds 已按升序，且“以允许开始”
+    if (bounds.size() % 2 != 0) {
+        // 若出现奇偶不配，说明bounds计算错误
+        return segs;
+    }
+
+    segs.reserve(bounds.size() / 2);
+    for (size_t i = 0; i < bounds.size(); i += 2) {
+        double L = bounds[i];
+        double R = bounds[i + 1];
+        if (R > L) segs.emplace_back(L, R);
+    }
+    return segs;
+}
+
+
+std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_cos_limits(
+    double a, double b, double c, int conf, double joint_l, double joint_u) {
+    // 1) 奇异点判断，这里虽然是奇异点，但是左导数，右导数都是存在的，故没有屏蔽这个点
+    if (std::abs(a * a + b * b - (c - 1) * (c - 1)) < EPS) {
+        double psi_sing = 2.0 * std::atan2(a, (b - (c - 1)));
+    }
+    if (std::abs(a * a + b * b - (c + 1) * (c + 1)) < EPS) {
+        double psi_sing = 2.0 * std::atan2(a, (b - (c + 1)));
+    }
+
+    // 2) 将关节上下限映射到 ψ（cos 型）
+    std::vector<double> pt1 = calc_cos_solution(a, b, c, joint_l);
+    std::vector<double> pt2 = calc_cos_solution(a, b, c, joint_u);
+
+    std::vector<double> ptlim;
+    ptlim.reserve(pt1.size() + pt2.size());
+    ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
+    ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
+
+
+    // 去重
+    std::sort(ptlim.begin(), ptlim.end());
+    ptlim.erase(std::unique(ptlim.begin(), ptlim.end(),
+                            [](double a, double b) { return std::abs(a - b) < EPS; }),
+                ptlim.end());
+
+    std::vector<std::pair<double, double> > allow_pairs;
+
+    auto theta_of = [&](double psi) {
+        double ct = a * std::sin(psi) + b * std::cos(psi) + c;
+        ct =  SupportFunctions::clamp(ct, -1.0, 1.0); // 保证 ct 的范围在 [-1, 1] 之间
+        return static_cast<double>(conf) * std::acos(ct);
+    };
+
+    // 3) 排序区间
+    std::sort(ptlim.begin(), ptlim.end());
+
+    if (!ptlim.empty()) {
+        // 4) 采样 + 交替
+        // 构造边界数组：[-π, cuts..., π]
+        std::vector<double> bounds;
+        bounds.reserve(ptlim.size() + 2);
+        bounds.push_back(-M_PI);
+        bounds.insert(bounds.end(), ptlim.begin(), ptlim.end());
+        bounds.push_back(M_PI);
+
+        // 5) 采样左侧点，判断是否允许
+        for (size_t i = 0; i + 1 < bounds.size(); ++i) {
+            double L = bounds[i];
+            double R = bounds[i + 1];
+            if (R <= L) continue;
+
+            double probe = (L + R) * 0.5; //取中点
+
+            if ( SupportFunctions::angle_in_wrap(theta_of(probe), joint_l, joint_u)) {
+                allow_pairs.emplace_back(L, R);
+            }
+        }
+    } else {
+        // 6) 无交点的情况：全允许或全禁止
+        double tlim = theta_of(0.0);
+        if ( SupportFunctions::angle_in_wrap(tlim, joint_l, joint_u)) {
+            allow_pairs = {{-M_PI, M_PI}};
+        } else {
+            allow_pairs.clear();
+        }
+    }
+
+    // 8) 合并
+    allow_pairs =  SupportFunctions::union_intervals(allow_pairs);
+
+    return allow_pairs;
+}
+
+
+bool JointsLimitAnalyzer::check_cos_solution(double a, double b, double c, double theta_target, double psi) {
+    // // 由 ψ 复原 θ：θ = acos( a sinψ + b cosψ + c )
+    // double ct = a * std::sin(psi) + b * std::cos(psi) + c;
+    // ct = clamp(ct, -1.0, 1.0);
+    // double theta = std::acos(ct);
+    // return std::abs(theta - theta_target) < 1e-6;
+
+    // ct = a sinψ + b cosψ + c  应等于 cos(theta_target)
+    double ct = a * std::sin(psi) + b * std::cos(psi) + c;
+    ct =  SupportFunctions::clamp(ct, -1.0, 1.0);
+
+    // 目标的 cos 值（对 conf 正负都成立）
+    double v = std::cos(theta_target);
+
+    return std::abs(ct - v) < EPS;
+}
+
+
+std::vector<double> JointsLimitAnalyzer::calc_cos_solution(double a, double b, double c, double theta) {
+    std::vector<double> out;
+
+    // v = cos(theta)
+    double v = std::cos(theta);
+
+    // 二次式：as * t^2 + bs * t + cs = 0， 其中 t = tan(ψ/2)
+    double as = v + b - c;
+    double bs = -2.0 * a;
+    double cs = v - b - c;
+
+    double D = bs * bs - 4.0 * as * cs;
+    if (D < 0.0) return out;
+    D = std::max(0.0, D);
+    double sqrtD = std::sqrt(D);
+
+    double psi1 = 2.0 * std::atan2(-(bs - sqrtD), 2.0 * as);
+    double psi2 = 2.0 * std::atan2(-(bs + sqrtD), 2.0 * as);
+
+    psi1 =  SupportFunctions::normalize_angle(psi1);
+    psi2 =  SupportFunctions::normalize_angle(psi2);
+
+    if (check_cos_solution(a, b, c, theta, psi1)) out.push_back(psi1);
+    if (check_cos_solution(a, b, c, theta, psi2) &&
+        std::abs(psi2 - psi1) > EPS)
+        out.push_back(psi2);
+
+    std::sort(out.begin(), out.end());
+    return out;
+}
+
+
+
+
+std::vector<std::pair<double, double> >
+JointsLimitAnalyzer::cal_offset_limits(double joint_l, double joint_u, double offset) {
+    double pL =  SupportFunctions::normalize_angle(joint_l + offset); // [-π, π]
+    double pU =  SupportFunctions::normalize_angle(joint_u + offset); // [-π, π]
+
+    std::vector<std::pair<double, double> > joint_ranges;
+
+    if (! SupportFunctions::wraps(pL, pU)) {
+        // 单段
+        if (pU > pL + EPS) {
+            joint_ranges.push_back({pL, pU});
+        }
+    } else {
+        // 跨 ±π，拆成两段 [-π, pU] ∪ [pL, π]
+        if (pU > -M_PI + EPS) joint_ranges.push_back({-M_PI, pU});
+        if (M_PI > pL + EPS) joint_ranges.push_back({pL, M_PI});
+    }
+    return joint_ranges;
+}
+
+std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_tan_limits(
+    double an, double ad, double bn, double bd, double cn, double cd, double joint_l, double joint_u, double offset) {
+    auto joint_ranges = cal_offset_limits(joint_l, joint_u, offset);
+    std::vector<std::pair<double, double> > out{};
+    for (auto [L, U]: joint_ranges) {
+        auto part = calc_tan_limits(an, ad, bn, bd, cn, cd, L, U);
+        out.insert(out.end(), part.begin(), part.end());
+    }
+    out =  SupportFunctions::union_intervals(out);
+    return out;
+}
+
+std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_cos_limits(
+    double a, double b, double c, int conf, double joint_l, double joint_u, double offset) {
+    auto joint_ranges = cal_offset_limits(joint_l, joint_u, offset);
+    std::vector<std::pair<double, double> > out{};
+    for (auto [L, U]: joint_ranges) {
+        auto part = calc_cos_limits(a, b, c, conf, L, U);
+        out.insert(out.end(), part.begin(), part.end());
+    }
+    out =  SupportFunctions::union_intervals(out);
+    return out;
+}
+
+
+
+
+
+
+
+// --- 前向预测（给 ψ 预测 θ̂，用于候选打分） ---
+inline double JointsLimitAnalyzer::predict_theta_tan(
+    double an, double ad, double bn, double bd, double cn, double cd, double psi, double offset) {
+    double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
+    double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
+    double phi = std::atan2(N, D);
+    return  SupportFunctions::normalize_angle(phi - offset); // θ̂ = φ - offset
+}
+
+inline double JointsLimitAnalyzer::predict_theta_cos(
+    double a, double b, double c, int conf, double psi, double offset) {
+    double ct = a * std::sin(psi) + b * std::cos(psi) + c;
+    ct =  SupportFunctions::clamp(ct, -1.0, 1.0);
+    double phi = static_cast<double>(conf) * std::acos(ct);
+    return  SupportFunctions::normalize_angle(phi - offset); // θ̂ = φ - offset
+}
+
+
+JointsLimitAnalyzer::PsiEstimateResult
+JointsLimitAnalyzer::estimate_psi_from_joints(
+    const Eigen::MatrixXd &s_mat, const Eigen::MatrixXd &w_mat,
+    const std::vector<double> &theta_c,
+    int s_conf, int /*e_conf*/, int w_conf,
+    double prefer_psi) {
+    PsiEstimateResult res;
+
+    // 尺寸检查
+    if (s_mat.rows() != 3 || s_mat.cols() != 9 || w_mat.rows() != 3 || w_mat.cols() != 9 || theta_c.size() != 7) {
+        res.ok = false;
+        return res;
+    }
+
+    // 拆块
+    const Eigen::Matrix3d As = s_mat.block<3, 3>(0, 0);
+    const Eigen::Matrix3d Bs = s_mat.block<3, 3>(0, 3);
+    const Eigen::Matrix3d Cs = s_mat.block<3, 3>(0, 6);
+
+    const Eigen::Matrix3d Aw = w_mat.block<3, 3>(0, 0);
+    const Eigen::Matrix3d Bw = w_mat.block<3, 3>(0, 3);
+    const Eigen::Matrix3d Cw = w_mat.block<3, 3>(0, 6);
+
+    const int s = s_conf;
+    const int w = w_conf;
+
+    std::vector<double> cands;
+
+    // —— 用每个关节各自反算 ψ 候选（与限位时的系数完全一致）——
+
+    // J1: tan
+    {
+        double an = -s * As(1, 1), ad = -s * As(0, 1);
+        double bn = -s * Bs(1, 1), bd = -s * Bs(0, 1);
+        double cn = -s * Cs(1, 1), cd = -s * Cs(0, 1);
+        auto v = calc_tan_solution(an, ad, bn, bd, cn, cd, theta_c[0]);
+        cands.insert(cands.end(), v.begin(), v.end());
+    }
+
+    // J2: cos  (同 limit_2；反算时不需要 conf 放入 cos() 里)
+    {
+        double a = -As(2, 1), b = -Bs(2, 1), c = -Cs(2, 1);
+        auto v = calc_cos_solution(a, b, c, theta_c[1]);
+        cands.insert(cands.end(), v.begin(), v.end());
+    }
+
+    // J3: tan  (同 limit_3)
+    {
+        double an = s * As(2, 2), ad = -s * As(2, 0);
+        double bn = s * Bs(2, 2), bd = -s * Bs(2, 0);
+        double cn = s * Cs(2, 2), cd = -s * Cs(2, 0);
+        auto v = calc_tan_solution(an, ad, bn, bd, cn, cd, theta_c[2]);
+        cands.insert(cands.end(), v.begin(), v.end());
+    }
+
+    // J5: tan + offset(+π/2)  (同 limit_5)
+    {
+        double an = w * Aw(1, 2), ad = w * Aw(0, 2);
+        double bn = w * Bw(1, 2), bd = w * Bw(0, 2);
+        double cn = w * Cw(1, 2), cd = w * Cw(0, 2);
+        double theta_adj = theta_c[4] + M_PI / 2.0;
+        auto v = calc_tan_solution(an, ad, bn, bd, cn, cd, theta_adj);
+        cands.insert(cands.end(), v.begin(), v.end());
+    }
+
+    // J6: cos + offset(+π/2)  (同 limit_6)
+    {
+        double a = Aw(2, 2), b = Bw(2, 2), c = Cw(2, 2);
+        double theta_adj = theta_c[5] + M_PI / 2.0;
+        auto v = calc_cos_solution(a, b, c, theta_adj);
+        cands.insert(cands.end(), v.begin(), v.end());
+    }
+
+    // J7: tan  (同 limit_7)
+    {
+        double an = w * Aw(2, 1), ad = -w * Aw(2, 0);
+        double bn = w * Bw(2, 1), bd = -w * Bw(2, 0);
+        double cn = w * Cw(2, 1), cd = -w * Cw(2, 0);
+        auto v = calc_tan_solution(an, ad, bn, bd, cn, cd, theta_c[6]);
+        cands.insert(cands.end(), v.begin(), v.end());
+    }
+
+    // 候选去重
+    std::sort(cands.begin(), cands.end());
+    cands.erase(std::unique(cands.begin(), cands.end(),
+                            [&](double a, double b) {
+                                return std::abs(SupportFunctions::normalize_angle(a - b)) < EPS;
+                            }), cands.end());
+
+    // if (cands.empty()) { res.ok=false; return res; }
+    // 认为设置
+    if (cands.empty()) {
+        const int K = 720; // 0.5° 网格
+        cands.reserve(K);
+        for (int i = 0; i < K; ++i) {
+            cands.push_back(-M_PI + (2.0 * M_PI) * (i + 0.5) / K);
+        }
+    }
+
+    // —— 用所有关节的前向解析 θ̂(ψ) 对每个候选打分，选总残差最小者 ——
+    auto wrap_diff = [&](double x) { return  SupportFunctions::normalize_angle(x); };
+
+    auto score_of = [&](double psi) {
+        double sc = 0.0;
+
+        // J1 tan
+        {
+            double an = -s * As(1, 1), ad = -s * As(0, 1);
+            double bn = -s * Bs(1, 1), bd = -s * Bs(0, 1);
+            double cn = -s * Cs(1, 1), cd = -s * Cs(0, 1);
+            double th = predict_theta_tan(an, ad, bn, bd, cn, cd, psi, 0.0);
+            sc += std::pow(wrap_diff(th - theta_c[0]), 2);
+        }
+        // J2 cos
+        {
+            double a = -As(2, 1), b = -Bs(2, 1), c = -Cs(2, 1);
+            double th = predict_theta_cos(a, b, c, s, psi, 0.0);
+            sc += std::pow(wrap_diff(th - theta_c[1]), 2);
+        }
+        // J3 tan
+        {
+            double an = s * As(2, 2), ad = -s * As(2, 0);
+            double bn = s * Bs(2, 2), bd = -s * Bs(2, 0);
+            double cn = s * Cs(2, 2), cd = -s * Cs(2, 0);
+            double th = predict_theta_tan(an, ad, bn, bd, cn, cd, psi, 0.0);
+            sc += std::pow(wrap_diff(th - theta_c[2]), 2);
+        }
+        // J5 tan + offset
+        {
+            double an = w * Aw(1, 2), ad = w * Aw(0, 2);
+            double bn = w * Bw(1, 2), bd = w * Bw(0, 2);
+            double cn = w * Cw(1, 2), cd = w * Cw(0, 2);
+            double th = predict_theta_tan(an, ad, bn, bd, cn, cd, psi, M_PI / 2.0);
+            sc += std::pow(wrap_diff(th - theta_c[4]), 2);
+        }
+        // J6 cos + offset
+        {
+            double a = Aw(2, 2), b = Bw(2, 2), c = Cw(2, 2);
+            double th = predict_theta_cos(a, b, c, w, psi, M_PI / 2.0);
+            sc += std::pow(wrap_diff(th - theta_c[5]), 2);
+        }
+        // J7 tan
+        {
+            double an = w * Aw(2, 1), ad = -w * Aw(2, 0);
+            double bn = w * Bw(2, 1), bd = -w * Bw(2, 0);
+            double cn = w * Cw(2, 1), cd = -w * Cw(2, 0);
+            double th = predict_theta_tan(an, ad, bn, bd, cn, cd, psi, 0.0);
+            sc += std::pow(wrap_diff(th - theta_c[6]), 2);
+        }
+        return sc;
+    };
+
+    double best = std::numeric_limits<double>::infinity();
+    double bestpsi = 0.0;
+
+    for (double psi: cands) {
+        double sc = score_of(psi);
+        res.candidates.emplace_back(psi, sc);
+        if (sc < best - 1e-12) {
+            best = sc;
+            bestpsi = psi;
+        } else if (std::abs(sc - best) <= 1e-12 && std::isfinite(prefer_psi)) {
+            // 并列时选更靠近 prefer_psi 的
+            if (std::abs( SupportFunctions::normalize_angle(psi - prefer_psi)) <
+                std::abs( SupportFunctions::normalize_angle(bestpsi - prefer_psi))) {
+                best = sc;
+                bestpsi = psi;
+            }
+        }
+    }
+
+    res.ok = true;
+    res.psi = bestpsi;
+    res.score = best;
+    return res;
+}
+
+
+std::vector<std::pair<double, double> > JointsLimitAnalyzer::calc_arm_angle_limits(
+    const Eigen::MatrixXd &s_mat, const Eigen::MatrixXd &w_mat,
+    std::vector<std::pair<double, double> > joints_limits,
+    int s_conf, int e_conf, int w_conf) {
+    // 期望 s_mat / w_mat 都是 3x9： [A | B | C]
+    if (s_mat.rows() != 3 || s_mat.cols() != 9 ||
+        w_mat.rows() != 3 || w_mat.cols() != 9) {
+        // 尺寸不对，直接返回空
+        return {};
+    }
+
+
+    const Eigen::Matrix3d As = s_mat.block<3, 3>(0, 0);
+    const Eigen::Matrix3d Bs = s_mat.block<3, 3>(0, 3);
+    const Eigen::Matrix3d Cs = s_mat.block<3, 3>(0, 6);
+
+    const Eigen::Matrix3d Aw = w_mat.block<3, 3>(0, 0);
+    const Eigen::Matrix3d Bw = w_mat.block<3, 3>(0, 3);
+    const Eigen::Matrix3d Cw = w_mat.block<3, 3>(0, 6);
+
+    auto s = s_conf;
+    auto w = w_conf;
+
+    auto limit_1 = calc_tan_limits(-s * As(1, 1), -s * As(0, 1), -s * Bs(1, 1), -s * Bs(0, 1),
+                                   -s * Cs(1, 1), -s * Cs(0, 1),
+                                   joints_limits[0].first, joints_limits[0].second);
+
+    auto limit_2 = calc_cos_limits(-As(2, 1), -Bs(2, 1), -Cs(2, 1), s,
+                                                      joints_limits[1].first, joints_limits[1].second);
+
+    auto limit_3 = calc_tan_limits(s * As(2, 2),
+                                                      -s * As(2, 0), s * Bs(2, 2), -s * Bs(2, 0),
+                                                      s * Cs(2, 2), -s * Cs(2, 0),
+                                                      joints_limits[2].first, joints_limits[2].second);
+
+    auto limit_5 = calc_tan_limits(w * Aw(1, 2),
+                                                      w * Aw(0, 2), w * Bw(1, 2), w * Bw(0, 2),
+                                                      w * Cw(1, 2), w * Cw(0, 2),
+                                                      joints_limits[4].first, joints_limits[4].second,M_PI / 2.0);
+
+    auto limit_6 = calc_cos_limits(Aw(2, 2), Bw(2, 2), Cw(2, 2), w,
+                                                      joints_limits[5].first, joints_limits[5].second,M_PI / 2.0);
+    auto limit_7 = calc_tan_limits(w * Aw(2, 1),
+                                                      -w * Aw(2, 0), w * Bw(2, 1), -w * Bw(2, 0),
+                                                      w * Cw(2, 1), -w * Cw(2, 0),
+                                                      joints_limits[6].first, joints_limits[6].second);
+
+
+    auto limits =  SupportFunctions::intersect_intervals(limit_1, limit_2);
+    limits =  SupportFunctions::intersect_intervals(limits, limit_3);
+    limits =  SupportFunctions::intersect_intervals(limits, limit_5);
+    limits =  SupportFunctions::intersect_intervals(limits, limit_6);
+    limits =  SupportFunctions::intersect_intervals(limits, limit_7);
+
+    SupportFunctions::print_intervals(limits);
+    return limits;
+}

src/ik_solver/opt_psi_limit_bias_solver/src/opt_psi_limit_bias_solver.cpp

@@ -0,0 +1,106 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+#include "opt_psi_limit_bias_solver/include/opt_psi_limit_bias_solver.h"
+
+using namespace cmvr;
+
+OptPsiLimitBiasSolver::OptPsiLimitBiasSolver() : IKSolver(){
+    bias_srs_ik_solver_ = std::make_shared<BiasSRSIkSolver>();
+    joints_limit_analyzer_ = std::make_shared<JointsLimitAnalyzer>();
+    opt_psi_selector_ = std::make_shared<OptPsiSelector>();
+
+    this->init();
+}
+
+bool OptPsiLimitBiasSolver::init() {
+    Eigen::Matrix4d T_tool_flange, T_arm_robot;
+    T_tool_flange << 0, 1, 0, -0.284077,
+            0, 0, 1, 0.00801525,
+            1, 0, 0, 0.00684256,
+            0, 0, 0, 1;
+
+
+    T_arm_robot << 0, 1, 0, 0,
+                   0, 0, -1, 0,
+                  -1, 0, 0, 0.042,
+                   0, 0, 0, 1;
+
+    setTcpTransform(T_tool_flange);
+    setArmBaseTransform(T_arm_robot);
+    opt_psi_selector_->set_update_params(0.6, 5.0, -1, 1e-4);
+    return true;
+}
+
+bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose, std::vector<double> &joints_angle) {
+    Eigen::Matrix4d target_cal_pose = SupportFunctions::invertHomogeneous(T_arm_robot_) * target_pose * SupportFunctions::invertHomogeneous(T_tool_flange_);
+    // 0 : 先验证一下 保存的最优臂角是否有问题
+    if (prev_opt_psi_.valid == true) {
+        auto cur_joints_angle = bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, prev_opt_psi_.value);
+        for (int i = 0; i < cur_joints_angle_.size(); i++) {
+            if (std::abs(cur_joints_angle[i] - cur_joints_angle_[i]) > 1e-3) {
+                prev_opt_psi_.valid = false;
+                break;
+            }
+        }
+    }
+
+    // 1：根据当前位置预估当前位置的最优臂角
+    if (prev_opt_psi_.valid == false) {
+        Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
+        Eigen::Matrix4d cur_pose;
+        fk(cur_joints_angle_, cur_pose);
+        bias_srs_ik_solver_->cal_coefficient_matrix(cur_pose, s_mat, w_mat);
+
+        auto s = bias_srs_ik_solver_->get_shoulder_config();
+        auto e = bias_srs_ik_solver_->get_elbow_config();
+        auto w = bias_srs_ik_solver_->get_wrist_config();
+
+        auto res = joints_limit_analyzer_->estimate_psi_from_joints(s_mat, w_mat,
+                                                                    cur_joints_angle_, s, e, w);
+
+        if (res.ok) {
+            prev_opt_psi_.valid = true;
+            prev_opt_psi_.value = res.psi;
+        }
+    }
+
+
+    // 2：求取臂角范围
+    Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
+    bias_srs_ik_solver_->cal_coefficient_matrix(target_cal_pose, s_mat, w_mat);
+
+    auto s = bias_srs_ik_solver_->get_shoulder_config();
+    auto e = bias_srs_ik_solver_->get_elbow_config();
+    auto w = bias_srs_ik_solver_->get_wrist_config();
+    auto joints_limits = bias_srs_ik_solver_->get_joints_limits();
+
+    auto limits = joints_limit_analyzer_->calc_arm_angle_limits(s_mat, w_mat,
+                                                                joints_limits, s, e, w);
+
+    // 3： 求取当前最优臂角
+    double best_psi{};
+    if (prev_opt_psi_.valid == false) {
+        return false;
+    } else {
+        opt_psi_selector_->update_psi(best_psi, prev_opt_psi_.value, limits);
+        prev_opt_psi_.value = best_psi;
+        prev_opt_psi_.valid = true;
+    }
+
+    // 4 : 求取关节角
+    joints_angle = bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, best_psi);
+    return true;
+}
+
+
+bool OptPsiLimitBiasSolver::fk(const std::vector<double> &joints_angle, Eigen::Matrix4d &cur_pose, bool robot_base) {
+
+    cur_pose = bias_srs_ik_solver_->calc_total_transform(joints_angle);
+    if (robot_base == true) {
+        cur_pose = T_arm_robot_ * cur_pose *T_tool_flange_;
+    }
+    return true;
+}
+

src/ik_solver/opt_psi_limit_bias_solver/src/opt_psi_selector.cpp

@@ -0,0 +1,108 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+#include "opt_psi_limit_bias_solver/include/opt_psi_selector.h"
+
+using namespace cmvr;
+
+bool OptPsiSelector::update_psi(
+    double &psi_best,
+    double psi_prev,
+    const std::vector<std::pair<double,double>>& psi_all
+){
+    // 没有可行区间：按需返回原值或报错；这里返回原值
+    if (psi_all.empty()) {
+        return false;
+    }
+
+    // 2) 找到包含 ψ_{t-1} 的可行段 Ψ_all,m = [L, U]
+    int hit = -1;
+    for (int i = 0; i < (int)psi_all.size(); ++i) {
+        double L = psi_all[i].first;
+        double U = psi_all[i].second;
+        if (psi_prev >= L - EPS && psi_prev <= U + EPS) {
+            hit = i; break;
+        }
+    }
+
+    auto clamp_step = [&](double d){
+        if (step_cap_ > 0.0)
+            return std::max(-step_cap_, std::min(step_cap_, d));
+        return d;
+    };
+
+    // 工具：到区间的环形距离（若在区间内则为 0）
+    auto ang_dist_to_interval = [&](double x, double L, double U){
+        x = SupportFunctions::normalize_angle(x);
+        // 不跨界，且 L<U
+        if (x >= L - EPS && x <= U + EPS) return 0.0;
+        auto wrap_abs = [&](double d){ return std::abs(SupportFunctions::normalize_angle(d)); };
+        return std::min(wrap_abs(x - L), wrap_abs(x - U));
+    };
+
+    // 3) 命中某个可行段 → 用论文的指数排斥公式
+    if (hit >= 0) {
+        double L = psi_all[hit].first;
+        double U = psi_all[hit].second;
+        double W = U - L;
+        if (W <= EPS) {
+            psi_best = SupportFunctions::normalize_angle(0.5 * (L + U));
+            return true;
+        }
+
+        // 归一化到 [0,1] 的左右边界距离
+        double sL = SupportFunctions::clamp((psi_prev - L) / W,0.0,1.0);  // ∈[0,1]
+        double sU = SupportFunctions::clamp((U - psi_prev) / W,0.0,1.0);  // ∈[0,1]
+
+        // 公式：
+        // ψ_t = ψ_{t-1} + K*(W/2) * [ exp(-α * sL) - exp(-α * sU) ]
+        double kick = k_ * (0.5 * W) * ( std::exp(-alpha_ * sL) - std::exp(-alpha_ * sU) );
+        kick = clamp_step(kick);
+
+        double psi_new = SupportFunctions::normalize_angle(psi_prev + kick);
+        double margin  = std::max(edge_margin_, 0.02 * W);
+        margin = std::min(margin, 0.25 * W);
+        if (psi_new < L + EPS || psi_new > U - EPS) {
+            psi_new = std::min(U - margin, std::max(L + margin, psi_new));
+        }
+        psi_best = psi_new;
+        return true;
+    }
+
+    // 4) 没命中的情况：把肘（ψ）“移入最近的可行段”
+    int best = -1;
+    double best_d = std::numeric_limits<double>::infinity();
+    for (int i = 0; i < (int)psi_all.size(); ++i) {
+        double L = psi_all[i].first;
+        double U = psi_all[i].second;
+        double d = ang_dist_to_interval(psi_prev, L, U);
+        if (d < best_d) { best_d = d; best = i; }
+    }
+    // 理论上 best 必定存在
+    double L = psi_all[best].first;
+    double U = psi_all[best].second;
+    double W = U - L;
+
+    // 选择最近的边界，并向内缩 margin
+    // 根据 ψ_{t-1} 与 [L,U] 的相对位置，决定吸向 L+δ 还是 U-δ
+    auto wrap = [&](double x){ return SupportFunctions::normalize_angle(x); };
+
+    // 判断离哪个端点近：用环形距离
+    auto wrap_abs = [&](double d){ return std::abs(SupportFunctions::normalize_angle(d)); };
+    bool closer_to_L = (wrap_abs(psi_prev - L) <= wrap_abs(psi_prev - U));
+
+    double margin = std::max(edge_margin_, 0.02 * W); // 至少内缩一点，或用 2% 的区间宽
+    margin = std::min(margin, 0.25 * W);               // 别缩太多
+
+    double target = closer_to_L ? (L + margin) : (U - margin);
+
+    // 如需要平滑，可按 step_cap 限制一步走到 target 的幅度
+    double delta = SupportFunctions::normalize_angle(target - psi_prev);
+    delta = clamp_step(delta);
+
+    psi_best = wrap(psi_prev + delta);
+
+
+    return true;
+}

src/ik_solver/opt_psi_limit_bias_solver/src/srs_ik_test.cpp

@@ -4,15 +4,17 @@
 
 
 #include "gtest/gtest.h"
-#include "manif/SE3.h"
-#include "srs_ik/srs_ik_slover.h"
-#include "srs_ik/ik_limit_analyzer.h"
+#include "ik_solver/opt_psi_limit_bias_solver/include/bias_srs_ik_slover.h"
+#include "ik_solver/opt_psi_limit_bias_solver/include/joints_limit_analyzer.h"
+#include "ik_solver/opt_psi_limit_bias_solver/include/opt_psi_selector.h"
 #include <iostream>
 #include <iomanip>
 #include <fstream>
 
-using namespace manif;
-using namespace cmvr::utils;
+#include "ik_solver/opt_psi_limit_bias_solver/include/opt_psi_limit_bias_solver.h"
+
+using namespace cmvr;
+
 
 struct IkSample {
     double psi;
@@ -48,7 +50,7 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
 
     // std::cout << std::fixed << std::setprecision(7);
 
-    SRSIkSlover slover;
+    BiasSRSIkSolver slover;
     std::vector<IkSample> samples;
     samples.reserve(4096);
 
@@ -65,13 +67,15 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
     Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
     slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
 
-    auto res = IkLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(),
-                                                         slover.get_elbow_config(), slover.get_wrist_config());
+    auto res = JointsLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(),
+                                                             slover.get_elbow_config(), slover.get_wrist_config());
 
     if (res.ok) {
         std::cout << "res.psi" << res.psi << std::endl;
     }
-    auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);
+    auto limits = JointsLimitAnalyzer::calc_arm_angle_limits(s_mat, w_mat, slover.get_joints_limits(),
+                                                             slover.get_shoulder_config(),
+                                                             slover.get_elbow_config(), slover.get_wrist_config());
 
     // 误差统计
     const double kPosTol = 1e-4; // 位置容差（m）
@@ -167,7 +171,7 @@ TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) {
 
     // std::cout << std::fixed << std::setprecision(7);
 
-    SRSIkSlover slover;
+    BiasSRSIkSolver slover;
     std::vector<IkSample> samples;
     samples.reserve(4096);
 
@@ -177,7 +181,7 @@ TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) {
     const auto cur_pose = slover.calc_total_transform(joint_angles);
     Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
     slover.cal_coefficient_matrix(cur_pose, s_mat, w_mat);
-    auto res = IkLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(),
+    auto res = JointsLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(),
                                                          slover.get_elbow_config(), slover.get_wrist_config());
 
     // 2: 目标位姿
@@ -186,10 +190,14 @@ TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) {
     slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
 
     // 3； 计算limit
-    auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);
+    auto limits = JointsLimitAnalyzer::calc_arm_angle_limits(s_mat, w_mat, slover.get_joints_limits(),
+                                                         slover.get_shoulder_config(),
+                                                         slover.get_elbow_config(), slover.get_wrist_config());
 
     // 4； 计算best
-    auto psi = IkLimitAnalyzer::update_psi(res.psi, limits, IkLimitAnalyzer::PsiUpdateParams());
+    OptPsiSelector opt_psi_selector;
+    double best_psi{};
+     opt_psi_selector.update_psi(best_psi,res.psi, limits);
 
     // 误差统计
     const double kPosTol = 1e-4; // 位置容差（m）
@@ -213,7 +221,7 @@ TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) {
     };
 
     // IK 解
-    auto q = slover.inverse_kinematics(target_pose, psi);
+    auto q = slover.inverse_kinematics(target_pose, best_psi);
 
     for (double q1: q) {
         std::cout << q1 << " , ";
@@ -232,46 +240,7 @@ TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) {
 
     // 表头
     cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n";
-    cout << psi << ", " << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
-}
-
-
-TEST(SRS_IK_TEST, INTERSECT_TEST) {
-    // 测试 1: 有交集的区间
-    std::vector<std::pair<double, double> > A = {{-3.0, -1.0}, {1.0, 4.0}};
-    std::vector<std::pair<double, double> > B = {{-2.0, 0.5}, {2.5, 5.0}};
-
-    std::cout << "Test 1: Intersecting intervals" << std::endl;
-    auto result1 = IkLimitAnalyzer::intersect(A, B);
-    IkLimitAnalyzer::print_intervals(result1);
-    // 预期输出: [-2.0, -1.0] [2.5, 4.0]
-
-    // 测试 2: 相邻但不重叠的区间
-    std::vector<std::pair<double, double> > C = {{-3.0, -1.0}, {2.0, 4.0}};
-    std::vector<std::pair<double, double> > D = {{-1.0, 0.0}, {1.0, 3.0}};
-
-    std::cout << "Test 2: Adjacent intervals" << std::endl;
-    auto result2 = IkLimitAnalyzer::intersect(C, D);
-    IkLimitAnalyzer::print_intervals(result2);
-    // 预期输出:  [2.0, 3.0]
-
-    // 测试 3: 无交集的区间
-    std::vector<std::pair<double, double> > E = {{-5.0, -3.0}, {2.0, 4.0}};
-    std::vector<std::pair<double, double> > F = {{5.0, 6.0}, {7.0, 8.0}};
-
-    std::cout << "Test 3: Non-intersecting intervals" << std::endl;
-    auto result3 = IkLimitAnalyzer::intersect(E, F);
-    IkLimitAnalyzer::print_intervals(result3);
-    // 预期输出: (无输出)
-
-    // 测试 4: 一个空的区间集
-    std::vector<std::pair<double, double> > G = {};
-    std::vector<std::pair<double, double> > H = {{1.0, 2.0}, {3.0, 4.0}};
-
-    std::cout << "Test 4: Empty intervals" << std::endl;
-    auto result4 = IkLimitAnalyzer::intersect(G, H);
-    IkLimitAnalyzer::print_intervals(result4);
-    // 预期输出: (无输出)
+    cout << best_psi << ", " << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
 }
 
 
@@ -279,53 +248,45 @@ TEST(SRS_IK_TEST, MOVE_L_SLOVER_TEST) {
     using std::cout;
     using std::endl;
 
-    SRSIkSlover slover;
+
     std::vector<IkSample> samples;
     samples.reserve(4096);
 
+    OptPsiLimitBiasSolver solver;
+
+
     // 1) 当前位姿（估计上一时刻 ψ 用）
     std::vector<double> joint_angles(7, 0);
     joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
-    const auto cur_pose = slover.calc_total_transform(joint_angles);
-
-    Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
-    slover.cal_coefficient_matrix(cur_pose, s_mat, w_mat);
-
-    auto res = IkLimitAnalyzer::estimate_psi_from_joints(
-        s_mat, w_mat,joint_angles ,
-        slover.get_shoulder_config(),
-        slover.get_elbow_config(),
-        slover.get_wrist_config()
-    );
-    samples.push_back(IkSample{res.psi, {joint_angles[0], joint_angles[1],
-        joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6]}});
+    solver.update_joints_state(joint_angles);
+    samples.push_back(IkSample{
+        0.0, {
+            joint_angles[0], joint_angles[1],
+            joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6]
+        }
+    });
 
     // 2) 目标位姿（作为直线的起点）
     joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 0.29, 0.59};
-    const auto target_pose = slover.calc_total_transform(joint_angles);
+    Eigen::Matrix4d target_pose;
+
+    solver.fk(joint_angles,target_pose,true);
 
     // 直线插补参数 —— 从 target_pose 出发沿 X 方向 L 米，共 N 段（N+1 个点，包含起点）
-    const int    N  = 100;                // 采样点数（间隔均匀）
-    const double L  = 0.20;              // 直线长度 0.20 m
+    const int N = 100; // 采样点数（间隔均匀）
+    const double L = 0.20; // 直线长度 0.20 m
     Eigen::Vector3d dir = Eigen::Vector3d::UnitX();
     dir.normalize();
 
     // 固定姿态（也可以改成对姿态做 Slerp）
-    const Eigen::Matrix3d R_fixed = target_pose.block<3,3>(0,0);
-    const Eigen::Vector3d p0      = target_pose.block<3,1>(0,3);
+    const Eigen::Matrix3d R_fixed = target_pose.block < 3,
+    3 > (0, 0);
+    const Eigen::Vector3d p0 = target_pose.block < 3,
+    1 > (0, 3);
 
     // 3) 初始 ψ：用估计得到的 ψ，再根据 target_pose 的可行区间做一次更新
-    slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
-    auto limits0 = slover.calc_arm_angle_limits(s_mat, w_mat);
-
-    IkLimitAnalyzer::PsiUpdateParams up;
-    up.K = 0.6;                 // 排斥强度
-    up.alpha = 3.0;             // 靠边越强
-    up.step_cap = -1; // 单步最大变化
-    up.edge_margin = 1e-4;      // 吸附到段里时的内缩
-
-    double psi_curr = IkLimitAnalyzer::update_psi(res.psi, limits0, up);
-    auto q = slover.inverse_kinematics(target_pose, psi_curr);
+    std::vector<double> q;
+    solver.ik(target_pose,q);
 
     // 4) 误差评估工具
     const auto clamp = [](double x, double lo, double hi) {
@@ -337,48 +298,76 @@ TEST(SRS_IK_TEST, MOVE_L_SLOVER_TEST) {
         return std::acos(c);
     };
 
-    samples.push_back(IkSample{psi_curr, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
+    samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
     cout << "idx, s(0..1), psi(rad), q1..q7, pos_err(m), rot_err(rad), rot_err(deg)\n";
 
     // 5) 直线采样 & 每点求 IK（带 ψ 更新）
     for (int k = 0; k <= N; ++k) {
-        const double s = static_cast<double>(k) / static_cast<double>(N);  // [0,1]
+        const double s = static_cast<double>(k) / static_cast<double>(N); // [0,1]
         Eigen::Vector3d p = p0 + s * L * dir;
 
         Eigen::Matrix4d T_goal = Eigen::Matrix4d::Identity();
-        T_goal.block<3,3>(0,0) = R_fixed;
-        T_goal.block<3,1>(0,3) = p;
+        T_goal.block<3, 3>(0, 0) = R_fixed;
+        T_goal.block<3, 1>(0, 3) = p;
 
         // 计算当前点的 arm-angle 可行区间，并基于上一时刻 psi_curr 更新一次
-        slover.cal_coefficient_matrix(T_goal, s_mat, w_mat);
-        auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);
-        psi_curr = IkLimitAnalyzer::update_psi(psi_curr, limits, up);
+
 
         // 逆解（带 ψ）
-        auto q = slover.inverse_kinematics(T_goal, psi_curr);
+        solver.ik(T_goal,q);
+        solver.update_joints_state(q);
 
         // 容错：若 IK 失败（大小不为 7），跳过但打印提示
         if (q.size() != 7) {
-            cout << k << ", " << s << ", " << psi_curr
-                 << ", IK_FAIL, , , , , , , , ,\n";
+            cout << k << ", " << s << ", " << 0.0
+                    << ", IK_FAIL, , , , , , , , ,\n";
             continue;
         }
 
         // 前向校验
-        const auto T_fk = slover.calc_total_transform(q);
-        const Eigen::Vector3d p_fk = T_fk.block<3,1>(0,3);
-        const Eigen::Matrix3d R_fk = T_fk.block<3,3>(0,0);
+        Eigen::Matrix4d T_fk;
+        solver.fk(q,T_fk,true);
+        const Eigen::Vector3d p_fk = T_fk.block < 3,
+        1 > (0, 3);
+        const Eigen::Matrix3d R_fk = T_fk.block < 3,
+        3 > (0, 0);
 
         const double pos_err = (p_fk - p).norm();
         const double rot_err = rot_err_rad(R_fixed, R_fk);
         const double rot_err_deg = rot_err * 180.0 / M_PI;
 
-        cout << k << ", " << s << ", " << psi_curr << ", "
-             << q[0] << ", " << q[1] << ", " << q[2] << ", "
-             << q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", "
-             << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
+        cout << k << ", " << s << ", " << 0.0 << ", "
+                << q[0] << ", " << q[1] << ", " << q[2] << ", "
+                << q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", "
+                << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
 
-        samples.push_back(IkSample{psi_curr, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
+        samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
     }
     write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);
 }
+
+
+TEST(SRS_IK_TEST, TR_TEST) {
+    using std::cout;
+    using std::endl;
+
+
+    OptPsiLimitBiasSolver solver;
+    Eigen::Matrix4d T;
+    T <<  9.99998311e-01, -1.78940420e-03,  4.20611000e-04,  3.83367005e-02,
+           6.06804000e-05, -1.96560031e-01, -9.80491790e-01, -1.08356411e-01,
+           1.83717140e-03,  9.80490159e-01, -1.96559590e-01, -7.19901808e-01,
+           0.0,              0.0,              0.0,              1.0;
+
+    std::vector<double> joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
+
+
+
+    // 目标位姿：FK(joint_angles)
+    Eigen::Matrix4d pose;
+    solver.fk(joint_angles,pose,true);
+    cout << "Target Pose (FK from seed joints):\n" << pose << endl;
+
+    // solver.ik(T,joint_angles);
+
+}

src/planner/CMakeLists.txt

@@ -0,0 +1,24 @@
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+find_package(osqp REQUIRED)
+find_package(OsqpEigen REQUIRED)
+find_package(PkgConfig REQUIRED)
+find_package(fcl REQUIRED)
+pkg_check_modules(TINYXML2 REQUIRED tinyxml2)
+
+include_directories(${TINYXML2_INCLUDE_DIRS})
+
+add_library(planner STATIC
+    joint_space_planner/src/joint_space_planner.cpp
+    joint_space_planner/src/joint_space_planner_creator.cpp
+)
+
+target_include_directories(planner PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+target_link_libraries(planner PRIVATE
+        Eigen3::Eigen
+        OsqpEigen::OsqpEigen
+        ${TINYXML2_LIBRARIES}
+        fcl
+)
+
+add_library(cmvr_es::planner ALIAS planner)

src/planner/joint_space_planner/include/joint_space_planner.h

@@ -0,0 +1,17 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+#pragma once
+
+#include "joint_space_planner/include/joint_space_planner_creator.h"
+namespace cmvr {
+    class JointSpacePlanner {
+    public:
+
+        JointSpacePlanner();
+
+        virtual ~JointSpacePlanner();
+
+    };
+}

src/planner/joint_space_planner/include/joint_space_planner_creator.h

@@ -0,0 +1,31 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+#pragma once
+
+#include <memory>
+#include <unordered_map>
+#include <joint_space_planner/include/joint_space_planner.h>
+namespace cmvr
+{
+    enum JointSpacePlannerType
+    {
+        JOINT_SPACE_PLANNER_UNKNOWN = 0,
+        TOPPRA_BSPLINE = 1
+    };
+    class JointSpacePlannerCreator
+    {
+    protected:
+        static std::unordered_map<JointSpacePlannerType, std::shared_ptr<JointSpacePlanner>>
+            registers_;
+        static std::shared_ptr<JointSpacePlanner> createNew(const JointSpacePlannerType type);
+
+    public:
+        static void clear()
+        {
+            registers_.clear();
+        }
+        static std::shared_ptr<JointSpacePlanner> create(const JointSpacePlannerType type);
+    };
+} // namespace parking

src/planner/joint_space_planner/src/joint_space_planner.cpp

@@ -0,0 +1,3 @@
+//
+// Created by lgv on 11/7/25.
+//

src/planner/joint_space_planner/src/joint_space_planner_creator.cpp

@@ -0,0 +1,39 @@
+//
+// Created by lgv on 11/7/25.
+//
+
+#include "joint_space_planner/include/joint_space_planner_creator.h"
+
+namespace cmvr
+{
+    std::unordered_map<JointSpacePlannerType, std::shared_ptr<JointSpacePlanner>>
+        JointSpacePlannerCreator::registers_;
+
+    std::shared_ptr<JointSpacePlanner> JointSpacePlannerCreator::create(const JointSpacePlannerType type)
+    {
+        static std::unordered_map<JointSpacePlannerType, std::shared_ptr<JointSpacePlanner>>
+            registered_planners;
+        auto planner_pair = registered_planners.find(type);
+        if (planner_pair == registered_planners.end())
+        {
+            auto res = createNew(type);
+            registered_planners.emplace(type, res);
+            return res;
+        }
+        return planner_pair->second;
+    }
+    std::shared_ptr<JointSpacePlanner>
+    JointSpacePlannerCreator::createNew(const JointSpacePlannerType type)
+    {
+        std::shared_ptr<JointSpacePlanner> result;
+        switch (type)
+        {
+        case JOINT_SPACE_PLANNER_UNKNOWN:
+            return nullptr;
+        case TOPPRA_BSPLINE:
+            // return std::make_shared<HybridAstarParkIn>();
+        default:
+            return nullptr;
+        }
+    }
+}

src/utils/CMakeLists.txt

@@ -20,8 +20,6 @@ add_library(utils STATIC
         math/se3.cpp
         math/so3.cpp
         controller/cartesian_controller.cpp
-        srs_ik/srs_ik_slover.cpp
-        srs_ik/ik_limit_analyzer.cpp
 )
 
 target_include_directories(utils PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -33,49 +31,4 @@ target_link_libraries(utils PRIVATE
         fcl
 )
 
-add_library(cmvr_es::utils ALIAS utils)
-
-
-# --------------------------------------------------------
-# Unit test
-# --------------------------------------------------------
-find_package(glog REQUIRED)
-find_package(PkgConfig REQUIRED)
-find_package(fcl REQUIRED)
-find_package(OpenCV REQUIRED)
-
-include_directories(
-        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
-)
-
-include_directories(
-        ${CMAKE_SOURCE_DIR}/third_party/manif/0.0.5/include
-)
-
-
-link_directories(
-        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
-)
-
-
-
-
-add_executable(srs_ik_test
-        ${CMAKE_CURRENT_SOURCE_DIR}/srs_ik/srs_ik_test.cpp
-)
-
-
-target_link_libraries(srs_ik_test
-        PRIVATE
-        gtest
-        gtest_main
-        pthread
-        glog::glog
-        proto-objects
-        ccd
-        fcl
-        ${OpenCV_LIBS}
-        Eigen3::Eigen
-        OsqpEigen::OsqpEigen
-        cmvr_es::utils
-)
+add_library(cmvr_es::utils ALIAS utils)

src/utils/srs_ik/ik_hybird_optimizer.cpp

@@ -1,5 +0,0 @@
-//
-// Created by lgv on 11/7/25.
-//
-
-#include "ik_hybird_optimizer.h"

src/utils/srs_ik/ik_hybird_optimizer.h

@@ -1,23 +0,0 @@
-//
-// Created by lgv on 11/7/25.
-//
-
-#ifndef CMVR_ES_HYBIRD_OPTIMIZER_H
-#define CMVR_ES_HYBIRD_OPTIMIZER_H
-
-
-#include <math>
-
-namespace cmvr {
-    namespace utils {
-        class IkHybirdOptimizer {
-
-        public:
-        private:
-
-        };
-    }
-}
-
-
-#endif //CMVR_ES_HYBIRD_OPTIMIZER_H

src/utils/srs_ik/ik_limit_analyzer.cpp

@@ -1,744 +0,0 @@
-//
-// Created by lgv on 2025/11/3.
-//
-
-#include "ik_limit_analyzer.h"
-#include <algorithm>
-#include <limits>
-
-using namespace cmvr::utils;
-double IkLimitAnalyzer::normalize_angle(double angle) {
-    double a = std::fmod(angle, 2.0 * M_PI);
-    if (a < -M_PI) a += 2.0 * M_PI;
-    if (a >  M_PI) a -= 2.0 * M_PI;
-
-    if (std::abs(a -  M_PI) < EPS) return  M_PI;
-    if (std::abs(a +  M_PI) < EPS) return -M_PI;
-    return a;
-}
-bool IkLimitAnalyzer::check_tan_solution(double an, double ad, double bn, double bd, double cn, double cd,
-                                         double theta_target, double psi) {
-    // 由 ψ 求 θ：θ = atan2( N, D )
-    // N = an*sinψ + bn*cosψ + cn
-    // D = ad*sinψ + bd*cosψ + cd
-    double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
-    double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
-    double theta = std::atan2(N, D);
-    double err = normalize_angle(theta - theta_target);
-    return std::abs(err) < 1e-6;
-}
-
-
-std::vector<double> IkLimitAnalyzer::calc_tan_solution(
-    double an, double ad, double bn, double bd, double cn, double cd, double theta)
-{
-    std::vector<double> out;
-
-    // 用 sin/cos(θ) 形成方程： (D sinθ - N cosθ) = 0
-    const double s = std::sin(theta);
-    const double c = std::cos(theta);
-
-    // 二次式 A t^2 + B t + C = 0,  t = tan(ψ/2)
-    double A = s*(cd - bd) - c*(cn - bn);
-    double B = 2.0*(s*ad - c*an);
-    double C = s*(bd + cd) - c*(bn + cn);
-
-    // 退化线性兜底
-    if (std::abs(A) < EPS) {
-        if (std::abs(B) < EPS) {
-            // A≈0 且 B≈0：按无解处理。
-            if (std::abs(C) < 1e-12) {
-                // 恒等：任意 ψ 都满足——按需要返回空或[-π,π]，这里返回空让上层判定。
-            }
-            return {};
-        }
-        // 线性：B t + C = 0
-        double psi = 2.0 * std::atan2(-C, B);
-        psi = normalize_angle(psi);
-        if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi))
-            return {psi};
-        return {};
-    }
-
-    // 判别式（带稳健夹零）
-    double D = B*B - 4.0*A*C;
-    if (D < -1e-14*(A*A + B*B + C*C)) return {}; // 为负，无解
-    D = std::max(0.0, D);
-    double sqrtD = std::sqrt(D);
-
-    // t = (-B ± sqrtD)/(2A)  → ψ = 2*atan(t)
-    double psi1 = 2.0 * std::atan2(-(B - sqrtD), 2.0*A);
-    double psi2 = 2.0 * std::atan2(-(B + sqrtD), 2.0*A);
-
-    psi1 = normalize_angle(psi1);
-    psi2 = normalize_angle(psi2);
-
-    if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi1))
-        out.push_back(psi1);
-    if (check_tan_solution(an, ad, bn, bd, cn, cd, theta, psi2) &&
-        std::abs(psi2 - psi1) > EPS)
-        out.push_back(psi2);
-
-    std::sort(out.begin(), out.end());
-    return out;
-}
-
-
-
-std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(double an, double ad, double bn, double bd,
-                                                                         double cn, double cd, double joint_l,
-                                                                         double joint_u) {
-    // 1) 微分系数
-    double at = bd * cn - bn * cd;
-    double bt = an * cd - ad * cn;
-    double ct = an * bd - ad * bn;
-
-    // 2) 奇异点（式 31）：at^2 + bt^2 - ct^2 = 0
-    double dt = at * at + bt * bt - ct * ct;
-    std::vector<std::pair<double, double> > singular_allow; // 允许区间列表
-    if (std::abs(at * at + bt * bt - ct * ct) < 1e-6) {
-        double psi_sing = normalize_angle(2.0 * std::atan2(at, (bt - ct)));
-        double safe = psi_sing_avid_;
-
-        double L = normalize_angle(psi_sing - safe);
-        double R = normalize_angle(psi_sing + safe);
-
-        // 允许集 = [-π, L] ∪ [R, π]
-        if (L <= R) {
-            singular_allow = {{-M_PI, L}, {R, M_PI}};
-        } else {
-            singular_allow = {{-M_PI, R}, {L, M_PI}};
-        }
-    }
-
-    // 3) 将上下限 ±jl 映射为 psi（tan 型）
-    std::vector<double> pt1 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_u);
-    std::vector<double> pt2 = calc_tan_solution(an, ad, bn, bd, cn, cd, joint_l);
-
-    std::vector<double> ptlim;
-    ptlim.reserve(pt1.size() + pt2.size());
-    ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
-    ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
-
-
-    // 去重
-    std::sort(ptlim.begin(), ptlim.end());
-    ptlim.erase(std::unique(ptlim.begin(), ptlim.end(),
-                            [](double a, double b) { return std::abs(a - b) < EPS; }),
-                ptlim.end());
-
-    std::vector<std::pair<double, double> > allow_pairs; // 最终允许区间
-
-    auto theta_of = [&](double psi) {
-        double N = an * std::sin(psi) + bn * std::cos(psi) + cn;
-        double D = ad * std::sin(psi) + bd * std::cos(psi) + cd;
-        return std::atan2(N, D);
-    };
-
-    if (!ptlim.empty()) {
-        // === 采样 + 交替
-        // 构造边界：[-π, cuts..., π]
-        std::vector<double> bounds;
-        bounds.reserve(ptlim.size() + 2);
-        bounds.push_back(-M_PI);
-        bounds.insert(bounds.end(), ptlim.begin(), ptlim.end());
-        bounds.push_back(M_PI);
-
-        // 左侧偏移采样，判断首段是否“允许”
-        for (size_t i = 0; i + 1 < bounds.size(); ++i) {
-            double L = bounds[i];
-            double R = bounds[i + 1];
-            if (R <= L) continue;
-
-            double probe = (L + R) * 0.5;      // 取中点
-
-            if (angle_in_wrap(theta_of(probe), joint_l, joint_u)) {
-                allow_pairs.emplace_back(L, R);
-            }
-        }
-    } else {
-        // 4) 无交点：整圈全允许或全禁止（用环形比较）
-        double tlim = theta_of(0.0);
-        if (angle_in_wrap(tlim, joint_l, joint_u)) {
-            allow_pairs = {{-M_PI, M_PI}};
-        } else {
-            allow_pairs.clear();
-        }
-    }
-
-    // 5) 去掉奇异点
-    if (!singular_allow.empty() && !allow_pairs.empty()) {
-        allow_pairs = intersect(allow_pairs, singular_allow);
-    }
-    // 合并小段
-    allow_pairs = union_intervals(allow_pairs);
-    return allow_pairs;
-}
-
-
-std::vector<std::pair<double, double> > IkLimitAnalyzer::intersect(
-    const std::vector<std::pair<double, double> > &A, const std::vector<std::pair<double, double> > &B) {
-    if (A.empty() || B.empty()) return {};
-
-    // 先复制并排序（按起点）
-    auto SA = A, SB = B;
-    std::sort(SA.begin(), SA.end(),
-              [](auto &x, auto &y) { return x.first < y.first; });
-    std::sort(SB.begin(), SB.end(),
-              [](auto &x, auto &y) { return x.first < y.first; });
-
-    // 双指针求交
-    std::vector<std::pair<double, double> > out;
-    size_t i = 0, j = 0;
-    while (i < SA.size() && j < SB.size()) {
-        double L = std::max(SA[i].first, SB[j].first);
-        double R = std::min(SA[i].second, SB[j].second);
-        if (R > L) out.emplace_back(L, R);
-
-        // 谁先结束谁前进
-        if (SA[i].second < SB[j].second) ++i;
-        else ++j;
-    }
-    // 合并可能相邻/重叠的小段
-    if (out.empty()) return out;
-
-    std::vector<std::pair<double, double> > merged;
-    merged.reserve(out.size());
-    std::sort(out.begin(), out.end(),
-              [](auto &x, auto &y) { return x.first < y.first; });
-
-    merged.push_back(out[0]);
-    for (size_t k = 1; k < out.size(); ++k) {
-        if (out[k].first <= merged.back().second + EPS) {
-            merged.back().second = std::max(merged.back().second, out[k].second);
-        } else {
-            merged.push_back(out[k]);
-        }
-    }
-    return merged;
-}
-
-std::vector<std::pair<double, double> > IkLimitAnalyzer::bounds_to_pairs(const std::vector<double> &bounds) {
-    std::vector<std::pair<double, double> > segs;
-    if (bounds.empty()) return segs;
-
-    // 要求：bounds 已按升序，且“以允许开始”
-    if (bounds.size() % 2 != 0) {
-        // 若出现奇偶不配，说明bounds计算错误
-        return segs;
-    }
-
-    segs.reserve(bounds.size() / 2);
-    for (size_t i = 0; i < bounds.size(); i += 2) {
-        double L = bounds[i];
-        double R = bounds[i + 1];
-        if (R > L) segs.emplace_back(L, R);
-    }
-    return segs;
-}
-
-
-std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
-    double a, double b, double c, int conf, double joint_l, double joint_u) {
-    // 1) 奇异点判断，这里虽然是奇异点，但是左导数，右导数都是存在的，故没有屏蔽这个点
-    if (std::abs(a * a + b * b - (c - 1) * (c - 1)) < EPS) {
-        double psi_sing = 2.0 * std::atan2(a, (b - (c - 1)));
-    }
-    if (std::abs(a * a + b * b - (c + 1) * (c + 1)) < EPS) {
-        double psi_sing = 2.0 * std::atan2(a, (b - (c + 1)));
-    }
-
-    // 2) 将关节上下限映射到 ψ（cos 型）
-    std::vector<double> pt1 = calc_cos_solution(a, b, c, joint_l);
-    std::vector<double> pt2 = calc_cos_solution(a, b, c, joint_u);
-
-    std::vector<double> ptlim;
-    ptlim.reserve(pt1.size() + pt2.size());
-    ptlim.insert(ptlim.end(), pt1.begin(), pt1.end());
-    ptlim.insert(ptlim.end(), pt2.begin(), pt2.end());
-
-
-    // 去重
-    std::sort(ptlim.begin(), ptlim.end());
-    ptlim.erase(std::unique(ptlim.begin(), ptlim.end(),
-                            [](double a, double b) { return std::abs(a - b) < EPS; }),
-                ptlim.end());
-
-    std::vector<std::pair<double, double> > allow_pairs;
-
-    auto theta_of = [&](double psi) {
-        double ct = a * std::sin(psi) + b * std::cos(psi) + c;
-        ct = clamp(ct, -1.0, 1.0); // 保证 ct 的范围在 [-1, 1] 之间
-        return static_cast<double>(conf) * std::acos(ct);
-    };
-
-    // 3) 排序区间
-    std::sort(ptlim.begin(), ptlim.end());
-
-    if (!ptlim.empty()) {
-
-
-        // 4) 采样 + 交替
-        // 构造边界数组：[-π, cuts..., π]
-        std::vector<double> bounds;
-        bounds.reserve(ptlim.size() + 2);
-        bounds.push_back(-M_PI);
-        bounds.insert(bounds.end(), ptlim.begin(), ptlim.end());
-        bounds.push_back(M_PI);
-
-        // 5) 采样左侧点，判断是否允许
-        for (size_t i = 0; i + 1 < bounds.size(); ++i) {
-            double L = bounds[i];
-            double R = bounds[i + 1];
-            if (R <= L) continue;
-
-            double probe = (L + R) * 0.5;      //取中点
-
-            if (angle_in_wrap(theta_of(probe), joint_l, joint_u)) {
-                allow_pairs.emplace_back(L, R);
-            }
-        }
-    } else {
-        // 6) 无交点的情况：全允许或全禁止
-        double tlim = theta_of(0.0);
-        if (angle_in_wrap(tlim, joint_l, joint_u)) {
-            allow_pairs = {{-M_PI, M_PI}};
-        } else {
-            allow_pairs.clear();
-        }
-    }
-
-    // 8) 合并
-    allow_pairs = union_intervals(allow_pairs);
-
-    return allow_pairs;
-}
-
-
-bool IkLimitAnalyzer::check_cos_solution(double a, double b, double c, double theta_target, double psi) {
-    // // 由 ψ 复原 θ：θ = acos( a sinψ + b cosψ + c )
-    // double ct = a * std::sin(psi) + b * std::cos(psi) + c;
-    // ct = clamp(ct, -1.0, 1.0);
-    // double theta = std::acos(ct);
-    // return std::abs(theta - theta_target) < 1e-6;
-
-    // ct = a sinψ + b cosψ + c  应等于 cos(theta_target)
-    double ct = a * std::sin(psi) + b * std::cos(psi) + c;
-    ct = clamp(ct, -1.0, 1.0);
-
-    // 目标的 cos 值（对 conf 正负都成立）
-    double v = std::cos(theta_target);
-
-    return std::abs(ct - v) < EPS;
-}
-
-
-std::vector<double> IkLimitAnalyzer::calc_cos_solution(double a, double b, double c, double theta) {
-    std::vector<double> out;
-
-    // v = cos(theta)
-    double v = std::cos(theta);
-
-    // 二次式：as * t^2 + bs * t + cs = 0， 其中 t = tan(ψ/2)
-    double as = v + b - c;
-    double bs = -2.0 * a;
-    double cs = v - b - c;
-
-    double D = bs * bs - 4.0 * as * cs;
-    if (D < 0.0) return out;
-    D = std::max(0.0, D);
-    double sqrtD = std::sqrt(D);
-
-    double psi1 = 2.0 * std::atan2(-(bs - sqrtD), 2.0 * as);
-    double psi2 = 2.0 * std::atan2(-(bs + sqrtD), 2.0 * as);
-
-    psi1 = normalize_angle(psi1);
-    psi2 = normalize_angle(psi2);
-
-    if (check_cos_solution(a, b, c, theta, psi1)) out.push_back(psi1);
-    if (check_cos_solution(a, b, c, theta, psi2) &&
-        std::abs(psi2 - psi1) > EPS)
-        out.push_back(psi2);
-
-    std::sort(out.begin(), out.end());
-    return out;
-}
-
-bool IkLimitAnalyzer::wraps(double L, double U) {
-    L = normalize_angle(L);   // [-π, π]
-    U = normalize_angle(U);   // [-π, π]
-    return (L > U);           // 在 [-π, π] 规范下仍成立
-}
-
-
-std::vector<std::pair<double, double>>
-IkLimitAnalyzer::cal_offset_limits(double joint_l, double joint_u, double offset) {
-
-
-    double pL = normalize_angle(joint_l + offset); // [-π, π]
-    double pU = normalize_angle(joint_u + offset); // [-π, π]
-
-    std::vector<std::pair<double, double>> joint_ranges;
-
-    if (!wraps(pL, pU)) {
-        // 单段
-        if (pU > pL + EPS) {
-            joint_ranges.push_back({pL, pU});
-        }
-    } else {
-        // 跨 ±π，拆成两段 [-π, pU] ∪ [pL, π]
-        if (pU > -M_PI + EPS) joint_ranges.push_back({-M_PI, pU});
-        if ( M_PI >  pL + EPS) joint_ranges.push_back({ pL,   M_PI});
-    }
-    return joint_ranges;
-}
-
-std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_tan_limits(
-    double an, double ad, double bn, double bd, double cn, double cd, double joint_l, double joint_u, double offset) {
-    auto joint_ranges = cal_offset_limits(joint_l, joint_u, offset);
-    std::vector<std::pair<double, double> > out{};
-    for (auto [L, U]: joint_ranges) {
-        auto part = calc_tan_limits(an, ad, bn, bd, cn, cd, L, U);
-        out.insert(out.end(), part.begin(), part.end());
-    }
-    out = union_intervals(out);
-    return out;
-}
-
-std::vector<std::pair<double, double> > IkLimitAnalyzer::calc_cos_limits(
-    double a, double b, double c, int conf, double joint_l, double joint_u, double offset) {
-    auto joint_ranges = cal_offset_limits(joint_l, joint_u, offset);
-    std::vector<std::pair<double, double> > out{};
-    for (auto [L, U]: joint_ranges) {
-        auto part = calc_cos_limits(a, b, c, conf, L, U);
-        out.insert(out.end(), part.begin(), part.end());
-    }
-    out = union_intervals(out);
-    return out;
-}
-
-
-bool IkLimitAnalyzer::angle_in_wrap(double x, double L, double U) {
-    x = normalize_angle(x);
-    L = normalize_angle(L);
-    U = normalize_angle(U);
-    if (L <= U) return (x > L - EPS && x < U + EPS);
-    return (x > L - EPS || x < U + EPS);
-}
-
-std::vector<std::pair<double, double> >
-IkLimitAnalyzer::union_intervals(const std::vector<std::pair<double, double> > &in) {
-
-    if (in.empty()) return {};
-    std::vector<std::pair<double, double> > v = in;
-    std::sort(v.begin(), v.end(), [](auto &a, auto &b) {
-        return (a.first < b.first) || (a.first == b.first && a.second < b.second);
-    });
-    std::vector<std::pair<double, double> > out;
-    double L = v[0].first, R = v[0].second;
-    for (size_t i = 1; i < v.size(); ++i) {
-        if (v[i].first <= R + EPS) R = std::max(R, v[i].second);
-        else {
-            out.push_back({L, R});
-            L = v[i].first;
-            R = v[i].second;
-        }
-    }
-    out.push_back({L, R});
-    return out;
-}
-
-
-
-// --- 前向预测（给 ψ 预测 θ̂，用于候选打分） ---
-inline double IkLimitAnalyzer::predict_theta_tan(
-    double an,double ad,double bn,double bd,double cn,double cd,double psi,double offset)
-{
-    double N = an*std::sin(psi) + bn*std::cos(psi) + cn;
-    double D = ad*std::sin(psi) + bd*std::cos(psi) + cd;
-    double phi = std::atan2(N, D);
-    return normalize_angle(phi - offset); // θ̂ = φ - offset
-}
-
-inline double IkLimitAnalyzer::predict_theta_cos(
-    double a,double b,double c,int conf,double psi,double offset)
-{
-    double ct = a*std::sin(psi) + b*std::cos(psi) + c;
-    ct = clamp(ct, -1.0, 1.0);
-    double phi = static_cast<double>(conf) * std::acos(ct);
-    return normalize_angle(phi - offset); // θ̂ = φ - offset
-}
-
-
-IkLimitAnalyzer::PsiEstimateResult
-IkLimitAnalyzer::estimate_psi_from_joints(
-    const Eigen::MatrixXd& s_mat, const Eigen::MatrixXd& w_mat,
-    const std::vector<double>& theta_c,
-    int s_conf, int /*e_conf*/, int w_conf,
-    double prefer_psi)
-{
-    PsiEstimateResult res;
-
-    // 尺寸检查
-    if (s_mat.rows()!=3 || s_mat.cols()!=9 || w_mat.rows()!=3 || w_mat.cols()!=9 || theta_c.size() != 7) {
-        res.ok = false; return res;
-    }
-
-    // 拆块
-    const Eigen::Matrix3d As = s_mat.block<3,3>(0,0);
-    const Eigen::Matrix3d Bs = s_mat.block<3,3>(0,3);
-    const Eigen::Matrix3d Cs = s_mat.block<3,3>(0,6);
-
-    const Eigen::Matrix3d Aw = w_mat.block<3,3>(0,0);
-    const Eigen::Matrix3d Bw = w_mat.block<3,3>(0,3);
-    const Eigen::Matrix3d Cw = w_mat.block<3,3>(0,6);
-
-    const int s = s_conf;
-    const int w = w_conf;
-
-    std::vector<double> cands;
-
-    // —— 用每个关节各自反算 ψ 候选（与限位时的系数完全一致）——
-
-    // J1: tan
-    {
-        double an = -s * As(1,1), ad = -s * As(0,1);
-        double bn = -s * Bs(1,1), bd = -s * Bs(0,1);
-        double cn = -s * Cs(1,1), cd = -s * Cs(0,1);
-        auto v = calc_tan_solution(an,ad,bn,bd,cn,cd, theta_c[0]);
-        cands.insert(cands.end(), v.begin(), v.end());
-    }
-
-    // J2: cos  (同 limit_2；反算时不需要 conf 放入 cos() 里)
-    {
-        double a = -As(2,1), b = -Bs(2,1), c = -Cs(2,1);
-        auto v = calc_cos_solution(a,b,c, theta_c[1]);
-        cands.insert(cands.end(), v.begin(), v.end());
-    }
-
-    // J3: tan  (同 limit_3)
-    {
-        double an =  s * As(2,2), ad = -s * As(2,0);
-        double bn =  s * Bs(2,2), bd = -s * Bs(2,0);
-        double cn =  s * Cs(2,2), cd = -s * Cs(2,0);
-        auto v = calc_tan_solution(an,ad,bn,bd,cn,cd, theta_c[2]);
-        cands.insert(cands.end(), v.begin(), v.end());
-    }
-
-    // J5: tan + offset(+π/2)  (同 limit_5)
-    {
-        double an =  w * Aw(1,2), ad =  w * Aw(0,2);
-        double bn =  w * Bw(1,2), bd =  w * Bw(0,2);
-        double cn =  w * Cw(1,2), cd =  w * Cw(0,2);
-        double theta_adj = theta_c[4] + M_PI/2.0;
-        auto v = calc_tan_solution(an,ad,bn,bd,cn,cd, theta_adj);
-        cands.insert(cands.end(), v.begin(), v.end());
-    }
-
-    // J6: cos + offset(+π/2)  (同 limit_6)
-    {
-        double a = Aw(2,2), b = Bw(2,2), c = Cw(2,2);
-        double theta_adj = theta_c[5] + M_PI/2.0;
-        auto v = calc_cos_solution(a,b,c, theta_adj);
-        cands.insert(cands.end(), v.begin(), v.end());
-    }
-
-    // J7: tan  (同 limit_7)
-    {
-        double an =  w * Aw(2,1), ad = -w * Aw(2,0);
-        double bn =  w * Bw(2,1), bd = -w * Bw(2,0);
-        double cn =  w * Cw(2,1), cd = -w * Cw(2,0);
-        auto v = calc_tan_solution(an,ad,bn,bd,cn,cd, theta_c[6]);
-        cands.insert(cands.end(), v.begin(), v.end());
-    }
-
-    // 候选去重
-    std::sort(cands.begin(), cands.end());
-    cands.erase(std::unique(cands.begin(), cands.end(),
-        [&](double a,double b){ return std::abs(normalize_angle(a - b)) < 1e-9; }), cands.end());
-
-    // if (cands.empty()) { res.ok=false; return res; }
-   // 认为设置
-    if (cands.empty()) {
-        const int K = 720; // 0.5° 网格
-        cands.reserve(K);
-        for (int i=0;i<K;++i){
-            cands.push_back(-M_PI + (2.0*M_PI)*(i+0.5)/K);
-        }
-    }
-
-    // —— 用所有关节的前向解析 θ̂(ψ) 对每个候选打分，选总残差最小者 ——
-    auto wrap_diff = [&](double x){ return normalize_angle(x); };
-
-    auto score_of = [&](double psi){
-        double sc = 0.0;
-
-        // J1 tan
-        {
-            double an = -s * As(1,1), ad = -s * As(0,1);
-            double bn = -s * Bs(1,1), bd = -s * Bs(0,1);
-            double cn = -s * Cs(1,1), cd = -s * Cs(0,1);
-            double th = predict_theta_tan(an,ad,bn,bd,cn,cd, psi, 0.0);
-            sc += std::pow(wrap_diff(th - theta_c[0]), 2);
-        }
-        // J2 cos
-        {
-            double a = -As(2,1), b = -Bs(2,1), c = -Cs(2,1);
-            double th = predict_theta_cos(a,b,c, s, psi, 0.0);
-            sc += std::pow(wrap_diff(th - theta_c[1]), 2);
-        }
-        // J3 tan
-        {
-            double an =  s * As(2,2), ad = -s * As(2,0);
-            double bn =  s * Bs(2,2), bd = -s * Bs(2,0);
-            double cn =  s * Cs(2,2), cd = -s * Cs(2,0);
-            double th = predict_theta_tan(an,ad,bn,bd,cn,cd, psi, 0.0);
-            sc += std::pow(wrap_diff(th - theta_c[2]), 2);
-        }
-        // J5 tan + offset
-        {
-            double an =  w * Aw(1,2), ad =  w * Aw(0,2);
-            double bn =  w * Bw(1,2), bd =  w * Bw(0,2);
-            double cn =  w * Cw(1,2), cd =  w * Cw(0,2);
-            double th = predict_theta_tan(an,ad,bn,bd,cn,cd, psi, M_PI/2.0);
-            sc += std::pow(wrap_diff(th - theta_c[4]), 2);
-        }
-        // J6 cos + offset
-        {
-            double a = Aw(2,2), b = Bw(2,2), c = Cw(2,2);
-            double th = predict_theta_cos(a,b,c, w, psi, M_PI/2.0);
-            sc += std::pow(wrap_diff(th - theta_c[5]), 2);
-        }
-        // J7 tan
-        {
-            double an =  w * Aw(2,1), ad = -w * Aw(2,0);
-            double bn =  w * Bw(2,1), bd = -w * Bw(2,0);
-            double cn =  w * Cw(2,1), cd = -w * Cw(2,0);
-            double th = predict_theta_tan(an,ad,bn,bd,cn,cd, psi, 0.0);
-            sc += std::pow(wrap_diff(th - theta_c[6]), 2);
-        }
-        return sc;
-    };
-
-    double best = std::numeric_limits<double>::infinity();
-    double bestpsi = 0.0;
-
-    for (double psi : cands) {
-        double sc = score_of(psi);
-        res.candidates.emplace_back(psi,sc);
-        if (sc < best - 1e-12) {
-            best = sc; bestpsi = psi;
-        } else if (std::abs(sc - best) <= 1e-12 && std::isfinite(prefer_psi)) {
-            // 并列时选更靠近 prefer_psi 的
-            if (std::abs(normalize_angle(psi - prefer_psi)) <
-                std::abs(normalize_angle(bestpsi - prefer_psi))) {
-                best = sc; bestpsi = psi;
-            }
-        }
-    }
-
-    res.ok = true;
-    res.psi = bestpsi;
-    res.score = best;
-    return res;
-}
-
-
-
-double IkLimitAnalyzer::update_psi(
-    double psi_prev,
-    const std::vector<std::pair<double,double>>& psi_all,
-    const PsiUpdateParams& p
-){
-    // 没有可行区间：按需返回原值或报错；这里返回原值
-    if (psi_all.empty()) {
-        return psi_prev;
-    }
-
-    const double EPS = 1e-12;
-
-    // 2) 找到包含 ψ_{t-1} 的可行段 Ψ_all,m = [L, U]
-    int hit = -1;
-    for (int i = 0; i < (int)psi_all.size(); ++i) {
-        double L = psi_all[i].first;
-        double U = psi_all[i].second;
-        if (psi_prev >= L - EPS && psi_prev <= U + EPS) {
-            hit = i; break;
-        }
-    }
-
-    auto clamp_step = [&](double d){
-        if (p.step_cap > 0.0)
-            return std::max(-p.step_cap, std::min(p.step_cap, d));
-        return d;
-    };
-
-    // 工具：到区间的环形距离（若在区间内则为 0）
-    auto ang_dist_to_interval = [&](double x, double L, double U){
-        x = normalize_angle(x);
-        // 不跨界，且 L<U
-        if (x >= L - EPS && x <= U + EPS) return 0.0;
-        auto wrap_abs = [&](double d){ return std::abs(normalize_angle(d)); };
-        return std::min(wrap_abs(x - L), wrap_abs(x - U));
-    };
-
-    // 3) 命中某个可行段 → 用论文的指数排斥公式
-    if (hit >= 0) {
-        double L = psi_all[hit].first;
-        double U = psi_all[hit].second;
-        double W = U - L;
-        if (W <= EPS) {
-            return normalize_angle(0.5 * (L + U));
-        }
-
-        // 归一化到 [0,1] 的左右边界距离
-        double sL = clamp((psi_prev - L) / W,0.0,1.0);  // ∈[0,1]
-        double sU = clamp((U - psi_prev) / W,0.0,1.0);  // ∈[0,1]
-
-        // 公式：
-        // ψ_t = ψ_{t-1} + K*(W/2) * [ exp(-α * sL) - exp(-α * sU) ]
-        double kick = p.K * (0.5 * W) * ( std::exp(-p.alpha * sL) - std::exp(-p.alpha * sU) );
-        kick = clamp_step(kick);
-
-        double psi_new = normalize_angle(psi_prev + kick);
-        double margin  = std::max(p.edge_margin, 0.02 * W);
-        margin = std::min(margin, 0.25 * W);
-        if (psi_new < L + 1e-9 || psi_new > U - 1e-9) {
-            psi_new = std::min(U - margin, std::max(L + margin, psi_new));
-        }
-        return psi_new;
-    }
-
-    // 4) 没命中的情况：把肘（ψ）“移入最近的可行段”
-    int best = -1;
-    double best_d = std::numeric_limits<double>::infinity();
-    for (int i = 0; i < (int)psi_all.size(); ++i) {
-        double L = psi_all[i].first;
-        double U = psi_all[i].second;
-        double d = ang_dist_to_interval(psi_prev, L, U);
-        if (d < best_d) { best_d = d; best = i; }
-    }
-    // 理论上 best 必定存在
-    double L = psi_all[best].first;
-    double U = psi_all[best].second;
-    double W = U - L;
-
-    // 选择最近的边界，并向内缩 margin
-    // 根据 ψ_{t-1} 与 [L,U] 的相对位置，决定吸向 L+δ 还是 U-δ
-    auto wrap = [&](double x){ return normalize_angle(x); };
-
-    // 判断离哪个端点近：用环形距离
-    auto wrap_abs = [&](double d){ return std::abs(normalize_angle(d)); };
-    bool closer_to_L = (wrap_abs(psi_prev - L) <= wrap_abs(psi_prev - U));
-
-    double margin = std::max(p.edge_margin, 0.02 * W); // 至少内缩一点，或用 2% 的区间宽
-    margin = std::min(margin, 0.25 * W);               // 别缩太多
-
-    double target = closer_to_L ? (L + margin) : (U - margin);
-
-    // 如需要平滑，可按 step_cap 限制一步走到 target 的幅度
-    double delta = normalize_angle(target - psi_prev);
-    delta = clamp_step(delta);
-
-    return wrap(psi_prev + delta);
-}

src/utils/srs_ik/ik_limit_analyzer.h

@@ -1,169 +0,0 @@
-//
-// Created by lgv on 2025/11/3.
-//
-
-#ifndef CMVR_ES_IK_LIMIT_ANALYZER_H
-#define CMVR_ES_IK_LIMIT_ANALYZER_H
-#include <complex.h>
-#include <vector>
-#include <array>
-#include <iostream>
-#include <Eigen/Core>
-
-namespace cmvr {
-    namespace utils {
-        class IkLimitAnalyzer {
-        public:
-
-            // Tan 型关节：给定 (an,ad,bn,bd,cn,cd) 与关节极限
-            // 返回 psi 的允许区间边界： [L1,R1,L2,R2,...]
-            static std::vector<std::pair<double, double> >
-            calc_tan_limits(double an, double ad,
-                            double bn, double bd,
-                            double cn, double cd,
-                            double joint_l, double joint_u);
-
-            static std::vector<std::pair<double, double> >
-            calc_tan_limits(double an, double ad,
-                            double bn, double bd,
-                            double cn, double cd,
-                            double joint_l, double joint_u, double offset);
-
-            static std::vector<std::pair<double, double> >
-            calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u);
-
-            static std::vector<std::pair<double, double> >
-            calc_cos_limits(double a, double b, double c, int conf, double joint_l, double joint_u, double offset);
-
-            // 两个“允许集”的交集
-            static std::vector<std::pair<double, double> >
-            intersect(const std::vector<std::pair<double, double> > &A,
-                      const std::vector<std::pair<double, double> > &B);
-
-            //打印区间
-            static void print_intervals(const std::vector<std::pair<double, double> > &intervals) {
-                for (const auto &interval: intervals) {
-                    std::cout << "[" << interval.first << ", " << interval.second << "] ";
-                }
-                std::cout << std::endl;
-            }
-
-            static void set_sing_avid(double value_deg){psi_sing_avid_ = deg2rad(value_deg);}
-
-        public:
-            //  反算 ψ 的返回结果
-            struct PsiEstimateResult {
-                bool ok{false};
-                double psi{0.0};    // 估计出来的 ψ
-                double score{0.0};  // 总残差（越小越好）
-                std::vector<std::pair<double, double>> candidates{}; // 候选 ψ ，以及分数
-            };
-
-            // 用当前关节角反算“上一时刻/当前估计”的臂角 ψ
-            // s_conf/e_conf/w_conf = {+1, -1}；prefer_psi  NaN 表示无偏好
-            static PsiEstimateResult estimate_psi_from_joints(
-                const Eigen::MatrixXd& s_mat,          // 3x9  [As|Bs|Cs]
-                const Eigen::MatrixXd& w_mat,          // 3x9  [Aw|Bw|Cw]
-                const std::vector<double>& theta_c,   // 当前 7 关节角
-                int s_conf, int e_conf, int w_conf,
-                double prefer_psi = std::numeric_limits<double>::quiet_NaN()
-            );
-
-        private:
-            // 打分时的前向预测（方程里的“φ”减去 offset 得 θ̂）
-            static inline double predict_theta_tan(double an,double ad,double bn,double bd,double cn,double cd,double psi,double offset);
-            static inline double predict_theta_cos(double a,double b,double c,int conf,double psi,double offset);
-
-        public:
-            // 论文 Fig.10 的“Calculate New ψ”一步更新所需参数
-            struct  PsiUpdateParams {
-                double K     = 0.6;   // [0,1] 排斥强度
-                double alpha = 5.0;   // >0  开始排斥的“敏感度”
-                double step_cap = 0.0; // psi 与上一次psi 的单步最大变化（rad），<=0 表示不限制
-                double edge_margin = 1e-4; // 落到区间边界时的内缩量
-            };
-
-            // 根据全局可行区间 Ψ_all（并集），按论文公式从 ψ_{t-1} 计算 ψ_{t}
-            static double update_psi(
-                double psi_prev,
-                const std::vector<std::pair<double,double>>& psi_all, // merged ∪-intervals, each L<R in [-π,π]
-                const PsiUpdateParams& p
-            );
-        private:
-            static constexpr double EPS = 1e-9;
-
-            static double deg2rad(double deg) { return deg * M_PI / 180.0; }
-            inline static double psi_sing_avid_ = deg2rad(5);
-
-
-            // tan 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解，式（32）
-            static std::vector<double>
-            calc_tan_solution(double an, double ad,
-                              double bn, double bd,
-                              double cn, double cd,
-                              double theta);
-
-            // cos 型：给定目标 theta，返回所有 ψ ∈ [-π, π] 的解
-            static std::vector<double>
-            calc_cos_solution(double a, double b, double c, double theta);
-
-
-            // 检核：tan 型
-            static bool check_tan_solution(double an, double ad,
-                                           double bn, double bd,
-                                           double cn, double cd,
-                                           double theta_target,
-                                           double psi);
-
-            // 检核：cos 型
-            static bool check_cos_solution(double a, double b, double c,
-                                           double theta_target,
-                                           double psi);
-
-            static std::vector<std::pair<double, double> >
-            bounds_to_pairs(const std::vector<double> &bounds);
-
-            // 根据DH 参数中的 关节角 θi (rad)  的偏移来计算实际的限位区间
-            static inline std::vector<std::pair<double, double> >
-            cal_offset_limits(double joint_l, double joint_u, double offset);
-
-
-            // static double deg2rad(double deg) { return deg * M_PI / 180.0; }
-            static double rad2deg(double rad) { return rad * 180.0 / M_PI; }
-
-            template<class T>
-            static constexpr int sign(T x, T eps) {
-                return (x > eps) - (x < -eps);
-            }
-
-            template<class T>
-            static constexpr int sign(T x) {
-                return sign(x, T(0));
-            }
-
-            template<typename T>
-            static T clamp(T v, T lo, T hi) {
-                return std::max(lo, std::min(hi, v));
-            }
-
-            // 将角度归一化到 [-π, π] 范围内
-            static double normalize_angle(double angle);
-
-            // 环形包含（ [L,U]）
-            static bool angle_in_wrap(double x, double L, double U);
-
-            // 判断环绕：区间 [L,U] 是否跨过 π
-            static inline bool wraps(double L, double U);
-
-            // 线性并集（输入输出都在 [-π,π] 且 L<=U；跨界已在上游拆分）
-            static std::vector<std::pair<double, double> >
-            union_intervals(const std::vector<std::pair<double, double> > &in);
-
-
-
-        };
-    };
-}
-
-
-#endif //CMVR_ES_IK_LIMIT_ANALYZER_H

third_party/toppra/0.6.2/include/toppra/algorithm.hpp

@@ -0,0 +1,168 @@
+#ifndef TOPPRA_ALGORITHM_HPP
+#define TOPPRA_ALGORITHM_HPP
+
+#include <stdexcept>
+#include <sstream>
+#include <toppra/constraint.hpp>
+#include <toppra/geometric_path.hpp>
+#include <toppra/solver.hpp>
+#include <toppra/toppra.hpp>
+
+namespace toppra {
+
+/// Return code for Path Parametrization algorithm.
+enum class ReturnCode {
+
+  /// Success
+  OK = 0,
+
+  /// Unknown error
+  ERR_UNKNOWN = 1,
+
+  /// Fail during computing controllable sets. Problem might be infeasible.
+  ERR_FAIL_CONTROLLABLE = 2,
+
+  /// Fail during forward pass. Numerical error occured.
+  ERR_FAIL_FORWARD_PASS = 3,
+
+  /// Problem is not initialized
+  ERR_UNINITIALIZED = 4,
+
+  /// Fail to ocmpute feasible sets.
+  ERR_FAIL_FEASIBLE = 5,
+};
+
+struct ParametrizationData {
+  /// \brief Grid-points used for solving the discretized problem.
+  /// The number of points must equal m_N + 1.
+  Vector gridpoints;
+
+  ///  Output parametrization (squared path velocity)
+  Vector parametrization;
+
+  Matrix controllable_sets;
+  Matrix feasible_sets;
+
+  ///  Return code of the algorithm.
+  ReturnCode ret_code = ReturnCode::ERR_UNINITIALIZED;
+};
+
+/** \brief Base class for time parametrization algorithms.
+ *
+ */
+class PathParametrizationAlgorithm {
+ public:
+  /** Construct the problem instance.
+   *
+   *  \param  constraints  List of constraints.
+   *  \param  path  The geometric path.
+   *
+   */
+  PathParametrizationAlgorithm(LinearConstraintPtrs constraints,
+                               const GeometricPathPtr &path);
+
+  /** \brief Set the level of discretization used by the solver.
+   *
+   * If is zero, will attempt to detect automatically the most
+   * suitable grid.  
+   * 
+   * TODO: Automatic gridpoint selection is not implemented in the CPP
+   * API.
+   */
+  void setN(int N) { m_N = N; m_initialized = false; };
+
+  /** \brief Set the gridpoints (points with the path intervals)
+   *
+   * If not set manually, then N equally distributed points is used.
+   */
+  void setGridpoints(const Vector& gridpoints);
+
+  /** \brief Set the LP/QP solver
+   *
+   * Default to \ref solver::qpOASESWrapper
+   */
+  void solver(SolverPtr solver) { m_solver.swap(solver); };
+
+  /** \brief Get output or result of algorithm.
+   */
+  const ParametrizationData& getParameterizationData() const { return m_data; };
+
+  /** Compute the time parametrization of the given path.
+   *
+   * \param vel_start
+   * \param vel_end
+   * \return Return code. When not ReturnCode::OK,
+   *         check PathParametrizationAlgorithm::getErrorMessage
+   */
+  virtual ReturnCode computePathParametrization(value_type vel_start = 0,
+                                                value_type vel_end = 0);
+
+  /** Compute the sets of feasible squared velocities.
+   */
+  ReturnCode computeFeasibleSets();
+
+  /** Set initial bounds on \f$ \dot{s}^2.
+   * This is helpfull when the solver encounters numerical issues.
+   */
+  void setInitialXBounds (const Bound& xbound)
+  {
+    m_initXBound = xbound;
+  }
+
+  /** Get the error message when PathParametrizationAlgorithm::computePathParametrization
+   * failed.
+   */
+  std::string getErrorMessage() const {
+    return m_errorStream.str();
+  }
+
+  virtual ~PathParametrizationAlgorithm() {}
+
+ protected:
+  /** \brief Select solver and gridpoints to use.
+   *
+   * This method implements a simple way to select gridpoints.
+   */
+  virtual void initialize();
+
+  /** \brief Compute the forward pass.
+   *
+   * Derived class should provide a suitable forward pass function,
+   * depending on the desired objective.
+   */
+  virtual ReturnCode computeForwardPass(value_type vel_start) = 0;
+
+  /** Compute the sets of controllable squared path velocities.
+   */
+  ReturnCode computeControllableSets(const Bound &vel_ends);
+
+  /** To be implemented in child method. */
+  LinearConstraintPtrs m_constraints;
+  GeometricPathPtr m_path;
+  SolverPtr m_solver;
+
+  std::stringstream m_errorStream;
+
+  /// Struct containing algorithm output.
+  ParametrizationData m_data;
+
+  /// \brief Number of segments in the discretized problems.
+  /// See m_gridpoints for more information.
+  int m_N = 100;
+
+  int m_initialized = false;
+
+  /** Set initial bounds on \f$ \dot{s}^2.
+   * \sa setInitialXBounds
+   * \todo The hard-coded bound below avoids numerical issues in LP / QP solvers
+   * when \f$ x \f$ becomes too big. This issue should be addressed in the
+   * solver wrapper themselfves as numerical behaviors is proper to each
+   * individual solver.
+   * See https://github.com/hungpham2511/toppra/issues/156
+   */
+  Bound m_initXBound = {0, 100};
+};
+
+}  // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/algorithm/toppra.hpp

@@ -0,0 +1,21 @@
+#ifndef TOPPRA_ALGORITHM_TOPPRA_HPP
+#define TOPPRA_ALGORITHM_TOPPRA_HPP
+
+#include <toppra/algorithm.hpp>
+#include <toppra/constraint.hpp>
+#include <toppra/geometric_path.hpp>
+#include "toppra/toppra.hpp"
+
+namespace toppra {
+namespace algorithm {
+class TOPPRA : public PathParametrizationAlgorithm {
+ public:
+  TOPPRA(LinearConstraintPtrs constraints, const GeometricPathPtr &path);
+
+ protected:
+  ReturnCode computeForwardPass(value_type vel_start);
+};
+}  // namespace algorithm
+}  // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/constraint.hpp

@@ -0,0 +1,174 @@
+#ifndef TOPPRA_CONSTRAINT_HPP
+#define TOPPRA_CONSTRAINT_HPP
+
+#include <ostream>
+#include <toppra/toppra.hpp>
+
+namespace toppra {
+/** Enum to mark different Discretization Scheme for LinearConstraint.
+ *  In general, the difference in speed is not too large. Should use
+ *  \ref Interpolation if possible.
+ * */
+enum DiscretizationType {
+    Collocation,   ///< smaller problem size, but lower accuracy.
+    Interpolation, ///< larger problem size, but higher accuracy.
+};
+
+/** \brief Abstract interface for constraints used in TOPPRA.
+ *
+ * This class of constraint is also known as Second-order Constraint.
+ *
+ * A Canonical Linear Constraint has the following form:
+ * \f{eqnarray}
+ *     \mathbf a_i u + \mathbf b_i x + \mathbf c_i &= v \\
+ *     \mathbf F_i v & \leq \mathbf g_i                \\
+ *     x^b_{i, 0} \leq x & \leq x^b_{i, 1} \\
+ *     u^b_{i, 0} \leq u & \leq u^b_{i, 1}
+ * \f}
+ *
+ * Here \f$u\f$ and \f$x\f$ represent the path acceleration and path velocity
+ * square. \f$v$\f is an auxilliary variable that represents either the robot
+ * joint/taskspace velocity, acceleration or torque, or just the squared path
+ * velocity term only.
+ *
+ * Alternatively, if \f$ \mathbf F_i \f$ is constant for all values
+ * of \f$i\f$, then we can consider the simpler constraint:
+ * \f[
+ *     \mathbf{F} v \leq \mathbf g
+ * \f]
+ *
+ * In this case, the returned value of \f$F\f$ by
+ * LinearConstraint::computeParams has shape (k, m) instead of (N, k, m),
+ * \f$ g \f$ (k) instead of (N, k) and the class attribute
+ * LinearConstraint::constantF will be \c true.
+ *
+ * \note Derived classes should at least implement the method
+ * LinearConstraint::computeParams_impl.
+ *
+ * \sa JointAccelerationConstraint, JointVelocityConstraint,
+ * CanonicalLinearSecondOrderConstraint
+ *
+ * */
+class LinearConstraint {
+  public:
+    DiscretizationType discretizationType () const
+    {
+      return m_discretizationType;
+    }
+
+    void discretizationType (DiscretizationType type);
+
+    /** Tells whether \f$ F, g \f$ matrices are the same over all the grid points.
+     * In this case, LinearConstraint::computeParams F and g parameters should
+     * only be of size 1.
+     * */
+    bool constantF () const
+    {
+      return m_constantF;
+    }
+
+    /// Dimension of \f$g\f$.
+    Eigen::Index nbConstraints () const
+    {
+      return m_k;
+    }
+
+    /// Dimension of \f$a, b, c, v\f$.
+    Eigen::Index nbVariables () const
+    {
+      return m_m;
+    }
+
+    bool hasLinearInequalities () const
+    {
+      return nbConstraints() > 0;
+    }
+
+    /** Whether this constraint has bounds on \f$u\f$.
+     * */
+    bool hasUbounds () const
+    {
+      return m_hasUbounds;
+    }
+
+    /** Whether this constraint has bounds on \f$x\f$.
+     * */
+    bool hasXbounds () const
+    {
+      return m_hasXbounds;
+    }
+
+    /**
+     * \param N number of gripoints (i.e. the number of intervals + 1)
+     * */
+    void allocateParams (std::size_t N,
+        Vectors& a, Vectors& b, Vectors& c,
+        Matrices& F, Vectors& g,
+        Bounds ubound, Bounds& xbound);
+
+    /** Compute numerical coefficients of the given constraint.
+     *
+     *  \param[in] path The geometric path.
+     *  \param[in] gridpoints Vector of size N+1. Gridpoint use for discretizing path.
+     *
+     *  \param[out] a N+1 Vector of size m.
+     *  \param[out] b N+1 Vector of size m.
+     *  \param[out] c N+1 Vector of size m.
+     *  \param[out] F N+1 Matrix of shape (k, m). If LinearConstraint::constantF
+     *              is \c true, there is only one such Matrix.
+     *  \param[out] g N+1 Vector of size m.
+     *  \param[out] ubound Shape (N + 1, 2). See notes.
+     *  \param[out] xbound Shape (N + 1, 2). See notes.
+     *
+     * \note the output must be allocated to correct sizes prior to calling this
+     * function.
+     *
+     * \todo check constness
+     *
+     * */
+    void computeParams(const GeometricPath& path, const Vector& gridpoints,
+        Vectors& a, Vectors& b, Vectors& c,
+        Matrices& F, Vectors& g,
+        Bounds& ubound, Bounds& xbound);
+
+    virtual std::ostream& print(std::ostream& os) const;
+
+    virtual ~LinearConstraint () {}
+
+  protected:
+    /**
+     * \param k number of inequality constraints.
+     * \param m number of internal variable (i.e. dimention of \f$v\f$).
+     * \param constantF whether \f$F\f$ and \f$g\f$ are constant.
+     * \param uBound whether \f$u\f$ is bounded.
+     * \param xBound whether \f$x\f$ is bounded.
+     * */
+    LinearConstraint(Eigen::Index k, Eigen::Index m, bool constantF,
+        bool uBound, bool xBound)
+      : m_discretizationType (Interpolation)
+      , m_k (k), m_m (m)
+      , m_constantF (constantF)
+      , m_hasUbounds (uBound)
+      , m_hasXbounds (xBound)
+    {}
+
+    virtual void computeParams_impl(const GeometricPath& path,
+        const Vector& gridpoints,
+        Vectors& a, Vectors& b, Vectors& c,
+        Matrices& F, Vectors& g,
+        Bounds& ubound, Bounds& xbound) = 0;
+
+    Eigen::Index m_k, m_m;
+    DiscretizationType m_discretizationType;
+    bool m_constantF, m_hasUbounds, m_hasXbounds;
+}; // class LinearConstraint
+
+/// \brief write a LinearConstraint to an output stream
+inline std::ostream& operator<< (std::ostream& os, const LinearConstraint& lc)
+{
+  return lc.print(os);
+}
+
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/constraint/cartesian_velocity_norm.hpp

@@ -0,0 +1,94 @@
+#ifndef TOPPRA_CONSTRAINT_CARTESIAN_VELOCITY_NORM_HPP
+#define TOPPRA_CONSTRAINT_CARTESIAN_VELOCITY_NORM_HPP
+
+#include <toppra/constraint.hpp>
+
+namespace toppra {
+namespace constraint {
+
+/**
+\brief A cartesian velocity constraint class.
+
+Given a path \f$ p(s) \f$, this class constraints the norm
+\f$||v||_S^2 = v^T S v \leq limit \f$ of the velocity \f$v\f$ of a frame.
+As \f$ v = J(p(s)) p'(s) \dot{s} \f$, it fits into a LinearConstraint as follows.
+
+\f{eqnarray}
+    p'(s)^T J(q)^T S J(q) p'(s) \dot{s}^2 &= w \\
+    w & \leq \mathbf g_i         \\
+\f}
+
+This class implements the case of constant velocity limits but can be derived to
+achieve varying velocity limits. E.g.
+\code
+class CartesianVelocityVarying : public CartesianVelocityNorm {
+public:
+  CartesianVelocityVarying(...) : CartesianVelocityNorm () {}
+protected:
+  void computeVelocityLimit(value_type time)
+  {
+    // Eventually, if one of the two is constant, it can be set in the constructor.
+    m_limit = ...;
+    m_S = ...;
+  }
+};
+\endcode
+*/
+class CartesianVelocityNorm : public LinearConstraint {
+  public:
+    virtual std::ostream& print(std::ostream& os) const;
+
+  protected:
+    /// Constructor for constant velocity limit.
+    CartesianVelocityNorm (const Matrix& S, const double& limit)
+      : LinearConstraint (1, 1, true, false, false)
+      , m_limit (limit)
+      , m_S (S)
+    {
+      check();
+    }
+
+    /// Constructor for varying velocity limit.
+    /// \note Attributes \ref m_S and \ref m_limit **must** be computed in
+    /// \ref computeVelocityLimit
+    CartesianVelocityNorm ()
+      : LinearConstraint (1, 1, false, false, false)
+      , m_limit (1.)
+      , m_S (6,6)
+    {
+      check();
+    }
+
+    /// Pure abstract method to compute the velocity from
+    /// \param q the current configuration
+    /// \param qdot the current velocity
+    /// \retval v the 6D frame velocity
+    virtual void computeVelocity (const Vector& q, const Vector& qdot,
+        Vector& v) = 0;
+
+    /**
+      \brief Computes the velocity limit at time \c time.
+
+      The result must be stored into attribute
+      CartesianVelocityNorm::m_limit
+      */
+    virtual void computeVelocityLimit(value_type time) { (void)time; }
+
+    /// The velocity limit
+    value_type m_limit;
+    /// The selection matrix
+    Matrix m_S;
+
+  private:
+    void check();
+
+    void computeParams_impl(const GeometricPath& path,
+        const Vector& gridpoints,
+        Vectors& a, Vectors& b, Vectors& c,
+        Matrices& F, Vectors& g,
+        Bounds& ubound, Bounds& xbound);
+}; // class CartesianVelocityNorm
+} // namespace constraint
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/constraint/cartesian_velocity_norm/pinocchio.hpp

@@ -0,0 +1,86 @@
+#ifndef TOPPRA_CONSTRAINT_CARTESIAN_VELOCITY_NORM_PINOCCIO_HPP
+#define TOPPRA_CONSTRAINT_CARTESIAN_VELOCITY_NORM_PINOCCIO_HPP
+
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/multibody/data.hpp>
+
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/algorithm/frames.hpp>
+
+#include <toppra/constraint/cartesian_velocity_norm.hpp>
+
+namespace toppra {
+namespace constraint {
+namespace cartesianVelocityNorm {
+
+/** Implementation of CartesianVelocityNorm using pinocchio function.
+ * \extends CartesianVelocityNorm
+ * */
+template<typename Model = pinocchio::Model>
+class Pinocchio;
+
+template<typename _Model>
+class Pinocchio : public CartesianVelocityNorm {
+  public:
+    typedef _Model Model;
+    typedef typename Model::Data Data;
+
+    std::ostream& print(std::ostream& os) const
+    {
+      return CartesianVelocityNorm::print(os << "Pinocchio - ");
+    }
+
+    void computeVelocity (const Vector& q, const Vector& qdot, Vector& v)
+    {
+      pinocchio::forwardKinematics(m_model, m_data, q, qdot);
+      v = pinocchio::getFrameVelocity(m_model, m_data, m_frame_id, m_reference_frame).toVector();
+    }
+
+    /// Constructor for constant velocity limit.
+    Pinocchio (const Model& model, const Matrix& S, const double& limit,
+        pinocchio::FrameIndex frame,
+        pinocchio::ReferenceFrame ref_frame = pinocchio::LOCAL_WORLD_ALIGNED)
+      : CartesianVelocityNorm (S, limit)
+      , m_model (model)
+      , m_data (model)
+      , m_frame_id (frame)
+      , m_reference_frame (ref_frame)
+    {
+    }
+
+    /// Move-assignment operator
+    Pinocchio (Pinocchio&& other)
+      : CartesianVelocityNorm(other)
+      , m_model (other.m_model)
+      , m_data (std::move(other.m_data))
+      , m_frame_id (m_frame_id)
+      , m_reference_frame (m_reference_frame)
+    {}
+
+    const Model& model() const { return m_model; }
+
+  protected:
+    /// Constructor for varying velocity limit.
+    Pinocchio (const Model& model,
+        pinocchio::FrameIndex frame,
+        pinocchio::ReferenceFrame ref_frame = pinocchio::LOCAL_WORLD_ALIGNED)
+      : CartesianVelocityNorm ()
+      , m_model (model)
+      , m_data (model)
+      , m_frame_id (frame)
+      , m_reference_frame (ref_frame)
+    {}
+
+
+  private:
+    const Model& m_model;
+    Data m_data;
+    pinocchio::FrameIndex m_frame_id;
+    pinocchio::ReferenceFrame m_reference_frame;
+}; // class Pinocchio
+
+} // namespace cartesianVelocityNorm
+} // namespace constraint
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/constraint/joint_torque.hpp

@@ -0,0 +1,113 @@
+#ifndef TOPPRA_CONSTRAINT_JOINT_TORQUE_HPP
+#define TOPPRA_CONSTRAINT_JOINT_TORQUE_HPP
+
+#include <toppra/constraint.hpp>
+
+namespace toppra {
+namespace constraint {
+
+/** Base class for joint torque constraints.
+ *
+ * A joint torque constraint is given by
+ * \f[
+ *   A(q) \ddot q + \dot q^\top B(q) \dot q + C(q) + D( \dot q )= \tau
+ * \f]
+ *
+ * where w is a vector that satisfies the polyhedral constraint:
+ * \f[ F \tau \leq g \f]
+ *
+ * Notice that \f$ inverseDynamics(q, qd, qdd) = \tau\f$ and that
+ * \f$ F, g \f$ are independant of the GeometricPath.
+ *
+ * To evaluate the constraint on a geometric path `p(s)`, multiple
+ * calls to \ref computeInverseDynamics are made. Specifically one
+ * can derive the second-order equation as follows
+ *
+ * \f{eqnarray}
+ *   A(q) p'(s) \ddot s &+ [A(q) p''(s) + p'(s)^\top B(q) p'(s)] \dot s^2 &+ C(q) + D( \dot q ) &= \tau \\
+ *         a(s) \ddot s &+                                  b(s) \dot s^2 &+ c(s) &= \tau
+ * \f}
+ *
+ * */
+class JointTorque : public LinearConstraint {
+  public:
+    virtual ~JointTorque () {}
+
+    virtual std::ostream& print(std::ostream& os) const;
+
+    /** Computes the joint torques from
+     * \param q robot configuration
+     * \param v robot velocity
+     * \param a robot acceleration
+     * \param[out] joint torques
+     * */
+    virtual void computeInverseDynamics (const Vector& q, const Vector& v, const Vector& a,
+        Vector& tau) = 0;
+
+    const Vector& lowerBounds () const
+    { return m_lower; }
+
+    void lowerBounds (const Vector& lb)
+    {
+      assert(lb.size() == m_lower.size());
+      m_lower = lb;
+    }
+
+    const Vector& upperBounds () const
+    { return m_upper; }
+
+    void upperBounds (const Vector& ub)
+    {
+      assert(ub.size() == m_upper.size());
+      m_upper = ub;
+    }
+
+    const Vector& frictionCoeffs () const
+    { return m_frictionCoeffs; }
+
+    void frictionCoeffs (const Vector& fc)
+    {
+      assert(fc.size() == m_frictionCoeffs.size());
+      m_frictionCoeffs = fc;
+    }
+
+  protected:
+    /**
+     * \param lowerTlimit lower torque limit
+     * \param upperTlimit upper torque limit
+     * \param frictionCoeffs dry friction coefficients of each joint.
+     *        size 0 when not considering friction.
+     * */
+    JointTorque (const Vector& lowerTlimit, const Vector& upperTlimit,
+        const Vector& frictionCoeffs)
+      : LinearConstraint (2*lowerTlimit.size(), lowerTlimit.size(), true, false, false)
+      , m_lower (lowerTlimit)
+      , m_upper (upperTlimit)
+      , m_frictionCoeffs (frictionCoeffs)
+    {
+      check();
+    }
+
+    /// Move-assignment constructor
+    JointTorque (JointTorque&& other)
+      : LinearConstraint (other)
+      , m_lower(std::move(other.m_lower))
+      , m_upper(std::move(other.m_upper))
+      , m_frictionCoeffs(std::move(other.m_frictionCoeffs))
+    {}
+
+  private:
+    void check();
+
+    void computeParams_impl(const GeometricPath& path,
+        const Vector& gridpoints,
+        Vectors& a, Vectors& b, Vectors& c,
+        Matrices& F, Vectors& g,
+        Bounds& ubound, Bounds& xbound);
+
+    Vector m_lower, m_upper, m_frictionCoeffs;
+}; // class JointTorque
+} // namespace constraint
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/constraint/joint_torque/pinocchio.hpp

@@ -0,0 +1,87 @@
+#ifndef TOPPRA_CONSTRAINT_JOINT_TORQUE_PINOCCIO_HPP
+#define TOPPRA_CONSTRAINT_JOINT_TORQUE_PINOCCIO_HPP
+
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/algorithm/rnea.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+
+#include <toppra/constraint/joint_torque.hpp>
+
+namespace toppra {
+namespace constraint {
+namespace jointTorque {
+
+/** Implementation of JointTorque using pinocchio::rnea function.
+ * \extends JointTorque
+ * */
+template<typename Model = pinocchio::Model>
+class Pinocchio;
+
+template<typename _Model>
+class Pinocchio : public JointTorque {
+  public:
+    typedef _Model Model;
+    typedef typename Model::Data Data;
+
+    std::ostream& print(std::ostream& os) const
+    {
+      return JointTorque::print(os << "Pinocchio - ");
+    }
+
+    void computeInverseDynamics (const Vector& q, const Vector& v, const Vector& a,
+        Vector& tau)
+    {
+      tau = pinocchio::rnea(m_model, m_data, q, v, a);
+    }
+
+    Pinocchio (const Model& model, const Vector& frictionCoeffs = Vector())
+      : JointTorque (-model.effortLimit, model.effortLimit, frictionCoeffs)
+      , m_model (model)
+      , m_data (model)
+    {
+    }
+
+    /// Move-assignment operator
+    Pinocchio (Pinocchio&& other)
+      : JointTorque(other)
+      , m_storage (std::move(other.m_storage))
+      , m_model (other.m_model)
+      , m_data (std::move(other.m_data))
+    {}
+
+    Pinocchio (const std::string& urdfFilename, const Vector& friction = Vector())
+      : Pinocchio (makeModel(urdfFilename), friction) {}
+
+    const Model& model() const { return m_model; }
+
+  private:
+    /// Build a pinocchio::Model
+    /// \param urdfFilename path to a URDF file.
+    static Model* makeModel (const std::string& urdfFilename)
+    {
+      Model* model (new Model);
+      pinocchio::urdf::buildModel(urdfFilename, *model);
+      return model;
+    }
+
+    /// Constructor that takes ownership of the model
+    Pinocchio (Model* model, const Vector& friction)
+      : JointTorque (-model->effortLimit, model->effortLimit, friction)
+      , m_storage (model)
+      , m_model (*model)
+      , m_data (m_model)
+    {
+    }
+
+    /// Store the pinocchio::Model object, in case this object owns it.
+    std::unique_ptr<Model> m_storage;
+    const Model& m_model;
+    Data m_data;
+}; // class Pinocchio
+
+} // namespace jointTorque
+} // namespace constraint
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/constraint/linear_joint_acceleration.hpp

@@ -0,0 +1,36 @@
+#ifndef TOPPRA_CONSTRAINT_LINEAR_JOINT_ACCELERATION_HPP
+#define TOPPRA_CONSTRAINT_LINEAR_JOINT_ACCELERATION_HPP
+
+#include <toppra/constraint.hpp>
+
+namespace toppra {
+namespace constraint {
+
+/// A Joint Acceleration Constraint class.
+class LinearJointAcceleration : public LinearConstraint {
+  public:
+    LinearJointAcceleration (const Vector& lowerAlimit, const Vector& upperAlimit)
+      : LinearConstraint (lowerAlimit.size() * 2, lowerAlimit.size(), true, false, false)
+      , m_lower (lowerAlimit)
+      , m_upper (upperAlimit)
+    {
+      check();
+    }
+
+    virtual std::ostream& print(std::ostream& os) const;
+
+  private:
+    void check();
+
+    void computeParams_impl(const GeometricPath& path,
+        const Vector& gridpoints,
+        Vectors& a, Vectors& b, Vectors& c,
+        Matrices& F, Vectors& g,
+        Bounds& ubound, Bounds& xbound);
+
+    Vector m_lower, m_upper;
+}; // class LinearJointAcceleration
+} // namespace constraint
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/constraint/linear_joint_velocity.hpp

@@ -0,0 +1,86 @@
+#ifndef TOPPRA_CONSTRAINT_LINEAR_JOINT_VELOCITY_HPP
+#define TOPPRA_CONSTRAINT_LINEAR_JOINT_VELOCITY_HPP
+
+#include <toppra/constraint.hpp>
+
+namespace toppra {
+namespace constraint {
+
+/**
+\brief A Joint Velocity Constraint class.
+
+This class implements the case of constant velocity limits but can be derived to
+achieve varying velocity limits. E.g.
+\code
+class LinearJointVelocityVarying : public LinearJointVelocity {
+public:
+  LinearJointVelocityVarying(...) : LinearJointVelocity (nDof) {}
+protected:
+  void computeVelocityLimits(value_type time)
+  {
+    m_lower = ...;
+    m_upper = ...;
+  }
+};
+\endcode
+*/
+class LinearJointVelocity : public LinearConstraint {
+  public:
+    LinearJointVelocity (const Vector& lowerVlimit, const Vector& upperVlimit)
+      : LinearConstraint (0, 0, true, false, true)
+      , m_lower (lowerVlimit)
+      , m_upper (upperVlimit)
+      , m_maxsd (1e8)
+    {
+      check();
+    }
+
+    /** Set the maximum allowed value of \f$\dot s\f$.
+     * \param maxsd should be strictly positive.
+     * */
+    void maxSDot (value_type maxsd)
+    {
+      assert(maxsd > 0);
+      m_maxsd = maxsd;
+    }
+
+    virtual std::ostream& print(std::ostream& os) const;
+
+  protected:
+    LinearJointVelocity (const int nDof)
+      : LinearConstraint (0, 0, true, false, true)
+      , m_lower (nDof)
+      , m_upper (nDof)
+      , m_maxsd (1e8)
+    {
+      check();
+    }
+
+    /**
+      \brief Computes the velocity limit at time \c time.
+
+      The result must be stored into attributes
+      LinearJointVelocity::m_lower and LinearJointVelocity::m_upper.
+      */
+    virtual void computeVelocityLimits(value_type time) { (void)time; }
+
+    /// The lower velocity limits
+    Vector m_lower;
+    /// The upper velocity limits
+    Vector m_upper;
+
+  private:
+    void check();
+
+    void computeParams_impl(const GeometricPath& path,
+        const Vector& gridpoints,
+        Vectors& a, Vectors& b, Vectors& c,
+        Matrices& F, Vectors& g,
+        Bounds& ubound, Bounds& xbound);
+
+    value_type m_maxsd;
+}; // class LinearJointVelocity
+} // namespace constraint
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/export.hpp

@@ -0,0 +1,43 @@
+
+#ifndef TOPPRA_EXPORT_H
+#define TOPPRA_EXPORT_H
+
+#ifdef TOPPRA_STATIC_DEFINE
+#  define TOPPRA_EXPORT
+#  define TOPPRA_NO_EXPORT
+#else
+#  ifndef TOPPRA_EXPORT
+#    ifdef toppra_EXPORTS
+        /* We are building this library */
+#      define TOPPRA_EXPORT __attribute__((visibility("default")))
+#    else
+        /* We are using this library */
+#      define TOPPRA_EXPORT __attribute__((visibility("default")))
+#    endif
+#  endif
+
+#  ifndef TOPPRA_NO_EXPORT
+#    define TOPPRA_NO_EXPORT __attribute__((visibility("hidden")))
+#  endif
+#endif
+
+#ifndef TOPPRA_DEPRECATED
+#  define TOPPRA_DEPRECATED __attribute__ ((__deprecated__))
+#endif
+
+#ifndef TOPPRA_DEPRECATED_EXPORT
+#  define TOPPRA_DEPRECATED_EXPORT TOPPRA_EXPORT TOPPRA_DEPRECATED
+#endif
+
+#ifndef TOPPRA_DEPRECATED_NO_EXPORT
+#  define TOPPRA_DEPRECATED_NO_EXPORT TOPPRA_NO_EXPORT TOPPRA_DEPRECATED
+#endif
+
+/* NOLINTNEXTLINE(readability-avoid-unconditional-preprocessor-if) */
+#if 0 /* DEFINE_NO_DEPRECATED */
+#  ifndef TOPPRA_NO_DEPRECATED
+#    define TOPPRA_NO_DEPRECATED
+#  endif
+#endif
+
+#endif /* TOPPRA_EXPORT_H */

third_party/toppra/0.6.2/include/toppra/geometric_path.hpp

@@ -0,0 +1,116 @@
+#ifndef TOPPRA_GEOMETRIC_PATH_HPP
+#define TOPPRA_GEOMETRIC_PATH_HPP
+
+#include <cstddef>
+#include <iostream>
+#include <ostream>
+#include <stdexcept>
+#include <toppra/algorithm.hpp>
+#include <toppra/toppra.hpp>
+#include <vector>
+
+namespace toppra {
+
+
+/**
+ * \brief Abstract interface for geometric paths.
+ */
+class GeometricPath {
+public:
+
+  GeometricPath()  = default;
+
+  /**
+   * Constructor of GeometricPath on vector spaces.
+   */
+  GeometricPath(int nDof) : m_configSize(nDof), m_dof (nDof) {}
+
+  /**
+   * Constructor of GeometricPath on non-vector spaces.
+   */
+  GeometricPath(int configSize, int nDof) : m_configSize(configSize), m_dof (nDof) {}
+
+  /**
+   * \brief Evaluate the path at given position.
+   */
+  virtual Vector eval_single(value_type, int order = 0) const = 0;
+
+  /**
+   * \brief Evaluate the path at given positions (vector).
+   *
+   * Default implementation: Evaluation each point one-by-one.
+   */
+  virtual Vectors eval(const Vector &positions, int order = 0) const;
+
+  /**
+     \brief Generate gridpoints that sufficiently cover the given path.
+     
+     This function operates in multiple passes through the geometric
+     path from the start to the end point. In each pass, for each
+     segment, the maximum interpolation error is estimated using the
+     following equation:
+
+        err_{est} = 0.5 * \mathrm{max}(\mathrm{abs}(p'' * d_{segment} ^ 2))
+
+     Here `p''` is the second derivative of the path and d_segment is
+     the length of the segment. If the estimated error `err_{test}` is
+     greater than the given threshold `max_err_threshold` then the
+     segment is divided in two half.
+     
+     Intuitively, at positions with higher curvature, there must be
+     more points in order to improve approximation
+     quality. Theoretically toppra performs the best when the proposed
+     gridpoint is optimally distributed.
+
+     @param maxErrThreshold Maximum worstcase error thrshold allowable.
+     @param maxIteration Maximum number of iterations.
+     @param maxSegLength All segments length should be smaller than this value.
+     @param minNbPoints Minimum number of points.
+     @param initialGridpoints Initial gridpoints to start the algorithm from. If
+            not provided, the path interval is used. If not empty, it must start
+            and end with the path interval limits.
+     @return The proposed gridpoints.
+
+   */
+  Vector proposeGridpoints(double maxErrThreshold=1e-4, int maxIteration=100, double maxSegLength=0.05, int minNbPoints=100, Vector initialGridpoints = Vector()) const;
+
+  /**
+   * \brief Dimension of the configuration space
+   */
+  int configSize() const
+  {
+    return m_configSize;
+  }
+
+  /**
+   * \return the number of degrees-of-freedom of the path.
+   */
+  int dof() const
+  {
+    return m_dof;
+  }
+
+  /**
+   * \brief Serialize path to stream.
+   */
+  virtual void serialize(std::ostream &O) const {};
+
+  /**
+   * \brief Deserialize stream to construct path.
+   */
+  virtual void deserialize(std::istream &I){};
+
+  /**
+   * \brief Starting and ending path positions.
+   */
+  virtual Bound pathInterval() const = 0;
+
+  virtual ~GeometricPath() {}
+
+ protected:
+  int m_configSize, m_dof;
+};
+
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/geometric_path/piecewise_poly_path.hpp

@@ -0,0 +1,161 @@
+#ifndef TOPPRA_PIECEWISE_POLY_PATH_HPP
+#define TOPPRA_PIECEWISE_POLY_PATH_HPP
+
+#include <array>
+#include <toppra/geometric_path.hpp>
+#include <toppra/toppra.hpp>
+#include <toppra/export.hpp>
+
+namespace toppra {
+
+
+struct BoundaryCond {
+  BoundaryCond() = default;
+
+  /**
+   * @brief Construct a new BoundaryCond object with a manually specified derivative.
+   * 
+   * @param order Order of the specified derivative.
+   * @param values Vector of values. Must have the same size as the path.
+   */
+  BoundaryCond(int order, const std::vector<value_type> &values);  
+
+  BoundaryCond(int order, const Vector values);
+
+  /**
+   * @brief Construct a new Boundary Cond object with well-known boundary condition.
+   * 
+   * @param bc_type Possible values: not-a-knot, clamped, natural and manual.
+   */
+  BoundaryCond(std::string bc_type); 
+
+  enum Type { NotAKnot, Clamped, Natural, Manual};
+  Type bc_type = NotAKnot;
+  int order = 0;
+  Vector values;
+};
+
+using BoundaryCondFull = std::array<BoundaryCond, 2>;
+
+
+/**
+ * \brief Piecewise polynomial geometric path.
+ *
+ * An implementation of a piecewise polynomial geometric path.
+ *
+ * The coefficient vector has shape (N, P, D), where N is the number
+ * segments. For each segment, the i-th row (P index) denotes the
+ * power, while the j-th column is the degree of freedom. In
+ * particular,
+ *
+ *  coeff(0) * dt ^ 3 + coeff(1) * dt ^ 2 + coeff(2) * dt + coeff(3)
+ *
+ *
+ */
+class PiecewisePolyPath : public GeometricPath {
+ public:
+  PiecewisePolyPath() = default;
+
+  /**
+   * \brief Construct new piecewise polynomial.
+   *
+   * See class docstring for details.
+   *
+   * @param coefficients Polynomial coefficients.
+   * @param breakpoints Vector of breakpoints.
+   */
+  PiecewisePolyPath(const Matrices &coefficients, std::vector<value_type> breakpoints);
+
+  /**
+   * /brief Evaluate the path at given position.
+   */
+  Vector eval_single(value_type, int order = 0) const override;
+
+  /**
+   * /brief Evaluate the path at given positions (vector).
+   */
+  Vectors eval(const Vector &, int order = 0) const override;
+
+  /**
+   * Return the starting and ending path positions.
+   */
+  Bound pathInterval() const override;
+  void serialize(std::ostream &O) const override;
+  void deserialize(std::istream &I) override;
+
+  /**
+   * @brief Construct a piecewise Cubic Hermite polynomial.
+   *
+   * See https://en.wikipedia.org/wiki/Cubic_Hermite_spline for a good
+   * description of this interplation scheme.
+   *
+   * This function is implemented based on scipy.interpolate.CubicHermiteSpline.
+   *
+   * Note that path generates by this function is not guaranteed to have
+   * continuous acceleration, or the path second-order derivative.
+   *
+   * @param positions Robot joints corresponding to the given times. Must have
+   * the same size as times.
+   * @param velocities Robot joint velocities.
+   * @param times Path positions or times. This is the independent variable.
+   * @return PiecewisePolyPath
+   */
+  static PiecewisePolyPath
+  CubicHermiteSpline(const Vectors &positions, const Vectors &velocities,
+                     const std::vector<value_type> times);
+
+  TOPPRA_DEPRECATED static PiecewisePolyPath
+  constructHermite(const Vectors &positions, const Vectors &velocities,
+                   const std::vector<value_type> times);
+
+
+  /**
+   * @brief Construct a cubic spline.
+   *
+   * Interpolate the given joint positions with a spline that is twice
+   * continuously differentiable. This means the position, velocity and
+   * acceleration are guaranteed to be continous but not jerk.
+   *
+   * This method is modelled after scipy.interpolate.CubicSpline.
+   *
+   * @param positions Robot joints corresponding to the given times. 
+   * @param times Path positions or times. This is the independent variable.
+   * @param bc_type Boundary condition. Currently on fixed boundary condition.
+   * @return PiecewisePolyPath
+   */
+  static PiecewisePolyPath CubicSpline(const Vectors &positions, const Vector &times, BoundaryCondFull bc_type);
+
+private:
+
+  /**
+   * @brief Calculate coefficients for Hermite spline.
+   * 
+   * @param positions See the constructor.
+   * @param velocities 
+   * @param times 
+   */
+  void initAsHermite(const Vectors &positions, const Vectors &velocities,
+                     const std::vector<value_type> times);
+
+protected:
+  static void computeCubicSplineCoefficients(const Vectors &positions,
+                                             const Vector &times,
+                                             const BoundaryCondFull &bc_type,
+                                             Matrices &coefficients);
+  // static void checkInputArgs(const Vectors &positions, const Vector &times,
+                            //  const BoundaryCondFull &bc_type);
+
+  // Cubic spline
+  void reset();
+  size_t findSegmentIndex(value_type pos) const;
+  void checkInputArgs();
+  void computeDerivativesCoefficients();
+  const Matrix &getCoefficient(size_t seg_index, int order) const;
+  Matrices m_coefficients, m_coefficients_1, m_coefficients_2;
+  std::vector<value_type> m_breakpoints;
+  int m_degree;
+};
+
+}  // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/parametrizer.hpp

@@ -0,0 +1,75 @@
+#ifndef TOPPRA_PARAMETRIZER_HPP
+#define TOPPRA_PARAMETRIZER_HPP
+
+#include <toppra/geometric_path.hpp>
+#include <toppra/toppra.hpp>
+
+namespace toppra {
+
+/**
+ * \brief Abstract output trajectory parametrizers.
+ *
+ * A parametrizer has the same interface as a geometric path. It
+ * receives as input the original geometric path and two arrays of
+ * gridpoints and parametrization (i.e. squared path velocities). A
+ * parametrizer should validate the input data. If not validated,
+ * evaluation results are not defined.
+ *
+ * Requirements: https://github.com/hungpham2511/toppra/issues/102
+ *
+ * Sub-classes should override the virtual private methods *_impl.
+ */
+class Parametrizer : public GeometricPath {
+ public:
+  /** Construct the parametrizer.
+   *
+   * \param path Input geometric path.
+   * \param gridpoints Shape (N+1,). Gridpoints of the parametrization, should be
+   * compatible with the path domain. \param vsquared Shape (N+1,). Path velocity
+   * squared, should have same shape as gridpoints as vsquared[i] corresponds to the
+   * velocity at gridpoints[i].
+   */
+  Parametrizer(GeometricPathPtr path, const Vector &gridpoints, const Vector &vsquared);
+
+  /** \brief Evaluate the path at given position.
+   */
+  Vector eval_single(value_type, int order = 0) const override;
+
+  /** \brief Evaluate the path at given positions (vector).
+   */
+  Vectors eval(const Vector &, int order = 0) const override;
+
+  /** \brief Return the starting and ending path positions.
+   */
+  Bound pathInterval() const override;
+
+  /** \brief Validate input data.
+   *
+   * Return false if something is wrong with the output
+   * trajectory. Only use the trajectory if validation successes.
+   */
+  bool validate() const;
+
+  virtual ~Parametrizer() {}
+
+  /** \brief Return the waypoint times.
+   */
+  virtual const Vector& getTimes() const = 0;
+
+ protected:
+  // Input geometric path
+  GeometricPathPtr m_path;
+  // Input gridpoints
+  Vector m_gridpoints;
+  // Input path velocities (not squared)
+  Vector m_vs;
+
+ private:
+  /// To be overwriten by derived classes
+  virtual Vectors eval_impl(const Vector &, int order = 0) const = 0;
+  virtual bool validate_impl() const = 0;
+  virtual Bound pathInterval_impl() const = 0;
+};
+};  // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/parametrizer/const_accel.hpp

@@ -0,0 +1,52 @@
+#ifndef TOPPRA_CONST_ACCEL_HPP
+#define TOPPRA_CONST_ACCEL_HPP
+
+#include <toppra/parametrizer.hpp>
+#include <toppra/toppra.hpp>
+
+namespace toppra {
+
+namespace parametrizer {
+
+/** \brief A path parametrizer with constant acceleration assumption.
+ *
+ * We assume that in each segment, the path acceleration is constant.
+ */
+class ConstAccel : public Parametrizer {
+ public:
+  ConstAccel(GeometricPathPtr path, const Vector &gridpoints, const Vector &vsquared);
+
+  const Vector& getTimes() const override { return m_ts; }
+ 
+ private:
+  /** Return joint derivatives at specified times. */
+  Vectors eval_impl(const Vector &times, int order = 0) const override;
+  bool validate_impl() const override;
+  Bound pathInterval_impl() const override;
+
+  /** \brief Evaluate path variables ss, vs, and us at the input time instances.
+   *
+   * For each t, a starting gridpoint will be selected. The selected
+   * path position, velocity and acceleration is then used to compute
+   * the corresponding output quantities.
+   *
+   * \param[in] ts Time instances to be evaluated, starting from zero seconds.
+   * \param[out] ss Path positions at the given time instances.
+   * \param[out] vs Path velocities at the given time instances.
+   * \param[out] us Path accelerations at the given time instances.
+   *
+   */
+  bool evalParams(const Vector &ts, Vector &ss, Vector &vs, Vector &us) const;
+  // Compute times and acclerations from given data (path, velocities)
+  void process_internals();
+  // Vector of time instances (corresponded to gridpoints)
+  Vector m_ts;
+  // Vector of accelerations (corresponded to gridpoints). Should have size
+  // shorter than m_ts and m_vs by 1.
+  Vector m_us;
+};
+}  // namespace parametrizer
+
+}  // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/parametrizer/spline.hpp

@@ -0,0 +1,47 @@
+#ifndef TOPPRA_SPLINE_HPP
+#define TOPPRA_SPLINE_HPP
+
+#include <toppra/parametrizer.hpp>
+#include <toppra/toppra.hpp>
+
+namespace toppra {
+
+namespace parametrizer {
+
+/** \brief A path parametrizer via a spline interpolation.
+ *
+ * This class computes the time and position at each gridpoint, then
+    fit and return a CubicSpline (continuous first and second
+    derivatives). Note that the boundary conditions are: first
+    derivatives at the start and end of the path equal q(s)' * s'.
+ */
+class Spline: public Parametrizer {
+public:
+    /**
+     * \brief Construct the spline parametrizer.
+     *
+     * See class docstring for details.
+     *
+     * @param path the input geometric path.
+     * @param gridpoints of the parametrization with shape (N+1,).
+     * @param vsquared the path velocity squared with shape (N+1,).
+    */
+    Spline(GeometricPathPtr path, const Vector &gridpoints, const Vector &vsquared);
+
+    const Vector& getTimes() const override { return m_ts; }
+
+private:
+    /** Return joint derivatives at specified times. */
+    Vectors eval_impl(const Vector &times, int order = 0) const override;
+    bool validate_impl() const override;
+    Bound pathInterval_impl() const override;
+
+    // Vector of time instances (corresponded to gridpoints)
+    Vector m_ts;
+};
+
+}  // namespace parametrizer
+
+}  // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/solver.hpp

@@ -0,0 +1,138 @@
+#ifndef TOPPRA_SOLVER_HPP
+#define TOPPRA_SOLVER_HPP
+
+#include <toppra/toppra.hpp>
+
+namespace toppra {
+
+/** \brief The base class for all solver wrappers.
+ *
+ *  All Solver can solve Linear/Quadratic Program subject to linear constraints
+ *  at the given stage, and possibly with additional auxiliary constraints.
+ *
+ *  All Solver derived class implements
+ *  - Solver::solveStagewiseOptim: core method needed by all Reachability
+ *    Analysis-based algorithms
+ *  - Solver::setupSolver, Solver::closeSolver: needed by some Solver
+ *    implementation, such as mosek and qpOASES with warmstart.
+ *
+ *  Note that some Solver only handle Linear Program while
+ *  some handle both.
+ *
+ *  Each solver wrapper should provide solver-specific constraint,
+ *  such as ultimate bound the variable u, x. For some solvers such as
+ *  ECOS, this is very important.
+ *
+ * */
+class Solver {
+  public:
+    /// \brief Create a solver based on the compilation option.
+    /// At the time of writing, the preference order is
+    /// - qpOASES
+    /// - GLPK
+    /// If none of these is available, this function returns a null pointer.
+    static SolverPtr createDefault();
+
+    /// \copydoc Solver::m_deltas
+    const Vector& deltas () const
+    {
+      return m_deltas;
+    }
+
+    /// \copydoc Solver::m_N
+    std::size_t nbStages () const
+    {
+      return m_N;
+    }
+
+    /// \copydoc Solver::m_nV
+    std::size_t nbVars () const
+    {
+      return m_nV;
+    }
+
+    /** Solve a stage-wise quadratic (or linear) optimization problem.
+     *
+     *  The quadratic optimization problem is described below:
+     *
+     *  \f{eqnarray}
+     *      \text{min  }  & 0.5 [u, x, v] H [u, x, v]^\top + [u, x, v] g    \\
+     *      \text{s.t.  } & [u, x] \text{ is feasible at stage } i          \\
+     *                    & x_{min} \leq x \leq x_{max}                     \\
+     *                    & x_{next, min} \leq x + 2 \Delta_i u \leq x_{next, max},
+     *  \f}
+     *
+     *  where `v` is an auxiliary variable, only exist if there are
+     *  non-canonical constraints.  The linear program is the
+     *  quadratic problem without the quadratic term.
+     *
+     *  \param i The stage index.
+     *  \param H Either a matrix of size (d, d), where d is \ref nbVars, in
+     *           which case a quadratic objective is defined, or a matrix
+     *           of size (0,0), in which case a linear objective is defined.
+     *  \param g Vector of size \ref nbVars. The linear term.
+     *  \param[out] solution in case of success, stores the optimal solution.
+     *
+     *  \return whether the resolution is successful, in which case \c solution
+     *          contains the optimal solution.
+     *  */
+    virtual bool solveStagewiseOptim(std::size_t i,
+        const Matrix& H, const Vector& g,
+        const Bound& x, const Bound& xNext,
+        Vector& solution) = 0;
+
+    /// \brief Initialize the solver
+    /// \note Child classes should call the parent implementation.
+    virtual void initialize (const LinearConstraintPtrs& constraints, const GeometricPathPtr& path,
+        const Vector& times);
+
+    /** \brief Initialize the wrapped solver
+     */
+    virtual void setupSolver ()
+    {}
+
+    /** \brief Free the wrapped solver
+     */
+    virtual void closeSolver ()
+    {}
+
+    virtual ~Solver () {}
+
+  protected:
+    Solver () {}
+
+    void init (const LinearConstraintPtrs& constraints, const GeometricPathPtr& path,
+        const Vector& times);
+
+    struct LinearConstraintParams {
+      int cid;
+      Vectors a, b, c, g;
+      Matrices F;
+    };
+    struct BoxConstraintParams {
+      int cid;
+      Bounds u, x;
+    };
+    struct ConstraintsParams {
+      std::vector<LinearConstraintParams> lin;
+      std::vector<BoxConstraintParams   > box;
+    } m_constraintsParams;
+
+    LinearConstraintPtrs m_constraints;
+    GeometricPathPtr m_path;
+    Vector m_times;
+
+  private:
+    /// \brief Number of stages.
+    /// The number of gridpoints equals N + 1, where N is the number of stages.
+    std::size_t m_N;
+    /// Total number of variables, including u, x.
+    std::size_t m_nV;
+    /// Time increment between each stage. Size \ref nbStages
+    Vector m_deltas;
+
+}; // class Solver
+
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/solver/glpk-wrapper.hpp

@@ -0,0 +1,54 @@
+#ifndef TOPPRA_SOLVER_GLPK_WRAPPER_HPP
+#define TOPPRA_SOLVER_GLPK_WRAPPER_HPP
+
+#include <memory.h>
+#include <toppra/solver.hpp>
+
+// Forward declare glpk solver.
+struct glp_prob;
+
+namespace toppra {
+namespace solver {
+
+/** Wrapper around GLPK library.
+ *
+ *  Internally, the problem is formulated as
+ *  \f{eqnarray}
+ *  min   & g^T y       \\
+ *  s.t   & z = A y     \\
+ *        & x_{min} <= x <= x_{max} \\
+ *        & l_1 <= z <= h_1 \\
+ *  \f}
+ *  where
+ *  \f{eqnarray}
+ *  y =& \begin{pmatrix} u & x \end{pmatrix}^T     \\
+ *  A =& \begin{pmatrix}
+ *     2\delta &       1 \\
+ *     F_i a_i & F_i b_i \\
+ *     \vdots  & \vdots \\
+ *     \end{pmatrix}\\
+ *  \f}
+ *
+ * */
+class GLPKWrapper : public Solver {
+  public:
+    GLPKWrapper () = default;
+
+    void initialize (const LinearConstraintPtrs& constraints, const GeometricPathPtr& path,
+        const Vector& times);
+
+    bool solveStagewiseOptim(std::size_t i,
+        const Matrix& H, const Vector& g,
+        const Bound& x, const Bound& xNext,
+        Vector& solution);
+
+    virtual ~GLPKWrapper();
+
+  private:
+    glp_prob* m_lp = NULL;
+}; // class GLPKWrapper
+
+} // namespace solver
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/solver/qpOASES-wrapper.hpp

@@ -0,0 +1,66 @@
+#ifndef TOPPRA_SOLVER_QPOASES_WRAPPER_HPP
+#define TOPPRA_SOLVER_QPOASES_WRAPPER_HPP
+
+#include <memory.h>
+#include <toppra/solver.hpp>
+
+namespace toppra {
+namespace solver {
+
+/** Wrapper around qpOASES::SQProblem
+ *
+ *  Internally, the problem is formulated as
+ *  \f{eqnarray}
+ *  min   & 0.5 y^T H y + g^T y \\
+ *  s.t   & lA <= Ay <= hA      \\
+ *        & l  <=  y <= h       \\
+ *  \f}
+ *
+ *  \todo Add a solver that inherits from qpOASESWrapper and that uses the warm
+ *  start capabilities of qpOASES
+ *
+ * */
+class qpOASESWrapper : public Solver {
+  public:
+    qpOASESWrapper ();
+
+    void initialize (const LinearConstraintPtrs& constraints, const GeometricPathPtr& path,
+        const Vector& times);
+
+    bool solveStagewiseOptim(std::size_t i,
+        const Matrix& H, const Vector& g,
+        const Bound& x, const Bound& xNext,
+        Vector& solution);
+
+    virtual ~qpOASESWrapper();
+
+    value_type setBoundary () const
+    {
+      return m_boundary;
+    }
+
+    void setBoundary (const value_type& v)
+    {
+      m_boundary = v;
+    }
+
+    static void setDefaultBoundary (const value_type& v);
+
+  private:
+    /// qpOASES uses row-major storage order.
+    typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic,
+            Eigen::RowMajor> RMatrix;
+    RMatrix m_H, m_A;
+    Vector m_lA, m_hA;
+    value_type m_boundary;
+
+    static value_type m_defaultBoundary;
+
+    struct Impl;
+    std::unique_ptr<Impl> m_impl;
+}; // class qpOASESWrapper
+
+} // namespace solver
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/solver/seidel.hpp

@@ -0,0 +1,43 @@
+#ifndef TOPPRA_SOLVER_SEIDEL_HPP
+#define TOPPRA_SOLVER_SEIDEL_HPP
+
+#include <memory>
+#include <array>
+#include <toppra/solver.hpp>
+
+namespace toppra {
+namespace solver {
+
+/** Implementation of Seidel algorithm.
+ *
+ * */
+class Seidel : public Solver {
+  public:
+    Seidel () = default;
+
+    void initialize (const LinearConstraintPtrs& constraints, const GeometricPathPtr& path,
+        const Vector& times);
+
+    bool solveStagewiseOptim(std::size_t i,
+        const Matrix& H, const Vector& g,
+        const Bound& x, const Bound& xNext,
+        Vector& solution);
+
+  private:
+    typedef Eigen::Matrix<value_type, Eigen::Dynamic, 2> MatrixX2;
+    typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3> MatrixX3;
+    typedef std::vector<MatrixX3, Eigen::aligned_allocator<MatrixX3> > MatricesX3;
+
+    MatricesX3 m_A;
+    MatrixX2 m_low, m_high;
+
+    MatrixX3 m_A_ordered;
+    MatrixX2 m_A_1d;
+    std::vector<int> m_index_map;
+    std::array<int, 2> m_active_c_up, m_active_c_down;
+}; // class Seidel
+
+} // namespace solver
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/include/toppra/toppra.hpp

@@ -0,0 +1,86 @@
+#ifndef TOPPRA_TOPPRA_HPP
+#define TOPPRA_TOPPRA_HPP
+
+#include <iostream>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+
+#include <toppra/export.hpp>
+
+
+#ifdef TOPPRA_DEBUG_ON
+#define TOPPRA_LOG_DEBUG(X) std::cout << "[DEBUG]: " <<  X << std::endl
+#else
+#define TOPPRA_LOG_DEBUG(X) ((void)0)
+#endif
+
+#if defined(TOPPRA_DEBUG_ON) || defined(TOPPRA_WARN_ON)
+#define TOPPRA_LOG_WARN(X) std::cout << "[WARN]: " <<  X << std::endl
+#else
+#define TOPPRA_LOG_WARN(X) ((void)0)
+#endif
+
+#define TOPPRA_UNUSED(x) (void)(x)
+
+// Use for checking if a quantity is very close to zero
+#define TOPPRA_NEARLY_ZERO 1e-8
+#define TOPPRA_REL_TOL 1e-6
+#define TOPPRA_ABS_TOL 1e-8
+
+
+/// The TOPP-RA namespace
+namespace toppra {
+  /// The scalar type
+  typedef double value_type;
+
+  constexpr value_type infty = std::numeric_limits<value_type>::infinity();
+
+  /// Column vector type
+  typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1> Vector;
+  /// Matrix type
+  typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> Matrix;
+
+  /// Vector of Vector
+  typedef std::vector<Vector, Eigen::aligned_allocator<Vector> > Vectors;
+  /// Vector of Matrix
+  typedef std::vector<Matrix, Eigen::aligned_allocator<Matrix> > Matrices;
+
+  // internal data rep for Matrix
+  typedef std::tuple<Eigen::Index, Eigen::Index, std::vector<value_type>> MatrixData;
+  typedef std::vector<MatrixData> MatricesData;
+
+  /// 2D vector that stores the upper and lower bound of a variable.
+  typedef Eigen::Matrix<value_type, 1, 2> Bound;
+  /// Vector of Bound
+  typedef std::vector<Bound, Eigen::aligned_allocator<Bound> > Bounds;
+
+  class LinearConstraint;
+  /// Shared pointer to a LinearConstraint
+  typedef std::shared_ptr<LinearConstraint> LinearConstraintPtr;
+  /// Vector of LinearConstraintPtr
+  typedef std::vector<LinearConstraintPtr> LinearConstraintPtrs;
+  namespace constraint {
+    class LinearJointVelocity;
+    class LinearJointAcceleration;
+    class JointTorque;
+  } // namespace constraint
+
+  class Solver;
+  typedef std::shared_ptr<Solver> SolverPtr;
+  namespace solver {
+    class qpOASESWrapper;
+  } // namespace solver
+
+  class GeometricPath;
+  typedef std::shared_ptr<GeometricPath> GeometricPathPtr;
+
+  class PathParametrizationAlgorithm;
+  typedef std::shared_ptr<PathParametrizationAlgorithm> PathParametrizationAlgorithmPtr;
+
+} // namespace toppra
+
+#endif

third_party/toppra/0.6.2/lib/cmake/toppra/toppraConfig.cmake

@@ -0,0 +1,13 @@
+set(TOPPRA_WITH_PINOCCHIO OFF)
+set(TOPPRA_WITH_qpOASES   OFF)
+set(TOPPRA_WITH_GLPK      OFF)
+
+find_package(Eigen3 REQUIRED)
+
+if(TOPPRA_WITH_PINOCCHIO)
+  find_package(pinocchio REQUIRED)
+endif()
+# qpOASES and GLPK do not need to be found again, because
+# they are not used as targets.
+
+include("${CMAKE_CURRENT_LIST_DIR}/toppraTargets.cmake")

third_party/toppra/0.6.2/lib/cmake/toppra/toppraConfigVersion.cmake

@@ -0,0 +1,65 @@
+# This is a basic version file for the Config-mode of find_package().
+# It is used by write_basic_package_version_file() as input file for configure_file()
+# to create a version-file which can be installed along a config.cmake file.
+#
+# The created file sets PACKAGE_VERSION_EXACT if the current version string and
+# the requested version string are exactly the same and it sets
+# PACKAGE_VERSION_COMPATIBLE if the current version is >= requested version,
+# but only if the requested major version is the same as the current one.
+# The variable CVF_VERSION must be set before calling configure_file().
+
+
+set(PACKAGE_VERSION "0.6.2")
+
+if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION)
+  set(PACKAGE_VERSION_COMPATIBLE FALSE)
+else()
+
+  if("0.6.2" MATCHES "^([0-9]+)\\.")
+    set(CVF_VERSION_MAJOR "${CMAKE_MATCH_1}")
+    if(NOT CVF_VERSION_MAJOR VERSION_EQUAL 0)
+      string(REGEX REPLACE "^0+" "" CVF_VERSION_MAJOR "${CVF_VERSION_MAJOR}")
+    endif()
+  else()
+    set(CVF_VERSION_MAJOR "0.6.2")
+  endif()
+
+  if(PACKAGE_FIND_VERSION_RANGE)
+    # both endpoints of the range must have the expected major version
+    math (EXPR CVF_VERSION_MAJOR_NEXT "${CVF_VERSION_MAJOR} + 1")
+    if (NOT PACKAGE_FIND_VERSION_MIN_MAJOR STREQUAL CVF_VERSION_MAJOR
+        OR ((PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL "INCLUDE" AND NOT PACKAGE_FIND_VERSION_MAX_MAJOR STREQUAL CVF_VERSION_MAJOR)
+          OR (PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL "EXCLUDE" AND NOT PACKAGE_FIND_VERSION_MAX VERSION_LESS_EQUAL CVF_VERSION_MAJOR_NEXT)))
+      set(PACKAGE_VERSION_COMPATIBLE FALSE)
+    elseif(PACKAGE_FIND_VERSION_MIN_MAJOR STREQUAL CVF_VERSION_MAJOR
+        AND ((PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL "INCLUDE" AND PACKAGE_VERSION VERSION_LESS_EQUAL PACKAGE_FIND_VERSION_MAX)
+        OR (PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL "EXCLUDE" AND PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION_MAX)))
+      set(PACKAGE_VERSION_COMPATIBLE TRUE)
+    else()
+      set(PACKAGE_VERSION_COMPATIBLE FALSE)
+    endif()
+  else()
+    if(PACKAGE_FIND_VERSION_MAJOR STREQUAL CVF_VERSION_MAJOR)
+      set(PACKAGE_VERSION_COMPATIBLE TRUE)
+    else()
+      set(PACKAGE_VERSION_COMPATIBLE FALSE)
+    endif()
+
+    if(PACKAGE_FIND_VERSION STREQUAL PACKAGE_VERSION)
+      set(PACKAGE_VERSION_EXACT TRUE)
+    endif()
+  endif()
+endif()
+
+
+# if the installed or the using project don't have CMAKE_SIZEOF_VOID_P set, ignore it:
+if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "" OR "8" STREQUAL "")
+  return()
+endif()
+
+# check that the installed version has the same 32/64bit-ness as the one which is currently searching:
+if(NOT CMAKE_SIZEOF_VOID_P STREQUAL "8")
+  math(EXPR installedBits "8 * 8")
+  set(PACKAGE_VERSION "${PACKAGE_VERSION} (${installedBits}bit)")
+  set(PACKAGE_VERSION_UNSUITABLE TRUE)
+endif()

third_party/toppra/0.6.2/lib/cmake/toppra/toppraTargets-debug.cmake

@@ -0,0 +1,19 @@
+#----------------------------------------------------------------
+# Generated CMake target import file for configuration "Debug".
+#----------------------------------------------------------------
+
+# Commands may need to know the format version.
+set(CMAKE_IMPORT_FILE_VERSION 1)
+
+# Import target "toppra::toppra" for configuration "Debug"
+set_property(TARGET toppra::toppra APPEND PROPERTY IMPORTED_CONFIGURATIONS DEBUG)
+set_target_properties(toppra::toppra PROPERTIES
+  IMPORTED_LOCATION_DEBUG "${_IMPORT_PREFIX}/lib/libtoppra.so"
+  IMPORTED_SONAME_DEBUG "libtoppra.so"
+  )
+
+list(APPEND _cmake_import_check_targets toppra::toppra )
+list(APPEND _cmake_import_check_files_for_toppra::toppra "${_IMPORT_PREFIX}/lib/libtoppra.so" )
+
+# Commands beyond this point should not need to know the version.
+set(CMAKE_IMPORT_FILE_VERSION)

third_party/toppra/0.6.2/lib/cmake/toppra/toppraTargets.cmake

@@ -0,0 +1,107 @@
+# Generated by CMake
+
+if("${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}" LESS 2.8)
+   message(FATAL_ERROR "CMake >= 2.8.0 required")
+endif()
+if(CMAKE_VERSION VERSION_LESS "2.8.12")
+   message(FATAL_ERROR "CMake >= 2.8.12 required")
+endif()
+cmake_policy(PUSH)
+cmake_policy(VERSION 2.8.12...3.28)
+#----------------------------------------------------------------
+# Generated CMake target import file.
+#----------------------------------------------------------------
+
+# Commands may need to know the format version.
+set(CMAKE_IMPORT_FILE_VERSION 1)
+
+# Protect against multiple inclusion, which would fail when already imported targets are added once more.
+set(_cmake_targets_defined "")
+set(_cmake_targets_not_defined "")
+set(_cmake_expected_targets "")
+foreach(_cmake_expected_target IN ITEMS toppra::toppra)
+  list(APPEND _cmake_expected_targets "${_cmake_expected_target}")
+  if(TARGET "${_cmake_expected_target}")
+    list(APPEND _cmake_targets_defined "${_cmake_expected_target}")
+  else()
+    list(APPEND _cmake_targets_not_defined "${_cmake_expected_target}")
+  endif()
+endforeach()
+unset(_cmake_expected_target)
+if(_cmake_targets_defined STREQUAL _cmake_expected_targets)
+  unset(_cmake_targets_defined)
+  unset(_cmake_targets_not_defined)
+  unset(_cmake_expected_targets)
+  unset(CMAKE_IMPORT_FILE_VERSION)
+  cmake_policy(POP)
+  return()
+endif()
+if(NOT _cmake_targets_defined STREQUAL "")
+  string(REPLACE ";" ", " _cmake_targets_defined_text "${_cmake_targets_defined}")
+  string(REPLACE ";" ", " _cmake_targets_not_defined_text "${_cmake_targets_not_defined}")
+  message(FATAL_ERROR "Some (but not all) targets in this export set were already defined.\nTargets Defined: ${_cmake_targets_defined_text}\nTargets not yet defined: ${_cmake_targets_not_defined_text}\n")
+endif()
+unset(_cmake_targets_defined)
+unset(_cmake_targets_not_defined)
+unset(_cmake_expected_targets)
+
+
+# Compute the installation prefix relative to this file.
+get_filename_component(_IMPORT_PREFIX "${CMAKE_CURRENT_LIST_FILE}" PATH)
+get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH)
+if(_IMPORT_PREFIX STREQUAL "/")
+  set(_IMPORT_PREFIX "")
+endif()
+
+# Create imported target toppra::toppra
+add_library(toppra::toppra SHARED IMPORTED)
+
+set_target_properties(toppra::toppra PROPERTIES
+  INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include"
+  INTERFACE_LINK_LIBRARIES "Eigen3::Eigen"
+)
+
+# Load information for each installed configuration.
+file(GLOB _cmake_config_files "${CMAKE_CURRENT_LIST_DIR}/toppraTargets-*.cmake")
+foreach(_cmake_config_file IN LISTS _cmake_config_files)
+  include("${_cmake_config_file}")
+endforeach()
+unset(_cmake_config_file)
+unset(_cmake_config_files)
+
+# Cleanup temporary variables.
+set(_IMPORT_PREFIX)
+
+# Loop over all imported files and verify that they actually exist
+foreach(_cmake_target IN LISTS _cmake_import_check_targets)
+  if(CMAKE_VERSION VERSION_LESS "3.28"
+      OR NOT DEFINED _cmake_import_check_xcframework_for_${_cmake_target}
+      OR NOT IS_DIRECTORY "${_cmake_import_check_xcframework_for_${_cmake_target}}")
+    foreach(_cmake_file IN LISTS "_cmake_import_check_files_for_${_cmake_target}")
+      if(NOT EXISTS "${_cmake_file}")
+        message(FATAL_ERROR "The imported target \"${_cmake_target}\" references the file
+   \"${_cmake_file}\"
+but this file does not exist.  Possible reasons include:
+* The file was deleted, renamed, or moved to another location.
+* An install or uninstall procedure did not complete successfully.
+* The installation package was faulty and contained
+   \"${CMAKE_CURRENT_LIST_FILE}\"
+but not all the files it references.
+")
+      endif()
+    endforeach()
+  endif()
+  unset(_cmake_file)
+  unset("_cmake_import_check_files_for_${_cmake_target}")
+endforeach()
+unset(_cmake_target)
+unset(_cmake_import_check_targets)
+
+# This file does not depend on other imported targets which have
+# been exported from the same project but in a separate export set.
+
+# Commands beyond this point should not need to know the version.
+set(CMAKE_IMPORT_FILE_VERSION)
+cmake_policy(POP)