refactor: optimize IK solver code

cmvr-es/common/CMakeLists.txt

@@ -3,6 +3,7 @@
 
 file(GLOB SRC
         ${CMAKE_CURRENT_SOURCE_DIR}/utils/config_helper/src/config_setting.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/curve/src/s_curve.cpp
 )
 
 

cmvr-es/common/curve/include/s_curve.h

@@ -0,0 +1,302 @@
+//
+// Created by lgv on 2026/3/2.
+//
+/**
+ * @file s_curve.h
+ * @brief 标准 7 段式 S 曲线轨迹生成器（用于平滑运动控制）
+ * 
+ * 实现标准 7 段式 S 曲线速度剖面：
+ * 1. 加加速度上升段（加速度逐渐增大）
+ * 2. 匀加速段
+ * 3. 加加速度下降段（加速度逐渐降到 0）
+ * 4. 匀速巡航段
+ * 5. 加加速度下降段（减速度逐渐增大）
+ * 6. 匀减速段
+ * 7. 加加速度上升段（减速度逐渐降到 0）
+ * 
+ * 特性：
+ * - 支持摇杆等实时增量更新
+ * - 支持生成完整轨迹（用于 MoveIt 路径再参数化）
+ * - 支持配置 max_velocity / max_acceleration / max_jerk
+ */
+
+#pragma once
+
+#include <cmath>
+#include <algorithm>
+#include <vector>
+
+
+namespace cmvr
+{
+
+/**
+ * @brief 单次点到点运动的 S 曲线剖面参数
+ */
+struct SCurveProfile
+{
+  // 7 个阶段的持续时间
+  double t1;  // 加加速度上升时间（加速阶段）
+  double t2;  // 匀加速时间
+  double t3;  // 加加速度下降时间（加速阶段结束）
+  double t4;  // 匀速时间（巡航）
+  double t5;  // 加加速度下降时间（减速阶段）
+  double t6;  // 匀减速时间
+  double t7;  // 加加速度上升时间（减速阶段结束）
+  
+  double total_time;
+  
+  // 运动约束
+  double j_max;   // 最大加加速度 (rad/s³)
+  double a_max;   // 最大加速度 (rad/s²)
+  double v_max;   // 最大速度 (rad/s)
+  
+  // 运动参数
+  double distance;      // 运动总距离
+  double direction;     // +1 或 -1
+  double v_cruise;      // 实际达到的巡航速度
+  double a_limit;       // 实际达到的加速度上限
+  
+  // 初始条件
+  double p0;  // 初始位置
+  double v0;  // 初始速度
+  double a0;  // 初始加速度
+  
+  SCurveProfile()
+    : t1(0), t2(0), t3(0), t4(0), t5(0), t6(0), t7(0)
+    , total_time(0)
+    , j_max(50.0), a_max(10.0), v_max(3.0)
+    , distance(0), direction(1.0), v_cruise(0), a_limit(0)
+    , p0(0), v0(0), a0(0)
+  {}
+};
+
+/**
+ * @brief 实时增量 S 曲线控制的状态量
+ */
+struct SCurveState
+{
+  double position;
+  double velocity;
+  double acceleration;
+  double jerk;
+  
+  // 目标跟踪
+  double target_position;
+  bool is_moving;
+  
+  SCurveState()
+    : position(0), velocity(0), acceleration(0), jerk(0)
+    , target_position(0), is_moving(false)
+  {}
+};
+
+/**
+ * @brief S 曲线轨迹生成器
+ * 
+ * 提供两种工作模式：
+ * 1. 实时模式：根据目标变化持续更新位置
+ * 2. 轨迹模式：针对给定位移生成完整轨迹
+ */
+class SCurve
+{
+public:
+  /**
+   * @brief 构造函数（带运动约束）
+   * @param max_velocity 最大速度 (rad/s)
+   * @param max_acceleration 最大加速度 (rad/s²)
+   * @param max_jerk 最大加加速度 (rad/s³)
+   */
+  SCurve(double max_velocity = 3.0, 
+                  double max_acceleration = 10.0, 
+                  double max_jerk = 50.0);
+  
+  /**
+   * @brief 设置运动约束
+   */
+  void setConstraints(double max_velocity, double max_acceleration, double max_jerk);
+  
+  /**
+   * @brief 获取当前运动约束
+   */
+  void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const;
+  
+  /**
+   * @brief 使用当前状态初始化生成器
+   * @param position 当前位置
+   * @param velocity 当前速度（默认 0）
+   * @param acceleration 当前加速度（默认 0）
+   */
+  void initialize(double position, double velocity = 0.0, double acceleration = 0.0);
+  
+  /**
+   * @brief 重置为初始状态
+   */
+  void reset();
+  
+  // ==================== 实时模式 ====================
+  
+  /**
+   * @brief 更新目标位置（实时模式）
+   * @param target_position 新目标位置
+   */
+  void setTarget(double target_position);
+  
+  /**
+   * @brief 使用 S 曲线剖面计算下一步位置（实时模式）
+   * @param dt 时间步长（秒）
+   * @return 更新后的位置
+   * 
+   * 本方法通过以下步骤实现实时 S 曲线控制：
+   * 1. 根据位置误差计算期望速度
+   * 2. 限制加速度变化（加加速度限制）
+   * 3. 限制加速度
+   * 4. 限制速度
+   * 5. 积分得到新的位置
+   */
+  double update(double dt);
+  
+  /**
+   * @brief 获取当前状态
+   */
+  const SCurveState& getState() const { return state_; }
+  
+  /**
+   * @brief 获取当前位置
+   */
+  double getPosition() const { return state_.position; }
+  
+  /**
+   * @brief 获取当前速度
+   */
+  double getVelocity() const { return state_.velocity; }
+  
+  /**
+   * @brief 获取当前加速度
+   */
+  double getAcceleration() const { return state_.acceleration; }
+  
+  /**
+   * @brief 是否处于运动状态
+   */
+  bool isMoving() const { return state_.is_moving; }
+  
+  // ==================== 轨迹模式 ====================
+  
+  /**
+   * @brief 计算点到点运动的完整 S 曲线剖面
+   * @param start_position 起始位置
+   * @param end_position 终止位置
+   * @param start_velocity 起始速度（默认 0）
+   * @param end_velocity 终止速度（默认 0）
+   * @return 计算得到的剖面参数
+   */
+  SCurveProfile calculateProfile(double start_position, double end_position,
+                                  double start_velocity = 0.0, double end_velocity = 0.0);
+  
+  /**
+   * @brief 给定剖面下，获取时刻 t 的位置
+   * @param profile S 曲线剖面
+   * @param t 从起点开始的时间
+   * @return 时刻 t 的位置
+   */
+  double getPositionAtTime(const SCurveProfile& profile, double t) const;
+  
+  /**
+   * @brief 给定剖面下，获取时刻 t 的速度
+   * @param profile S 曲线剖面
+   * @param t 从起点开始的时间
+   * @return 时刻 t 的速度
+   */
+  double getVelocityAtTime(const SCurveProfile& profile, double t) const;
+  
+  /**
+   * @brief 给定剖面下，获取时刻 t 的加速度
+   * @param profile S 曲线剖面
+   * @param t 从起点开始的时间
+   * @return 时刻 t 的加速度
+   */
+  double getAccelerationAtTime(const SCurveProfile& profile, double t) const;
+  
+  /**
+   * @brief 按固定时间间隔生成轨迹点
+   * @param profile S 曲线剖面
+   * @param dt 轨迹点时间间隔
+   * @param positions 输出：每个时间点的位置
+   * @param velocities 输出：每个时间点的速度
+   * @param accelerations 输出：每个时间点的加速度
+   */
+  void generateTrajectory(const SCurveProfile& profile, double dt,
+                          std::vector<double>& positions,
+                          std::vector<double>& velocities,
+                          std::vector<double>& accelerations) const;
+
+private:
+  // 运动约束
+  double max_velocity_;
+  double max_acceleration_;
+  double max_jerk_;
+  
+  // 实时状态
+  SCurveState state_;
+  
+  // 实时模式的位置跟踪增益
+  double position_gain_;
+  
+  // 小量阈值
+  static constexpr double EPSILON = 1e-9;
+  static constexpr double VELOCITY_THRESHOLD = 1e-6;
+  static constexpr double POSITION_THRESHOLD = 1e-7;
+  
+  /**
+   * @brief 在最大减速度约束下，计算减速到 0 的时间
+   */
+  double calculateStoppingTime(double velocity, double acceleration) const;
+  
+  /**
+   * @brief 按 S 曲线减速剖面计算制动距离
+   */
+  double calculateStoppingDistance(double velocity, double acceleration) const;
+  
+  /**
+   * @brief 计算带加加速度限制的加速度更新
+   */
+  double computeJerkLimitedAcceleration(double current_acc, double desired_acc, double dt) const;
+  
+  /**
+   * @brief 判断所处的段并计算对应加加速度
+   */
+  double computeSegmentJerk(const SCurveProfile& profile, double t) const;
+  
+  /**
+   * @brief 计算短距离剖面（可能达不到最大速度）
+   */
+  void calculateShortProfile(SCurveProfile& profile) const;
+  
+  /**
+   * @brief 计算长距离剖面（可达到最大速度）
+   */
+  void calculateLongProfile(SCurveProfile& profile) const;
+  
+  /**
+   * @brief 将数值夹紧到指定范围
+   */
+  static double clamp(double value, double min_val, double max_val)
+  {
+    return std::max(min_val, std::min(max_val, value));
+  }
+  
+  /**
+   * @brief 符号函数
+   */
+  static double sign(double value)
+  {
+    if (value > EPSILON) return 1.0;
+    if (value < -EPSILON) return -1.0;
+    return 0.0;
+  }
+};
+
+}  // namespace 
+
+

cmvr-es/common/curve/src/s_curve.cpp

@@ -0,0 +1,655 @@
+//
+// Created by lgv on 2026/3/2.
+//
+
+
+/**
+ * @file s_curve_generator.cpp
+ * @brief S 曲线轨迹生成器实现
+ */
+
+#include "common/curve/include/s_curve.h"
+#include <cmath>
+#include <algorithm>
+#include <stdexcept>
+
+namespace cmvr
+{
+
+SCurve::SCurve(double max_velocity, double max_acceleration, double max_jerk)
+  : max_velocity_(max_velocity)
+  , max_acceleration_(max_acceleration)
+  , max_jerk_(max_jerk)
+  , position_gain_(10.0)  // 位置跟踪增益
+{
+  reset();
+}
+
+void SCurve::setConstraints(double max_velocity, double max_acceleration, double max_jerk)
+{
+  max_velocity_ = std::abs(max_velocity);
+  max_acceleration_ = std::abs(max_acceleration);
+  max_jerk_ = std::abs(max_jerk);
+}
+
+void SCurve::getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const
+{
+  max_velocity = max_velocity_;
+  max_acceleration = max_acceleration_;
+  max_jerk = max_jerk_;
+}
+
+void SCurve::initialize(double position, double velocity, double acceleration)
+{
+  state_.position = position;
+  state_.velocity = velocity;
+  state_.acceleration = acceleration;
+  state_.jerk = 0.0;
+  state_.target_position = position;
+  state_.is_moving = false;
+}
+
+void SCurve::reset()
+{
+  state_ = SCurveState();
+}
+
+void SCurve::setTarget(double target_position)
+{
+  state_.target_position = target_position;
+}
+
+double SCurve::update(double dt)
+{
+  if (dt <= 0.0 || dt > 0.1) {
+    return state_.position;
+  }
+  
+  // 计算位置误差
+  double position_error = state_.target_position - state_.position;
+  
+  // 判断是否已足够接近目标
+  if (std::abs(position_error) < POSITION_THRESHOLD && 
+      std::abs(state_.velocity) < VELOCITY_THRESHOLD) {
+    state_.velocity = 0.0;
+    state_.acceleration = 0.0;
+    state_.jerk = 0.0;
+    state_.is_moving = false;
+    state_.position = state_.target_position;
+    return state_.position;
+  }
+  
+  state_.is_moving = true;
+  
+  // ==================== S 曲线实时算法 ====================
+  // 通过加加速度限制实现实时 S 曲线控制
+  
+  // Step 1：根据位置误差计算期望速度
+  // 使用比例控制并施加速度上限
+  double desired_velocity = position_gain_ * position_error;
+  desired_velocity = clamp(desired_velocity, -max_velocity_, max_velocity_);
+  
+  // Step 2：根据当前速度估算制动距离（考虑 S 曲线减速剖面）
+  double stopping_distance = calculateStoppingDistance(state_.velocity, state_.acceleration);
+  
+  // Step 3：判断是否需要开始减速
+  double direction = sign(position_error);
+  double velocity_direction = sign(state_.velocity);
+  
+  // 如果正在远离目标，或距离不足以刹停，则需要减速
+  bool need_decelerate = false;
+  if (velocity_direction != 0 && velocity_direction != direction) {
+    // 运动方向错误：必须减速
+    need_decelerate = true;
+  } else if (std::abs(position_error) <= std::abs(stopping_distance) * 1.1) {
+    // 接近目标：提前开始减速（留 10% 裕量）
+    need_decelerate = true;
+  }
+  
+  // Step 4：计算期望加速度
+  double desired_acceleration;
+  if (need_decelerate) {
+    // 减速：加速度方向应与速度相反
+    if (std::abs(state_.velocity) < VELOCITY_THRESHOLD) {
+      desired_acceleration = 0.0;
+    } else {
+      desired_acceleration = -sign(state_.velocity) * max_acceleration_;
+    }
+  } else {
+    // 朝目标方向加速
+    double velocity_error = desired_velocity - state_.velocity;
+    desired_acceleration = clamp(velocity_error / dt, -max_acceleration_, max_acceleration_);
+  }
+  
+  // Step 5：施加加加速度限制（S 曲线平滑性的关键）
+  double new_acceleration = computeJerkLimitedAcceleration(
+    state_.acceleration, desired_acceleration, dt);
+  
+  // Step 6：更新加加速度
+  state_.jerk = (new_acceleration - state_.acceleration) / dt;
+  
+  // Step 7：更新加速度
+  state_.acceleration = new_acceleration;
+  
+  // Step 8：在加速度限制下更新速度
+  double new_velocity = state_.velocity + state_.acceleration * dt;
+  new_velocity = clamp(new_velocity, -max_velocity_, max_velocity_);
+  state_.velocity = new_velocity;
+  
+  // Step 9：更新位置
+  // 使用梯形积分提高精度
+  double avg_velocity = (state_.velocity + new_velocity) * 0.5;
+  state_.position += avg_velocity * dt;
+  
+  // Step 10：非常接近目标且速度很小则直接夹紧到目标
+  if (std::abs(state_.target_position - state_.position) < POSITION_THRESHOLD * 10 &&
+      std::abs(state_.velocity) < VELOCITY_THRESHOLD * 10) {
+    state_.position = state_.target_position;
+  }
+  
+  return state_.position;
+}
+
+double SCurve::calculateStoppingTime(double velocity, double acceleration) const
+{
+  // 将加速度减到 0 所需时间
+  double t_jerk = std::abs(acceleration) / max_jerk_;
+  
+  // 加加速度阶段的速度变化量
+  double v_change_jerk = 0.5 * std::abs(acceleration) * t_jerk;
+  
+  // 加加速度阶段结束后的剩余速度
+  double v_remaining = std::abs(velocity) - v_change_jerk;
+  
+  if (v_remaining <= 0) {
+    // 仅靠加加速度阶段即可停止
+    return std::sqrt(2.0 * std::abs(velocity) / max_jerk_);
+  }
+  
+  // 需要进入匀减速阶段
+  // 匀减速阶段持续时间
+  double t_const = v_remaining / max_acceleration_;
+  
+  // 最后一个加加速度阶段：将加速度带回到 0
+  double t_jerk_final = max_acceleration_ / max_jerk_;
+  
+  return t_jerk + t_const + t_jerk_final;
+}
+
+double SCurve::calculateStoppingDistance(double velocity, double acceleration) const
+{
+  double v = std::abs(velocity);
+  double a = std::abs(acceleration);
+  
+  if (v < VELOCITY_THRESHOLD) {
+    return 0.0;
+  }
+  
+  // 简化版 S 曲线制动距离估算
+  // 完整推导需要考虑 7 个阶段；实时控制中使用近似即可获得平滑减速效果
+  
+  // 将加速度减到 0 的时间（加加速度阶段 1）
+  double t1 = a / max_jerk_;
+  double d1 = v * t1 + 0.5 * a * t1 * t1 - (1.0/6.0) * max_jerk_ * t1 * t1 * t1;
+  double v1 = v + a * t1 - 0.5 * max_jerk_ * t1 * t1;
+  
+  if (v1 <= 0) {
+    // 在第一段加加速度阶段内停止
+    return std::abs(d1);
+  }
+  
+  // 匀减速阶段
+  double t2 = (v1 - max_acceleration_ * max_acceleration_ / (2.0 * max_jerk_)) / max_acceleration_;
+  if (t2 < 0) t2 = 0;
+  double d2 = v1 * t2 - 0.5 * max_acceleration_ * t2 * t2;
+  double v2 = v1 - max_acceleration_ * t2;
+  
+  // 最后一个加加速度阶段
+  double t3 = max_acceleration_ / max_jerk_;
+  double d3 = v2 * t3 - 0.5 * max_acceleration_ * t3 * t3 + (1.0/6.0) * max_jerk_ * t3 * t3 * t3;
+  
+  double total_distance = d1 + d2 + d3;
+  
+  // 增加安全裕量
+  return std::abs(total_distance) * 1.2;
+}
+
+double SCurve::computeJerkLimitedAcceleration(double current_acc, double desired_acc, double dt) const
+{
+  double acc_change = desired_acc - current_acc;
+  double max_acc_change = max_jerk_ * dt;
+  
+  // 按加加速度上限夹紧加速度变化量
+  acc_change = clamp(acc_change, -max_acc_change, max_acc_change);
+  
+  double new_acc = current_acc + acc_change;
+  
+  // 同时夹紧加速度本身
+  new_acc = clamp(new_acc, -max_acceleration_, max_acceleration_);
+  
+  return new_acc;
+}
+
+// ==================== 轨迹模式实现 ====================
+
+SCurveProfile SCurve::calculateProfile(double start_position, double end_position,
+                                                 double start_velocity, double end_velocity [[maybe_unused]])
+{
+  SCurveProfile profile;
+  profile.j_max = max_jerk_;
+  profile.a_max = max_acceleration_;
+  profile.v_max = max_velocity_;
+  profile.p0 = start_position;
+  profile.v0 = start_velocity;
+  profile.a0 = 0.0;  // Assume starting from zero acceleration
+  
+  double displacement = end_position - start_position;
+  profile.distance = std::abs(displacement);
+  profile.direction = (displacement >= 0) ? 1.0 : -1.0;
+  
+  if (profile.distance < EPSILON) {
+    // 无需运动
+    profile.total_time = 0.0;
+    return profile;
+  }
+  
+  // 达到最大加速度所需时间（加加速度阶段）
+  double t_j = profile.a_max / profile.j_max;
+  
+  // 单个加加速度阶段获得的速度增量
+  double v_j = 0.5 * profile.j_max * t_j * t_j;
+  
+  // 加速阶段（含两个加加速度段）的位移
+  // 假设能够达到最大加速度
+  double d_acc = v_j * t_j + profile.a_max * t_j * t_j + v_j;
+  
+  // 判断是否能达到最大速度
+  double v_acc = 2 * v_j;  // Velocity gained during full acceleration phase
+  
+  if (v_acc >= profile.v_max) {
+    // 达不到最大加速度：三角形剖面
+    calculateShortProfile(profile);
+  } else {
+    // 判断是否能在半程之前达到最大速度
+    double d_to_vmax = d_acc + (profile.v_max - v_acc) * (profile.v_max - v_acc) / (2.0 * profile.a_max);
+    
+    if (2.0 * d_to_vmax > profile.distance) {
+      // 达不到最大速度：梯形加速度剖面
+      calculateShortProfile(profile);
+    } else {
+      // 可达到最大速度：完整 S 曲线
+      calculateLongProfile(profile);
+    }
+  }
+  
+  return profile;
+}
+
+void SCurve::calculateShortProfile(SCurveProfile& profile) const
+{
+  // 短距离情况下，可能达不到最大速度，甚至达不到最大加速度
+  // 使用对称剖面：t1=t3=t5=t7，t2=t6，t4=0
+  
+  double j = profile.j_max;
+  double a = profile.a_max;
+  // double v = profile.v_max;  // 短剖面不会达到最大速度，因此不使用
+  double d = profile.distance;
+  
+  // 达到最大加速度所需时间
+  double t_j = a / j;
+  
+  // 判断是否能达到最大加速度
+  // 纯加加速度剖面（t1=t3，无匀加速）的位移
+  double d_jerk_only = j * t_j * t_j * t_j / 3.0;
+  
+  if (d < 2.0 * d_jerk_only) {
+    // 极短距离：纯加加速度剖面
+    double t1 = std::cbrt(d * 1.5 / j);
+    profile.t1 = t1;
+    profile.t2 = 0.0;
+    profile.t3 = t1;
+    profile.t4 = 0.0;
+    profile.t5 = t1;
+    profile.t6 = 0.0;
+    profile.t7 = t1;
+    profile.v_cruise = j * t1 * t1;  // 峰值速度
+    profile.a_limit = j * t1;        // 峰值加速度
+  } else {
+    // 需要匀加速阶段
+    // 在位移约束下求解 t1 与 t2
+    // d = 2 * (v_j * t_j + 0.5 * a * t_j^2 + a * t_a * t_j + 0.5 * a * t_a^2)
+    
+    profile.t1 = t_j;
+    profile.t3 = t_j;
+    profile.t5 = t_j;
+    profile.t7 = t_j;
+    
+    // t1 结束时速度
+    double v1 = 0.5 * j * t_j * t_j;
+    
+    // 4 个加加速度段覆盖的位移
+    double d_jerk = 4.0 * (v1 * t_j / 2.0 + j * t_j * t_j * t_j / 6.0);
+    
+    // 匀加速/匀减速阶段的剩余位移
+    double d_const = d - d_jerk;
+    
+    // 求解 t2（匀加速时间）
+    // 使用一元二次方程
+    double A = a;
+    double B = 2.0 * v1 + a * t_j;
+    double C = -d_const / 2.0;
+    
+    double discriminant = B * B - 4.0 * A * C;
+    if (discriminant < 0) discriminant = 0;
+    
+    double t2 = (-B + std::sqrt(discriminant)) / (2.0 * A);
+    if (t2 < 0) t2 = 0;
+    
+    profile.t2 = t2;
+    profile.t6 = t2;
+    profile.t4 = 0.0;  // No cruise phase
+    
+    profile.v_cruise = v1 + a * (t_j + t2);
+    profile.a_limit = a;
+  }
+  
+  profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 +
+                       profile.t5 + profile.t6 + profile.t7;
+}
+
+void SCurve::calculateLongProfile(SCurveProfile& profile) const
+{
+  // 含巡航段的完整 7 段式 S 曲线
+  double j = profile.j_max;
+  double a = profile.a_max;
+  double v = profile.v_max;
+  double d = profile.distance;
+  
+  // 加加速度段持续时间
+  double t_j = a / j;
+  
+  // 加速阶段加加速度段获得的速度增量
+  double v_j = 0.5 * j * t_j * t_j;
+  
+  // 匀加速达到最大速度所需时间
+  double t_a = (v - 2.0 * v_j) / a;
+  if (t_a < 0) t_a = 0;
+  
+  profile.t1 = t_j;
+  profile.t2 = t_a;
+  profile.t3 = t_j;
+  profile.t5 = t_j;
+  profile.t6 = t_a;
+  profile.t7 = t_j;
+  
+  // 加速阶段（t1+t2+t3）覆盖的位移
+  double d_acc = v_j * t_j + 0.5 * a * t_j * t_j +  // t1
+                 (v_j + 0.5 * a * t_j) * t_a + 0.5 * a * t_a * t_a +  // t2
+                 (v_j + a * t_j + a * t_a) * t_j + 0.5 * a * t_j * t_j - j * t_j * t_j * t_j / 6.0;  // t3
+  
+  // 减速阶段对称
+  double d_dec = d_acc;
+  
+  // 巡航段位移
+  double d_cruise = d - d_acc - d_dec;
+  if (d_cruise < 0) d_cruise = 0;
+  
+  // 巡航段时间
+  profile.t4 = d_cruise / v;
+  
+  profile.v_cruise = v;
+  profile.a_limit = a;
+  
+  profile.total_time = profile.t1 + profile.t2 + profile.t3 + profile.t4 +
+                       profile.t5 + profile.t6 + profile.t7;
+}
+
+double SCurve::computeSegmentJerk(const SCurveProfile& profile, double t) const
+{
+  double j = profile.j_max * profile.direction;
+  
+  // 判断所处阶段
+  double t_end1 = profile.t1;
+  double t_end2 = t_end1 + profile.t2;
+  double t_end3 = t_end2 + profile.t3;
+  double t_end4 = t_end3 + profile.t4;
+  double t_end5 = t_end4 + profile.t5;
+  double t_end6 = t_end5 + profile.t6;
+  // double t_end7 = t_end6 + profile.t7;  // = total_time
+  
+  if (t < t_end1) {
+    return j;  // 段 1：正加加速度（加速）
+  } else if (t < t_end2) {
+    return 0.0;  // 段 2：加加速度为 0（匀加速）
+  } else if (t < t_end3) {
+    return -j;  // 段 3：负加加速度（减小加速度）
+  } else if (t < t_end4) {
+    return 0.0;  // 段 4：加加速度为 0（巡航）
+  } else if (t < t_end5) {
+    return -j;  // 段 5：负加加速度（开始减速）
+  } else if (t < t_end6) {
+    return 0.0;  // 段 6：加加速度为 0（匀减速）
+  } else {
+    return j;  // 段 7：正加加速度（减速结束）
+  }
+}
+
+double SCurve::getPositionAtTime(const SCurveProfile& profile, double t) const
+{
+  if (t <= 0) return profile.p0;
+  if (t >= profile.total_time) return profile.p0 + profile.distance * profile.direction;
+  
+  double j = profile.j_max * profile.direction;
+  double p = profile.p0;
+  double v = profile.v0;
+  double a = profile.a0;
+  
+  // 时间边界
+  double t_end1 = profile.t1;
+  double t_end2 = t_end1 + profile.t2;
+  double t_end3 = t_end2 + profile.t3;
+  double t_end4 = t_end3 + profile.t4;
+  double t_end5 = t_end4 + profile.t5;
+  double t_end6 = t_end5 + profile.t6;
+  
+  // 依次处理各阶段
+  auto processSegment = [&](double dt, double jerk) {
+    p += v * dt + 0.5 * a * dt * dt + (1.0/6.0) * jerk * dt * dt * dt;
+    v += a * dt + 0.5 * jerk * dt * dt;
+    a += jerk * dt;
+  };
+  
+  // 段 1
+  if (t <= t_end1) {
+    processSegment(t, j);
+    return p;
+  }
+  processSegment(profile.t1, j);
+  
+  // 段 2
+  if (t <= t_end2) {
+    processSegment(t - t_end1, 0.0);
+    return p;
+  }
+  processSegment(profile.t2, 0.0);
+  
+  // 段 3
+  if (t <= t_end3) {
+    processSegment(t - t_end2, -j);
+    return p;
+  }
+  processSegment(profile.t3, -j);
+  
+  // 段 4（巡航）
+  if (t <= t_end4) {
+    processSegment(t - t_end3, 0.0);
+    return p;
+  }
+  processSegment(profile.t4, 0.0);
+  
+  // 段 5
+  if (t <= t_end5) {
+    processSegment(t - t_end4, -j);
+    return p;
+  }
+  processSegment(profile.t5, -j);
+  
+  // 段 6
+  if (t <= t_end6) {
+    processSegment(t - t_end5, 0.0);
+    return p;
+  }
+  processSegment(profile.t6, 0.0);
+  
+  // 段 7
+  processSegment(t - t_end6, j);
+  return p;
+}
+
+double SCurve::getVelocityAtTime(const SCurveProfile& profile, double t) const
+{
+  if (t <= 0) return profile.v0;
+  if (t >= profile.total_time) return 0.0;  // 假设末端静止
+  
+  double j = profile.j_max * profile.direction;
+  double v = profile.v0;
+  double a = profile.a0;
+  
+  // 时间边界
+  double t_end1 = profile.t1;
+  double t_end2 = t_end1 + profile.t2;
+  double t_end3 = t_end2 + profile.t3;
+  double t_end4 = t_end3 + profile.t4;
+  double t_end5 = t_end4 + profile.t5;
+  double t_end6 = t_end5 + profile.t6;
+  
+  auto processSegment = [&](double dt, double jerk) {
+    v += a * dt + 0.5 * jerk * dt * dt;
+    a += jerk * dt;
+  };
+  
+  if (t <= t_end1) {
+    v += a * t + 0.5 * j * t * t;
+    return v;
+  }
+  processSegment(profile.t1, j);
+  
+  if (t <= t_end2) {
+    v += a * (t - t_end1);
+    return v;
+  }
+  processSegment(profile.t2, 0.0);
+  
+  if (t <= t_end3) {
+    double dt = t - t_end2;
+    v += a * dt + 0.5 * (-j) * dt * dt;
+    return v;
+  }
+  processSegment(profile.t3, -j);
+  
+  if (t <= t_end4) {
+    v += a * (t - t_end3);
+    return v;
+  }
+  processSegment(profile.t4, 0.0);
+  
+  if (t <= t_end5) {
+    double dt = t - t_end4;
+    v += a * dt + 0.5 * (-j) * dt * dt;
+    return v;
+  }
+  processSegment(profile.t5, -j);
+  
+  if (t <= t_end6) {
+    v += a * (t - t_end5);
+    return v;
+  }
+  processSegment(profile.t6, 0.0);
+  
+  double dt = t - t_end6;
+  v += a * dt + 0.5 * j * dt * dt;
+  return v;
+}
+
+double SCurve::getAccelerationAtTime(const SCurveProfile& profile, double t) const
+{
+  if (t <= 0 || t >= profile.total_time) return 0.0;
+  
+  double j = profile.j_max * profile.direction;
+  double a = profile.a0;
+  
+  // 时间边界
+  double t_end1 = profile.t1;
+  double t_end2 = t_end1 + profile.t2;
+  double t_end3 = t_end2 + profile.t3;
+  double t_end4 = t_end3 + profile.t4;
+  double t_end5 = t_end4 + profile.t5;
+  double t_end6 = t_end5 + profile.t6;
+  
+  if (t <= t_end1) {
+    return a + j * t;
+  }
+  a += j * profile.t1;
+  
+  if (t <= t_end2) {
+    return a;
+  }
+  
+  if (t <= t_end3) {
+    return a + (-j) * (t - t_end2);
+  }
+  a += (-j) * profile.t3;
+  
+  if (t <= t_end4) {
+    return a;  // Should be ~0
+  }
+  
+  if (t <= t_end5) {
+    return a + (-j) * (t - t_end4);
+  }
+  a += (-j) * profile.t5;
+  
+  if (t <= t_end6) {
+    return a;
+  }
+  
+  return a + j * (t - t_end6);
+}
+
+void SCurve::generateTrajectory(const SCurveProfile& profile, double dt,
+                                          std::vector<double>& positions,
+                                          std::vector<double>& velocities,
+                                          std::vector<double>& accelerations) const
+{
+  positions.clear();
+  velocities.clear();
+  accelerations.clear();
+  
+  if (profile.total_time <= 0 || dt <= 0) {
+    positions.push_back(profile.p0);
+    velocities.push_back(profile.v0);
+    accelerations.push_back(profile.a0);
+    return;
+  }
+  
+  int num_points = static_cast<int>(std::ceil(profile.total_time / dt)) + 1;
+  positions.reserve(num_points);
+  velocities.reserve(num_points);
+  accelerations.reserve(num_points);
+  
+  for (double t = 0; t <= profile.total_time; t += dt) {
+    positions.push_back(getPositionAtTime(profile, t));
+    velocities.push_back(getVelocityAtTime(profile, t));
+    accelerations.push_back(getAccelerationAtTime(profile, t));
+  }
+  
+  // 确保包含最终点
+  if (positions.empty() || 
+      std::abs(positions.back() - (profile.p0 + profile.distance * profile.direction)) > EPSILON) {
+    positions.push_back(profile.p0 + profile.distance * profile.direction);
+    velocities.push_back(0.0);
+    accelerations.push_back(0.0);
+  }
+}
+
+}  // namespace el_a3_hardware

cmvr-es/controller/include/ibvs_controller.h

@@ -258,6 +258,8 @@ private:
 private:
     // 是否初始化成功。
     bool initialized_{false};
+    // 速度 IK 使用的基座 frame 名称。
+    std::string base_frame_name_;
     // 速度 IK 使用的末端相机 frame 名称。
     std::string camera_frame_name_;
     // 相机对象。
@@ -325,9 +327,9 @@ private:
     // 是否成功读取到 URDF 关节限位。
     bool has_joint_position_limits_{false};
     // 本链关节位置下限（rad）。
-    std::vector<double> q_lower_limits_;
+    Eigen::VectorXd q_lower_limits_;
     // 本链关节位置上限（rad）。
-    std::vector<double> q_upper_limits_;
+    Eigen::VectorXd q_upper_limits_;
     // 最近一次用于控制的 tag id。
     int last_used_tag_id_{-1};
 

cmvr-es/controller/src/ibvs_controller.cpp

@@ -77,6 +77,7 @@ bool IbvsController::init(const std::shared_ptr<device::AbstractCamera>& camera,
         return false;
     }
 
+    base_frame_name_ = base_link;
     camera_frame_name_ = camera_link;
     dls_solver_ = std::make_unique<PinocchioDlsIKSolver>(
         urdf_path, base_link, flange_link, camera_frame_name_, 100, 1e-6, 1e-6, mu_);
@@ -91,8 +92,8 @@ bool IbvsController::init(const std::shared_ptr<device::AbstractCamera>& camera,
             dls_solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_);
     } else {
         has_joint_position_limits_ = false;
-        q_lower_limits_.clear();
+        q_lower_limits_.resize(0);
-        q_upper_limits_.clear();
+        q_upper_limits_.resize(0);
     }
     last_compute_status_ = initialized_ ? ComputeStatus::OK : ComputeStatus::NOT_READY;
     return initialized_;
@@ -303,9 +304,11 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
     twist_ee_pin.head<3>() = R_camera_urdf_ * twist_ee.head<3>();
     twist_ee_pin.tail<3>() = R_camera_urdf_ * twist_ee.tail<3>();
 
+    dls_solver_->update_joints_state(joints_angle);
     std::vector<double> qdot;
-    const bool ok = dls_solver_->velocityIk(
+    const bool ok = dls_solver_->ik(
-        joints_angle, twist_ee_pin, qdot, camera_frame_name_, mu_, std::numeric_limits<double>::infinity());
+        base_frame_name_, camera_frame_name_, twist_ee_pin,
+        qdot, mu_, std::numeric_limits<double>::infinity());
     if (!ok || qdot.size() != joints_angle.size()) {
         last_compute_status_ = ComputeStatus::IK_FAILED;
         return false;
@@ -362,7 +365,8 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
 
 void IbvsController::clampJointCommandInPlace(std::vector<double>& q) const {
     if (!has_joint_position_limits_) return;
-    if (q.size() != q_lower_limits_.size() || q.size() != q_upper_limits_.size()) return;
+    if (q.size() != static_cast<size_t>(q_lower_limits_.size()) ||
+        q.size() != static_cast<size_t>(q_upper_limits_.size())) return;
 
     for (size_t i = 0; i < q.size(); ++i) {
         const double lo = q_lower_limits_[i];

cmvr-es/ik_solver/CMakeLists.txt

@@ -1,6 +1,9 @@
 
 add_library(ik_solver SHARED
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/ik_solver.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/src/ik_solver_creator.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/urdf_parser.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_ik_base.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_dls_ik_solver.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/src/pinocchio_qp_ik_solver.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/src/bias_srs_ik_slover.cpp
@@ -62,5 +65,3 @@ target_link_libraries(ik_test
         glog
         cmvr_es::proto
 )
-
-install(TARGETS srs_ik_test RUNTIME DESTINATION bin)

cmvr-es/ik_solver/include/ik_solver.h

@@ -6,12 +6,33 @@
 #include <common/consts/constant.h>
 #include "ik_solver.h"
 #include <vector>
+#include <string>
+#include <memory>
 #include <Eigen/Core>
 
 namespace cmvr {
+    class UrdfParser;
+
     class IKSolver {
     public:
-        IKSolver()=default;
+        /**
+         * @brief 构造时按 URDF 路径初始化链路缓存。
+         * @param urdf_path URDF 文件路径。
+         * @param base_frame_name 链路基座 frame 名称。
+         * @param tip_frame_name 链路末端 frame 名称。
+         */
+        IKSolver(const std::string& urdf_path,
+                 const std::string& base_frame_name,
+                 const std::string& tip_frame_name);
+        /**
+         * @brief 构造时按外部解析器初始化链路缓存。
+         * @param parser 已加载模型的 URDF 解析器。
+         * @param base_frame_name 链路基座 frame 名称。
+         * @param tip_frame_name 链路末端 frame 名称。
+         */
+        IKSolver(std::shared_ptr<const UrdfParser> parser,
+                 const std::string& base_frame_name,
+                 const std::string& tip_frame_name);
         virtual ~IKSolver()=default;
 
         virtual bool init() {
@@ -43,30 +64,79 @@ namespace cmvr {
             cur_joints_angle_ = std::move(cur_joints_angle);
         }
 
+        /**
+         * @brief 获取当前缓存链路的关节位置限位。
+         * @param lower 输出下限，单位 rad。
+         * @param upper 输出上限，单位 rad。
+         * @return 成功返回 true，失败返回 false。
+         */
+        bool getJointPositionLimits(Eigen::VectorXd& lower,
+                                    Eigen::VectorXd& upper) const;
+
+        /**
+         * @brief 获取当前缓存链路的关节速度限位。
+         * @param limits 输出速度上限（绝对值），单位 rad/s。
+         * @return 成功返回 true，失败返回 false。
+         */
+        bool getJointVelocityLimits(Eigen::VectorXd& limits) const;
+
+        /**
+         * @brief 获取当前缓存链路的关节名称（base->tip 顺序）。
+         * @param names 输出关节名称列表。
+         * @return 成功返回 true，失败返回 false。
+         */
+        bool getChainJointNames(std::vector<std::string>& names) const;
+
 
     protected:
-        //机械臂urdf中末端工具坐标系相对于MDH中末端法兰的变换矩阵
+        /**
-        Eigen::Matrix4d T_tool_flange_{};
+         * @brief 使用 URDF 路径初始化链路信息缓存（关节限位/关节名）。
+         * @param urdf_path URDF 文件路径。
+         * @param base_frame_name 链路基座 frame 名称。
+         * @param tip_frame_name 链路末端 frame 名称。
+         * @return 成功返回 true，失败返回 false。
+         */
+        bool initUrdfChain(const std::string& urdf_path,
+                           const std::string& base_frame_name,
+                           const std::string& tip_frame_name);
 
-        // 机械臂MDH基座相对于机器人urdf基座的变换矩阵
+        /**
-        Eigen::Matrix4d T_arm_robot_{};
+         * @brief 使用外部 UrdfParser 初始化链路信息缓存（关节限位/关节名）。
-
+         * @param parser 已加载模型的 URDF 解析器。
-        //  机械臂urdf 中法兰姿态相对于 MDH 中法兰姿态的变换矩阵
+         * @param base_frame_name 链路基座 frame 名称。
-         Eigen::Matrix4d T_flange_urdf_mdh_{};
+         * @param tip_frame_name 链路末端 frame 名称。
+         * @return 成功返回 true，失败返回 false。
+         */
+        bool initUrdfChain(const std::shared_ptr<const UrdfParser>& parser,
+                           const std::string& base_frame_name,
+                           const std::string& tip_frame_name);
 
         // 上次调用ik 时计算的结果
         std::vector<double> cur_joints_angle_{};
 
-        // 设置 机械臂末端工具坐标系相对于末端法兰的变换矩阵
+        // URDF 解析器（用于统一管理链路限位/关节名）。
-        void setTcpTransform(const Eigen::Matrix4d& T_tool_flange) {
+        std::shared_ptr<const UrdfParser> urdf_parser_{};
-            T_tool_flange_ = T_tool_flange;
-        }
 
+        // 当前缓存链路的基座 frame 名称。
+        std::string chain_base_frame_name_{};
 
+        // 当前缓存链路的末端 frame 名称。
+        std::string chain_tip_frame_name_{};
 
-        void setArmBaseTransform(const Eigen::Matrix4d& T_arm_robot) {
+        // 当前缓存链路关节位置下限（rad）。
-            T_arm_robot_ = T_arm_robot;
+        Eigen::VectorXd joint_pos_lower_limits_{};
-        }
+
+        // 当前缓存链路关节位置上限（rad）。
+        Eigen::VectorXd joint_pos_upper_limits_{};
+
+        // 当前缓存链路关节速度上限（rad/s, 绝对值）。
+        Eigen::VectorXd joint_vel_limits_{};
+
+        // 当前缓存链路关节名称（base->tip 顺序）。
+        std::vector<std::string> chain_joint_names_{};
+
+        // 链路缓存是否有效。
+        bool urdf_chain_cached_{false};
 
     };
 }

cmvr-es/ik_solver/include/lawba_ik_solver.h

@@ -42,6 +42,15 @@ public:
     }
 
 private:
+    // 机械臂 URDF 中末端工具坐标系相对于 MDH 中末端法兰的变换矩阵。
+    Eigen::Matrix4d T_tool_flange_{Eigen::Matrix4d::Identity()};
+
+    // 机械臂 MDH 基座相对于机器人 URDF 基座的变换矩阵。
+    Eigen::Matrix4d T_arm_robot_{Eigen::Matrix4d::Identity()};
+
+    // 机械臂 URDF 中法兰姿态相对于 MDH 中法兰姿态的变换矩阵。
+    Eigen::Matrix4d T_flange_urdf_mdh_{Eigen::Matrix4d::Identity()};
+
     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};

cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h

@@ -1,11 +1,7 @@
 // Created by lgv on 11/28/25.
 #pragma once
 
-#include "ik_solver/include/ik_solver.h"
+#include "ik_solver/include/pinocchio_ik_base.h"
-
-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/multibody/data.hpp>
-#include <pinocchio/spatial/se3.hpp>
 
 #include <Eigen/Core>
 #include <limits>
@@ -15,8 +11,31 @@
 
 namespace cmvr {
 
-class PinocchioDlsIKSolver : public IKSolver {
+/**
+ * @brief 基于 Pinocchio 的 DLS 逆运动学求解器。
+ *
+ * 提供以下能力：
+ * - 位姿 IK（`ik`）
+ * - 正运动学 FK（`fk`）
+ * - 末端 twist 到关节速度的微分 IK（`ik` 重载）
+ * - 直线 MoveL（S 曲线标量规划 + 局部系微分 IK）
+ *
+ * 求解链路由 URDF 中 `base_frame_name -> flange_frame_name` 自动提取，
+ * 并支持可选 TCP frame。
+ */
+class PinocchioDlsIKSolver : public PinocchioIKBase {
 public:
+    /**
+     * @brief 构造 `PinocchioDlsIKSolver`。
+     * @param urdf_path URDF 文件路径。
+     * @param base_frame_name 机械臂基座 frame 名称。
+     * @param flange_frame_name 机械臂法兰 frame 名称。
+     * @param tcp_frame_name TCP frame 名称；为空表示不使用 TCP。
+     * @param max_iters IK 最大迭代次数。
+     * @param pos_eps 平移收敛阈值（m）。
+     * @param rot_eps 旋转收敛阈值（rad）。
+     * @param damping DLS 阻尼系数，建议默认值大于 `1e-6` 以提升稳定性。
+     */
     PinocchioDlsIKSolver(const std::string &urdf_path,
                          const std::string &base_frame_name,
                          const std::string &flange_frame_name,
@@ -28,26 +47,57 @@ public:
 
     ~PinocchioDlsIKSolver() override = default;
 
+    /**
+     * @brief 初始化求解器（加载 URDF、提取链路、缓存限位与基座位姿）。
+     * @return 成功返回 `true`，失败返回 `false`。
+     */
     bool init() override;
 
-    // target_pose: base 坐标系下的目标位姿
+    /**
+     * @brief 位姿 IK 求解。
+     * @param target_pose base 坐标系下目标位姿。
+     * @param joints_angle 输出关节角（链空间）。
+     * @param is_tcp 为 `true` 时以 TCP 为末端，否则以 flange 为末端。
+     * @return 成功返回 `true`，失败返回 `false`。
+     */
     bool ik(const Eigen::Matrix4d &target_pose,
             std::vector<double> &joints_angle,
             bool is_tcp = true) override;
 
-    // cur_pose: base 坐标系下当前位姿
+    /**
-    bool fk(const std::vector<double> &joints_angle,
+     * @brief 位姿 IK 求解（显式指定 base_link 与 ee_link）。
-            Eigen::Matrix4d &cur_pose,
+     *
-            bool is_tcp = true) override;
+     * 约束：`base_link` 与 `ee_link` 必须位于初始化单一链（base->tip）
-
+     * 对应分支内，且 `base_link` 需是 `ee_link` 的祖先（或同一 link）。
-    // 指定 base_link 与 ee_link，返回 ee 在 base 下的位姿。
+     *
-    bool fk(const std::string& base_link,
+     * @param base_link 基座 frame 名称（目标位姿参考系）。
+     * @param ee_link 末端 frame 名称。
+     * @param target_pose `ee_link` 在 `base_link` 下的目标位姿。
+     * @param joints_angle 输出关节角（链空间）。
+     * @return 成功返回 `true`，失败返回 `false`。
+     */
+    bool ik(const std::string& base_link,
             const std::string& ee_link,
-            const std::vector<double>& joints_angle,
+            const Eigen::Matrix4d& target_pose,
-            Eigen::Matrix4d& cur_pose);
+            std::vector<double>& joints_angle);
 
-    // MoveL：S 曲线（限 jerk）速度规划 + 微分IK（LOCAL）生成 q(t)
+    /**
-    // target_pose_base: base 下目标位姿（只用平移；姿态保持起点姿态）
+     * @brief 生成 MoveL 关节轨迹（局部系微分 IK + S 曲线标量规划）。
+     *
+     * 该接口仅跟踪平移直线，姿态保持起点姿态。
+     *
+     * @param target_pose_base base 坐标系下目标位姿（平移有效，姿态仅用于接口一致性）。
+     * @param q_start 起始关节角（链空间）。
+     * @param q_traj 输出关节轨迹序列。
+     * @param t_traj 输出时间戳序列（秒）。
+     * @param dt 控制周期（秒）。
+     * @param v_tcp_max TCP 线速度上限（m/s）。
+     * @param a_tcp_max TCP 线加速度上限（m/s^2）。
+     * @param j_tcp_max TCP 线加加速度上限（m/s^3）。
+     * @param qd_max 关节速度上限（rad/s），长度应等于 `chain_dof_`。
+     * @param is_tcp 为 `true` 时以 TCP 为末端，否则以 flange 为末端。
+     * @return 成功返回 `true`，失败返回 `false`。
+     */
     bool moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_base,
                            const std::vector<double>& q_start,
                            std::vector<std::vector<double>>& q_traj,
@@ -60,23 +110,24 @@ public:
                            bool is_tcp = true);
 
     /**
-     * @brief 用 DLS 微分逆解将末端局部坐标系（如相机系）twist 映射为本链关节速度。
+     * @brief 用 DLS 微分逆解将末端局部坐标系 twist 映射为关节速度。
      *
-     * @param cur_angle 当前关节角，支持 size==chain_dof_ 或 size==model_.nq。
+     * 当前关节状态取自内部 `cur_joints_angle_`（调用前需先 `update_joints_state`）。
-     * @param ee_velocity 末端期望速度 [vx vy vz wx wy wz]^T，单位 m/s 与 rad/s，坐标系为 ee_frame_name 局部系。
+     *
-     * @param joints_vel 输出的关节速度，size=chain_v_dof_，单位 rad/s。
+     * @param base_link 基坐标系 frame 名称。
      * @param ee_link 末端 frame 名称。
+     * @param target_vel 末端期望速度 [vx vy vz wx wy wz]^T，单位 m/s 与 rad/s，坐标系为 `ee_link` 局部系。
+     * @param joints_vel 输出的关节速度，size=chain_v_dof_，单位 rad/s。
      * @param damping DLS 阻尼系数，<=0 时使用成员 damping_。
      * @param qdot_abs_max 关节速度绝对值限幅；同时会叠加 URDF velocityLimit。
-     * @return true 计算成功；false 输入非法或 frame 不存在等失败。
+     * @return 成功返回 `true`，失败返回 `false`。
-     * @note 函数内部会将 ee 局部 twist 转到 base 坐标系，并在 base 坐标系中完成 DLS 求解。
      */
-    bool velocityIk(const std::vector<double>& cur_angle,
+    bool ik(const std::string& base_link,
-                    const Eigen::Matrix<double,6,1>& ee_velocity,
+            const std::string& ee_link,
-                    std::vector<double>& joints_vel,
+            const Eigen::Matrix<double,6,1>& target_vel,
-                    const std::string& ee_link,
+            std::vector<double>& joints_vel,
-                    double damping = -1.0,
+            double damping = -1.0,
-                    double qdot_abs_max = std::numeric_limits<double>::infinity());
+            double qdot_abs_max = std::numeric_limits<double>::infinity());
 
     /**
      * @brief 配置关节限位避障 null-space 项。
@@ -92,64 +143,103 @@ public:
                                 double max_push = 0.25);
 
     /**
-     * @brief 读取本链关节位置限位（来自 URDF）。
+     * @brief 设置 IK 最大迭代次数。
-     * @param lower 输出下限，size=chain_dof_。
+     * @param iters 最大迭代次数。
-     * @param upper 输出上限，size=chain_dof_。
-     * @return 成功返回 `true`。
      */
-    bool getJointPositionLimits(std::vector<double>& lower,
-                                std::vector<double>& upper) const;
-
     void setMaxIters(int iters) { max_iters_ = iters; }
+    /**
+     * @brief 设置 DLS 阻尼系数。
+     * @param d 阻尼系数。
+     */
     void setDamping(double d)   { damping_ = d; }
+    /**
+     * @brief 设置 IK 收敛阈值。
+     * @param pos_eps 平移阈值（m）。
+     * @param rot_eps 旋转阈值（rad）。
+     */
     void setEps(double pos_eps, double rot_eps) { pos_eps_ = pos_eps; rot_eps_ = rot_eps; }
 
-    using IKSolver::setTcpTransform;
-    using IKSolver::setArmBaseTransform;
-
 private:
-    static pinocchio::SE3 matrix4ToSE3(const Eigen::Matrix4d &T);
+    /**
-    static Eigen::Matrix4d se3ToMatrix4(const pinocchio::SE3 &M);
+     * @brief 计算阻尼伪逆。
+     * @param J 输入雅可比矩阵。
+     * @param lambda 阻尼系数。
+     * @return 阻尼伪逆矩阵。
+     */
     Eigen::MatrixXd dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda);
 
+    /**
+     * @brief 将链关节向量组装为 full-model `q`。
+     * @param q_chain 输入链关节向量。
+     * @param q_full 输出 full-model 关节向量。
+     * @param context 调用上下文（用于日志）。
+     * @return 组装成功返回 `true`。
+     */
+    bool buildFullQFromChain(const Eigen::VectorXd& q_chain,
+                             Eigen::VectorXd& q_full,
+                             const char* context) const;
+    /**
+     * @brief 从 full Jacobian 中提取当前链对应列。
+     * @param J_full full-model Jacobian。
+     * @return 链空间 Jacobian。
+     */
+    Eigen::MatrixXd extractChainJacobian(const Eigen::Matrix<double,6,Eigen::Dynamic>& J_full) const;
+    /**
+     * @brief 获取 base 在 world 下位姿（固定基座时优先使用缓存）。
+     * @return base 在 world 下的位姿。
+     */
+    const pinocchio::SE3& getBasePoseWorld() const;
+    /**
+     * @brief 计算关节限位避障的二级速度项（未投影）。
+     * @param q_chain 当前链关节位置。
+     * @return 二级目标速度向量。
+     */
+    Eigen::VectorXd computeJointLimitAvoidanceVelocity(const Eigen::VectorXd& q_chain) const;
+    /**
+     * @brief 将二级目标速度投影到任务零空间。
+     * @param J_pinv 任务雅可比伪逆。
+     * @param J 任务雅可比。
+     * @param secondary 二级目标速度。
+     * @return 零空间投影结果 `N * secondary`，其中 `N = I - J_pinv * J`。
+     */
+    Eigen::VectorXd projectToNullspace(const Eigen::MatrixXd& J_pinv,
+                                       const Eigen::MatrixXd& J,
+                                       const Eigen::VectorXd& secondary) const;
+
 private:
-    std::string urdf_path_;
+    /** @brief 当前链在 full-model `q` 中的起始索引。 */
-    std::string base_frame_name_;
-    std::string flange_frame_name_;
-    std::string tcp_frame_name_;
-
-    pinocchio::Model model_;
-    std::unique_ptr<pinocchio::Data> data_;
-
-    pinocchio::FrameIndex base_frame_id_{(pinocchio::FrameIndex)(-1)};
-    pinocchio::FrameIndex flange_frame_id_{(pinocchio::FrameIndex)(-1)};
-    pinocchio::FrameIndex tcp_frame_id_{(pinocchio::FrameIndex)(-1)};
-    bool has_tcp_{false};
-
-    // chain range in q and v
     int chain_q_start_{0};
+    /** @brief 当前链关节位置自由度数量。 */
     int chain_dof_{0};
+    /** @brief 当前链在 full-model `v` 中的起始索引。 */
     int chain_v_start_{0};
+    /** @brief 当前链关节速度自由度数量。 */
     int chain_v_dof_{0};
 
-    Eigen::VectorXd q_lower_chain_;
+    /** @brief 是否启用 null-space 关节限位避障。 */
-    Eigen::VectorXd q_upper_chain_;
-
-    // null-space 关节限位避障参数
     bool limit_avoidance_enabled_{false};
+    /** @brief 限位避障增益。 */
     double limit_avoidance_gain_{0.2};
+    /** @brief 限位触发边界比例（占关节行程比例）。 */
     double limit_avoidance_margin_ratio_{0.15};
+    /** @brief 每关节最大推回速度（rad/s），<=0 表示不额外限幅。 */
     double limit_avoidance_max_push_{0.25};
 
-    // base pose cache (PELVIS_S fixed)
+    /** @brief 是否启用基座位姿缓存。 */
     bool base_pose_cached_{false};
+    /** @brief 缓存的 base 在 world 下位姿。 */
     pinocchio::SE3 oM_base_cached_;
 
+    /** @brief 求解器是否已完成初始化。 */
     bool initialized_{false};
 
+    /** @brief IK 最大迭代次数。 */
     int max_iters_;
+    /** @brief IK 平移收敛阈值（m）。 */
     double pos_eps_;
+    /** @brief IK 旋转收敛阈值（rad）。 */
     double rot_eps_;
+    /** @brief DLS 阻尼系数。 */
     double damping_;
 };
 

cmvr-es/ik_solver/include/pinocchio_ik_base.h

@@ -0,0 +1,142 @@
+// Created by Codex on 2026/3/3.
+#pragma once
+
+#include "ik_solver/include/ik_solver.h"
+#include "ik_solver/include/urdf_parser.h"
+
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/spatial/se3.hpp>
+
+#include <Eigen/Core>
+
+#include <memory>
+#include <string>
+
+namespace cmvr {
+
+/**
+ * @brief Pinocchio 系 IK 求解器公共基类。
+ *
+ * 该类负责管理：
+ * - Pinocchio 模型/数据
+ * - base/flange/tcp frame 索引
+ * - 4x4 与 SE3 的互转
+ * - FK + frame placement 更新
+ *
+ * 业务算法（DLS/QP）仍由子类实现。
+ */
+class PinocchioIKBase : public IKSolver {
+public:
+    /**
+     * @brief 按 URDF 路径构造。
+     */
+    PinocchioIKBase(const std::string& urdf_path,
+                    const std::string& base_frame_name,
+                    const std::string& flange_frame_name,
+                    const std::string& tcp_frame_name = std::string());
+
+    /**
+     * @brief 按外部 URDF 解析器构造。
+     */
+    PinocchioIKBase(std::shared_ptr<const UrdfParser> parser,
+                    const std::string& base_frame_name,
+                    const std::string& flange_frame_name,
+                    const std::string& tcp_frame_name = std::string());
+
+    ~PinocchioIKBase() override = default;
+
+    /**
+     * @brief 正运动学（默认 base->(tcp/flange)）。
+     * @param joints_angle 输入关节角，支持链长度或全模型长度。
+     * @param cur_pose 输出位姿（base 坐标系）。
+     * @param is_tcp 为 `true` 时输出 TCP 位姿，否则输出 flange 位姿。
+     * @return 成功返回 `true`，失败返回 `false`。
+     */
+    bool fk(const std::vector<double>& joints_angle,
+            Eigen::Matrix4d& cur_pose,
+            bool is_tcp = true) override;
+
+    /**
+     * @brief 正运动学（自定义 base_link 与 ee_link）。
+     * @param base_link 基座 frame 名称。
+     * @param ee_link 末端 frame 名称。
+     * @param joints_angle 输入关节角，支持链长度或全模型长度。
+     * @param cur_pose 输出位姿（`base_link` 坐标系）。
+     * @return 成功返回 `true`，失败返回 `false`。
+     */
+    bool fk(const std::string& base_link,
+            const std::string& ee_link,
+            const std::vector<double>& joints_angle,
+            Eigen::Matrix4d& cur_pose);
+
+protected:
+    /**
+     * @brief 从父类缓存的 URDF 链信息初始化 Pinocchio 运行时对象与 frame 索引。
+     *
+     * 调用前要求：
+     * - `urdf_parser_` 已有效并 loaded
+     * - `chain_base_frame_name_` 与 `chain_tip_frame_name_` 已设置
+     *
+     * @param chain_info_out 可选输出链信息（用于子类读取 q/v 区间）。
+     * @param error 可选错误信息输出。
+     * @return 成功返回 true，失败返回 false。
+     */
+    bool initPinocchioFromUrdfChain(UrdfParser::ChainInfo* chain_info_out = nullptr,
+                                    std::string* error = nullptr);
+
+    /**
+     * @brief 将输入关节向量组装为 full-model `q`（支持链长度或全模型长度）。
+     * @param joints 输入关节向量。
+     * @param q_full 输出 full-model 关节向量。
+     * @param context 调用上下文（用于日志）。
+     * @return 组装成功返回 `true`。
+     */
+    bool buildFullQFromInput(const std::vector<double>& joints,
+                             Eigen::VectorXd& q_full,
+                             const char* context) const;
+
+    /**
+     * @brief 在当前 `model_` 上更新 FK 与 frame placement。
+     */
+    void updateKinematics(const Eigen::VectorXd& q_full);
+
+    /**
+     * @brief `Eigen::Matrix4d` 转 `pinocchio::SE3`。
+     */
+    static pinocchio::SE3 matrix4ToSE3(const Eigen::Matrix4d& T);
+
+    /**
+     * @brief `pinocchio::SE3` 转 `Eigen::Matrix4d`。
+     */
+    static Eigen::Matrix4d se3ToMatrix4(const pinocchio::SE3& M);
+
+protected:
+    /** @brief TCP frame 名称（可为空）。 */
+    std::string tcp_frame_name_;
+
+    /** @brief Pinocchio 模型。 */
+    pinocchio::Model model_;
+    /** @brief Pinocchio 运行时数据。 */
+    std::unique_ptr<pinocchio::Data> data_;
+
+    /** @brief base frame 在 `model_.frames` 中的索引。 */
+    pinocchio::FrameIndex base_frame_id_{(pinocchio::FrameIndex)(-1)};
+    /** @brief flange frame 在 `model_.frames` 中的索引。 */
+    pinocchio::FrameIndex flange_frame_id_{(pinocchio::FrameIndex)(-1)};
+    /** @brief tcp frame 在 `model_.frames` 中的索引。 */
+    pinocchio::FrameIndex tcp_frame_id_{(pinocchio::FrameIndex)(-1)};
+    /** @brief 是否存在有效 TCP frame。 */
+    bool has_tcp_{false};
+
+    /** @brief 当前链在 full-model `q` 中的起始索引。 */
+    int chain_q_start_{0};
+    /** @brief 当前链关节位置自由度数量。 */
+    int chain_q_dof_{0};
+    /** @brief 当前链在 full-model `v` 中的起始索引。 */
+    int chain_v_start_{0};
+    /** @brief 当前链关节速度自由度数量。 */
+    int chain_v_dof_{0};
+};
+
+} // namespace cmvr

cmvr-es/ik_solver/include/pinocchio_qp_ik_solver.h

@@ -7,13 +7,9 @@
 
 #pragma once
 
-#include "ik_solver/include/ik_solver.h"
+#include "ik_solver/include/pinocchio_ik_base.h"
 #include "common/utils/math/qp_solver.h"
 
-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/multibody/data.hpp>
-#include <pinocchio/spatial/se3.hpp>
-
 #include <Eigen/Core>
 #include <memory>
 #include <string>
@@ -23,7 +19,7 @@
 
 namespace cmvr {
 
-class PinocchioQpIKSolver : public IKSolver {
+class PinocchioQpIKSolver : public PinocchioIKBase {
 public:
     /// urdf_path          : URDF 路径（可以是单臂，也可以是双臂整机）
     /// base_frame_name    : 作为 IK 基坐标系的 frame 名（例：PELVIS_S）
@@ -61,39 +57,16 @@ public:
             std::vector<double> &joints_angle,
             bool is_tcp = true) override;
 
-    /// joints_angle : 当前子链关节角（长度 = 子链 DOF）
-    /// cur_pose     : URDF base 下当前 TCP / 法兰位姿
-    bool fk(const std::vector<double> &joints_angle,
-            Eigen::Matrix4d &cur_pose,
-            bool is_tcp = true) override;
-
     /// 如你有更严格的速度 / 加速度限位，可以覆盖默认值
     void setVelocityLimits(const Eigen::VectorXd &qd_max);
     void setAccelerationLimits(const Eigen::VectorXd &qdd_max);
 
     using IKSolver::update_joints_state;
-    using IKSolver::setTcpTransform;
-    using IKSolver::setArmBaseTransform;
-
-private:
-    static pinocchio::SE3  matrix4ToSE3(const Eigen::Matrix4d &T);
-    static Eigen::Matrix4d se3ToMatrix4(const pinocchio::SE3 &M);
-
-    /// 根据 base_frame_name_ 和 flange_frame_name_ 构建 base→flange 子链
-    /// 并建立：
-    ///   active_joints_   : 子链上的 joint index
-    ///   active_q_idx_    : 对应在 q_full_ 里的索引
-    ///   active_v_idx_    : 对应在 v 里的索引（做雅可比抽列用）
-    ///   q_min/q_max/...  : 子链限位
-    bool buildActiveChain();
 
 private:
     // 配置
     config::PinocchioQpIKConfig config_;
     std::string urdf_path_;
-    std::string base_frame_name_;
-    std::string flange_frame_name_;
-    std::string tcp_frame_name_;
 
     double lambda_;
     double w_posrot_;
@@ -101,34 +74,11 @@ private:
     double tol_;
     double qp_time_limit_;
 
-    // Pinocchio 模型
+    // 子链上的加速度限位（位置/速度限位复用父类 IKSolver 缓存）
-    pinocchio::Model model_;
-    std::unique_ptr<pinocchio::Data> data_;
-
-    int full_dof_{0};   // 整机 nq
-    int dof_{0};        // 子链 DOF（只在这条链上做 IK）
-
-    pinocchio::FrameIndex base_frame_id_{(pinocchio::FrameIndex)(-1)};
-    pinocchio::FrameIndex flange_frame_id_{(pinocchio::FrameIndex)(-1)};
-    pinocchio::FrameIndex tcp_frame_id_{(pinocchio::FrameIndex)(-1)};
-    bool has_tcp_{false};
-
-    // base→flange 子链（只支持每个关节 nq=1,nv=1 的情况）
-    std::vector<pinocchio::JointIndex> active_joints_;
-    std::vector<int> active_q_idx_;  // 子链每个关节在 q_full_ (nq) 中的索引
-    std::vector<int> active_v_idx_;  // 子链每个关节在 v (nv) 中的索引
-
-    // 子链上的限位
-    Eigen::VectorXd q_min_global_;
-    Eigen::VectorXd q_max_global_;
-    Eigen::VectorXd qd_max_global_;
     Eigen::VectorXd qdd_max_global_;
 
     bool initialized_{false};
 
-    // 整机 q（长度 = full_dof_），用于 Pinocchio 正解 / 雅可比
-    Eigen::VectorXd q_full_;
-
     // QP 求解器（OsqpEigen 封装）
     QPSolver solver_;
 };

cmvr-es/ik_solver/include/urdf_parser.h

@@ -0,0 +1,102 @@
+// Created by Codex on 2026/3/2.
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include <pinocchio/multibody/model.hpp>
+
+namespace cmvr {
+
+/**
+ * @brief 基于 Pinocchio 的 URDF 解析工具类。
+ *
+ * 主要职责：
+ * - 加载 URDF 到 Pinocchio 模型
+ * - 提取 base->tip 链路信息
+ * - 输出链路 q/v 索引范围、关节顺序、位置/速度限位
+ */
+class UrdfParser {
+public:
+    /**
+     * @brief 链路上的单个关节信息。
+     */
+    struct JointSegment {
+        /** 关节在 Pinocchio 模型中的 JointIndex。 */
+        pinocchio::JointIndex joint_id{0};
+        /** 关节名称（URDF/model 名称）。 */
+        std::string name;
+        /** 该关节在 model.q 中的起始下标。 */
+        int q_index{0};
+        /** 该关节配置维度（q 维度）。 */
+        int nq{0};
+        /** 该关节在 model.v 中的起始下标。 */
+        int v_index{0};
+        /** 该关节速度维度（v 维度）。 */
+        int nv{0};
+    };
+
+    /**
+     * @brief base->tip 链路的聚合信息。
+     */
+    struct ChainInfo {
+        /** 基坐标系 frame id。 */
+        pinocchio::FrameIndex base_frame_id{(pinocchio::FrameIndex)(-1)};
+        /** 末端坐标系 frame id。 */
+        pinocchio::FrameIndex tip_frame_id{(pinocchio::FrameIndex)(-1)};
+        /** 链路在 model.q 中的起始下标。 */
+        int q_start{0};
+        /** 链路 q 维度总数。 */
+        int q_dof{0};
+        /** 链路在 model.v 中的起始下标。 */
+        int v_start{0};
+        /** 链路 v 维度总数。 */
+        int v_dof{0};
+        /** 链路关节位置下限（q 段）。 */
+        Eigen::VectorXd q_lower;
+        /** 链路关节位置上限（q 段）。 */
+        Eigen::VectorXd q_upper;
+        /** 链路关节速度上限（v 段）。 */
+        Eigen::VectorXd v_limit;
+        /** 按 base->tip 顺序排列的关节信息。 */
+        std::vector<JointSegment> joints;
+    };
+
+    /**
+     * @brief 加载 URDF 模型。
+     * @param urdf_path URDF 文件路径。
+     * @param error 可选错误信息输出。
+     * @return 成功返回 true，失败返回 false。
+     */
+    bool loadModel(const std::string& urdf_path, std::string* error = nullptr);
+
+    /**
+     * @brief 提取 base->tip 链路信息。
+     *
+     * 实现方式：从 tip 的父关节向上回溯到 base 的父关节，
+     * 再反转为 base->tip 顺序。
+     *
+     * @param base_frame_name 基坐标系 frame 名称。
+     * @param tip_frame_name 末端 frame 名称。
+     * @param out 输出链路信息。
+     * @param error 可选错误信息输出。
+     * @return 成功返回 true，失败返回 false。
+     */
+    bool extractChain(const std::string& base_frame_name,
+                      const std::string& tip_frame_name,
+                      ChainInfo& out,
+                      std::string* error = nullptr) const;
+
+    /** @brief 获取已加载的 Pinocchio 模型。 */
+    const pinocchio::Model& model() const { return model_; }
+    /** @brief 当前是否已成功加载模型。 */
+    bool loaded() const { return loaded_; }
+
+private:
+    pinocchio::Model model_;
+    bool loaded_{false};
+};
+
+} // namespace cmvr

cmvr-es/ik_solver/src/ik_solver.cpp

@@ -0,0 +1,103 @@
+//
+// Created by Codex on 2026/3/2.
+//
+
+#include "ik_solver/include/ik_solver.h"
+#include "ik_solver/include/urdf_parser.h"
+
+#include <iostream>
+
+namespace cmvr {
+
+IKSolver::IKSolver(const std::string& urdf_path,
+                   const std::string& base_frame_name,
+                   const std::string& tip_frame_name) {
+    if (!urdf_path.empty() && !base_frame_name.empty() && !tip_frame_name.empty()) {
+        initUrdfChain(urdf_path, base_frame_name, tip_frame_name);
+    }
+}
+
+IKSolver::IKSolver(std::shared_ptr<const UrdfParser> parser,
+                   const std::string& base_frame_name,
+                   const std::string& tip_frame_name) {
+    if (parser && !base_frame_name.empty() && !tip_frame_name.empty()) {
+        initUrdfChain(parser, base_frame_name, tip_frame_name);
+    }
+}
+
+bool IKSolver::initUrdfChain(const std::string& urdf_path,
+                             const std::string& base_frame_name,
+                             const std::string& tip_frame_name) {
+    auto parser = std::make_shared<UrdfParser>();
+    std::string err;
+    if (!parser->loadModel(urdf_path, &err)) {
+        std::cerr << "[IKSolver] Failed to load URDF: " << err << "\n";
+        return false;
+    }
+    return initUrdfChain(parser, base_frame_name, tip_frame_name);
+}
+
+bool IKSolver::initUrdfChain(const std::shared_ptr<const UrdfParser>& parser,
+                             const std::string& base_frame_name,
+                             const std::string& tip_frame_name) {
+    if (!parser) {
+        std::cerr << "[IKSolver] initUrdfChain failed: parser is null\n";
+        return false;
+    }
+    if (!parser->loaded()) {
+        std::cerr << "[IKSolver] initUrdfChain failed: parser model not loaded\n";
+        return false;
+    }
+
+    UrdfParser::ChainInfo chain_info;
+    std::string err;
+    if (!parser->extractChain(base_frame_name, tip_frame_name, chain_info, &err)) {
+        std::cerr << "[IKSolver] Failed to extract chain: " << err << "\n";
+        return false;
+    }
+
+    urdf_parser_ = parser;
+    chain_base_frame_name_ = base_frame_name;
+    chain_tip_frame_name_ = tip_frame_name;
+
+    chain_joint_names_.clear();
+
+    joint_pos_lower_limits_ = chain_info.q_lower;
+    joint_pos_upper_limits_ = chain_info.q_upper;
+    joint_vel_limits_ = chain_info.v_limit;
+    chain_joint_names_.reserve(chain_info.joints.size());
+    for (const auto& joint : chain_info.joints) {
+        chain_joint_names_.push_back(joint.name);
+    }
+
+    urdf_chain_cached_ = true;
+    return true;
+}
+
+bool IKSolver::getJointPositionLimits(Eigen::VectorXd& lower,
+                                      Eigen::VectorXd& upper) const {
+    if (!urdf_chain_cached_ || joint_pos_lower_limits_.size() == 0 || joint_pos_upper_limits_.size() == 0) {
+        return false;
+    }
+    lower = joint_pos_lower_limits_;
+    upper = joint_pos_upper_limits_;
+    return true;
+}
+
+bool IKSolver::getJointVelocityLimits(Eigen::VectorXd& limits) const {
+    if (!urdf_chain_cached_ || joint_vel_limits_.size() == 0) {
+        return false;
+    }
+    limits = joint_vel_limits_;
+    return true;
+}
+
+bool IKSolver::getChainJointNames(std::vector<std::string>& names) const {
+    if (!urdf_chain_cached_ || chain_joint_names_.empty()) {
+        return false;
+    }
+    names = chain_joint_names_;
+    return true;
+}
+
+} // namespace cmvr

cmvr-es/ik_solver/src/ik_test.cpp

@@ -185,13 +185,13 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
         {-0.26,   1.57},
     }};
 
-    constexpr int N_SAMPLES = 100;  // 样本数：1000 组随机关节角
+    constexpr int N_SAMPLES = 10;  // 样本数：1000 组随机关节角
 
     // ========== 2. 创建三个求解器实例 ==========
 
     // 数值优化类 QP IK
     PinocchioQpIKSolver qp_solver(
-        "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
+        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
         "PELVIS_S",
         "R_WRIST_R_S",        // 法兰 frame
         "R_FINGER_TIP_FIXED"  // TCP frame
@@ -199,7 +199,7 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
 
     // 基于广义逆雅可比矩阵的数值增量 IK
     PinocchioDlsIKSolver pinv_solver(
-        "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
+        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
         "PELVIS_S",
         "R_WRIST_R_S",        // 法兰 frame
         "R_FINGER_TIP_FIXED"  // TCP frame
@@ -211,8 +211,8 @@ void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
     // 统一容器，方便 for 循环
     std::vector<IKSolver*> solvers = {
         &psi_solver,
-        // &pinv_solver,
+        &pinv_solver,
-        // &qp_solver
+        &qp_solver
     };
 
 
@@ -395,13 +395,13 @@ void benchmarkIkSolversRandomJoints()
 
     }};
 
-    constexpr int N_SAMPLES = 10000;  // 样本数：1000 组随机关节角
+    constexpr int N_SAMPLES = 100;  // 样本数：1000 组随机关节角
 
     // ========== 2. 创建三个求解器实例 ==========
 
     // 数值优化类 QP IK
     PinocchioQpIKSolver qp_solver(
-        "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
+        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
         "PELVIS_S",
         "R_WRIST_R_S",        // 法兰 frame
         "R_FINGER_TIP_FIXED"  // TCP frame
@@ -409,7 +409,7 @@ void benchmarkIkSolversRandomJoints()
 
     // 基于广义逆雅可比矩阵的数值增量 IK
     PinocchioDlsIKSolver pinv_solver(
-        "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
+        "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
         "PELVIS_S",
         "R_WRIST_R_S",        // 法兰 frame
         "R_FINGER_TIP_FIXED"  // TCP frame
@@ -420,8 +420,8 @@ void benchmarkIkSolversRandomJoints()
 
     // 统一容器，方便 for 循环
     std::vector<IKSolver*> solvers = {
-        &psi_solver,
+        // &psi_solver,
-        // &pinv_solver,
+        &pinv_solver,
         // &qp_solver
     };
 

cmvr-es/ik_solver/src/lawba_ik_solver.cpp

@@ -7,7 +7,7 @@
 
 using namespace cmvr;
 
-LawbaIKSolver::LawbaIKSolver() : IKSolver() {
+LawbaIKSolver::LawbaIKSolver() : IKSolver("", "", "") {
     bias_srs_ik_solver_ = std::make_shared<BiasSRSIkSolver>();
     joints_limit_analyzer_ = std::make_shared<JointsLimitAnalyzer>();
     opt_psi_selector_ = std::make_shared<OptPsiSelector>();
@@ -33,8 +33,8 @@ bool LawbaIKSolver::init() {
             1, 0, 0, 0,
             0, 0, 0, 1;
 
-    setTcpTransform(T_tool_flange);
+    T_tool_flange_ = T_tool_flange;
-    setArmBaseTransform(T_arm_robot);
+    T_arm_robot_ = T_arm_robot;
 
     // 臂角更新参数
     opt_psi_selector_->set_update_params(0.6, 5.0, -1, 1e-4);

cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp

@@ -1,13 +1,15 @@
 // Created by lgv on 11/28/25.
 
 #include "ik_solver/include/pinocchio_dls_ik_solver.h"
+#include "ik_solver/include/urdf_parser.h"
 
-#include <pinocchio/parsers/urdf.hpp>
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/algorithm/jacobian.hpp>
 #include <pinocchio/spatial/explog.hpp>
 
+#include <Eigen/SVD>
+
 #include <algorithm>
 #include <cmath>
 #include <iostream>
@@ -43,112 +45,106 @@ PinocchioDlsIKSolver::PinocchioDlsIKSolver(const std::string &urdf_path,
                                            double pos_eps,
                                            double rot_eps,
                                            double damping)
-    : urdf_path_(urdf_path)
+    : PinocchioIKBase(urdf_path, base_frame_name, flange_frame_name, tcp_frame_name)
-    , base_frame_name_(base_frame_name)
-    , flange_frame_name_(flange_frame_name)
-    , tcp_frame_name_(tcp_frame_name)
     , max_iters_(max_iters)
     , pos_eps_(pos_eps)
     , rot_eps_(rot_eps)
     , damping_(damping)
 {
-    T_tool_flange_.setIdentity();
-    T_arm_robot_.setIdentity();
 }
 
-pinocchio::SE3 PinocchioDlsIKSolver::matrix4ToSE3(const Eigen::Matrix4d &T) {
+bool PinocchioDlsIKSolver::buildFullQFromChain(const Eigen::VectorXd& q_chain,
-    pinocchio::SE3 M;
+                                                Eigen::VectorXd& q_full,
-    M.rotation() = T.block<3,3>(0,0);
+                                                const char* context) const {
-    M.translation() = T.block<3,1>(0,3);
+    if (q_chain.size() != chain_dof_) {
-    return M;
+        if (context != nullptr) {
+            std::cerr << "[PinocchioDlsIKSolver] " << context << " chain q size mismatch\n";
+        }
+        return false;
+    }
+    q_full = pinocchio::neutral(model_);
+    q_full.segment(chain_q_start_, chain_dof_) = q_chain;
+    return true;
 }
 
-Eigen::Matrix4d PinocchioDlsIKSolver::se3ToMatrix4(const pinocchio::SE3 &M) {
+Eigen::MatrixXd PinocchioDlsIKSolver::extractChainJacobian(
-    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+    const Eigen::Matrix<double,6,Eigen::Dynamic>& J_full) const {
-    T.block<3,3>(0,0) = M.rotation();
+    return J_full.middleCols(chain_v_start_, chain_v_dof_);
-    T.block<3,1>(0,3) = M.translation();
+}
-    return T;
+
+const pinocchio::SE3& PinocchioDlsIKSolver::getBasePoseWorld() const {
+    return base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_];
+}
+
+Eigen::VectorXd PinocchioDlsIKSolver::computeJointLimitAvoidanceVelocity(
+    const Eigen::VectorXd& q_chain) const {
+    if (!limit_avoidance_enabled_ ||
+        limit_avoidance_gain_ <= 0.0 ||
+        chain_v_dof_ != chain_dof_ ||
+        q_chain.size() != chain_dof_ ||
+        joint_pos_lower_limits_.size() != chain_dof_ ||
+        joint_pos_upper_limits_.size() != chain_dof_) {
+        return Eigen::VectorXd::Zero(chain_v_dof_);
+    }
+
+    Eigen::VectorXd qdot_avoid = Eigen::VectorXd::Zero(chain_v_dof_);
+    for (int i = 0; i < chain_dof_; ++i) {
+        const double lo = joint_pos_lower_limits_[i];
+        const double hi = joint_pos_upper_limits_[i];
+        if (!std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) {
+            continue;
+        }
+
+        const double span = hi - lo;
+        const double margin = std::max(1e-4, limit_avoidance_margin_ratio_ * span);
+        double push = 0.0;
+
+        if (q_chain[i] < lo + margin) {
+            const double s = (lo + margin - q_chain[i]) / margin; // 0..1+
+            push += limit_avoidance_gain_ * s * s;
+        } else if (q_chain[i] > hi - margin) {
+            const double s = (q_chain[i] - (hi - margin)) / margin; // 0..1+
+            push -= limit_avoidance_gain_ * s * s;
+        }
+
+        if (limit_avoidance_max_push_ > 0.0) {
+            push = clampd(push, -limit_avoidance_max_push_, limit_avoidance_max_push_);
+        }
+        qdot_avoid[i] = push;
+    }
+
+    return qdot_avoid;
+}
+
+Eigen::VectorXd PinocchioDlsIKSolver::projectToNullspace(const Eigen::MatrixXd& J_pinv,
+                                                         const Eigen::MatrixXd& J,
+                                                         const Eigen::VectorXd& secondary) const {
+    if (secondary.size() != J.cols()) {
+        return Eigen::VectorXd::Zero(J.cols());
+    }
+    const Eigen::MatrixXd N =
+        Eigen::MatrixXd::Identity(J.cols(), J.cols()) - J_pinv * J;
+    return N * secondary;
 }
 
 bool PinocchioDlsIKSolver::init() {
-    try {
+    UrdfParser::ChainInfo chain_info;
-        pinocchio::urdf::buildModel(urdf_path_, model_);
+    std::string err;
-    } catch (const std::exception &e) {
+    if (!initPinocchioFromUrdfChain(&chain_info, &err)) {
-        std::cerr << "[PinocchioDlsIKSolver] Failed to load URDF: " << e.what() << "\n";
+        std::cerr << "[PinocchioDlsIKSolver] Failed to init pinocchio base: " << err << "\n";
         return false;
     }
-    data_ = std::make_unique<pinocchio::Data>(model_);
+    chain_q_start_ = chain_info.q_start;
+    chain_dof_ = chain_info.q_dof;
+    chain_v_start_ = chain_info.v_start;
+    chain_v_dof_ = chain_info.v_dof;
 
-    if (!model_.existFrame(flange_frame_name_)) {
+    // limits already cached in IKSolver base class.
-        std::cerr << "[PinocchioDlsIKSolver] flange frame not found: " << flange_frame_name_ << "\n";
-        return false;
-    }
-    if (!model_.existFrame(base_frame_name_)) {
-        std::cerr << "[PinocchioDlsIKSolver] base frame not found: " << base_frame_name_ << "\n";
-        return false;
-    }
-
-    flange_frame_id_ = model_.getFrameId(flange_frame_name_);
-    base_frame_id_   = model_.getFrameId(base_frame_name_);
-
-    // chain joints base->flange
-    std::vector<pinocchio::JointIndex> chain_joints;
-    {
-        const auto &base_frame = model_.frames[base_frame_id_];
-        const auto &flange_frame = model_.frames[flange_frame_id_];
-
-        pinocchio::JointIndex base_joint   = base_frame.parent;
-        pinocchio::JointIndex flange_joint = flange_frame.parent;
-
-        pinocchio::JointIndex j = flange_joint;
-        while (j != 0 && j != base_joint) {
-            chain_joints.push_back(j);
-            j = model_.parents[j];
-        }
-        if (j == 0 && base_joint != 0) {
-            std::cerr << "[PinocchioDlsIKSolver] base is not ancestor of flange\n";
-            return false;
-        }
-        if (base_joint != 0) chain_joints.push_back(base_joint);
-        if (chain_joints.empty()) {
-            std::cerr << "[PinocchioDlsIKSolver] empty chain\n";
-            return false;
-        }
-        std::reverse(chain_joints.begin(), chain_joints.end());
-
-        chain_q_start_ = model_.idx_qs[chain_joints.front()];
-        int last_q = model_.idx_qs[chain_joints.back()] + model_.nqs[chain_joints.back()] - 1;
-        chain_dof_ = last_q - chain_q_start_ + 1;
-
-        chain_v_start_ = model_.idx_vs[chain_joints.front()];
-        int last_v = model_.idx_vs[chain_joints.back()] + model_.nvs[chain_joints.back()] - 1;
-        chain_v_dof_ = last_v - chain_v_start_ + 1;
-    }
-
-    // limits
-    q_lower_chain_ = model_.lowerPositionLimit.segment(chain_q_start_, chain_dof_);
-    q_upper_chain_ = model_.upperPositionLimit.segment(chain_q_start_, chain_dof_);
-
-    // tcp frame
-    has_tcp_ = false;
-    if (!tcp_frame_name_.empty() && model_.existFrame(tcp_frame_name_)) {
-        tcp_frame_id_ = model_.getFrameId(tcp_frame_name_);
-        has_tcp_ = true;
-
-        Eigen::VectorXd q0 = pinocchio::neutral(model_);
-        pinocchio::forwardKinematics(model_, *data_, q0);
-        pinocchio::updateFramePlacements(model_, *data_);
-
-        const pinocchio::SE3 &oM_flange = data_->oMf[flange_frame_id_];
-        const pinocchio::SE3 &oM_tcp    = data_->oMf[tcp_frame_id_];
-        T_tool_flange_ = se3ToMatrix4(oM_flange.inverse() * oM_tcp);
-    }
 
     // cache base pose (PELVIS_S fixed)
     {
         Eigen::VectorXd q0 = pinocchio::neutral(model_);
-        pinocchio::forwardKinematics(model_, *data_, q0);
+        updateKinematics(q0);
-        pinocchio::updateFramePlacements(model_, *data_);
         oM_base_cached_ = data_->oMf[base_frame_id_];
         base_pose_cached_ = true;
     }
@@ -157,15 +153,15 @@ bool PinocchioDlsIKSolver::init() {
     cur_joints_angle_.assign(chain_dof_, 0.0);
 
     initialized_ = true;
-    std::cout << "[PinocchioDlsIKSolver] Chain '" << base_frame_name_ << "' -> '" << flange_frame_name_
+    std::cout << "[PinocchioDlsIKSolver] Chain '" << chain_base_frame_name_ << "' -> '" << chain_tip_frame_name_
               << "': q_start=" << chain_q_start_ << " q_dof=" << chain_dof_
               << ", v_start=" << chain_v_start_ << " v_dof=" << chain_v_dof_
               << ", nq=" << model_.nq << " nv=" << model_.nv << "\n";
     std::cout << "[PinocchioDlsIKSolver] Chain joint position limits (rad):\n";
-    for (const auto j : chain_joints) {
+    for (const auto& seg : chain_info.joints) {
-        const int q_idx = model_.idx_qs[j];
+        const int q_idx = seg.q_index;
-        const int nq = model_.nqs[j];
+        const int nq = seg.nq;
-        const std::string& jname = model_.names[j];
+        const std::string& jname = seg.name;
         if (nq <= 0) continue;
 
         for (int k = 0; k < nq; ++k) {
@@ -173,10 +169,10 @@ bool PinocchioDlsIKSolver::init() {
             if (qi < 0 || qi >= chain_dof_) continue;
             if (nq == 1) {
                 std::cout << "  - " << jname
-                          << ": [" << q_lower_chain_[qi] << ", " << q_upper_chain_[qi] << "]\n";
+                          << ": [" << joint_pos_lower_limits_[qi] << ", " << joint_pos_upper_limits_[qi] << "]\n";
             } else {
                 std::cout << "  - " << jname << "[" << k << "]"
-                          << ": [" << q_lower_chain_[qi] << ", " << q_upper_chain_[qi] << "]\n";
+                          << ": [" << joint_pos_lower_limits_[qi] << ", " << joint_pos_upper_limits_[qi] << "]\n";
             }
         }
     }
@@ -188,27 +184,72 @@ bool PinocchioDlsIKSolver::ik(const Eigen::Matrix4d &target_pose_base,
                               std::vector<double> &joints_angle,
                               bool is_tcp)
 {
+    const std::string& ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : chain_tip_frame_name_;
+    return ik(chain_base_frame_name_, ee_link, target_pose_base, joints_angle);
+}
+
+bool PinocchioDlsIKSolver::ik(const std::string& base_link,
+                              const std::string& ee_link,
+                              const Eigen::Matrix4d& target_pose,
+                              std::vector<double>& joints_angle) {
     if (!initialized_) return false;
     if ((int)cur_joints_angle_.size() != chain_dof_) return false;
+    if (!model_.existFrame(base_link)) {
+        std::cerr << "[PinocchioDlsIKSolver] base frame not found: " << base_link << "\n";
+        return false;
+    }
+    if (!model_.existFrame(ee_link)) {
+        std::cerr << "[PinocchioDlsIKSolver] ee frame not found: " << ee_link << "\n";
+        return false;
+    }
+    const pinocchio::FrameIndex base_frame_id = model_.getFrameId(base_link);
+    const pinocchio::FrameIndex ee_frame_id = model_.getFrameId(ee_link);
 
-    pinocchio::FrameIndex target_frame_id =
+    const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent;
-        (is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_;
+    const pinocchio::JointIndex base_joint = model_.frames[base_frame_id].parent;
+    const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent;
+    const pinocchio::JointIndex ee_joint = model_.frames[ee_frame_id].parent;
 
-    const pinocchio::SE3 base_M_target = matrix4ToSE3(target_pose_base);
+    auto jointOnParentPath = [this](pinocchio::JointIndex from,
-    const pinocchio::SE3 &oM_base = base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_];
+                                    pinocchio::JointIndex target) {
-    const pinocchio::SE3 oM_target = oM_base * base_M_target;
+        if (target == 0) return true;
+        pinocchio::JointIndex j = from;
+        while (j != 0) {
+            if (j == target) return true;
+            j = model_.parents[j];
+        }
+        return false;
+    };
+
+    auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) {
+        return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint);
+    };
+
+    const bool base_on_branch = jointOnConfiguredBranch(base_joint);
+    const bool ee_on_branch = jointOnConfiguredBranch(ee_joint);
+    const bool base_is_ancestor_of_ee =
+        jointOnParentPath(ee_joint, base_joint);
+    if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) {
+        std::cerr << "[PinocchioDlsIKSolver] base/ee must be on configured single chain and base must be ancestor of ee\n";
+        return false;
+    }
+
+    const pinocchio::SE3 base_M_target = matrix4ToSE3(target_pose);
 
     Eigen::VectorXd q_chain = Eigen::Map<Eigen::VectorXd>(cur_joints_angle_.data(), chain_dof_);
 
     bool success = false;
     for (int iter = 0; iter < max_iters_; ++iter) {
-        Eigen::VectorXd q_full = pinocchio::neutral(model_);
+        Eigen::VectorXd q_full;
-        q_full.segment(chain_q_start_, chain_dof_) = q_chain;
+        if (!buildFullQFromChain(q_chain, q_full, "ik(base,ee)")) {
+            return false;
+        }
+        updateKinematics(q_full);
 
-        pinocchio::forwardKinematics(model_, *data_, q_full);
+        const pinocchio::SE3& oM_base =
-        pinocchio::updateFramePlacements(model_, *data_);
+            (base_frame_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_frame_id];
-
+        const pinocchio::SE3 oM_target = oM_base * base_M_target;
-        const pinocchio::SE3 &oM_cur = data_->oMf[target_frame_id];
+        const pinocchio::SE3 &oM_cur = data_->oMf[ee_frame_id];
         pinocchio::SE3 dM = oM_cur.inverse() * oM_target;
 
         Eigen::Matrix<double,6,1> err = pinocchio::log6(dM).toVector();
@@ -219,19 +260,19 @@ bool PinocchioDlsIKSolver::ik(const Eigen::Matrix4d &target_pose_base,
 
         Eigen::Matrix<double,6,Eigen::Dynamic> J_full(6, model_.nv);
         pinocchio::computeFrameJacobian(model_, *data_, q_full,
-                                        target_frame_id,
+                                        ee_frame_id,
                                         pinocchio::ReferenceFrame::LOCAL,
                                         J_full);
-        Eigen::MatrixXd J = J_full.middleCols(chain_v_start_, chain_v_dof_);
+        Eigen::MatrixXd J = extractChainJacobian(J_full);
         Eigen::MatrixXd J_pinv = dampedPseudoInverse(J, damping_);
 
         Eigen::VectorXd dq = J_pinv * err;
         q_chain += dq;
-        q_chain = q_chain.cwiseMax(q_lower_chain_).cwiseMin(q_upper_chain_);
+        q_chain = q_chain.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_);
     }
 
     if (!success) {
-        std::cerr << "[PinocchioDlsIKSolver] IK did not converge\n";
+        std::cerr << "[PinocchioDlsIKSolver] IK solve failed\n";
         return false;
     }
 
@@ -240,55 +281,6 @@ bool PinocchioDlsIKSolver::ik(const Eigen::Matrix4d &target_pose_base,
     return true;
 }
 
-bool PinocchioDlsIKSolver::fk(const std::vector<double> &joints_angle,
-                              Eigen::Matrix4d &cur_pose_base,
-                              bool is_tcp)
-{
-    const std::string &ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : flange_frame_name_;
-    return fk(base_frame_name_, ee_link, joints_angle, cur_pose_base);
-}
-
-bool PinocchioDlsIKSolver::fk(const std::string& base_link,
-                              const std::string& ee_link,
-                              const std::vector<double>& joints_angle,
-                              Eigen::Matrix4d& cur_pose)
-{
-    if (!initialized_) return false;
-    if (!model_.existFrame(base_link)) {
-        std::cerr << "[PinocchioDlsIKSolver] base frame not found: " << base_link << "\n";
-        return false;
-    }
-    if (!model_.existFrame(ee_link)) {
-        std::cerr << "[PinocchioDlsIKSolver] ee frame not found: " << ee_link << "\n";
-        return false;
-    }
-
-    const int size = (int)joints_angle.size();
-    if (size != chain_dof_ && size != model_.nq) {
-        std::cerr << "[PinocchioDlsIKSolver] fk(base,ee) joints size mismatch\n";
-        return false;
-    }
-
-    Eigen::VectorXd q_full = pinocchio::neutral(model_);
-    if (size == model_.nq) {
-        q_full = Eigen::Map<const Eigen::VectorXd>(joints_angle.data(), model_.nq);
-    } else {
-        Eigen::Map<const Eigen::VectorXd> q_chain(joints_angle.data(), chain_dof_);
-        q_full.segment(chain_q_start_, chain_dof_) = q_chain;
-    }
-
-    pinocchio::forwardKinematics(model_, *data_, q_full);
-    pinocchio::updateFramePlacements(model_, *data_);
-
-    const pinocchio::FrameIndex base_id = model_.getFrameId(base_link);
-    const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
-    const pinocchio::SE3 &oM_base = data_->oMf[base_id];
-    const pinocchio::SE3 &oM_ee = data_->oMf[ee_id];
-    pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
-    cur_pose = se3ToMatrix4(base_M_ee);
-    return true;
-}
-
 void PinocchioDlsIKSolver::setJointLimitAvoidance(bool enable,
                                                    double gain,
                                                    double margin_ratio,
@@ -299,15 +291,19 @@ void PinocchioDlsIKSolver::setJointLimitAvoidance(bool enable,
     limit_avoidance_max_push_ = max_push;
 }
 
-bool PinocchioDlsIKSolver::velocityIk(const std::vector<double>& cur_angle,
+bool PinocchioDlsIKSolver::ik(const std::string& base_link,
-                                      const Eigen::Matrix<double,6,1>& ee_velocity,
+                              const std::string& ee_link,
-                                      std::vector<double>& joints_vel,
+                              const Eigen::Matrix<double,6,1>& target_vel,
-                                      const std::string& ee_link,
+                              std::vector<double>& joints_vel,
-                                      double damping,
+                              double damping,
-                                      double qdot_abs_max)
+                              double qdot_abs_max)
 {
     // 1) 基本状态与输入合法性检查。
     if (!initialized_) return false;
+    if (!model_.existFrame(base_link)) {
+        std::cerr << "[PinocchioDlsIKSolver] base frame not found: " << base_link << "\n";
+        return false;
+    }
     if (ee_link.empty()) {
         std::cerr << "[PinocchioDlsIKSolver] ee_frame_name is empty\n";
         return false;
@@ -317,40 +313,64 @@ bool PinocchioDlsIKSolver::velocityIk(const std::vector<double>& cur_angle,
         return false;
     }
 
-    const int size = (int)cur_angle.size();
-    if (size != chain_dof_ && size != model_.nq) {
-        std::cerr << "[PinocchioDlsIKSolver] velocityIk joints size mismatch\n";
-        return false;
-    }
     if (chain_v_dof_ <= 0) {
         std::cerr << "[PinocchioDlsIKSolver] invalid chain_v_dof\n";
         return false;
     }
 
-    // 2) 组装整机 q（Pinocchio 统一在 full model 上做 FK/Jacobian）。
+    if (static_cast<int>(cur_joints_angle_.size()) != chain_dof_) {
-    Eigen::VectorXd q_full = pinocchio::neutral(model_);
+        std::cerr << "[PinocchioDlsIKSolver] ik(velocity) current joint state not initialized\n";
-    if (size == model_.nq) {
+        return false;
-        q_full = Eigen::Map<const Eigen::VectorXd>(cur_angle.data(), model_.nq);
+    }
-    } else {
+
-        Eigen::Map<const Eigen::VectorXd> q_chain(cur_angle.data(), chain_dof_);
+    const pinocchio::FrameIndex base_id = model_.getFrameId(base_link);
-        q_full.segment(chain_q_start_, chain_dof_) = q_chain;
+    const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
+
+    const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent;
+    const pinocchio::JointIndex base_joint = model_.frames[base_id].parent;
+    const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent;
+    const pinocchio::JointIndex ee_joint = model_.frames[ee_id].parent;
+
+    auto jointOnParentPath = [this](pinocchio::JointIndex from,
+                                    pinocchio::JointIndex target) {
+        if (target == 0) return true;
+        pinocchio::JointIndex j = from;
+        while (j != 0) {
+            if (j == target) return true;
+            j = model_.parents[j];
+        }
+        return false;
+    };
+    auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) {
+        return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint);
+    };
+    const bool base_on_branch = jointOnConfiguredBranch(base_joint);
+    const bool ee_on_branch = jointOnConfiguredBranch(ee_joint);
+    const bool base_is_ancestor_of_ee = jointOnParentPath(ee_joint, base_joint);
+    if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) {
+        std::cerr << "[PinocchioDlsIKSolver] base/ee must be on configured single chain and base must be ancestor of ee\n";
+        return false;
+    }
+
+    // 2) 组装整机 q（内部当前状态）。
+    Eigen::VectorXd q_full;
+    if (!buildFullQFromInput(cur_joints_angle_, q_full, "ik(velocity)")) {
+        return false;
     }
 
     // 3) 在当前关节角下更新位姿。
-    pinocchio::forwardKinematics(model_, *data_, q_full);
+    updateKinematics(q_full);
-    pinocchio::updateFramePlacements(model_, *data_);
 
-    const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
+    const pinocchio::SE3 &oM_base =
-
+        (base_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_id];
-    const pinocchio::SE3 &oM_base = base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_];
     const pinocchio::SE3 &oM_ee   = data_->oMf[ee_id];
 
     // 4) ee 局部系 twist -> base 系 twist。
     const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
     const Eigen::Matrix3d R_be = base_M_ee.rotation(); // ee -> base
     Eigen::Matrix<double,6,1> twist_base;
-    twist_base.head<3>() = R_be * ee_velocity.head<3>();
+    twist_base.head<3>() = R_be * target_vel.head<3>();
-    twist_base.tail<3>() = R_be * ee_velocity.tail<3>();
+    twist_base.tail<3>() = R_be * target_vel.tail<3>();
 
     // 5) 计算 Jacobian，并从 world 表达转到 base 表达。
     Eigen::Matrix<double,6,Eigen::Dynamic> J_world(6, model_.nv);
@@ -358,7 +378,7 @@ bool PinocchioDlsIKSolver::velocityIk(const std::vector<double>& cur_angle,
                                     ee_id,
                                     pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED,
                                     J_world);
-    Eigen::MatrixXd J = J_world.middleCols(chain_v_start_, chain_v_dof_);
+    Eigen::MatrixXd J = extractChainJacobian(J_world);
     const Eigen::Matrix3d R_bo = oM_base.rotation().transpose(); // world -> base
     J.topRows(3) = R_bo * J.topRows(3);
     J.bottomRows(3) = R_bo * J.bottomRows(3);
@@ -367,61 +387,29 @@ bool PinocchioDlsIKSolver::velocityIk(const std::vector<double>& cur_angle,
     const double lambda = (damping > 0.0) ? damping : damping_;
     Eigen::Matrix<double,6,6> A = J * J.transpose();
     A.diagonal().array() += (lambda * lambda);
-    Eigen::VectorXd qdot = J.transpose() * A.ldlt().solve(twist_base);
+    Eigen::LDLT<Eigen::Matrix<double,6,6>> ldlt(A);
+    if (ldlt.info() != Eigen::Success) {
+        std::cerr << "[PinocchioDlsIKSolver] ik(velocity) LDLT failed\n";
+        return false;
+    }
+    Eigen::VectorXd qdot = J.transpose() * ldlt.solve(twist_base);
 
     // 6.1) null-space 关节限位避障：在主任务零空间叠加“离限位推回”速度。
-    if (limit_avoidance_enabled_ &&
+    const Eigen::VectorXd q_chain = q_full.segment(chain_q_start_, chain_dof_);
-        limit_avoidance_gain_ > 0.0 &&
+    const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain);
-        chain_v_dof_ == chain_dof_ &&
+    if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
-        q_lower_chain_.size() == chain_dof_ &&
+        const Eigen::Matrix<double,6,6> A_inv =
-        q_upper_chain_.size() == chain_dof_) {
+            ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
-        const Eigen::VectorXd q_chain = q_full.segment(chain_q_start_, chain_dof_);
+        const Eigen::MatrixXd J_pinv = J.transpose() * A_inv; // n x 6
-        Eigen::VectorXd qdot_avoid = Eigen::VectorXd::Zero(chain_v_dof_);
+        qdot += projectToNullspace(J_pinv, J, qdot_avoid);
-
-        for (int i = 0; i < chain_dof_; ++i) {
-            const double lo = q_lower_chain_[i];
-            const double hi = q_upper_chain_[i];
-            if (!std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) {
-                continue;
-            }
-
-            const double span = hi - lo;
-            const double margin = std::max(1e-4, limit_avoidance_margin_ratio_ * span);
-            double push = 0.0;
-
-            if (q_chain[i] < lo + margin) {
-                const double s = (lo + margin - q_chain[i]) / margin; // 0..1+
-                push += limit_avoidance_gain_ * s * s;
-            } else if (q_chain[i] > hi - margin) {
-                const double s = (q_chain[i] - (hi - margin)) / margin; // 0..1+
-                push -= limit_avoidance_gain_ * s * s;
-            }
-
-            if (limit_avoidance_max_push_ > 0.0) {
-                push = clampd(push, -limit_avoidance_max_push_, limit_avoidance_max_push_);
-            }
-            qdot_avoid[i] = push;
-        }
-
-        if (qdot_avoid.squaredNorm() > 1e-16) {
-            const Eigen::Matrix<double,6,6> A_inv =
-                A.ldlt().solve(Eigen::Matrix<double,6,6>::Identity());
-            const Eigen::MatrixXd J_pinv = J.transpose() * A_inv; // n x 6
-            const Eigen::MatrixXd N =
-                Eigen::MatrixXd::Identity(chain_v_dof_, chain_v_dof_) - J_pinv * J;
-            qdot += N * qdot_avoid;
-        }
     }
 
     // 7) 关节速度限幅：取调用者限幅与 URDF velocityLimit 的更严格值。
     joints_vel.resize(chain_v_dof_);
     for (int i = 0; i < chain_v_dof_; ++i) {
         double vel_limit = qdot_abs_max;
-        if (model_.velocityLimit.size() == model_.nv) {
+        if (joint_vel_limits_.size() == chain_v_dof_) {
-            const int v_idx = chain_v_start_ + i;
+            vel_limit = std::min(vel_limit, std::abs(joint_vel_limits_[i]));
-            if (v_idx >= 0 && v_idx < model_.nv) {
-                vel_limit = std::min(vel_limit, std::abs(model_.velocityLimit[v_idx]));
-            }
         }
 
         double qi = qdot[i];
@@ -434,26 +422,6 @@ bool PinocchioDlsIKSolver::velocityIk(const std::vector<double>& cur_angle,
     return true;
 }
 
-bool PinocchioDlsIKSolver::getJointPositionLimits(std::vector<double>& lower,
-                                                  std::vector<double>& upper) const {
-    if (!initialized_ || chain_dof_ <= 0) {
-        return false;
-    }
-    if (q_lower_chain_.size() != chain_dof_ || q_upper_chain_.size() != chain_dof_) {
-        return false;
-    }
-
-    lower.assign(q_lower_chain_.data(), q_lower_chain_.data() + q_lower_chain_.size());
-    upper.assign(q_upper_chain_.data(), q_upper_chain_.data() + q_upper_chain_.size());
-    return true;
-}
-
-#include <Eigen/SVD>
-#include <cmath>
-#include <iostream>
-#include <vector>
-#include <algorithm>
-
 // ===== 小工具 =====
 static inline double clamp01(double x) { return std::max(0.0, std::min(1.0, x)); }
 
@@ -766,13 +734,13 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
         T_des_base.block<3,1>(0,3) = p_des_base;
 
         // build q_full
-        Eigen::VectorXd q_full = pinocchio::neutral(model_);
+        Eigen::VectorXd q_full;
-        q_full.segment(chain_q_start_, chain_dof_) = q_chain;
+        if (!buildFullQFromChain(q_chain, q_full, "moveL_SCurveLocal")) {
+            return false;
+        }
+        updateKinematics(q_full);
 
-        pinocchio::forwardKinematics(model_, *data_, q_full);
+        const pinocchio::SE3& oM_base = getBasePoseWorld(); // world<-base
-        pinocchio::updateFramePlacements(model_, *data_);
-
-        const pinocchio::SE3 oM_base = data_->oMf[base_frame_id_]; // world<-base
         const Eigen::Matrix3d R_ob   = oM_base.rotation();
 
         const pinocchio::SE3 &oM_cur = data_->oMf[ee_id];
@@ -798,7 +766,7 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
                                         J_full);
 
         // !!! use v_start/v_dof (more correct)
-        Eigen::MatrixXd J = J_full.middleCols(chain_v_start_, chain_v_dof_);
+        Eigen::MatrixXd J = extractChainJacobian(J_full);
         Eigen::MatrixXd J_pinv = dampedPseudoInverse(J, damping_);
 
         // C) singular monitor
@@ -831,9 +799,7 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
 
         // nullspace posture
         const Eigen::VectorXd dq_posture = -k_posture * (q_chain - q_ref);
-        const Eigen::MatrixXd I = Eigen::MatrixXd::Identity(chain_dof_, chain_dof_);
+        const Eigen::VectorXd dq_raw = dq_task + projectToNullspace(J_pinv, J, dq_posture);
-        const Eigen::MatrixXd N = I - J_pinv * J;   // DLS approx projector
-        const Eigen::VectorXd dq_raw = dq_task + N * dq_posture;
 
         // ---------------- gamma_speed (joint speed) ----------------
         double gamma_speed = 1.0;
@@ -850,11 +816,11 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
             if (std::abs(dqi) < eps_dq) continue;
 
             if (dqi > 0.0) {
-                const double margin = q_upper_chain_[i] - q_chain[i];
+                const double margin = joint_pos_upper_limits_[i] - q_chain[i];
                 const double g = margin / (dqi * step_real);
                 gamma_lim = std::min(gamma_lim, g);
             } else { // dqi < 0
-                const double margin = q_chain[i] - q_lower_chain_[i];
+                const double margin = q_chain[i] - joint_pos_lower_limits_[i];
                 const double g = margin / ((-dqi) * step_real);
                 gamma_lim = std::min(gamma_lim, g);
             }
@@ -879,7 +845,7 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
         q_chain += (gamma_tot * dq_raw) * step_real;
 
         // (理论上 gamma_lim 已保证不越界，这里只是数值保险)
-        q_chain = q_chain.cwiseMax(q_lower_chain_).cwiseMin(q_upper_chain_);
+        q_chain = q_chain.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_);
 
         // ---- advance profile time (time scaling) ----
         // IMPORTANT: tau advances by gamma_tot*dt_real => if slowed by limits, profile slows too
@@ -893,8 +859,8 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
             bool near_limit = false;
             int near_cnt = 0;
             for (int i = 0; i < chain_dof_; ++i) {
-                const bool nl = (q_upper_chain_[i] - q_chain[i] < 1e-8) ||
+                const bool nl = (joint_pos_upper_limits_[i] - q_chain[i] < 1e-8) ||
-                                (q_chain[i] - q_lower_chain_[i] < 1e-8);
+                                (q_chain[i] - joint_pos_lower_limits_[i] < 1e-8);
                 if (nl) { near_limit = true; near_cnt++; }
             }
 

cmvr-es/ik_solver/src/pinocchio_ik_base.cpp

@@ -0,0 +1,154 @@
+// Created by Codex on 2026/3/3.
+
+#include "ik_solver/include/pinocchio_ik_base.h"
+
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+
+#include <iostream>
+
+namespace cmvr {
+
+PinocchioIKBase::PinocchioIKBase(const std::string& urdf_path,
+                                 const std::string& base_frame_name,
+                                 const std::string& flange_frame_name,
+                                 const std::string& tcp_frame_name)
+    : IKSolver(urdf_path, base_frame_name, flange_frame_name)
+    , tcp_frame_name_(tcp_frame_name) {
+}
+
+PinocchioIKBase::PinocchioIKBase(std::shared_ptr<const UrdfParser> parser,
+                                 const std::string& base_frame_name,
+                                 const std::string& flange_frame_name,
+                                 const std::string& tcp_frame_name)
+    : IKSolver(parser, base_frame_name, flange_frame_name)
+    , tcp_frame_name_(tcp_frame_name) {
+}
+
+bool PinocchioIKBase::initPinocchioFromUrdfChain(UrdfParser::ChainInfo* chain_info_out,
+                                                  std::string* error) {
+    if (!urdf_parser_ || !urdf_parser_->loaded()) {
+        if (error) *error = "urdf parser not initialized";
+        return false;
+    }
+    if (chain_base_frame_name_.empty() || chain_tip_frame_name_.empty()) {
+        if (error) *error = "chain base/tip frame name is empty";
+        return false;
+    }
+    if (!urdf_chain_cached_) {
+        if (!initUrdfChain(urdf_parser_, chain_base_frame_name_, chain_tip_frame_name_)) {
+            if (error) *error = "initUrdfChain failed";
+            return false;
+        }
+    }
+
+    model_ = urdf_parser_->model();
+    data_ = std::make_unique<pinocchio::Data>(model_);
+
+    UrdfParser::ChainInfo chain_info;
+    std::string chain_err;
+    if (!urdf_parser_->extractChain(chain_base_frame_name_, chain_tip_frame_name_, chain_info, &chain_err)) {
+        if (error) *error = chain_err;
+        return false;
+    }
+
+    base_frame_id_ = chain_info.base_frame_id;
+    flange_frame_id_ = chain_info.tip_frame_id;
+    chain_q_start_ = chain_info.q_start;
+    chain_q_dof_ = chain_info.q_dof;
+    chain_v_start_ = chain_info.v_start;
+    chain_v_dof_ = chain_info.v_dof;
+
+    has_tcp_ = false;
+    tcp_frame_id_ = (pinocchio::FrameIndex)(-1);
+    if (!tcp_frame_name_.empty() && model_.existFrame(tcp_frame_name_)) {
+        tcp_frame_id_ = model_.getFrameId(tcp_frame_name_);
+        has_tcp_ = true;
+    }
+
+    if (chain_info_out != nullptr) {
+        *chain_info_out = chain_info;
+    }
+    return true;
+}
+
+bool PinocchioIKBase::buildFullQFromInput(const std::vector<double>& joints,
+                                          Eigen::VectorXd& q_full,
+                                          const char* context) const {
+    const int size = static_cast<int>(joints.size());
+    if (size != chain_q_dof_ && size != model_.nq) {
+        if (context != nullptr) {
+            std::cerr << "[PinocchioIKBase] " << context << " joints size mismatch\n";
+        }
+        return false;
+    }
+
+    q_full = pinocchio::neutral(model_);
+    if (size == model_.nq) {
+        q_full = Eigen::Map<const Eigen::VectorXd>(joints.data(), model_.nq);
+    } else {
+        Eigen::Map<const Eigen::VectorXd> q_chain(joints.data(), chain_q_dof_);
+        q_full.segment(chain_q_start_, chain_q_dof_) = q_chain;
+    }
+    return true;
+}
+
+void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) {
+    pinocchio::forwardKinematics(model_, *data_, q_full);
+    pinocchio::updateFramePlacements(model_, *data_);
+}
+
+bool PinocchioIKBase::fk(const std::vector<double>& joints_angle,
+                         Eigen::Matrix4d& cur_pose,
+                         bool is_tcp) {
+    const std::string& ee_link = (is_tcp && has_tcp_) ? tcp_frame_name_ : chain_tip_frame_name_;
+    return fk(chain_base_frame_name_, ee_link, joints_angle, cur_pose);
+}
+
+bool PinocchioIKBase::fk(const std::string& base_link,
+                         const std::string& ee_link,
+                         const std::vector<double>& joints_angle,
+                         Eigen::Matrix4d& cur_pose) {
+    if (!data_) {
+        std::cerr << "[PinocchioIKBase] fk called before pinocchio init\n";
+        return false;
+    }
+    if (!model_.existFrame(base_link)) {
+        std::cerr << "[PinocchioIKBase] base frame not found: " << base_link << "\n";
+        return false;
+    }
+    if (!model_.existFrame(ee_link)) {
+        std::cerr << "[PinocchioIKBase] ee frame not found: " << ee_link << "\n";
+        return false;
+    }
+
+    Eigen::VectorXd q_full;
+    if (!buildFullQFromInput(joints_angle, q_full, "fk")) {
+        return false;
+    }
+    updateKinematics(q_full);
+
+    const pinocchio::FrameIndex base_id = model_.getFrameId(base_link);
+    const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
+    const pinocchio::SE3& oM_base = data_->oMf[base_id];
+    const pinocchio::SE3& oM_ee = data_->oMf[ee_id];
+    const pinocchio::SE3 base_M_ee = oM_base.inverse() * oM_ee;
+    cur_pose = se3ToMatrix4(base_M_ee);
+    return true;
+}
+
+pinocchio::SE3 PinocchioIKBase::matrix4ToSE3(const Eigen::Matrix4d& T) {
+    pinocchio::SE3 M;
+    M.rotation() = T.block<3,3>(0,0);
+    M.translation() = T.block<3,1>(0,3);
+    return M;
+}
+
+Eigen::Matrix4d PinocchioIKBase::se3ToMatrix4(const pinocchio::SE3& M) {
+    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+    T.block<3,3>(0,0) = M.rotation();
+    T.block<3,1>(0,3) = M.translation();
+    return T;
+}
+
+} // namespace cmvr

cmvr-es/ik_solver/src/pinocchio_qp_ik_solver.cpp

@@ -4,7 +4,6 @@
 
 #include "ik_solver/include/pinocchio_qp_ik_solver.h"
 
-#include <pinocchio/parsers/urdf.hpp>
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/algorithm/jacobian.hpp>
@@ -29,286 +28,151 @@ namespace cmvr {
                                              int max_iters,
                                              double tol,
                                              double qp_time_limit)
-        : urdf_path_(urdf_path)
+        : PinocchioIKBase(urdf_path, base_frame_name, flange_frame_name, tcp_frame_name)
-          , base_frame_name_(base_frame_name)
+          , urdf_path_(urdf_path)
-          , flange_frame_name_(flange_frame_name)
-          , tcp_frame_name_(tcp_frame_name)
           , lambda_(lambda)
           , w_posrot_(w_posrot)
           , max_iters_(max_iters)
           , tol_(tol)
           , qp_time_limit_(qp_time_limit)
           , solver_() {
-        // 不再在 IK/FK 里使用 T_tool_flange_ / T_arm_robot_ 做额外变换，统一 URDF base
-        T_tool_flange_.setIdentity();
-        T_arm_robot_.setIdentity();
     }
 
-    PinocchioQpIKSolver::PinocchioQpIKSolver():solver_(){
+    PinocchioQpIKSolver::PinocchioQpIKSolver()
+        : PinocchioIKBase("", "", "", "")
+        , solver_() {
         config::PinocchioQpIKConfig config;
         if (ConfigHelper::getPinocchioQpIkSolverConfig(config))
         {
             urdf_path_ = config.urdf_path();
-            base_frame_name_ = config.base_frame_name();
+            chain_base_frame_name_ = config.base_frame_name();
-            flange_frame_name_ = config.flange_frame_name();
+            chain_tip_frame_name_ = config.flange_frame_name();
             tcp_frame_name_ = config.tcp_frame_name();
             lambda_ = config.lambda();
             w_posrot_ = config.w_posrot();
             max_iters_ = config.max_iters();
             tol_ = config.tol();
             qp_time_limit_ = config.qp_time_limit();
+            if (!urdf_path_.empty() &&
+                !chain_base_frame_name_.empty() &&
+                !chain_tip_frame_name_.empty()) {
+                initUrdfChain(urdf_path_, chain_base_frame_name_, chain_tip_frame_name_);
+            }
         }
-        // 不再在 IK/FK 里使用 T_tool_flange_ / T_arm_robot_ 做额外变换，统一 URDF base
-        T_tool_flange_.setIdentity();
-        T_arm_robot_.setIdentity();
     }
 
-    PinocchioQpIKSolver::PinocchioQpIKSolver(const config::PinocchioQpIKConfig& config):config_(config),solver_()
+    PinocchioQpIKSolver::PinocchioQpIKSolver(const config::PinocchioQpIKConfig& config)
+        : PinocchioIKBase(config.urdf_path(),
+                          config.base_frame_name(),
+                          config.flange_frame_name(),
+                          config.tcp_frame_name())
+        , config_(config)
+        , solver_()
     {
         urdf_path_ = config.urdf_path();
-        base_frame_name_ = config.base_frame_name();
-        flange_frame_name_ = config.flange_frame_name();
-        tcp_frame_name_ = config.tcp_frame_name();
         lambda_ = config.lambda();
         w_posrot_ = config.w_posrot();
         max_iters_ = config.max_iters();
         tol_ = config.tol();
         qp_time_limit_ = config.qp_time_limit();
-
-        T_tool_flange_.setIdentity();
-        T_arm_robot_.setIdentity();
-    }
-
-    pinocchio::SE3 PinocchioQpIKSolver::matrix4ToSE3(const Matrix4d &T) {
-        pinocchio::SE3 M;
-        M.rotation() = T.block<3, 3>(0, 0);
-        M.translation() = T.block<3, 1>(0, 3);
-        return M;
-    }
-
-    Matrix4d PinocchioQpIKSolver::se3ToMatrix4(const pinocchio::SE3 &M) {
-        Matrix4d T = Matrix4d::Identity();
-        T.block<3, 3>(0, 0) = M.rotation();
-        T.block<3, 1>(0, 3) = M.translation();
-        return T;
     }
 
     bool PinocchioQpIKSolver::init() {
-        try {
+        if (!urdf_chain_cached_) {
-            pinocchio::urdf::buildModel(urdf_path_, model_);
+            if (urdf_path_.empty() ||
-        } catch (const std::exception &e) {
+                chain_base_frame_name_.empty() ||
-            std::cerr << "[PinocchioQpIKSolver] Failed to load URDF: "
+                chain_tip_frame_name_.empty()) {
-                    << e.what() << std::endl;
+                std::cerr << "[PinocchioQpIKSolver] missing urdf/base/flange config before init().\n";
-            return false;
+                return false;
-        }
-
-        data_ = std::make_unique<pinocchio::Data>(model_);
-        full_dof_ = static_cast<int>(model_.nq);
-
-        // 1) base frame
-        if (!model_.existFrame(base_frame_name_)) {
-            std::cerr << "[PinocchioQpIKSolver] Base frame '" << base_frame_name_
-                    << "' not found. Available frames:" << std::endl;
-            for (std::size_t i = 0; i < model_.frames.size(); ++i) {
-                std::cerr << "  [" << i << "] " << model_.frames[i].name << std::endl;
             }
-            return false;
+            if (!initUrdfChain(urdf_path_, chain_base_frame_name_, chain_tip_frame_name_)) {
-        }
+                std::cerr << "[PinocchioQpIKSolver] Failed to cache URDF chain metadata in IKSolver.\n";
-        base_frame_id_ = model_.getFrameId(base_frame_name_);
+                return false;
-        if (base_frame_id_ >= model_.frames.size()) {
-            std::cerr << "[PinocchioQpIKSolver] Base frame id out of range: "
-                    << base_frame_id_ << std::endl;
-            return false;
-        }
-
-        // 2) flange frame
-        if (!model_.existFrame(flange_frame_name_)) {
-            std::cerr << "[PinocchioQpIKSolver] Flange frame '" << flange_frame_name_
-                    << "' not found. Available frames:" << std::endl;
-            for (std::size_t i = 0; i < model_.frames.size(); ++i) {
-                std::cerr << "  [" << i << "] " << model_.frames[i].name << std::endl;
-            }
-            return false;
-        }
-        flange_frame_id_ = model_.getFrameId(flange_frame_name_);
-        if (flange_frame_id_ >= model_.frames.size()) {
-            std::cerr << "[PinocchioQpIKSolver] Flange frame id out of range: "
-                    << flange_frame_id_ << std::endl;
-            return false;
-        }
-
-        // 3) TCP frame
-        has_tcp_ = false;
-        if (!tcp_frame_name_.empty()) {
-            if (!model_.existFrame(tcp_frame_name_)) {
-                std::cerr << "[PinocchioQpIKSolver] TCP frame '" << tcp_frame_name_
-                        << "' not found, fallback to flange only." << std::endl;
-            } else {
-                tcp_frame_id_ = model_.getFrameId(tcp_frame_name_);
-                if (tcp_frame_id_ >= model_.frames.size()) {
-                    std::cerr << "[PinocchioQpIKSolver] TCP frame id out of range: "
-                            << tcp_frame_id_ << std::endl;
-                } else {
-                    has_tcp_ = true;
-
-                    // 中立位姿算一次 法兰->TCP 固定变换，存到 T_tool_flange_
-                    Eigen::VectorXd q0 = pinocchio::neutral(model_);
-                    pinocchio::forwardKinematics(model_, *data_, q0);
-                    pinocchio::updateFramePlacements(model_, *data_);
-
-                    const pinocchio::SE3 &oM_flange = data_->oMf[flange_frame_id_];
-                    const pinocchio::SE3 &oM_tcp = data_->oMf[tcp_frame_id_];
-
-                    pinocchio::SE3 flange_M_tcp = oM_flange.inverse() * oM_tcp;
-                    T_tool_flange_ = se3ToMatrix4(flange_M_tcp);
-                }
             }
         }
 
-        // 4) 构建 base→flange 子链并提取限位
+        UrdfParser::ChainInfo chain_info;
-        if (!buildActiveChain()) {
+        std::string err;
+        if (!initPinocchioFromUrdfChain(&chain_info, &err)) {
+            std::cerr << "[PinocchioQpIKSolver] Failed to init pinocchio base: " << err << std::endl;
             return false;
         }
 
-        // 5) 初始化整机 q_full_，用中立位姿
+        if (chain_q_dof_ <= 0 || chain_v_dof_ <= 0) {
-        q_full_ = pinocchio::neutral(model_);
+            std::cerr << "[PinocchioQpIKSolver] Invalid chain dof: q=" << chain_q_dof_
+                      << " v=" << chain_v_dof_ << std::endl;
+            return false;
+        }
+        if (chain_q_dof_ != chain_v_dof_) {
+            std::cerr << "[PinocchioQpIKSolver] requires q_dof == v_dof, got q="
+                      << chain_q_dof_ << " v=" << chain_v_dof_ << std::endl;
+            return false;
+        }
 
-        // 6) 子链当前关节角（外部 update_joints_state 也会覆盖）
+        // 当前实现仅支持 nq=1,nv=1 且链索引在 q/v 上连续的关节序列。
+        int expected_q = chain_q_start_;
+        int expected_v = chain_v_start_;
+        for (const auto& seg : chain_info.joints) {
+            if (seg.nq != 1 || seg.nv != 1) {
+                std::cerr << "[PinocchioQpIKSolver] only supports nq=1,nv=1 joints. joint="
+                          << seg.name << " nq=" << seg.nq << " nv=" << seg.nv << std::endl;
+                return false;
+            }
+            if (seg.q_index != expected_q || seg.v_index != expected_v) {
+                std::cerr << "[PinocchioQpIKSolver] chain q/v index must be contiguous. joint="
+                          << seg.name << " q_index=" << seg.q_index << " expect_q=" << expected_q
+                          << " v_index=" << seg.v_index << " expect_v=" << expected_v << std::endl;
+                return false;
+            }
+            expected_q += seg.nq;
+            expected_v += seg.nv;
+        }
+
+        if (joint_pos_lower_limits_.size() != chain_q_dof_ ||
+            joint_pos_upper_limits_.size() != chain_q_dof_) {
+            std::cerr << "[PinocchioQpIKSolver] position limits size mismatch with q_dof." << std::endl;
+            return false;
+        }
+
+        const double big = 1e6;
+        if (joint_vel_limits_.size() != chain_v_dof_) {
+            joint_vel_limits_.resize(chain_v_dof_);
+            joint_vel_limits_.setConstant(big);
+        }
+        qdd_max_global_.resize(chain_q_dof_);
+        qdd_max_global_.setConstant(big);
+
+        // 子链当前关节角（外部 update_joints_state 也会覆盖）
         if (cur_joints_angle_.empty()) {
-            cur_joints_angle_.assign(dof_, 0.0);
+            cur_joints_angle_.assign(chain_q_dof_, 0.0);
         }
 
         initialized_ = true;
-        std::cout << "[PinocchioQpIKSolver] Init OK. full nq = " << full_dof_
+        std::cout << "[PinocchioQpIKSolver] Init OK. full nq = " << model_.nq
-                << ", active dof = " << dof_
+                << ", active dof = " << chain_q_dof_
-                << ", base frame = " << base_frame_name_
+                << ", q_start=" << chain_q_start_ << ", v_start=" << chain_v_start_
-                << ", flange frame = " << flange_frame_name_
+                << ", base frame = " << chain_base_frame_name_
+                << ", flange frame = " << chain_tip_frame_name_
                 << ", tcp frame = " << (has_tcp_ ? tcp_frame_name_ : "<none>")
                 << std::endl;
         return true;
     }
 
-    bool PinocchioQpIKSolver::buildActiveChain() {
-        active_joints_.clear();
-        active_q_idx_.clear();
-        active_v_idx_.clear();
-        dof_ = 0;
-
-        const auto &base_frame = model_.frames[base_frame_id_];
-        const auto &flange_frame = model_.frames[flange_frame_id_];
-
-        pinocchio::JointIndex base_joint = base_frame.parent; // 作为“子链起点”的 joint（不一定被包含）
-        pinocchio::JointIndex flange_joint = flange_frame.parent; // 子链末端 joint
-
-        if (flange_joint == 0) {
-            std::cerr << "[PinocchioQpIKSolver] flange_frame parent joint is 0, "
-                    << "cannot build chain." << std::endl;
-            return false;
-        }
-
-        // 从 flange_joint 一路往上爬，直到：
-        //   1）遇到 base_joint（如果 base_joint != 0），则停止
-        //   2）爬到 root(0) 且 base_joint==0，也停止
-        //   3）爬到 root(0) 且 base_joint!=0，说明 base 不在祖先链上，报错
-        std::vector<pinocchio::JointIndex> chain; // flange → base（反向）
-        pinocchio::JointIndex j = flange_joint;
-        while (true) {
-            if (j == base_joint) {
-                if (j != 0) {
-                    chain.push_back(j); // base_joint != 0 时，把它也算进去
-                }
-                break;
-            }
-            if (j == 0) {
-                if (base_joint == 0) {
-                    // base_joint 是 root，已经到头了，root 不加入链条
-                    break;
-                } else {
-                    std::cerr << "[PinocchioQpIKSolver] base joint is not an ancestor of flange joint."
-                            << " (flange_joint=" << flange_joint
-                            << ", base_joint=" << base_joint << ")" << std::endl;
-                    return false;
-                }
-            }
-
-            chain.push_back(j);
-            j = model_.parents[j];
-        }
-
-        if (chain.empty()) {
-            std::cerr << "[PinocchioQpIKSolver] Active chain is empty (base==flange?)." << std::endl;
-            return false;
-        }
-
-        // 现在 chain 是 flange→base，反转得到 base→flange
-        std::reverse(chain.begin(), chain.end());
-
-        full_dof_ = static_cast<int>(model_.nq);
-
-        // 建立子链 DOF 映射（这里假定每个 joint nq=1,nv=1，对典型 6/7 自由度机械臂成立）
-        for (auto joint: chain) {
-            // 如果你是 Pinocchio 3，可以在这里检查 nqs/nvs：
-            // int nq_j = model_.nqs[joint];
-            // int nv_j = model_.nvs[joint];
-            // if (nq_j != 1 || nv_j != 1) { ... }
-
-            int q_idx = model_.idx_qs[joint];
-            int v_idx = model_.idx_vs[joint];
-
-            active_joints_.push_back(joint);
-            active_q_idx_.push_back(q_idx);
-            active_v_idx_.push_back(v_idx);
-            ++dof_;
-        }
-
-        // 从全局 limit 里抽取子链 limit
-        q_min_global_.resize(dof_);
-        q_max_global_.resize(dof_);
-        for (int i = 0; i < dof_; ++i) {
-            q_min_global_(i) = model_.lowerPositionLimit(active_q_idx_[i]);
-            q_max_global_(i) = model_.upperPositionLimit(active_q_idx_[i]);
-        }
-
-        // 速度限位：优先用 URDF 的 velocityLimit（nv 维），否则给大值
-        double big = 1e6;
-        qd_max_global_.resize(dof_);
-        if (model_.velocityLimit.size() == model_.nv) {
-            for (int i = 0; i < dof_; ++i) {
-                qd_max_global_(i) = model_.velocityLimit(active_v_idx_[i]);
-            }
-        } else {
-            qd_max_global_.setConstant(big);
-        }
-
-
-        // 加速度限位：URDF 没有，先给大值；如果你有更精细的，用 setAccelerationLimits 覆盖
-        qdd_max_global_.resize(dof_);
-        qdd_max_global_.setConstant(big);
-
-        std::cout << "[PinocchioQpIKSolver] Active chain joints: ";
-        for (auto joint: active_joints_) {
-            std::cout << joint << " ";
-        }
-        std::cout << "(dof = " << dof_ << ")" << std::endl;
-
-        return true;
-    }
-
     void PinocchioQpIKSolver::setVelocityLimits(const Eigen::VectorXd &qd_max) {
-        if (qd_max.size() == dof_) {
+        if (qd_max.size() == chain_v_dof_) {
-            qd_max_global_ = qd_max;
+            joint_vel_limits_ = qd_max;
         } else {
             std::cerr << "[PinocchioQpIKSolver] setVelocityLimits size mismatch. got "
-                    << qd_max.size() << ", expect " << dof_ << std::endl;
+                    << qd_max.size() << ", expect " << chain_v_dof_ << std::endl;
         }
     }
 
     void PinocchioQpIKSolver::setAccelerationLimits(const Eigen::VectorXd &qdd_max) {
-        if (qdd_max.size() == dof_) {
+        if (qdd_max.size() == chain_q_dof_) {
             qdd_max_global_ = qdd_max;
         } else {
             std::cerr << "[PinocchioQpIKSolver] setAccelerationLimits size mismatch. got "
-                    << qdd_max.size() << ", expect " << dof_ << std::endl;
+                    << qdd_max.size() << ", expect " << chain_q_dof_ << std::endl;
         }
     }
 
@@ -319,9 +183,9 @@ namespace cmvr {
             std::cerr << "[PinocchioQpIKSolver] IK called before init()." << std::endl;
             return false;
         }
-        if (cur_joints_angle_.size() != static_cast<std::size_t>(dof_)) {
+        if (cur_joints_angle_.size() != static_cast<std::size_t>(chain_q_dof_)) {
             std::cerr << "[PinocchioQpIKSolver] cur_joints_angle_ size mismatch: "
-                    << cur_joints_angle_.size() << " vs dof = " << dof_ << std::endl;
+                    << cur_joints_angle_.size() << " vs dof = " << chain_q_dof_ << std::endl;
             return false;
         }
 
@@ -346,30 +210,27 @@ namespace cmvr {
         Eigen::VectorXd q_local = Eigen::Map<Eigen::VectorXd>(
             cur_joints_angle_.data(), cur_joints_angle_.size());
 
-        // 把 q_local 写入 q_full_ 的对应索引
+        Eigen::VectorXd q_full;
-        if (q_full_.size() != full_dof_) {
+        if (!buildFullQFromInput(cur_joints_angle_, q_full, "ik(qp)")) {
-            q_full_ = pinocchio::neutral(model_);
+            return false;
-        }
-        for (int i = 0; i < dof_; ++i) {
-            q_full_(active_q_idx_[i]) = q_local(i);
         }
+        q_full.segment(chain_q_start_, chain_q_dof_) = q_local;
 
         // 初始化 QP（变量 = 子链 DOF，约束 = 子链 DOF，因为只做 box 约束）
-        solver_.Setup(dof_, dof_, qp_time_limit_);
+        solver_.Setup(chain_q_dof_, chain_q_dof_, qp_time_limit_);
         solver_.ResetIsFirst();
 
         MatrixXd A_cost;
         VectorXd b_cost;
 
-        const VectorXd &q_min = q_min_global_;
+        const VectorXd &q_min = joint_pos_lower_limits_;
-        const VectorXd &q_max = q_max_global_;
+        const VectorXd &q_max = joint_pos_upper_limits_;
-        const VectorXd &qd_max = qd_max_global_;
+        const VectorXd &qd_max = joint_vel_limits_;
         const VectorXd &qdd_max = qdd_max_global_;
 
         for (int iter = 0; iter < max_iters_; ++iter) {
-            // 1) 用 q_full_ 做正解 + frame pose + J_full
+            // 1) 用 q_full 做正解 + frame pose + J_full
-            pinocchio::forwardKinematics(model_, *data_, q_full_);
+            updateKinematics(q_full);
-            pinocchio::updateFramePlacements(model_, *data_);
 
             const pinocchio::SE3 &oM_curr = data_->oMf[target_frame_id];
 
@@ -391,34 +252,31 @@ namespace cmvr {
 
             // 6×nv Jacobian（LOCAL）
             Eigen::Matrix<double, 6, Eigen::Dynamic> J_full(6, model_.nv);
-            pinocchio::computeFrameJacobian(model_, *data_, q_full_,
+            pinocchio::computeFrameJacobian(model_, *data_, q_full,
                                             target_frame_id,
                                             pinocchio::ReferenceFrame::LOCAL,
                                             J_full);
 
-            // 抽取子链对应列 => 6×dof_ 雅可比
+            // 抽取子链对应列 => 6×chain_q_dof_ 雅可比
-            MatrixXd J(6, dof_);
+            MatrixXd J = J_full.middleCols(chain_v_start_, chain_v_dof_);
-            for (int i = 0; i < dof_; ++i) {
-                J.col(i) = J_full.col(active_v_idx_[i]);
-            }
 
             // 2) 构造代价：|| A_cost * dq - b_cost ||^2
             constexpr std::size_t n_eq = 6;
-            A_cost.resize(n_eq + dof_, dof_);
+            A_cost.resize(n_eq + chain_q_dof_, chain_q_dof_);
-            b_cost.resize(n_eq + dof_);
+            b_cost.resize(n_eq + chain_q_dof_);
 
             // 任务部分
-            A_cost.block(0, 0, 6, dof_) = J;
+            A_cost.block(0, 0, 6, chain_q_dof_) = J;
             b_cost.segment(0, 6) = dx;
 
             // 阻尼项
-            A_cost.bottomRows(dof_) = std::sqrt(lambda_) *
+            A_cost.bottomRows(chain_q_dof_) = std::sqrt(lambda_) *
-                                      MatrixXd::Identity(dof_, dof_);
+                                      MatrixXd::Identity(chain_q_dof_, chain_q_dof_);
-            b_cost.tail(dof_).setZero();
+            b_cost.tail(chain_q_dof_).setZero();
 
             // 3) box 约束：lb <= dq_local <= ub
-            VectorXd lb(dof_), ub(dof_);
+            VectorXd lb(chain_q_dof_), ub(chain_q_dof_);
-            for (int i = 0; i < dof_; ++i) {
+            for (int i = 0; i < chain_q_dof_; ++i) {
                 double pos_lb = q_min(i) - q_local(i);
                 double pos_ub = q_max(i) - q_local(i);
 
@@ -437,7 +295,7 @@ namespace cmvr {
 
             // 4) 调用 QP solver
             solver_.SetCostFunction(A_cost, b_cost);
-            solver_.SetConstraintsFunction(MatrixXd::Identity(dof_, dof_), lb, ub);
+            solver_.SetConstraintsFunction(MatrixXd::Identity(chain_q_dof_, chain_q_dof_), lb, ub);
 
             VectorXd dq_local;
             try {
@@ -451,12 +309,12 @@ namespace cmvr {
                 return false;
             }
 
-            // 5) 更新 q_local & 写回 q_full_
+            // 5) 更新 q_local 并写回 q_full
             q_local += dq_local;
-            for (int i = 0; i < dof_; ++i) {
+            for (int i = 0; i < chain_q_dof_; ++i) {
                 q_local(i) = std::clamp(q_local(i), q_min(i), q_max(i));
-                q_full_(active_q_idx_[i]) = q_local(i);
             }
+            q_full.segment(chain_q_start_, chain_q_dof_) = q_local;
 
             // 6) 收敛检查
             double err_norm = b_cost.head(n_eq).norm();
@@ -479,48 +337,4 @@ namespace cmvr {
     }
 
 
-    bool PinocchioQpIKSolver::fk(const std::vector<double> &joints_angle,
-                                 Matrix4d &cur_pose,
-                                 bool is_tcp) {
-        if (!initialized_) {
-            std::cerr << "[PinocchioQpIKSolver] FK called before init()." << std::endl;
-            return false;
-        }
-        if (static_cast<int>(joints_angle.size()) != dof_) {
-            std::cerr << "[PinocchioQpIKSolver] FK joints size mismatch. got "
-                    << joints_angle.size() << ", expect " << dof_ << std::endl;
-            return false;
-        }
-
-        pinocchio::FrameIndex target_frame_id;
-        if (is_tcp && has_tcp_) {
-            target_frame_id = tcp_frame_id_;
-        } else {
-            target_frame_id = flange_frame_id_;
-        }
-
-        Eigen::VectorXd q_local = Eigen::Map<const Eigen::VectorXd>(
-            joints_angle.data(), joints_angle.size());
-
-        if (q_full_.size() != full_dof_) {
-            q_full_ = pinocchio::neutral(model_);
-        }
-        for (int i = 0; i < dof_; ++i) {
-            q_full_(active_q_idx_[i]) = q_local(i);
-        }
-
-        pinocchio::forwardKinematics(model_, *data_, q_full_);
-        pinocchio::updateFramePlacements(model_, *data_);
-
-        // 0T_base, 0T_target
-        const pinocchio::SE3 &oM_base = data_->oMf[base_frame_id_];
-        const pinocchio::SE3 &oM_target = data_->oMf[target_frame_id];
-
-        // baseTtarget = (0Tbase)^-1 * (0Ttarget)
-        pinocchio::SE3 base_M_target = oM_base.inverse() * oM_target;
-
-        // 返回的是：相对于 base_frame_name_ 的位姿
-        cur_pose = se3ToMatrix4(base_M_target);
-        return true;
-    }
 } // namespace cmvr

cmvr-es/ik_solver/src/srs_ik_test.cpp

@@ -1106,16 +1106,16 @@ TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) {
 
         // 目标位姿：base X 方向走 0.25m，姿态保持起点
         Eigen::Matrix4d Tg = T0;
-        Tg(0,3) += 0.13;
+        Tg(0,3) += 0.1;
-        Tg(2,3) += 0.3;
+        // Tg(2,3) += 0.3;
         Eigen::Vector3d dp_check = Tg.block<3,1>(0,3) - T0.block<3,1>(0,3);
         std::cerr << "dp(base)=" << dp_check.transpose() << "\n";
 
         // 轨迹生成参数（生成 dt 不必极小，1ms~2ms 足够；真正平滑靠 S 曲线 + MuJoCo 伺服滤波）
         const double dt_gen = 0.002;
-        const double v_tcp  = 0.13;
+        const double v_tcp  = 1.0;
         const double a_tcp  = 10.0;
-        const double j_tcp  = 10.00;
+        const double j_tcp  = 100.00;
         std::vector<double> qd_max(7, 3.0);
 
         std::vector<std::vector<double>> q_traj;

cmvr-es/ik_solver/src/urdf_parser.cpp

@@ -0,0 +1,120 @@
+// Created by Codex on 2026/3/2.
+
+#include "ik_solver/include/urdf_parser.h"
+
+#include <algorithm>
+
+#include <pinocchio/parsers/urdf.hpp>
+
+namespace cmvr {
+
+bool UrdfParser::loadModel(const std::string& urdf_path, std::string* error) {
+    try {
+        // 使用 Pinocchio 官方 URDF 解析入口构建模型。
+        pinocchio::urdf::buildModel(urdf_path, model_);
+        loaded_ = true;
+        return true;
+    } catch (const std::exception& e) {
+        loaded_ = false;
+        if (error) {
+            *error = e.what();
+        }
+        return false;
+    }
+}
+
+bool UrdfParser::extractChain(const std::string& base_frame_name,
+                              const std::string& tip_frame_name,
+                              ChainInfo& out,
+                              std::string* error) const {
+    // 1) 基础合法性检查。
+    if (!loaded_) {
+        if (error) {
+            *error = "model not loaded";
+        }
+        return false;
+    }
+    if (!model_.existFrame(base_frame_name)) {
+        if (error) {
+            *error = "base frame not found: " + base_frame_name;
+        }
+        return false;
+    }
+    if (!model_.existFrame(tip_frame_name)) {
+        if (error) {
+            *error = "tip frame not found: " + tip_frame_name;
+        }
+        return false;
+    }
+
+    out = ChainInfo{};
+    out.base_frame_id = model_.getFrameId(base_frame_name);
+    out.tip_frame_id = model_.getFrameId(tip_frame_name);
+
+    const auto& base_frame = model_.frames[out.base_frame_id];
+    const auto& tip_frame = model_.frames[out.tip_frame_id];
+    const pinocchio::JointIndex base_joint = base_frame.parent;
+    const pinocchio::JointIndex tip_joint = tip_frame.parent;
+
+    // 2) 从 tip 向上回溯到 base，再反转得到 base->tip 关节序列。
+    std::vector<pinocchio::JointIndex> chain_joints;
+    pinocchio::JointIndex j = tip_joint;
+    while (j != 0 && j != base_joint) {
+        chain_joints.push_back(j);
+        j = model_.parents[j];
+    }
+    if (j == 0 && base_joint != 0) {
+        if (error) {
+            *error = "base is not ancestor of tip";
+        }
+        return false;
+    }
+    if (base_joint != 0) {
+        chain_joints.push_back(base_joint);
+    }
+    if (chain_joints.empty()) {
+        if (error) {
+            *error = "empty chain";
+        }
+        return false;
+    }
+    std::reverse(chain_joints.begin(), chain_joints.end());
+
+    // 3) 计算链路在 q/v 中的连续区间。
+    out.q_start = model_.idx_qs[chain_joints.front()];
+    const int last_q = model_.idx_qs[chain_joints.back()] + model_.nqs[chain_joints.back()] - 1;
+    out.q_dof = last_q - out.q_start + 1;
+
+    out.v_start = model_.idx_vs[chain_joints.front()];
+    const int last_v = model_.idx_vs[chain_joints.back()] + model_.nvs[chain_joints.back()] - 1;
+    out.v_dof = last_v - out.v_start + 1;
+
+    if (out.q_dof <= 0 || out.v_dof <= 0) {
+        if (error) {
+            *error = "invalid chain dof";
+        }
+        return false;
+    }
+
+    // 4) 读取链路对应的位置/速度限位。
+    out.q_lower = model_.lowerPositionLimit.segment(out.q_start, out.q_dof);
+    out.q_upper = model_.upperPositionLimit.segment(out.q_start, out.q_dof);
+    out.v_limit = model_.velocityLimit.segment(out.v_start, out.v_dof);
+
+    // 5) 输出关节明细（名称、q/v 下标与维度）。
+    out.joints.clear();
+    out.joints.reserve(chain_joints.size());
+    for (const auto joint_id : chain_joints) {
+        JointSegment seg;
+        seg.joint_id = joint_id;
+        seg.name = model_.names[joint_id];
+        seg.q_index = model_.idx_qs[joint_id];
+        seg.nq = model_.nqs[joint_id];
+        seg.v_index = model_.idx_vs[joint_id];
+        seg.nv = model_.nvs[joint_id];
+        out.joints.push_back(seg);
+    }
+    return true;
+}
+
+} // namespace cmvr