# ==============================================================================
# File Name: S3RTT.py
# Author: feecat
# Version: V1.0
# Description: Trapezoidal Velocity Profile Generator
# Website: https://github.com/feecat/S7RTT
# License: Apache License Version 2.0
# ==============================================================================
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
# Description:
# This module generates trajectory data using a Trapezoidal Velocity Profile
# (T-Curve) rather than an S-Curve (Sigmoid/Jerk-limited profile).
#
# The T-Curve approach significantly reduces computational complexity and
# CPU consumption, making it highly suitable for resource-constrained
# embedded devices.
#
# Note:
# This Python version is intended for algorithm verification and testing.
# For the production-ready embedded implementation, please refer to the
# C language header file 'S3RTT.h'.
# ==============================================================================
import tkinter as tk
from tkinter import ttk
import math
import matplotlib.pyplot as plt
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
S3_EPS = 1e-5
class MotionState:
def __init__(self, dt=0.0, p=0.0, v=0.0, a=0.0):
self.dt = dt
self.p = p
self.v = v
self.a = a
class Path:
def __init__(self):
self.nodes = [] # List of MotionState
def push(self, dt, p, v, a):
if dt > S3_EPS:
self.nodes.append(MotionState(dt, p, v, a))
def total_time(self):
return sum(n.dt for n in self.nodes)
def s3_sign(x):
if x >= 0: return 1.0
return -1.0
def s3_plan(start_state, target_p, target_v, v_max, a_max):
path = Path()
# Extract parameters and convert to float
vi, pi = float(start_state.v), float(start_state.p)
pf, vf = float(target_p), float(target_v)
acc, v_cap = abs(float(a_max)), abs(float(v_max))
# Clamp the target velocity within the allowable maximum velocity limits
vf = max(-v_cap, min(v_cap, vf))
# If acceleration is negligible, return a constant velocity path immediately
if acc < S3_EPS:
path.push(0.0, pi, vi, 0.0)
return path
dist = pf - pi
# Determine the direction of motion (1 for positive, -1 for negative)
s = s3_sign(dist) if abs(dist) > S3_EPS else 1.0
# ==========================================
# Phase 1: Kinematic Violation Handling
# ==========================================
# Check 1: Wrong Direction or Overshoot
# Detect if the current velocity is moving away from the target or if inertia makes stopping impossible
is_wrong_dir = (vi * s < -S3_EPS)
is_overshoot = False
# Only check for overshoot if we are moving towards the target
if not is_wrong_dir and (vi * s > S3_EPS):
# Case A: Moving towards target, but target velocity requires moving backwards (must stop and reverse)
if vf * s < -S3_EPS:
is_overshoot = True
# Case B: Current kinetic energy is too high to slow down to 'vf' within distance 'dist'
# Formula check: v_initial^2 > v_final^2 + 2 * a * distance
elif vi**2 > vf**2 + 2.0 * acc * abs(dist) + S3_EPS:
is_overshoot = True
if is_wrong_dir or is_overshoot:
# Strategy: Perform a full stop, then plan recursively from the stopped state.
# Calculate the distance required to come to a complete stop
d_stop = (vi**2) / (2.0 * acc) * s3_sign(vi)
stop_pos = pi + d_stop
# Add the braking segment to zero velocity
path.push(abs(vi)/acc, pi, vi, -s3_sign(vi)*acc)
# Recursively plan from the stop position to the original target
path.nodes.extend(s3_plan(MotionState(0, stop_pos, 0, 0), pf, vf, v_max, a_max).nodes)
return path
# Check 2: Insufficient Run-up Distance
# If we need to accelerate to a high 'vf' but don't have enough distance to reach it
if abs(vf) > abs(vi) and (vf * s > 0):
max_reachable_sq = vi**2 + 2.0 * acc * abs(dist)
if vf**2 > max_reachable_sq + S3_EPS:
# Strategy: Move backward first to gain runway (Run-up).
# Calculate the turnaround point needed to generate enough acceleration distance
p_turn = pf - s * ((vf**2)/(2.0*acc))
# Plan segment to the turnaround point
path.nodes.extend(s3_plan(MotionState(0, pi, vi, 0), p_turn, 0, v_max, a_max).nodes)
# Plan segment from turnaround point to the target
path.nodes.extend(s3_plan(MotionState(0, p_turn, 0, 0), pf, vf, v_max, a_max).nodes)
return path
# ==========================================
# Phase 2: Standard Profile Generation
# ==========================================
# Calculate the theoretical peak velocity squared for a triangular profile
# Derived from: 2 * a * d = (v_peak^2 - v_i^2) + (v_peak^2 - v_f^2)
vp_sq = (2.0 * acc * abs(dist) + vi**2 + vf**2) / 2.0
vp = math.sqrt(max(0.0, vp_sq))
# Apply velocity constraints (Transition from Triangular to Trapezoidal profile)
v_peak = min(vp, v_cap) * s
# 1. Acceleration Phase: Move from current velocity to peak velocity
t1 = abs(v_peak - vi) / acc
path.push(t1, pi, vi, s3_sign(v_peak - vi) * acc)
# Update current position after acceleration
p_curr = pi + (vi + v_peak) * t1 * 0.5
# 2. Cruising Phase: Constant velocity (exists only if profile is Trapezoidal)
# Calculate distance required for the final deceleration phase
t3 = abs(vf - v_peak) / acc
d3 = (v_peak + vf) * t3 * 0.5
# Calculate remaining distance available for cruising
d_cruise = (pf - p_curr) - d3
# If there is meaningful distance left, insert a cruise segment
if d_cruise * s > S3_EPS:
path.push(abs(d_cruise)/abs(v_peak), p_curr, v_peak, 0.0)
p_curr += d_cruise
# 3. Deceleration/Adjustment Phase: Move from peak velocity to target velocity
path.push(t3, p_curr, v_peak, s3_sign(vf - v_peak) * acc)
return path
def s3_plan_velocity(start_state, target_v, v_max, a_max):
path = Path()
vi = start_state.v
pi = start_state.p
acc = abs(a_max)
v_cap = abs(v_max)
vf = max(-v_cap, min(v_cap, target_v))
if acc < S3_EPS or abs(vf - vi) < S3_EPS:
path.push(0.0, pi, vi, 0.0)
return path
duration = abs(vf - vi) / acc
sign = 1.0 if vf > vi else -1.0
path.push(duration, pi, vi, sign * acc)
return path
def s3_at_time(path, t):
if not path.nodes: return MotionState()
if t < 0: t = 0
elapsed = 0.0
for node in path.nodes:
if t < elapsed + node.dt:
dt = t - elapsed
res = MotionState()
res.a = node.a
res.v = node.v + node.a * dt
res.p = node.p + node.v * dt + 0.5 * node.a * dt * dt
return res
elapsed += node.dt
# Extrapolate last
last = path.nodes[-1]
dt_seg = last.dt
v_end = last.v + last.a * dt_seg
p_end = last.p + last.v * dt_seg + 0.5 * last.a * dt_seg * dt_seg
dt_ex = t - elapsed
res = MotionState()
res.a = 0.0
res.v = v_end
res.p = p_end + v_end * dt_ex
return res
# ==========================================
# UI & Plotting
# ==========================================
class App:
def __init__(self, root):
self.root = root
self.root.title("S3RTT C-Port Simulation (Overshoot Logic)")
self.root.geometry("1200x800")
# Controls Frame
control_frame = ttk.LabelFrame(root, text="Parameters")
control_frame.pack(fill="x", padx=10, pady=5)
self.entries = {}
params = [
("Start Pos", "0.0"),
("Start Vel", "0.0"),
("Target Pos", "1000"),
("Target Vel", "0.0"),
("Max Vel", "1000.0"),
("Max Acc", "10000.0")
]
for i, (label, val) in enumerate(params):
ttk.Label(control_frame, text=label).pack(side="left", padx=5)
e = ttk.Entry(control_frame, width=8)
e.insert(0, val)
e.pack(side="left", padx=5)
self.entries[label] = e
btn = ttk.Button(control_frame, text="Calculate & Plot", command=self.calculate)
btn.pack(side="left", padx=20)
# Info Label
self.info_lbl = ttk.Label(root, text="Ready", foreground="blue")
self.info_lbl.pack(pady=5)
# Plot Area
self.fig, self.axs = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
self.canvas = FigureCanvasTkAgg(self.fig, master=root)
self.canvas.get_tk_widget().pack(fill="both", expand=True)
def get_float(self, name):
try:
return float(self.entries[name].get())
except ValueError:
return 0.0
def calculate(self):
start_p = self.get_float("Start Pos")
start_v = self.get_float("Start Vel")
target_p = self.get_float("Target Pos")
target_v = self.get_float("Target Vel")
v_max = self.get_float("Max Vel")
a_max = self.get_float("Max Acc")
start_s = MotionState(0, start_p, start_v, 0)
# Run Algorithm
path = s3_plan(start_s, target_p, target_v, v_max, a_max)
# Sampling
total_t = path.total_time()
sim_duration = total_t * 1.1 + 0.0001
times = []
pos = []
vel = []
acc = []
steps = 1000
dt = sim_duration / steps
final_s = MotionState()
for i in range(steps + 1):
t = i * dt
s = s3_at_time(path, t)
times.append(t)
pos.append(s.p)
vel.append(s.v)
acc.append(s.a)
if i == steps: final_s = s
# Update Info
err = abs(final_s.p - target_p)
self.info_lbl.config(text=f"Total Time: {total_t:.4f}s | Segments: {len(path.nodes)} | Final P: {final_s.p:.4f} (Err: {err:.4f}) | Final V: {final_s.v:.4f}")
# Plotting
for ax in self.axs: ax.clear()
self.axs[0].plot(times, pos, label="Position", color="blue")
self.axs[0].axhline(y=target_p, color="green", linestyle="--", label="Target P")
self.axs[0].set_ylabel("Position")
self.axs[0].grid(True)
self.axs[0].legend()
self.axs[1].plot(times, vel, label="Velocity", color="red")
self.axs[1].axhline(y=target_v, color="orange", linestyle="--", label="Target V")
self.axs[1].set_ylabel("Velocity")
self.axs[1].grid(True)
self.axs[2].plot(times, acc, label="Acceleration", color="purple")
self.axs[2].set_ylabel("Acceleration")
self.axs[2].set_xlabel("Time (s)")
self.axs[2].grid(True)
self.canvas.draw()
if __name__ == "__main__":
root = tk.Tk()
app = App(root)
root.mainloop()