Skip to content

Commit 80e8949

Browse files
feecatfeecat
committed
Optimize "_try_time_optimal_plan()"
1 parent 169f675 commit 80e8949

File tree

8 files changed

+1048
-554
lines changed

8 files changed

+1048
-554
lines changed

‎README.md‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ S7RTT is not intended to replace Ruckig for multi-DOF cases, but it offers disti
2828
* In many edge cases, performance is comparable.
2929
![](doc/img/Compare_Complex.png)
3030
* Ruckig does exhibit better solutions under certain boundary conditions.
31-
![](doc/img/Compare_Boundary_2.png)
31+
![](doc/img/Compare_Boundary_4.png)
3232
* However, more often than not, even within the constraints, Ruckig still attempts to insert a Brake motion, which leads to a suboptimal final solution.
3333
![](doc/img/Compare_Boundary_3.png)
3434
* More seriously, Ruckig frequently reports solution failures, which is unacceptable in practical applications. Below are the results of stress tests under random conditions.
@@ -43,8 +43,8 @@ CATEGORY | S7RTT | RUCKIG
4343
Plan Failures | 0 | 751
4444
Sim Acc Failures | 0 | 0
4545
----------------------------------------
46-
Faster Count | 742 | 9
47-
Draws | 8498 | 8498
46+
Faster Count | 756 | 1
47+
Draws | 8492 | 8492
4848
################################################################################
4949
```
5050

‎c/S7RTT.h‎

Lines changed: 154 additions & 63 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
// ==============================================================================
22
// File Name: S7RTT.h
33
// Author: feecat
4-
// Version: V1.6.1
4+
// Version: V1.7.1
55
// Description: Simple 7seg Real-Time Trajectory Generator
66
// Website: https://github.com/feecat/S7RTT
77
// License: Apache License Version 2.0
@@ -93,15 +93,15 @@ static inline S7RTT_MotionState s7rtt_state_init(double dt, double p, double v,
9393

9494
#ifdef S7RTT_IMPLEMENTATION
9595

96-
/* --- Constants --- */
97-
#define S7_EPS_TIME 1e-9
98-
#define S7_EPS_DIST 1e-8
99-
#define S7_EPS_VEL 1e-7
100-
#define S7_EPS_ACC 1e-6
101-
#define S7_EPS_SOLVER 1e-3
102-
#define S7_MATH_EPS 1e-9
103-
#define S7_SOLVER_TOL 1e-8
104-
#define S7_SOLVER_ITER 30
96+
/* --- Constants (Updated to V1.7) --- */
97+
#define S7_EPS_TIME 1e-10
98+
#define S7_EPS_DIST 1e-10
99+
#define S7_EPS_VEL 1e-10
100+
#define S7_EPS_ACC 1e-10
101+
#define S7_MATH_EPS 1e-10
102+
#define S7_EPS_SOLVER 1e-4
103+
#define S7_SOLVER_TOL 1e-10
104+
#define S7_SOLVER_ITER 50
105105

106106
#define S7_ONE_SIXTH (1.0 / 6.0)
107107
#define S7_ONE_HALF 0.5
@@ -112,12 +112,6 @@ static inline double s7_fclamp(double x, double lower, double upper) {
112112
return fmax(lower, fmin(x, upper));
113113
}
114114

115-
static inline double s7_fsign(double x) {
116-
if (x > 0.0) return 1.0;
117-
if (x < 0.0) return -1.0;
118-
return 0.0;
119-
}
120-
121115
static inline double s7_copysign(double x, double y) {
122116
return copysign(x, y);
123117
}
@@ -135,7 +129,14 @@ static void s7_path_init(S7RTT_Path* path, int initial_cap) {
135129

136130
static void s7_path_push(S7RTT_Path* path, S7RTT_MotionState s) {
137131
if (path->nodes == NULL) return;
138-
if (path->count >= path->capacity) return;
132+
if (path->count >= path->capacity) {
133+
int new_cap = path->capacity * 2;
134+
if (new_cap < 8) new_cap = 8;
135+
S7RTT_MotionState* new_nodes = (S7RTT_MotionState*)realloc(path->nodes, sizeof(S7RTT_MotionState) * new_cap);
136+
if (new_nodes == NULL) return; /* Allocation failed */
137+
path->nodes = new_nodes;
138+
path->capacity = new_cap;
139+
}
139140
path->nodes[path->count++] = s;
140141
}
141142

@@ -168,7 +169,7 @@ static inline S7RTT_MotionState s7_integrate_step(S7RTT_MotionState s, double dt
168169
return next;
169170
}
170171

171-
/* --- TinyProfile Replacement (Stack Array) --- */
172+
/* --- TinyProfile (Stack Array) --- */
172173
#define S7_MAX_SEGS 8
173174
typedef struct {
174175
struct { double dt; double j; } segs[S7_MAX_SEGS];
@@ -319,7 +320,8 @@ static inline S7_VelChange s7_calc_vel_change_times(double v0, double a0, double
319320
t3 = t3_max;
320321
} else {
321322
double term = j_max * dv_req + 0.5 * _a0_scaled * _a0_scaled;
322-
double a_peak = (term > 0) ? sqrt(term) : 0.0;
323+
if (term < 0) term = 0.0;
324+
double a_peak = sqrt(term);
323325
t1 = (a_peak - _a0_scaled) / j_max;
324326
t3 = a_peak / j_max;
325327
if (t1 < 0) t1 = 0.0;
@@ -353,13 +355,13 @@ static double s7_solve_brent(S7_SolverFunc func, void* ctx, double lower, double
353355
double d = b - a, e = b - a;
354356

355357
for (int i = 0; i < S7_SOLVER_ITER; ++i) {
356-
if (fabs(fb) < S7_SOLVER_TOL) return b;
357358
if (fabs(fc) < fabs(fb)) {
358359
a = b; b = c; c = a;
359360
fa = fb; fb = fc; fc = fa;
360361
}
361362
double xm = 0.5 * (c - b);
362-
if (fabs(xm) < S7_SOLVER_TOL) return b;
363+
364+
if (fabs(xm) < S7_SOLVER_TOL || fb == 0.0) return b;
363365

364366
if (fabs(e) >= S7_SOLVER_TOL && fabs(fa) > fabs(fb)) {
365367
double s = fb / fa;
@@ -451,22 +453,92 @@ static double s7_time_opt_cost(double t, void* data) {
451453
return s.p - c->target_p;
452454
}
453455

454-
static double s7_solve_time_optimal(S7RTT_MotionState curr, double target_p, double target_v,
455-
double a_max, double j_max, double v_max, double j_action) {
456-
S7_TimeCtx ctx = {curr, target_p, target_v, a_max, j_max, j_action};
456+
/* Struct to hold results of a potential jerk choice */
457+
typedef struct {
458+
bool valid;
459+
double total_duration;
460+
double best_t;
461+
double j_apply;
462+
} S7_OptCandidate;
463+
464+
static S7_OptCandidate s7_eval_jerk_choice(S7RTT_MotionState curr, double target_p, double target_v,
465+
double a_max, double j_max, double v_max, double j_apply) {
466+
S7_OptCandidate res;
467+
res.valid = false;
468+
res.j_apply = j_apply;
457469

470+
S7_TimeCtx ctx = {curr, target_p, target_v, a_max, j_max, j_apply};
471+
472+
/* 1. Estimate Search Horizon */
458473
double t_est = (a_max > 0) ? (fabs(curr.v) + v_max) / a_max : 1.0;
459-
double t_search_max = t_est * 3.0 + 5.0;
474+
double search_horizon = t_est * 2.0 + 5.0;
475+
476+
/* 2. Boundary Checks */
477+
double err_0 = s7_time_opt_cost(0.0, &ctx);
478+
479+
/* Check 1: Zero Drift */
480+
if (fabs(err_0) < S7_EPS_SOLVER) {
481+
res.best_t = 0.0;
482+
}
483+
/* Check 2: Unreachable */
484+
else {
485+
double err_hor = s7_time_opt_cost(search_horizon, &ctx);
486+
if (err_0 * err_hor > 0) {
487+
return res; /* Fail */
488+
}
489+
/* Check 3: Solve */
490+
res.best_t = s7_solve_brent(s7_time_opt_cost, &ctx, 0.0, search_horizon);
491+
if (fabs(s7_time_opt_cost(res.best_t, &ctx)) > S7_EPS_SOLVER) {
492+
return res; /* Fail precision check */
493+
}
494+
}
460495

461-
double e0 = s7_time_opt_cost(0.0, &ctx);
462-
double e_max = s7_time_opt_cost(t_search_max, &ctx);
496+
/* 3. Calculate Total Duration */
497+
/* Reconstruct phases to sum up dt. We simulate locally without allocating nodes. */
498+
double total_dt = 0.0;
499+
S7RTT_MotionState sim_s = curr;
500+
501+
/* Phase 1: Saturated integration segments */
502+
double t = res.best_t;
503+
double limit_a = (j_apply > 0) ? a_max : -a_max;
504+
double dist_to_lim = limit_a - sim_s.a;
505+
double t_ramp = 0.0;
506+
507+
if (fabs(j_apply) >= S7_MATH_EPS) {
508+
bool same_dir = (j_apply > 0) ? (dist_to_lim > -S7_MATH_EPS) : (dist_to_lim < S7_MATH_EPS);
509+
t_ramp = same_dir ? (dist_to_lim / j_apply) : 0.0;
510+
} else {
511+
t_ramp = INFINITY;
512+
}
463513

464-
if (e0 * e_max > 0) return -1.0;
514+
if (t <= t_ramp) {
515+
if (t > S7_EPS_TIME) {
516+
total_dt += t;
517+
s7_integrate_state_inplace(&sim_s, t, j_apply);
518+
}
519+
} else {
520+
if (t_ramp > S7_EPS_TIME) {
521+
total_dt += t_ramp;
522+
s7_integrate_state_inplace(&sim_s, t_ramp, j_apply);
523+
}
524+
sim_s.a = limit_a;
525+
double t_hold = t - t_ramp;
526+
if (t_hold > S7_EPS_TIME) {
527+
total_dt += t_hold;
528+
s7_integrate_state_inplace(&sim_s, t_hold, 0.0);
529+
}
530+
}
465531

466-
double best_t = s7_solve_brent(s7_time_opt_cost, &ctx, 0.0, t_search_max);
532+
/* Phase 2: Velocity profile */
533+
S7_Profile rem_shapes;
534+
s7_build_vel_profile(&rem_shapes, sim_s, target_v, a_max, j_max);
535+
for (int i=0; i<rem_shapes.count; ++i) {
536+
total_dt += rem_shapes.segs[i].dt;
537+
}
467538

468-
if (fabs(s7_time_opt_cost(best_t, &ctx)) > S7_EPS_SOLVER) return -1.0;
469-
return best_t;
539+
res.total_duration = total_dt;
540+
res.valid = true;
541+
return res;
470542
}
471543

472544
/* --- Planning Steps --- */
@@ -520,8 +592,9 @@ static void s7_append_fallback_cruise(S7RTT_Path* nodes, S7RTT_MotionState curr,
520592
}
521593

522594
static void s7_refine_trajectory(S7RTT_Path* nodes, S7RTT_MotionState start, double target_p) {
523-
if (nodes->count == 0) return;
595+
if (!nodes || nodes->count == 0) return;
524596

597+
/* Phase 1: Simulation and Identification */
525598
S7RTT_MotionState sim_s = start;
526599
int correction_idx = -1;
527600
double max_cruise_dt = -1.0;
@@ -537,22 +610,34 @@ static void s7_refine_trajectory(S7RTT_Path* nodes, S7RTT_MotionState start, dou
537610
}
538611

539612
double pos_error = target_p - sim_s.p;
540-
if (fabs(pos_error) <= S7_EPS_DIST || correction_idx == -1) return;
541-
542-
double v_cruise = nodes->nodes[correction_idx].v;
543-
if (fabs(v_cruise) > S7_EPS_VEL) {
544-
double dt_fix = pos_error / v_cruise;
545-
double new_dt = nodes->nodes[correction_idx].dt + dt_fix;
546-
if (new_dt < S7_EPS_TIME) new_dt = S7_EPS_TIME;
547-
548-
nodes->nodes[correction_idx].dt = new_dt;
549-
550-
S7RTT_MotionState curr = start;
551-
for (int k = 0; k < nodes->count; ++k) {
552-
nodes->nodes[k].p = curr.p;
553-
nodes->nodes[k].v = curr.v;
554-
nodes->nodes[k].a = curr.a;
555-
s7_integrate_state_inplace(&curr, nodes->nodes[k].dt, nodes->nodes[k].j);
613+
614+
/* Phase 2: Correction and Propagation */
615+
if (fabs(pos_error) > S7_EPS_DIST && correction_idx != -1) {
616+
double v_cruise = nodes->nodes[correction_idx].v;
617+
if (fabs(v_cruise) > S7_EPS_VEL) {
618+
double dt_fix = pos_error / v_cruise;
619+
double new_dt = nodes->nodes[correction_idx].dt + dt_fix;
620+
if (new_dt < S7_EPS_TIME) new_dt = S7_EPS_TIME;
621+
622+
nodes->nodes[correction_idx].dt = new_dt;
623+
624+
/* Propagate changes re-calculating start states */
625+
S7RTT_MotionState curr = start;
626+
/* Integrate up to the modified node */
627+
for (int k = 0; k < correction_idx; ++k) {
628+
s7_integrate_state_inplace(&curr, nodes->nodes[k].dt, nodes->nodes[k].j);
629+
}
630+
631+
/* Apply modification and continue */
632+
s7_integrate_state_inplace(&curr, nodes->nodes[correction_idx].dt, nodes->nodes[correction_idx].j);
633+
634+
/* Update subsequent nodes */
635+
for (int k = correction_idx + 1; k < nodes->count; ++k) {
636+
nodes->nodes[k].p = curr.p;
637+
nodes->nodes[k].v = curr.v;
638+
nodes->nodes[k].a = curr.a;
639+
s7_integrate_state_inplace(&curr, nodes->nodes[k].dt, nodes->nodes[k].j);
640+
}
556641
}
557642
}
558643
}
@@ -571,7 +656,7 @@ S7RTT_Path s7rtt_plan(S7RTT_MotionState start_state, double target_p, double tar
571656
/* 1. Safety Decel */
572657
curr = s7_append_safety_decel(&path, curr, a_max, j_max);
573658

574-
/* 2. Feasibility */
659+
/* 2. Feasibility Check */
575660
double dist_req = target_p - curr.p;
576661
double d_pos_lim = s7_calc_max_reach(curr, v_max, target_v, a_max, j_max);
577662
double d_neg_lim = s7_calc_max_reach(curr, -v_max, target_v, a_max, j_max);
@@ -582,21 +667,25 @@ S7RTT_Path s7rtt_plan(S7RTT_MotionState start_state, double target_p, double tar
582667
}
583668

584669
bool opt_success = false;
670+
671+
/* 3. Execution Strategy */
585672
if (use_optimal) {
586-
S7_Profile stop_shapes;
587-
s7_build_vel_profile(&stop_shapes, curr, target_v, a_max, j_max);
588-
S7RTT_MotionState s_stop = curr;
589-
s7_simulate_endpoint_inplace(&s_stop, &stop_shapes);
590-
591-
bool is_pos_move = (fabs(curr.v) > S7_EPS_VEL) ? (curr.v > 0) : (target_p > curr.p);
592-
double gap = target_p - s_stop.p;
593-
double j_action = (is_pos_move) ? ((gap < -S7_EPS_DIST) ? -j_max : j_max)
594-
: ((gap > S7_EPS_DIST) ? j_max : -j_max);
595-
596-
double best_t = s7_solve_time_optimal(curr, target_p, target_v, a_max, j_max, v_max, j_action);
673+
/* Bidirectional Competition */
674+
S7_OptCandidate c1 = s7_eval_jerk_choice(curr, target_p, target_v, a_max, j_max, v_max, j_max);
675+
S7_OptCandidate c2 = s7_eval_jerk_choice(curr, target_p, target_v, a_max, j_max, v_max, -j_max);
676+
677+
S7_OptCandidate* winner = NULL;
678+
679+
if (c1.valid && c2.valid) {
680+
winner = (c1.total_duration < c2.total_duration) ? &c1 : &c2;
681+
} else if (c1.valid) {
682+
winner = &c1;
683+
} else if (c2.valid) {
684+
winner = &c2;
685+
}
597686

598-
if (best_t >= 0.0) {
599-
curr = s7_append_saturated_profile(&path, curr, best_t, j_action, a_max);
687+
if (winner) {
688+
curr = s7_append_saturated_profile(&path, curr, winner->best_t, winner->j_apply, a_max);
600689
S7_Profile rem;
601690
s7_build_vel_profile(&rem, curr, target_v, a_max, j_max);
602691
s7_append_from_profile(&path, curr, &rem);
@@ -605,9 +694,11 @@ S7RTT_Path s7rtt_plan(S7RTT_MotionState start_state, double target_p, double tar
605694
}
606695

607696
if (!opt_success) {
697+
/* Fallback */
608698
s7_append_fallback_cruise(&path, curr, target_p, target_v, v_max, a_max, j_max);
609699
}
610700

701+
/* 4. Precision Refinement */
611702
s7_refine_trajectory(&path, start_state, target_p);
612703

613704
return path;
@@ -659,4 +750,4 @@ S7RTT_MotionState s7rtt_at_time(const S7RTT_Path* path, double t) {
659750
return s7_integrate_step(final_s, t - elapsed, 0.0);
660751
}
661752

662-
#endif /* S7RTT_IMPLEMENTATION */
753+
#endif /* S7RTT_IMPLEMENTATION */

0 commit comments

Comments
 (0)