S7RTT/python/S3RTT.py at main · feecat/S7RTT · GitHub

History

327 lines (267 loc) · 11.8 KB

Raw

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

# ==============================================================================

# 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()

S7RTT/python/S3RTT.py at main · feecat/S7RTT · GitHub

Expand file tree

Breadcrumbs

S3RTT.py

Latest commit

History

Breadcrumbs

S3RTT.py

File metadata and controls