Skip to content

Commit eac1171

Browse files
feecatfeecat
committed
Optimize online calculate
1 parent d7afcf6 commit eac1171

File tree

4 files changed

+727
-178
lines changed

4 files changed

+727
-178
lines changed

‎python/S7RTT.py‎

Lines changed: 93 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
# ==============================================================================
22
# File Name: S7RTT.py
33
# Author: feecat
4-
# Version: V1.1
4+
# Version: V1.2
55
# Description: Simple 7seg Real-Time Trajectory Generator
66
# Website: https://github.com/feecat/S7RTT
77
# License: Apache License Version 2.0
@@ -261,98 +261,130 @@ def _compute_trajectory_dist(self, start_state, v_peak, target_v, a_max, j_max):
261261
def plan(self, start_state, target_p, target_v, v_max, a_max, j_max):
262262
"""
263263
Planning Method: Position Control.
264-
Calculates a trajectory to move from start_state to target_p with final velocity target_v.
264+
Optimized with caching to reduce redundant computations.
265265
"""
266+
# 1. Input Validation
266267
if v_max <= 0 or a_max <= 0 or j_max <= 0:
267268
return []
268269

269270
current_state = start_state.copy()
270271
final_trajectory = []
271272

272-
# --- 1. Acceleration Recovery ---
273-
# If current acceleration exceeds limits, ramp it down to safe limits first.
273+
# 2. Acceleration Recovery (Safety)
274274
if current_state.a > a_max + S7RTT.EPS_VAL:
275275
t_recover = (current_state.a - a_max) / j_max
276276
rec_state = current_state.copy()
277277
rec_state.dt = t_recover
278278
rec_state.j = -j_max
279279
final_trajectory.append(rec_state)
280-
281280
current_state = self._integrate_state(current_state, t_recover, -j_max)
282281
current_state.a = a_max
283-
284282
elif current_state.a < -a_max - S7RTT.EPS_VAL:
285283
t_recover = (-a_max - current_state.a) / j_max
286284
rec_state = current_state.copy()
287285
rec_state.dt = t_recover
288286
rec_state.j = j_max
289287
final_trajectory.append(rec_state)
290-
291288
current_state = self._integrate_state(current_state, t_recover, j_max)
292289
current_state.a = -a_max
293290

294291
dist_req = target_p - current_state.p
295-
296-
# --- 2. Boundary Check ---
297-
# Calculate the distance traveled if we hit +V_max and -V_max respectively.
298-
d_upper, acc_up, dec_up = self._compute_trajectory_dist(
299-
current_state, v_max, target_v, a_max, j_max)
300-
301-
d_lower, acc_lo, dec_lo = self._compute_trajectory_dist(
302-
current_state, -v_max, target_v, a_max, j_max)
303-
304-
shape_list = []
305-
306-
# --- 3. Logic Branching ---
307-
if dist_req > d_upper + S7RTT.EPS_DIST:
308-
# Distance is large: Accelerate to V_max, Cruise, then Decelerate.
309-
gap = dist_req - d_upper
310-
t_cruise = gap / v_max
311-
312-
shape_list.extend(acc_up)
313-
if t_cruise > S7RTT.EPS_TIME:
314-
shape_list.append(MotionState(t_cruise, 0,0,0, 0.0))
315-
shape_list.extend(dec_up)
316-
317-
elif dist_req < d_lower - S7RTT.EPS_DIST:
318-
# Distance is large (negative): Accelerate to -V_max, Cruise, then Decelerate.
319-
gap = dist_req - d_lower
320-
t_cruise = gap / (-v_max)
321-
322-
shape_list.extend(acc_lo)
323-
if t_cruise > S7RTT.EPS_TIME:
324-
shape_list.append(MotionState(t_cruise, 0,0,0, 0.0))
325-
shape_list.extend(dec_lo)
292+
293+
# 3. Inertial Reference (Used for logic direction)
294+
t_to_zero = abs(current_state.a) / j_max
295+
j_to_zero = -j_max if current_state.a > 0 else j_max
296+
v_inertial = current_state.v + current_state.a * t_to_zero + 0.5 * j_to_zero * t_to_zero**2
297+
298+
# 4. Pre-check: Direct Profile (Fast Path)
299+
direct_shape = self._build_profile(current_state.v, current_state.a, target_v, a_max, j_max)
300+
d_direct = self._integrate_dist_only(current_state.v, current_state.a, direct_shape)
301+
302+
if abs(d_direct - dist_req) <= S7RTT.EPS_DIST:
303+
shape_list = direct_shape
326304

327305
else:
328-
# Distance is within bounds: No cruising phase at V_max is needed.
329-
# Solve for the specific peak velocity that yields the exact distance.
330-
def get_error(v_p):
331-
d, _, _ = self._compute_trajectory_dist(
332-
current_state, v_p, target_v, a_max, j_max)
333-
return d - dist_req
334-
335-
max_abs_v = max(v_max, abs(current_state.v), abs(target_v)) * 2.0
336-
best_v = Solver.solve_monotonic_brent(get_error, -max_abs_v, max_abs_v)
337-
338-
#best_v = Solver.solve_monotonic_brent(get_error, -v_max, v_max)
339-
#if abs(best_v - current_state.v) < 0.1:
340-
# best_v = current_state.v
341-
342-
_, acc_fin, dec_fin = self._compute_trajectory_dist(
343-
current_state, best_v, target_v, a_max, j_max)
344-
345-
shape_list.extend(acc_fin)
346-
shape_list.extend(dec_fin)
347-
348-
# --- 4. Stitching ---
349-
# Convert relative shape segments into absolute trajectory points.
306+
# 5. Boundary Calculation
307+
d_upper, acc_up, dec_up = self._compute_trajectory_dist(
308+
current_state, v_max, target_v, a_max, j_max)
309+
d_lower, acc_lo, dec_lo = self._compute_trajectory_dist(
310+
current_state, -v_max, target_v, a_max, j_max)
311+
312+
shape_list = []
313+
314+
if dist_req > d_upper + S7RTT.EPS_DIST:
315+
# Target beyond positive limit
316+
gap = dist_req - d_upper
317+
t_cruise = gap / v_max
318+
shape_list.extend(acc_up)
319+
if t_cruise > S7RTT.EPS_TIME:
320+
shape_list.append(MotionState(t_cruise, 0,0,0, 0.0))
321+
shape_list.extend(dec_up)
322+
323+
elif dist_req < d_lower - S7RTT.EPS_DIST:
324+
# Target beyond negative limit
325+
gap = dist_req - d_lower
326+
t_cruise = gap / (-v_max)
327+
shape_list.extend(acc_lo)
328+
if t_cruise > S7RTT.EPS_TIME:
329+
shape_list.append(MotionState(t_cruise, 0,0,0, 0.0))
330+
shape_list.extend(dec_lo)
331+
332+
else:
333+
# Case: Within Bounds (Solver Required)
334+
d_inertial, acc_inertial, dec_inertial = self._compute_trajectory_dist(
335+
current_state, v_inertial, target_v, a_max, j_max)
336+
337+
# Strategy A: Inertial Snapping (Deadband)
338+
if abs(d_inertial - dist_req) <= S7RTT.EPS_DIST:
339+
shape_list.extend(acc_inertial)
340+
shape_list.extend(dec_inertial)
341+
342+
else:
343+
# Strategy B: Buffered Directional Search
344+
def get_error(v_p):
345+
d, _, _ = self._compute_trajectory_dist(
346+
current_state, v_p, target_v, a_max, j_max)
347+
return d - dist_req
348+
349+
limit_safe = max(v_max, abs(current_state.v), abs(target_v)) * 1.5 + 10.0
350+
overlap_v = S7RTT.EPS_VAL * 10.0
351+
352+
if dist_req > d_inertial:
353+
s_low = v_inertial - overlap_v
354+
s_high = limit_safe
355+
else:
356+
s_high = v_inertial + overlap_v
357+
s_low = -limit_safe
358+
359+
if s_low >= s_high: s_low = s_high - S7RTT.EPS_VAL
360+
361+
best_v = Solver.solve_monotonic_brent(get_error, s_low, s_high)
362+
363+
# Strategy C: Fallback Mechanism
364+
hit_low = abs(best_v - s_low) < S7RTT.EPS_VAL
365+
hit_high = abs(best_v - s_high) < S7RTT.EPS_VAL
366+
367+
if hit_low or hit_high:
368+
best_v = Solver.solve_monotonic_brent(get_error, -limit_safe, limit_safe)
369+
370+
# Post-Process: Micro-noise filtering
371+
# If result implies v_inertial, use the cached shapes to save calculation
372+
if abs(best_v - v_inertial) < S7RTT.EPS_VAL:
373+
shape_list.extend(acc_inertial)
374+
shape_list.extend(dec_inertial)
375+
else:
376+
# Only recalculate if we actually deviated from v_inertial
377+
_, acc_fin, dec_fin = self._compute_trajectory_dist(
378+
current_state, best_v, target_v, a_max, j_max)
379+
shape_list.extend(acc_fin)
380+
shape_list.extend(dec_fin)
381+
382+
# 6. Stitching
350383
for shape in shape_list:
351384
node = current_state.copy()
352385
node.dt = shape.dt
353386
node.j = shape.j
354387
final_trajectory.append(node)
355-
356388
current_state = self._integrate_state(current_state, shape.dt, shape.j)
357389

358390
return final_trajectory

0 commit comments

Comments
 (0)