2 """Translate equidistant path to equitemporal (isochronic) trajectory.
4 Path does not need to be equidistant, in fact. Typically, the last point
5 of the path segment is closer due to interpolation.
7 It is expected that the point of the path has five values: x, y,
8 heading, speed, and type, where type is +1 for left turn, -1 for right
9 turn, and 0 for straight segment.
11 from math import sin, atan, cos, tan, pi, acos, asin, atan2, sqrt
14 from dataclasses import dataclass
17 PLAN_FILE = "orig-forward.json"
18 DT = 0.02 # seconds, sampling period
22 return (0 < x) - (x < 0)
33 def find_segments_of(path):
35 # Direction in the input path is not consistent. The first point
36 # seems to be correct, the rest can be derived from it.late it ourselves.
37 direction = sgn(path[0][3])
38 current_segment = PathSegment(direction)
40 sp = SegmentPoint(x=p[0], y=p[1], heading=p[2], turn_type=p[4])
41 current_segment.append(sp)
43 if cusp and len(current_segment) > 1:
44 current_segment.precalc_dist()
45 segments.append(current_segment)
47 current_segment = PathSegment(direction)
48 current_segment.append(copy(sp))
49 if len(current_segment) > 1:
50 current_segment.precalc_dist()
51 segments.append(current_segment)
62 dist_from_seg_start: float = None
64 def distance_to(self, other):
65 return sqrt ((other.x - self.x) * (other.x - self.x) +
66 (other.y - self.y) * (other.y - self.y))
69 class PathSegment(list):
70 def __init__(self, direction: int):
71 self.direction = direction # ±1
72 self.last_pose_index = None
74 def precalc_dist(self):
76 self[0].dist_from_seg_start = dist
77 for i in range(1, len(self)):
78 dist += self[i-1].distance_to(self[i])
79 self[i].dist_from_seg_start = dist
81 def total_length(self):
82 return self[-1].dist_from_seg_start
84 def pose_at(self, distance):
85 """Return pose at specific distance from the start of the segment.
87 if distance > self.total_length():
88 raise Exception("distance to high")
90 i = self.last_pose_index if self.last_pose_index is not None else 0
92 while distance < self[i].dist_from_seg_start:
94 while distance > self[i+1].dist_from_seg_start:
97 ratio = (distance - self[i].dist_from_seg_start) / \
98 (self[i+1].dist_from_seg_start - self[i].dist_from_seg_start)
101 pose.x = self[i].x + ratio * (self[i+1].x - self[i].x)
102 pose.y = self[i].y + ratio * (self[i+1].y - self[i].y)
103 pose.heading = self[i].heading + ratio * (self[i+1].heading - self[i].heading)
107 ctc = 11.2531 # this is guess resulting to max_wa ~= 35.99998076387121
111 df = 2.895 + 0.9 # this is guess
112 dr = 4.918 - df # length - df
114 max_sp = 2.5 / 3.6 # mps
117 mtr = ((ctc / 2)**2 - wb**2)**0.5 - ft/2
118 max_wa = atan(wb / mtr) # rad
119 max_wa_rate = deg_to_rad(50) # rad per second
121 def __init__(self, x=0, y=0, h=0, forward=True):
122 self.x = x # x, y is center of rear axle
126 self.wa = 0 # wheel angle
127 self.dwa = 0 # desired wa
129 self.forward = forward # gear: forward/backward
132 self.traj.append(self.pose()) # Start trajectory with the initial pose
135 return f"BicycleCar{{x:{self.x:.3f} y:{self.y:.3f} h:{self.h:.3f}}}"
138 return [self.x, self.y, self.h, self.wa, self.sp, self.acc, self.forward]
141 def bd(self, acc, t=None):
142 """Return braking distance.
144 :param t: Time to stop (speed = 0).
146 See https://en.wikipedia.org/wiki/Braking_distance
147 See https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction
148 See https://en.wikipedia.org/wiki/Standard_gravity
150 But see https://www.nabla.cz/obsah/fyzika/mechanika/rovnomerne-zrychleny-primocary-pohyb.php
152 s = v_0 * t + 1/2 * a * t**2
155 t = abs(self.sp / acc)
156 return abs(self.sp) * t + 1/2 * -1*abs(acc) * t**2
158 def set_wa_based_on_dwa(self, dt):
159 if abs(self.dwa - self.wa) > self.max_wa_rate * dt:
160 if self.dwa > self.wa:
161 self.wa += self.max_wa_rate * dt
163 self.wa -= self.max_wa_rate * dt
166 if abs(self.wa) > self.max_wa:
167 self.wa = self.max_wa * sgn(self.wa)
171 self.set_wa_based_on_dwa(dt)
172 self.sp += self.acc * dt
173 if abs(self.sp) > self.max_sp:
174 self.sp = self.max_sp * sgn(self.sp)
176 self.x += dt * self.sp * cos(self.h)
177 self.y += dt * self.sp * sin(self.h)
178 self.h += dt * self.sp / self.wb * tan(self.wa)
184 self.traj.append(self.pose())
187 if __name__ == "__main__":
189 if len(sys.argv) == 2:
190 PLAN_FILE = sys.argv[1]
191 with open(f"{PLAN_FILE}", "r") as f:
193 plan["nnpath"] = list(plan["path"])
194 #plan["path"] = normalize_path(plan["path"])
195 path = list(plan["path"])
196 #cusps = find_cusps_in(path)
197 path_segments = find_segments_of(path)
198 print(f"path segments ({len(path_segments)}):")
199 for s in path_segments:
200 print(f"- dir:{s.direction:+d} len:{s.total_length()} {s}")
201 p = path_segments[0][0]
202 c = BicycleCar(p.x, p.y, p.heading, path_segments[0].direction == +1)
204 # Interpolate over segments.
205 for i in range(len(path_segments)):
208 # wait 2 second, prepare wheel angle and gear
211 c.dwa = s[1].turn_type * c.max_wa
212 c.forward = s.direction == +1
213 for _ in range(int(2/DT)):
218 while dist < s.total_length():
220 dist_to_end = s.total_length() - dist
221 c.acc = s.direction * c.max_acc
222 if dist_to_end <= c.bd(c.max_acc):
224 c.dwa = p.turn_type * c.max_wa
225 sp = c.sp + DT/2 * c.acc # avg. speed in the sample period
228 print(f"seg{i}: t0:{t0:5.2f}s t_end:{c.time:5.2f}s")
229 # Add time to change gear and steer to other side
232 for _ in range(int(1/DT)): # wait 1 second
236 plan["traj"] = c.traj
237 with open(f"{PLAN_FILE.split('.')[0]}.traj.json", "w") as f:
238 json.dump(plan, f, indent="\t")
239 with open(f"trajectory.h", "w") as f:
240 f.write("#pragma once\n")
241 f.write("#include <stdbool.h>\n")
242 f.write("struct command { double wa; double sp; double acc; bool forward; };\n")
243 f.write("extern struct command commands[];\n")
244 f.write("extern const unsigned commands_count;\n")
245 with open(f"traj-{PLAN_FILE.split('.')[0]}.cpp", "w") as f:
246 f.write('#include "trajectory.h"\n')
247 f.write(f"const unsigned commands_count = {len(c.traj)};\n")
248 f.write("struct command commands[] = {\n")
250 f.write(f" {{{rad_to_deg(p[3])}, {p[4]}, {p[5]}, {'true' if p[6] else 'false'}}},\n")