fix: fix endpoint speed & acc & jerk != 0
This commit is contained in:
parent
178c8b1dbe
commit
57ffeddee9
@ -254,8 +254,20 @@ bool PinocchioDlsIKSolver::fk(const std::vector<double> &joints_angle,
|
||||
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)); }
|
||||
|
||||
// ============================================================================
|
||||
// S-curve profile
|
||||
// S-curve profile (7 segments), with EXACT per-segment integration sampling
|
||||
// - compute(): your original
|
||||
// - jerk_at(): your original
|
||||
// - sample(tau): new; returns s, sd, sdd exactly under the piecewise-constant jerk
|
||||
// ============================================================================
|
||||
namespace {
|
||||
|
||||
@ -277,7 +289,6 @@ struct SCurveProfile1D {
|
||||
|
||||
static double cbrt_pos(double x) { return std::cbrt(std::max(0.0, x)); }
|
||||
|
||||
// 半程（加速段）距离：s_half = a*(t_j^2 + 1.5 t_j t_a + 0.5 t_a^2)
|
||||
static double half_dist_a_reached(double a, double t_j, double t_a) {
|
||||
return a * (t_j*t_j + 1.5*t_j*t_a + 0.5*t_a*t_a);
|
||||
}
|
||||
@ -296,12 +307,12 @@ struct SCurveProfile1D {
|
||||
if (v_max <= 1e-12 || a_max <= 1e-12 || j_max <= 1e-12) return;
|
||||
|
||||
const double j = j_max;
|
||||
const double t_j_a = a_max / j; // 达到 a_max 需要的 jerk 时间
|
||||
const double t_j_a = a_max / j;
|
||||
|
||||
// Case X：v_max 很低，达不到 a_max（纯 jerk 三角加速到 v_max）
|
||||
// Case X：达不到 a_max
|
||||
if (v_max < a_max * t_j_a) {
|
||||
const double t_j_v = std::sqrt(v_max / j); // v = j t_j^2
|
||||
const double s_half_v = j * std::pow(t_j_v, 3); // s_half = j t_j^3
|
||||
const double t_j_v = std::sqrt(v_max / j);
|
||||
const double s_half_v = j * std::pow(t_j_v, 3);
|
||||
const double s_min = 2.0 * s_half_v;
|
||||
|
||||
if (L > s_min + 1e-12) {
|
||||
@ -314,7 +325,6 @@ struct SCurveProfile1D {
|
||||
valid = true;
|
||||
return;
|
||||
} else {
|
||||
// 距离太短，达不到 v_max：纯 jerk-only
|
||||
t_a = 0.0;
|
||||
t_v = 0.0;
|
||||
t_j = cbrt_pos(L / (2.0*j));
|
||||
@ -327,12 +337,11 @@ struct SCurveProfile1D {
|
||||
}
|
||||
|
||||
// Case Y：能达到 a_max
|
||||
const double t_a_v = v_max / a_max - t_j_a; // >=0
|
||||
const double t_a_v = v_max / a_max - t_j_a;
|
||||
const double s_half_v = half_dist_a_reached(a_max, t_j_a, t_a_v);
|
||||
const double s_min = 2.0 * s_half_v;
|
||||
|
||||
if (L > s_min + 1e-12) {
|
||||
// 有匀速
|
||||
t_j = t_j_a;
|
||||
t_a = t_a_v;
|
||||
t_v = (L - s_min) / v_max;
|
||||
@ -342,13 +351,11 @@ struct SCurveProfile1D {
|
||||
valid = true;
|
||||
return;
|
||||
} else {
|
||||
// 无匀速：解 t_a 使得 2*s_half == L
|
||||
const double tj = t_j_a;
|
||||
const double D = tj*tj + 4.0 * (L / a_max);
|
||||
double t_a_sol = (-3.0*tj + std::sqrt(std::max(0.0, D))) * 0.5;
|
||||
|
||||
if (t_a_sol < 0.0) {
|
||||
// 距离短到连 a_max 都达不到 -> jerk-only
|
||||
t_a = 0.0;
|
||||
t_v = 0.0;
|
||||
t_j = cbrt_pos(L / (2.0*j));
|
||||
@ -397,25 +404,68 @@ struct SCurveProfile1D {
|
||||
if (t < d7) return +j;
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// ===== 新增：解析采样（按段常 jerk 精确积分）=====
|
||||
void sample(double tau, double& s, double& sd, double& sdd, double& j_out) const {
|
||||
if (!valid) { s=sd=sdd=j_out=0.0; return; }
|
||||
|
||||
tau = std::min(std::max(0.0, tau), T);
|
||||
|
||||
auto advance = [&](double dt, double jerk,
|
||||
double& s, double& v, double& a) {
|
||||
// constant jerk exact integration
|
||||
s += 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;
|
||||
};
|
||||
|
||||
// segments
|
||||
const double segT[7] = {t_j, t_a, t_j, t_v, t_j, t_a, t_j};
|
||||
const double segJ[7] = {+j_max, 0.0, -j_max, 0.0, -j_max, 0.0, +j_max};
|
||||
|
||||
double s_acc = 0.0;
|
||||
double v_acc = 0.0;
|
||||
double a_acc = 0.0;
|
||||
|
||||
double rem = tau;
|
||||
int k_last = 0;
|
||||
for (int k = 0; k < 7; ++k) {
|
||||
const double dt = std::min(rem, segT[k]);
|
||||
if (dt > 0.0) {
|
||||
advance(dt, segJ[k], s_acc, v_acc, a_acc);
|
||||
k_last = k;
|
||||
}
|
||||
rem -= dt;
|
||||
if (rem <= 1e-15) { k_last = k; break; }
|
||||
}
|
||||
|
||||
s = std::min(std::max(0.0, s_acc), L);
|
||||
sd = v_acc;
|
||||
sdd = a_acc;
|
||||
|
||||
// current jerk
|
||||
j_out = jerk_at(tau);
|
||||
|
||||
// force perfect stop at end
|
||||
if (tau >= T - 1e-12) { sd = 0.0; sdd = 0.0; j_out = 0.0; }
|
||||
if (tau <= 1e-12) { /* sd already ~0 */ }
|
||||
}
|
||||
};
|
||||
|
||||
static inline double smoothstep01(double x) {
|
||||
x = clamp01(x);
|
||||
return x*x*(3.0 - 2.0*x);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
#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)); }
|
||||
|
||||
// ============================================================================
|
||||
// MoveL with tau time-scaling (anti-wobble + correct stop):
|
||||
// - tau: profile time (S-curve runs to the end => sd->0)
|
||||
// - t : real time (your control period)
|
||||
// - gamma_tot scales BOTH dq and tau-step
|
||||
// - gamma_lim prevents hitting joint limits (avoid clamp chattering)
|
||||
// - tau: profile time (S-curve runs fully => sd->0)
|
||||
// - t : real time (control dt)
|
||||
// - gamma_tot scales BOTH dq and tau-step (time scaling)
|
||||
// - gamma_lim keeps q_next inside limits (avoid clamp chattering)
|
||||
// - posture/feedback fades out near end (avoid nullspace leakage stop wiggle)
|
||||
// - add final hold sample => diff-speed == 0
|
||||
// ============================================================================
|
||||
bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_base,
|
||||
@ -438,7 +488,7 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
const pinocchio::FrameIndex ee_id =
|
||||
(is_tcp && has_tcp_) ? tcp_frame_id_ : flange_frame_id_;
|
||||
|
||||
// 起点 FK（base下），姿态固定为起点姿态
|
||||
// start FK (base)
|
||||
Eigen::Matrix4d T0_base;
|
||||
if (!fk(q_start, T0_base, is_tcp)) return false;
|
||||
|
||||
@ -448,16 +498,12 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
const Eigen::Vector3d pg = target_pose_base.block<3,1>(0,3);
|
||||
const Eigen::Vector3d dp = pg - p0;
|
||||
const double L = dp.norm();
|
||||
if (L < 1e-9) {
|
||||
q_traj = {q_start};
|
||||
t_traj = {0.0};
|
||||
return true;
|
||||
}
|
||||
if (L < 1e-9) { q_traj = {q_start}; t_traj = {0.0}; return true; }
|
||||
const Eigen::Vector3d dir_base = dp / L;
|
||||
|
||||
std::cerr << "dp(base)= " << dp.transpose() << "\n";
|
||||
|
||||
// S-curve profile (along s)
|
||||
// profile
|
||||
SCurveProfile1D prof;
|
||||
prof.compute(L, v_tcp_max, a_tcp_max, j_tcp_max);
|
||||
if (!prof.valid) {
|
||||
@ -466,53 +512,59 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
}
|
||||
|
||||
// ---------------- tuning ----------------
|
||||
// 横向只纠偏垂直误差，避免沿线方向“抢速度”
|
||||
const double Kp_perp = 1.0; // 1/s
|
||||
const double Kp_rot = 1.0; // 1/s
|
||||
const double k_posture = 1.0; // 1/s (nullspace posture)
|
||||
const double eps_dq = 1e-9;
|
||||
const double Kp_perp0 = 1.0; // 1/s
|
||||
const double Kp_rot0 = 1.0; // 1/s
|
||||
const double k_posture0 = 1.0; // 1/s
|
||||
const double eps_dq = 1e-9;
|
||||
|
||||
// 关节速度上限
|
||||
// speed limits
|
||||
Eigen::VectorXd qd_lim(chain_dof_);
|
||||
for (int i = 0; i < chain_dof_; ++i) qd_lim[i] = std::max(1e-6, std::abs(qd_max[i]));
|
||||
|
||||
// posture reference
|
||||
Eigen::VectorXd q_ref = Eigen::Map<const Eigen::VectorXd>(q_start.data(), chain_dof_);
|
||||
|
||||
// states
|
||||
// state
|
||||
Eigen::VectorXd q_chain = Eigen::Map<const Eigen::VectorXd>(q_start.data(), chain_dof_);
|
||||
|
||||
double t = 0.0; // real time
|
||||
double tau = 0.0; // profile time
|
||||
|
||||
// profile states at tau
|
||||
double s = 0.0; // along-line distance
|
||||
double sd = 0.0; // ds/dtau
|
||||
double sdd = 0.0; // d2s/dtau2
|
||||
|
||||
// output buffers
|
||||
// output
|
||||
q_traj.clear();
|
||||
t_traj.clear();
|
||||
q_traj.reserve((size_t)(prof.T / dt_real) + 10);
|
||||
t_traj.reserve((size_t)(prof.T / dt_real) + 10);
|
||||
|
||||
q_traj.push_back(q_start);
|
||||
t_traj.push_back(0.0);
|
||||
|
||||
// logs (A/B/C)
|
||||
// logs A/B/C
|
||||
const double log_period = 0.05;
|
||||
double next_log_t = 0.0;
|
||||
|
||||
const double limit_eps = 1e-8;
|
||||
const double sigma_min_th = 1e-4;
|
||||
const double cond_th = 1e4;
|
||||
|
||||
// gamma smoothing (optional but helps remove kinks)
|
||||
double gamma_prev = 1.0;
|
||||
const double gamma_tau = 0.03; // 30ms low-pass
|
||||
|
||||
// stuck detection
|
||||
int stuck_cnt = 0;
|
||||
|
||||
while (tau < prof.T - 1e-12) {
|
||||
const double step_real = std::min(dt_real, 0.01); // 你也可以删掉 0.01，这里只是防极端大dt
|
||||
const double step_real = dt_real;
|
||||
|
||||
// desired pose at current profile state (tau)
|
||||
// ===== sample profile at tau (exact) =====
|
||||
double s=0.0, sd=0.0, sdd=0.0, j_now=0.0;
|
||||
prof.sample(tau, s, sd, sdd, j_now);
|
||||
|
||||
// ===== end-fade (avoid nullspace leakage at stop) =====
|
||||
const double w_end = smoothstep01(sd / std::max(1e-6, 0.15 * prof.v_peak));
|
||||
const double Kp_perp = Kp_perp0 * w_end;
|
||||
const double Kp_rot = Kp_rot0 * w_end;
|
||||
const double k_posture = k_posture0 * w_end;
|
||||
|
||||
// desired pose in base
|
||||
const Eigen::Vector3d p_des_base = p0 + dir_base * s;
|
||||
|
||||
Eigen::Matrix4d T_des_base = Eigen::Matrix4d::Identity();
|
||||
@ -551,8 +603,8 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
pinocchio::ReferenceFrame::LOCAL,
|
||||
J_full);
|
||||
|
||||
// 取本链列（这里用 q_start_，nv==nq 且 1DOF 关节时OK）
|
||||
Eigen::MatrixXd J = J_full.middleCols(chain_q_start_, chain_dof_);
|
||||
// !!! use v_start/v_dof (more correct)
|
||||
Eigen::MatrixXd J = J_full.middleCols(chain_v_start_, chain_v_dof_);
|
||||
Eigen::MatrixXd J_pinv = dampedPseudoInverse(J, damping_);
|
||||
|
||||
// C) singular monitor
|
||||
@ -583,10 +635,10 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
// task dq
|
||||
const Eigen::VectorXd dq_task = J_pinv * xi;
|
||||
|
||||
// nullspace posture (reduce redundancy wobble)
|
||||
// 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;
|
||||
const Eigen::MatrixXd N = I - J_pinv * J; // DLS approx projector
|
||||
const Eigen::VectorXd dq_raw = dq_task + N * dq_posture;
|
||||
|
||||
// ---------------- gamma_speed (joint speed) ----------------
|
||||
@ -597,8 +649,7 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
}
|
||||
gamma_speed = clamp01(gamma_speed);
|
||||
|
||||
// ---------------- gamma_lim (joint position limits) ----------------
|
||||
// ensure q_next = q + gamma*dq_raw*dt_real stays within [lower, upper]
|
||||
// ---------------- gamma_lim (keep q_next within limits) ----------------
|
||||
double gamma_lim = 1.0;
|
||||
for (int i = 0; i < chain_dof_; ++i) {
|
||||
const double dqi = dq_raw[i];
|
||||
@ -609,45 +660,36 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
const double g = margin / (dqi * step_real);
|
||||
gamma_lim = std::min(gamma_lim, g);
|
||||
} else { // dqi < 0
|
||||
const double margin = q_lower_chain_[i] - q_chain[i]; // <=0
|
||||
const double g = margin / (dqi * step_real); // (-)/(-) => +
|
||||
const double margin = q_chain[i] - q_lower_chain_[i];
|
||||
const double g = margin / ((-dqi) * step_real);
|
||||
gamma_lim = std::min(gamma_lim, g);
|
||||
}
|
||||
}
|
||||
gamma_lim = clamp01(gamma_lim);
|
||||
|
||||
// total gamma
|
||||
const double gamma_tot = std::min(gamma_speed, gamma_lim);
|
||||
double gamma_target = std::min(gamma_speed, gamma_lim);
|
||||
|
||||
if (gamma_tot < 1e-6) {
|
||||
stuck_cnt++;
|
||||
} else {
|
||||
stuck_cnt = 0;
|
||||
}
|
||||
if (stuck_cnt > 20) { // 连续一段时间动不了
|
||||
std::cerr << "[moveL_SCurveLocal] stuck by limits/speed: gamma_tot too small\n";
|
||||
// optional gamma smoothing to remove kinks
|
||||
const double alpha = step_real / (gamma_tau + step_real);
|
||||
const double gamma_tot = gamma_prev + alpha * (gamma_target - gamma_prev);
|
||||
gamma_prev = gamma_tot;
|
||||
|
||||
if (gamma_tot < 1e-6) stuck_cnt++; else stuck_cnt = 0;
|
||||
if (stuck_cnt > 50) {
|
||||
std::cerr << "[moveL_SCurveLocal] stuck: gamma_tot too small (limits/speed)\n";
|
||||
return false;
|
||||
}
|
||||
|
||||
// apply dq in real time
|
||||
const Eigen::VectorXd dq = gamma_tot * dq_raw;
|
||||
q_chain += dq * step_real;
|
||||
// apply dq (real time)
|
||||
q_chain += (gamma_tot * dq_raw) * step_real;
|
||||
|
||||
// (理论上 gamma_lim 已保证不越界，这里只是数值保险)
|
||||
q_chain = q_chain.cwiseMax(q_lower_chain_).cwiseMin(q_upper_chain_);
|
||||
|
||||
// ---------------- advance profile time tau (time scaling) ----------------
|
||||
const double step_prof = gamma_tot * step_real; // <-- 关键：profile 慢放
|
||||
const double j = prof.jerk_at(tau);
|
||||
|
||||
// integrate s(tau) with jerk-limited profile
|
||||
s = s + sd*step_prof + 0.5*sdd*step_prof*step_prof + (1.0/6.0)*j*step_prof*step_prof*step_prof;
|
||||
sd = sd + sdd*step_prof + 0.5*j*step_prof*step_prof;
|
||||
sdd = sdd + j*step_prof;
|
||||
|
||||
// clamp physically
|
||||
s = std::min(std::max(0.0, s), L);
|
||||
sd = std::max(0.0, sd);
|
||||
|
||||
tau += step_prof;
|
||||
// ---- advance profile time (time scaling) ----
|
||||
// IMPORTANT: tau advances by gamma_tot*dt_real => if slowed by limits, profile slows too
|
||||
tau = std::min(prof.T, tau + gamma_tot * step_real);
|
||||
t += step_real;
|
||||
|
||||
// log A/B/C
|
||||
@ -657,8 +699,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 = (std::abs(q_chain[i] - q_lower_chain_[i]) < limit_eps) ||
|
||||
(std::abs(q_chain[i] - q_upper_chain_[i]) < limit_eps);
|
||||
const bool nl = (q_upper_chain_[i] - q_chain[i] < 1e-8) ||
|
||||
(q_chain[i] - q_lower_chain_[i] < 1e-8);
|
||||
if (nl) { near_limit = true; near_cnt++; }
|
||||
}
|
||||
|
||||
@ -675,6 +717,7 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
<< " |e_perp|=" << e_perp.norm()
|
||||
<< " sigma_min=" << sigma_min
|
||||
<< " cond=" << cond
|
||||
<< " w_end=" << w_end
|
||||
<< "\n";
|
||||
|
||||
if (sigma_min < sigma_min_th || cond > cond_th) {
|
||||
@ -689,19 +732,17 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
|
||||
}
|
||||
}
|
||||
|
||||
// save sample (real time)
|
||||
// save
|
||||
std::vector<double> q_out(chain_dof_);
|
||||
Eigen::Map<Eigen::VectorXd>(q_out.data(), chain_dof_) = q_chain;
|
||||
q_traj.push_back(std::move(q_out));
|
||||
t_traj.push_back(t);
|
||||
}
|
||||
|
||||
// ---- force end conditions (profile complete => sd should be ~0) ----
|
||||
// 给差分速度一个“严格为0”的结尾：再加一个 hold 点
|
||||
// ---- ensure end stop for finite difference velocity ----
|
||||
if (!q_traj.empty() && !t_traj.empty()) {
|
||||
const double hold_dt = dt_real;
|
||||
t_traj.push_back(t_traj.back() + hold_dt);
|
||||
q_traj.push_back(q_traj.back()); // same q => p doesn't change => v=0 by diff
|
||||
t_traj.push_back(t_traj.back() + dt_real);
|
||||
q_traj.push_back(q_traj.back()); // hold => diff-speed becomes 0
|
||||
}
|
||||
|
||||
return !q_traj.empty();
|
||||
|
||||
@ -1106,13 +1106,14 @@ 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;
|
||||
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.33;
|
||||
const double v_tcp = 0.13;
|
||||
const double a_tcp = 10.0;
|
||||
const double j_tcp = 10.00;
|
||||
std::vector<double> qd_max(7, 3.0);
|
||||
|
||||
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