fix: fix endpoint speed & acc & jerk != 0

cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp

@@ -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 t_j_v = std::sqrt(v_max / j);
-            const double s_half_v = j * std::pow(t_j_v, 3); // s_half = j t_j^3
+            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)
+//  - tau: profile time (S-curve runs fully => sd->0)
-//  - t  : real time (your control period)
+//  - t  : real time (control dt)
-//  - gamma_tot scales BOTH dq and tau-step
+//  - gamma_tot scales BOTH dq and tau-step  (time scaling)
-//  - gamma_lim prevents hitting joint limits (avoid clamp chattering)
+//  - 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) {
+    if (L < 1e-9) { q_traj = {q_start}; t_traj = {0.0}; return true; }
-        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_perp0   = 1.0;   // 1/s
-    const double Kp_perp   = 1.0;  // 1/s
+    const double Kp_rot0    = 1.0;   // 1/s
-    const double Kp_rot    = 1.0;  // 1/s
+    const double k_posture0 = 1.0;   // 1/s
-    const double k_posture = 1.0;  // 1/s (nullspace posture)
+    const double eps_dq     = 1e-9;
-    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
+    // output
-    double s   = 0.0;   // along-line distance
-    double sd  = 0.0;   // ds/dtau
-    double sdd = 0.0;   // d2s/dtau2
-
-    // output buffers
     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）
+        // !!! use v_start/v_dof (more correct)
-        Eigen::MatrixXd J = J_full.middleCols(chain_q_start_, chain_dof_);
+        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) ----------------
+        // ---------------- gamma_lim (keep q_next within limits) ----------------
-        // ensure q_next = q + gamma*dq_raw*dt_real stays within [lower, upper]
         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 margin = q_chain[i] - q_lower_chain_[i];
-                const double g = margin / (dqi * step_real);           // (-)/(-) => +
+                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) {
+        // optional gamma smoothing to remove kinks
-            stuck_cnt++;
+        const double alpha = step_real / (gamma_tau + step_real);
-        } else {
+        const double gamma_tot = gamma_prev + alpha * (gamma_target - gamma_prev);
-            stuck_cnt = 0;
+        gamma_prev = gamma_tot;
-        }
+
-        if (stuck_cnt > 20) { // 连续一段时间动不了
+        if (gamma_tot < 1e-6) stuck_cnt++; else stuck_cnt = 0;
-            std::cerr << "[moveL_SCurveLocal] stuck by limits/speed: gamma_tot too small\n";
+        if (stuck_cnt > 50) {
+            std::cerr << "[moveL_SCurveLocal] stuck: gamma_tot too small (limits/speed)\n";
             return false;
         }
 
-        // apply dq in real time
+        // apply dq (real time)
-        const Eigen::VectorXd dq = gamma_tot * dq_raw;
+        q_chain += (gamma_tot * dq_raw) * step_real;
-        q_chain += dq * step_real;
+
+        // (理论上 gamma_lim 已保证不越界，这里只是数值保险)
         q_chain = q_chain.cwiseMax(q_lower_chain_).cwiseMin(q_upper_chain_);
 
-        // ---------------- advance profile time tau (time scaling) ----------------
+        // ---- advance profile time (time scaling) ----
-        const double step_prof = gamma_tot * step_real; // <-- 关键：profile 慢放
+        // IMPORTANT: tau advances by gamma_tot*dt_real => if slowed by limits, profile slows too
-        const double j = prof.jerk_at(tau);
+        tau = std::min(prof.T, tau + gamma_tot * step_real);
-
-        // 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;
         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) ||
+                const bool nl = (q_upper_chain_[i] - q_chain[i] < 1e-8) ||
-                                (std::abs(q_chain[i] - q_upper_chain_[i]) < limit_eps);
+                                (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) ----
+    // ---- ensure end stop for finite difference velocity ----
-    // 给差分速度一个“严格为0”的结尾：再加一个 hold 点
     if (!q_traj.empty() && !t_traj.empty()) {
-        const double hold_dt = dt_real;
+        t_traj.push_back(t_traj.back() + dt_real);
-        t_traj.push_back(t_traj.back() + hold_dt);
+        q_traj.push_back(q_traj.back()); // hold => diff-speed becomes 0
-        q_traj.push_back(q_traj.back()); // same q => p doesn't change => v=0 by diff
     }
 
     return !q_traj.empty();

cmvr-es/ik_solver/src/srs_ik_test.cpp

@@ -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);