refactor: optimize IK solver code
This commit is contained in:
parent
774e8a16e5
commit
441a5e6e64
@ -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
|
||||
)
|
||||
|
||||
|
||||
|
||||
302
cmvr-es/common/curve/include/s_curve.h
Normal file
302
cmvr-es/common/curve/include/s_curve.h
Normal file
@ -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
|
||||
|
||||
|
||||
655
cmvr-es/common/curve/src/s_curve.cpp
Normal file
655
cmvr-es/common/curve/src/s_curve.cpp
Normal file
@ -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
|
||||
@ -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};
|
||||
|
||||
|
||||
@ -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_upper_limits_.clear();
|
||||
q_lower_limits_.resize(0);
|
||||
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(
|
||||
joints_angle, twist_ee_pin, qdot, camera_frame_name_, mu_, std::numeric_limits<double>::infinity());
|
||||
const bool ok = dls_solver_->ik(
|
||||
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];
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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_{};
|
||||
|
||||
// 机械臂urdf 中法兰姿态相对于 MDH 中法兰姿态的变换矩阵
|
||||
Eigen::Matrix4d T_flange_urdf_mdh_{};
|
||||
/**
|
||||
* @brief 使用外部 UrdfParser 初始化链路信息缓存（关节限位/关节名）。
|
||||
* @param parser 已加载模型的 URDF 解析器。
|
||||
* @param base_frame_name 链路基座 frame 名称。
|
||||
* @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_{};
|
||||
|
||||
// 设置 机械臂末端工具坐标系相对于末端法兰的变换矩阵
|
||||
void setTcpTransform(const Eigen::Matrix4d& T_tool_flange) {
|
||||
T_tool_flange_ = T_tool_flange;
|
||||
}
|
||||
// URDF 解析器（用于统一管理链路限位/关节名）。
|
||||
std::shared_ptr<const UrdfParser> urdf_parser_{};
|
||||
|
||||
// 当前缓存链路的基座 frame 名称。
|
||||
std::string chain_base_frame_name_{};
|
||||
|
||||
// 当前缓存链路的末端 frame 名称。
|
||||
std::string chain_tip_frame_name_{};
|
||||
|
||||
void setArmBaseTransform(const Eigen::Matrix4d& T_arm_robot) {
|
||||
T_arm_robot_ = T_arm_robot;
|
||||
}
|
||||
// 当前缓存链路关节位置下限（rad）。
|
||||
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};
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
@ -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};
|
||||
|
||||
@ -1,11 +1,7 @@
|
||||
// Created by lgv on 11/28/25.
|
||||
#pragma once
|
||||
|
||||
#include "ik_solver/include/ik_solver.h"
|
||||
|
||||
#include <pinocchio/multibody/model.hpp>
|
||||
#include <pinocchio/multibody/data.hpp>
|
||||
#include <pinocchio/spatial/se3.hpp>
|
||||
#include "ik_solver/include/pinocchio_ik_base.h"
|
||||
|
||||
#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,
|
||||
Eigen::Matrix4d &cur_pose,
|
||||
bool is_tcp = true) override;
|
||||
|
||||
// 指定 base_link 与 ee_link，返回 ee 在 base 下的位姿。
|
||||
bool fk(const std::string& base_link,
|
||||
/**
|
||||
* @brief 位姿 IK 求解（显式指定 base_link 与 ee_link）。
|
||||
*
|
||||
* 约束：`base_link` 与 `ee_link` 必须位于初始化单一链（base->tip）
|
||||
* 对应分支内，且 `base_link` 需是 `ee_link` 的祖先（或同一 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,
|
||||
Eigen::Matrix4d& cur_pose);
|
||||
const Eigen::Matrix4d& target_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。
|
||||
* @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。
|
||||
* 当前关节状态取自内部 `cur_joints_angle_`（调用前需先 `update_joints_state`）。
|
||||
*
|
||||
* @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 不存在等失败。
|
||||
* @note 函数内部会将 ee 局部 twist 转到 base 坐标系，并在 base 坐标系中完成 DLS 求解。
|
||||
* @return 成功返回 `true`，失败返回 `false`。
|
||||
*/
|
||||
bool velocityIk(const std::vector<double>& cur_angle,
|
||||
const Eigen::Matrix<double,6,1>& ee_velocity,
|
||||
std::vector<double>& joints_vel,
|
||||
const std::string& ee_link,
|
||||
double damping = -1.0,
|
||||
double qdot_abs_max = std::numeric_limits<double>::infinity());
|
||||
bool ik(const std::string& base_link,
|
||||
const std::string& ee_link,
|
||||
const Eigen::Matrix<double,6,1>& target_vel,
|
||||
std::vector<double>& joints_vel,
|
||||
double damping = -1.0,
|
||||
double qdot_abs_max = std::numeric_limits<double>::infinity());
|
||||
|
||||
/**
|
||||
* @brief 配置关节限位避障 null-space 项。
|
||||
@ -92,64 +143,103 @@ public:
|
||||
double max_push = 0.25);
|
||||
|
||||
/**
|
||||
* @brief 读取本链关节位置限位（来自 URDF）。
|
||||
* @param lower 输出下限，size=chain_dof_。
|
||||
* @param upper 输出上限，size=chain_dof_。
|
||||
* @return 成功返回 `true`。
|
||||
* @brief 设置 IK 最大迭代次数。
|
||||
* @param iters 最大迭代次数。
|
||||
*/
|
||||
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_;
|
||||
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
|
||||
/** @brief 当前链在 full-model `q` 中的起始索引。 */
|
||||
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_;
|
||||
Eigen::VectorXd q_upper_chain_;
|
||||
|
||||
// null-space 关节限位避障参数
|
||||
/** @brief 是否启用 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_;
|
||||
};
|
||||
|
||||
|
||||
142
cmvr-es/ik_solver/include/pinocchio_ik_base.h
Normal file
142
cmvr-es/ik_solver/include/pinocchio_ik_base.h
Normal file
@ -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
|
||||
@ -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 模型
|
||||
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_;
|
||||
// 子链上的加速度限位（位置/速度限位复用父类 IKSolver 缓存）
|
||||
Eigen::VectorXd qdd_max_global_;
|
||||
|
||||
bool initialized_{false};
|
||||
|
||||
// 整机 q（长度 = full_dof_），用于 Pinocchio 正解 / 雅可比
|
||||
Eigen::VectorXd q_full_;
|
||||
|
||||
// QP 求解器（OsqpEigen 封装）
|
||||
QPSolver solver_;
|
||||
};
|
||||
|
||||
102
cmvr-es/ik_solver/include/urdf_parser.h
Normal file
102
cmvr-es/ik_solver/include/urdf_parser.h
Normal file
@ -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
|
||||
103
cmvr-es/ik_solver/src/ik_solver.cpp
Normal file
103
cmvr-es/ik_solver/src/ik_solver.cpp
Normal file
@ -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
|
||||
@ -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,
|
||||
// &qp_solver
|
||||
&pinv_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,
|
||||
// &pinv_solver,
|
||||
// &psi_solver,
|
||||
&pinv_solver,
|
||||
// &qp_solver
|
||||
};
|
||||
|
||||
|
||||
@ -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);
|
||||
setArmBaseTransform(T_arm_robot);
|
||||
T_tool_flange_ = T_tool_flange;
|
||||
T_arm_robot_ = T_arm_robot;
|
||||
|
||||
// 臂角更新参数
|
||||
opt_psi_selector_->set_update_params(0.6, 5.0, -1, 1e-4);
|
||||
|
||||
@ -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)
|
||||
, base_frame_name_(base_frame_name)
|
||||
, flange_frame_name_(flange_frame_name)
|
||||
, tcp_frame_name_(tcp_frame_name)
|
||||
: PinocchioIKBase(urdf_path, base_frame_name, flange_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) {
|
||||
pinocchio::SE3 M;
|
||||
M.rotation() = T.block<3,3>(0,0);
|
||||
M.translation() = T.block<3,1>(0,3);
|
||||
return M;
|
||||
bool PinocchioDlsIKSolver::buildFullQFromChain(const Eigen::VectorXd& q_chain,
|
||||
Eigen::VectorXd& q_full,
|
||||
const char* context) const {
|
||||
if (q_chain.size() != chain_dof_) {
|
||||
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::Matrix4d T = Eigen::Matrix4d::Identity();
|
||||
T.block<3,3>(0,0) = M.rotation();
|
||||
T.block<3,1>(0,3) = M.translation();
|
||||
return T;
|
||||
Eigen::MatrixXd PinocchioDlsIKSolver::extractChainJacobian(
|
||||
const Eigen::Matrix<double,6,Eigen::Dynamic>& J_full) const {
|
||||
return J_full.middleCols(chain_v_start_, chain_v_dof_);
|
||||
}
|
||||
|
||||
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 {
|
||||
pinocchio::urdf::buildModel(urdf_path_, model_);
|
||||
} catch (const std::exception &e) {
|
||||
std::cerr << "[PinocchioDlsIKSolver] Failed to load URDF: " << e.what() << "\n";
|
||||
UrdfParser::ChainInfo chain_info;
|
||||
std::string err;
|
||||
if (!initPinocchioFromUrdfChain(&chain_info, &err)) {
|
||||
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_)) {
|
||||
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);
|
||||
}
|
||||
// limits already cached in IKSolver base class.
|
||||
|
||||
// cache base pose (PELVIS_S fixed)
|
||||
{
|
||||
Eigen::VectorXd q0 = pinocchio::neutral(model_);
|
||||
pinocchio::forwardKinematics(model_, *data_, q0);
|
||||
pinocchio::updateFramePlacements(model_, *data_);
|
||||
updateKinematics(q0);
|
||||
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) {
|
||||
const int q_idx = model_.idx_qs[j];
|
||||
const int nq = model_.nqs[j];
|
||||
const std::string& jname = model_.names[j];
|
||||
for (const auto& seg : chain_info.joints) {
|
||||
const int q_idx = seg.q_index;
|
||||
const int nq = seg.nq;
|
||||
const std::string& jname = seg.name;
|
||||
if (nq <= 0) continue;
|
||||
|
||||
for (int k = 0; k < nq; ++k) {
|
||||
@ -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 =
|
||||
(is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_;
|
||||
const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent;
|
||||
const pinocchio::JointIndex base_joint = model_.frames[base_frame_id].parent;
|
||||
const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent;
|
||||
const pinocchio::JointIndex ee_joint = model_.frames[ee_frame_id].parent;
|
||||
|
||||
const pinocchio::SE3 base_M_target = matrix4ToSE3(target_pose_base);
|
||||
const pinocchio::SE3 &oM_base = base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_];
|
||||
const pinocchio::SE3 oM_target = oM_base * base_M_target;
|
||||
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;
|
||||
}
|
||||
|
||||
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_);
|
||||
q_full.segment(chain_q_start_, chain_dof_) = q_chain;
|
||||
Eigen::VectorXd q_full;
|
||||
if (!buildFullQFromChain(q_chain, q_full, "ik(base,ee)")) {
|
||||
return false;
|
||||
}
|
||||
updateKinematics(q_full);
|
||||
|
||||
pinocchio::forwardKinematics(model_, *data_, q_full);
|
||||
pinocchio::updateFramePlacements(model_, *data_);
|
||||
|
||||
const pinocchio::SE3 &oM_cur = data_->oMf[target_frame_id];
|
||||
const pinocchio::SE3& oM_base =
|
||||
(base_frame_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_frame_id];
|
||||
const pinocchio::SE3 oM_target = oM_base * base_M_target;
|
||||
const pinocchio::SE3 &oM_cur = data_->oMf[ee_frame_id];
|
||||
pinocchio::SE3 dM = oM_cur.inverse() * oM_target;
|
||||
|
||||
Eigen::Matrix<double,6,1> err = pinocchio::log6(dM).toVector();
|
||||
@ -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,
|
||||
const Eigen::Matrix<double,6,1>& ee_velocity,
|
||||
std::vector<double>& joints_vel,
|
||||
const std::string& ee_link,
|
||||
double damping,
|
||||
double qdot_abs_max)
|
||||
bool PinocchioDlsIKSolver::ik(const std::string& base_link,
|
||||
const std::string& ee_link,
|
||||
const Eigen::Matrix<double,6,1>& target_vel,
|
||||
std::vector<double>& joints_vel,
|
||||
double damping,
|
||||
double qdot_abs_max)
|
||||
{
|
||||
// 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）。
|
||||
Eigen::VectorXd q_full = pinocchio::neutral(model_);
|
||||
if (size == model_.nq) {
|
||||
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_);
|
||||
q_full.segment(chain_q_start_, chain_dof_) = q_chain;
|
||||
if (static_cast<int>(cur_joints_angle_.size()) != chain_dof_) {
|
||||
std::cerr << "[PinocchioDlsIKSolver] ik(velocity) current joint state not initialized\n";
|
||||
return false;
|
||||
}
|
||||
|
||||
const pinocchio::FrameIndex base_id = model_.getFrameId(base_link);
|
||||
const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
|
||||
|
||||
const pinocchio::JointIndex chain_base_joint = model_.frames[base_frame_id_].parent;
|
||||
const pinocchio::JointIndex base_joint = model_.frames[base_id].parent;
|
||||
const pinocchio::JointIndex flange_joint = model_.frames[flange_frame_id_].parent;
|
||||
const pinocchio::JointIndex ee_joint = model_.frames[ee_id].parent;
|
||||
|
||||
auto jointOnParentPath = [this](pinocchio::JointIndex from,
|
||||
pinocchio::JointIndex target) {
|
||||
if (target == 0) return true;
|
||||
pinocchio::JointIndex j = from;
|
||||
while (j != 0) {
|
||||
if (j == target) return true;
|
||||
j = model_.parents[j];
|
||||
}
|
||||
return false;
|
||||
};
|
||||
auto jointOnConfiguredBranch = [&](pinocchio::JointIndex j) {
|
||||
return jointOnParentPath(flange_joint, j) && jointOnParentPath(j, chain_base_joint);
|
||||
};
|
||||
const bool base_on_branch = jointOnConfiguredBranch(base_joint);
|
||||
const bool ee_on_branch = jointOnConfiguredBranch(ee_joint);
|
||||
const bool base_is_ancestor_of_ee = jointOnParentPath(ee_joint, base_joint);
|
||||
if (!base_on_branch || !ee_on_branch || !base_is_ancestor_of_ee) {
|
||||
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);
|
||||
pinocchio::updateFramePlacements(model_, *data_);
|
||||
updateKinematics(q_full);
|
||||
|
||||
const pinocchio::FrameIndex ee_id = model_.getFrameId(ee_link);
|
||||
|
||||
const pinocchio::SE3 &oM_base = base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_];
|
||||
const pinocchio::SE3 &oM_base =
|
||||
(base_id == base_frame_id_) ? getBasePoseWorld() : data_->oMf[base_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.tail<3>() = R_be * ee_velocity.tail<3>();
|
||||
twist_base.head<3>() = R_be * target_vel.head<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_ &&
|
||||
limit_avoidance_gain_ > 0.0 &&
|
||||
chain_v_dof_ == chain_dof_ &&
|
||||
q_lower_chain_.size() == chain_dof_ &&
|
||||
q_upper_chain_.size() == chain_dof_) {
|
||||
const Eigen::VectorXd q_chain = q_full.segment(chain_q_start_, chain_dof_);
|
||||
Eigen::VectorXd qdot_avoid = Eigen::VectorXd::Zero(chain_v_dof_);
|
||||
|
||||
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;
|
||||
}
|
||||
const Eigen::VectorXd q_chain = q_full.segment(chain_q_start_, chain_dof_);
|
||||
const Eigen::VectorXd qdot_avoid = computeJointLimitAvoidanceVelocity(q_chain);
|
||||
if (qdot_avoid.size() == chain_v_dof_ && qdot_avoid.squaredNorm() > 1e-16) {
|
||||
const Eigen::Matrix<double,6,6> A_inv =
|
||||
ldlt.solve(Eigen::Matrix<double,6,6>::Identity());
|
||||
const Eigen::MatrixXd J_pinv = J.transpose() * A_inv; // n x 6
|
||||
qdot += projectToNullspace(J_pinv, J, 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) {
|
||||
const int v_idx = chain_v_start_ + i;
|
||||
if (v_idx >= 0 && v_idx < model_.nv) {
|
||||
vel_limit = std::min(vel_limit, std::abs(model_.velocityLimit[v_idx]));
|
||||
}
|
||||
if (joint_vel_limits_.size() == chain_v_dof_) {
|
||||
vel_limit = std::min(vel_limit, std::abs(joint_vel_limits_[i]));
|
||||
}
|
||||
|
||||
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_);
|
||||
q_full.segment(chain_q_start_, chain_dof_) = q_chain;
|
||||
Eigen::VectorXd q_full;
|
||||
if (!buildFullQFromChain(q_chain, q_full, "moveL_SCurveLocal")) {
|
||||
return false;
|
||||
}
|
||||
updateKinematics(q_full);
|
||||
|
||||
pinocchio::forwardKinematics(model_, *data_, q_full);
|
||||
pinocchio::updateFramePlacements(model_, *data_);
|
||||
|
||||
const pinocchio::SE3 oM_base = data_->oMf[base_frame_id_]; // world<-base
|
||||
const pinocchio::SE3& oM_base = getBasePoseWorld(); // 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::MatrixXd N = I - J_pinv * J; // DLS approx projector
|
||||
const Eigen::VectorXd dq_raw = dq_task + N * dq_posture;
|
||||
const Eigen::VectorXd dq_raw = dq_task + projectToNullspace(J_pinv, J, 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) ||
|
||||
(q_chain[i] - q_lower_chain_[i] < 1e-8);
|
||||
const bool nl = (joint_pos_upper_limits_[i] - q_chain[i] < 1e-8) ||
|
||||
(q_chain[i] - joint_pos_lower_limits_[i] < 1e-8);
|
||||
if (nl) { near_limit = true; near_cnt++; }
|
||||
}
|
||||
|
||||
|
||||
154
cmvr-es/ik_solver/src/pinocchio_ik_base.cpp
Normal file
154
cmvr-es/ik_solver/src/pinocchio_ik_base.cpp
Normal file
@ -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
|
||||
@ -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)
|
||||
, base_frame_name_(base_frame_name)
|
||||
, flange_frame_name_(flange_frame_name)
|
||||
, tcp_frame_name_(tcp_frame_name)
|
||||
: PinocchioIKBase(urdf_path, base_frame_name, flange_frame_name, tcp_frame_name)
|
||||
, urdf_path_(urdf_path)
|
||||
, lambda_(lambda)
|
||||
, w_posrot_(w_posrot)
|
||||
, max_iters_(max_iters)
|
||||
, tol_(tol)
|
||||
, qp_time_limit_(qp_time_limit)
|
||||
, solver_() {
|
||||
// 不再在 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();
|
||||
flange_frame_name_ = config.flange_frame_name();
|
||||
chain_base_frame_name_ = config.base_frame_name();
|
||||
chain_tip_frame_name_ = config.flange_frame_name();
|
||||
tcp_frame_name_ = config.tcp_frame_name();
|
||||
lambda_ = config.lambda();
|
||||
w_posrot_ = config.w_posrot();
|
||||
max_iters_ = config.max_iters();
|
||||
tol_ = config.tol();
|
||||
qp_time_limit_ = config.qp_time_limit();
|
||||
if (!urdf_path_.empty() &&
|
||||
!chain_base_frame_name_.empty() &&
|
||||
!chain_tip_frame_name_.empty()) {
|
||||
initUrdfChain(urdf_path_, chain_base_frame_name_, chain_tip_frame_name_);
|
||||
}
|
||||
}
|
||||
// 不再在 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 {
|
||||
pinocchio::urdf::buildModel(urdf_path_, model_);
|
||||
} catch (const std::exception &e) {
|
||||
std::cerr << "[PinocchioQpIKSolver] Failed to load URDF: "
|
||||
<< e.what() << std::endl;
|
||||
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;
|
||||
if (!urdf_chain_cached_) {
|
||||
if (urdf_path_.empty() ||
|
||||
chain_base_frame_name_.empty() ||
|
||||
chain_tip_frame_name_.empty()) {
|
||||
std::cerr << "[PinocchioQpIKSolver] missing urdf/base/flange config before init().\n";
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
base_frame_id_ = model_.getFrameId(base_frame_name_);
|
||||
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);
|
||||
}
|
||||
if (!initUrdfChain(urdf_path_, chain_base_frame_name_, chain_tip_frame_name_)) {
|
||||
std::cerr << "[PinocchioQpIKSolver] Failed to cache URDF chain metadata in IKSolver.\n";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// 4) 构建 base→flange 子链并提取限位
|
||||
if (!buildActiveChain()) {
|
||||
UrdfParser::ChainInfo chain_info;
|
||||
std::string err;
|
||||
if (!initPinocchioFromUrdfChain(&chain_info, &err)) {
|
||||
std::cerr << "[PinocchioQpIKSolver] Failed to init pinocchio base: " << err << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 5) 初始化整机 q_full_，用中立位姿
|
||||
q_full_ = pinocchio::neutral(model_);
|
||||
if (chain_q_dof_ <= 0 || chain_v_dof_ <= 0) {
|
||||
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_
|
||||
<< ", active dof = " << dof_
|
||||
<< ", base frame = " << base_frame_name_
|
||||
<< ", flange frame = " << flange_frame_name_
|
||||
std::cout << "[PinocchioQpIKSolver] Init OK. full nq = " << model_.nq
|
||||
<< ", active dof = " << chain_q_dof_
|
||||
<< ", q_start=" << chain_q_start_ << ", v_start=" << chain_v_start_
|
||||
<< ", base frame = " << chain_base_frame_name_
|
||||
<< ", flange frame = " << chain_tip_frame_name_
|
||||
<< ", tcp frame = " << (has_tcp_ ? tcp_frame_name_ : "<none>")
|
||||
<< std::endl;
|
||||
return true;
|
||||
}
|
||||
|
||||
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_) {
|
||||
qd_max_global_ = qd_max;
|
||||
if (qd_max.size() == chain_v_dof_) {
|
||||
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_ 的对应索引
|
||||
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);
|
||||
Eigen::VectorXd q_full;
|
||||
if (!buildFullQFromInput(cur_joints_angle_, q_full, "ik(qp)")) {
|
||||
return false;
|
||||
}
|
||||
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_max = q_max_global_;
|
||||
const VectorXd &qd_max = qd_max_global_;
|
||||
const VectorXd &q_min = joint_pos_lower_limits_;
|
||||
const VectorXd &q_max = joint_pos_upper_limits_;
|
||||
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
|
||||
pinocchio::forwardKinematics(model_, *data_, q_full_);
|
||||
pinocchio::updateFramePlacements(model_, *data_);
|
||||
// 1) 用 q_full 做正解 + frame pose + J_full
|
||||
updateKinematics(q_full);
|
||||
|
||||
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_ 雅可比
|
||||
MatrixXd J(6, dof_);
|
||||
for (int i = 0; i < dof_; ++i) {
|
||||
J.col(i) = J_full.col(active_v_idx_[i]);
|
||||
}
|
||||
// 抽取子链对应列 => 6×chain_q_dof_ 雅可比
|
||||
MatrixXd J = J_full.middleCols(chain_v_start_, chain_v_dof_);
|
||||
|
||||
// 2) 构造代价：|| A_cost * dq - b_cost ||^2
|
||||
constexpr std::size_t n_eq = 6;
|
||||
A_cost.resize(n_eq + dof_, dof_);
|
||||
b_cost.resize(n_eq + dof_);
|
||||
A_cost.resize(n_eq + chain_q_dof_, chain_q_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_) *
|
||||
MatrixXd::Identity(dof_, dof_);
|
||||
b_cost.tail(dof_).setZero();
|
||||
A_cost.bottomRows(chain_q_dof_) = std::sqrt(lambda_) *
|
||||
MatrixXd::Identity(chain_q_dof_, chain_q_dof_);
|
||||
b_cost.tail(chain_q_dof_).setZero();
|
||||
|
||||
// 3) box 约束：lb <= dq_local <= ub
|
||||
VectorXd lb(dof_), ub(dof_);
|
||||
for (int i = 0; i < dof_; ++i) {
|
||||
VectorXd lb(chain_q_dof_), ub(chain_q_dof_);
|
||||
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
|
||||
|
||||
@ -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(2,3) += 0.3;
|
||||
Tg(0,3) += 0.1;
|
||||
// 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;
|
||||
|
||||
120
cmvr-es/ik_solver/src/urdf_parser.cpp
Normal file
120
cmvr-es/ik_solver/src/urdf_parser.cpp
Normal file
@ -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
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue
Block a user