path-to-traj.py

   1 #!/usr/bin/env python3
   2 """Translate equidistant path to equitemporal (isochronic) trajectory.
   3
   4 Path does not need to be equidistant, in fact. Typically, the last point
   5 of the path segment is closer due to interpolation.
   6
   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.
  10 """
  11 from math import sin, atan, cos, tan, pi, acos, asin, atan2
  12 import json
  13 import sys
  14
  15 PLAN_FILE = "orig-forward.json"
  16 DT = 0.02  # seconds, sampling period
  17 WD = 0.5  # m, waypoint distance
  18
  19
  20 def sgn(x):
  21     return (0 < x) - (x < 0)
  22
  23
  24 def ed(p1, p2):
  25     return ((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2)**0.5
  26
  27
  28 def arc_len(p1, p2):
  29     # see https://math.stackexchange.com/questions/1595872/arclength-between-two-points-on-a-circle-not-knowing-theta
  30     r = BicycleCar().mtr
  31     d = ed(p1, p2)
  32     return 2 * r * asin(d / 2 / r)
  33
  34
  35 def rad_to_deg(x):
  36     return x * 180 / pi
  37
  38
  39 def deg_to_rad(x):
  40     return x / 180 * pi
  41
  42
  43 def count_to(seconds):
  44     return abs(int(seconds / DT)) + 1
  45
  46
  47 def normalize_path(path):
  48     x = path[0][0]
  49     y = path[0][1]
  50     for p in path:
  51         p[0] -= x
  52         p[1] -= y
  53     while path[0] == path[1]:
  54         del path[0]
  55     return path
  56
  57
  58 def check_limits(x, max_x):
  59     if x > max_x:
  60         return max_x
  61     elif x < -max_x:
  62         return -max_x
  63     else:
  64         return x
  65
  66
  67 def set_max(x, max_x):
  68     if x > 0:
  69         return max_x
  70     elif x < 0:
  71         return -max_x
  72     else:
  73         return 0
  74
  75
  76 def lines_intersect(li1, li2):
  77     x1 = li1[0][0]
  78     y1 = li1[0][1]
  79     x2 = li1[1][0]
  80     y2 = li1[1][1]
  81     x3 = li2[0][0]
  82     y3 = li2[0][1]
  83     x4 = li2[1][0]
  84     y4 = li2[1][1]
  85     deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
  86     if deno == 0:
  87         return False
  88     t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)
  89     t /= deno
  90     u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)
  91     u *= -1
  92     u /= deno
  93     if t < 0 or t > 1 or u < 0 or u > 1:
  94         return False
  95     return x1 + t * (x2 - x1), y1 + t * (y2 - y1)
  96
  97
  98 def point_on_right_side_of(p, li):
  99     x1 = li[0][0]
 100     y1 = li[0][1]
 101     x2 = li[1][0]
 102     y2 = li[1][1]
 103     x3 = p[0]
 104     y3 = p[1]
 105     if sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0:
 106         return False
 107     else:
 108         return True
 109
 110
 111 def min_angle_between(p1, pp, p2):
 112     d1x = p1[0] - pp[0]
 113     d1y = p1[1] - pp[1]
 114     d2x = p2[0] - p1[0]
 115     d2y = p2[1] - p1[1]
 116     dot = d1x * d2x + d1y * d2y
 117     d1 = (d1x * d1x + d1y * d1y)**0.5
 118     d2 = (d2x * d2x + d2y * d2y)**0.5
 119     delta = acos(dot / (d1 * d2))
 120     return min(delta, pi - delta)
 121
 122
 123 def find_segments_of(path):
 124     segments = []
 125     last_segment = PathSegment(0, path[0])
 126     for i in range(len(path)):
 127         if (path[i][4] != last_segment.type()
 128                 or sgn(path[i][3]) != last_segment.direction()):
 129             last_segment.set_goal(i, path[i])
 130             segments.append(last_segment)
 131             last_segment = PathSegment(i, path[i])
 132     last_segment.set_goal(len(path) - 1, path[-1])
 133     segments.append(last_segment)
 134     # Fix speed when the segment direction is 0.
 135     for i in range(1, len(segments) - 1):
 136         ls = segments[i-1]
 137         cs = segments[i]
 138         ns = segments[i+1]
 139         if cs.direction() == 0 and ls.direction() == ns.direction():
 140             cs.init_pose[3] = ls.init_pose[3]
 141         elif cs.direction() == 0:
 142             cs.init_pose[3] = ns.init_pose[3]
 143     return segments
 144
 145
 146 def find_cusps_in(segments):
 147     """Cusp pose is the first pose where the direction changed."""
 148     cusps = []
 149     for i in range(1, len(segments)):
 150         if segments[i-1].direction() != segments[i].direction():
 151             cusps.append(segments[i].init_index)
 152     return cusps
 153
 154
 155 class PathSegment:
 156     """Class describing path segment.
 157
 158     :param init_index: Initial index
 159     :param init_pose: Initial pose
 160     :param goal_index: Goal index
 161     :param goal_pose: Goal pose
 162     """
 163     def __init__(self, ii, ip):
 164         """Create new PathSegment object.
 165
 166         :param ii: Initial index
 167         :param ip: Initial pose
 168         """
 169         if ip[4] not in [0, 1, -1]:
 170             raise ValueError(f"Unrecognized PathSegment type '{ip[4]}'")
 171         self.init_index = ii
 172         self.init_pose = list(ip)
 173         self.goal_index = None
 174         self.goal_pose = None
 175
 176     def __repr__(self):
 177         st = self.type()
 178         if st == 1:
 179             st = "left"
 180         elif st == -1:
 181             st = "right"
 182         else:
 183             st = "straight"
 184         di = self.direction()
 185         if di > 0:
 186             di = "forward"
 187         elif di < 0:
 188             di = "backward"
 189         else:
 190             di = "stopped"
 191         sl = self.len()
 192         return f"[{self.init_index}, {self.goal_index}] {st} {di} {sl}"
 193
 194     def set_goal(self, gi, gp):
 195         self.goal_index = gi
 196         self.goal_pose = gp
 197
 198     def type(self):
 199         """Return segment type.
 200
 201         Segment type can be:
 202
 203         - 1 ~ turning left
 204         - -1 ~ turning right
 205         - 0 ~ straight
 206         """
 207         return self.init_pose[4]
 208
 209     def direction(self):
 210         """Return segment direction.
 211
 212         Segment direction can be:
 213
 214         - 1 ~ forward
 215         - -1 ~ backward
 216         - 0 ~ stopped
 217         """
 218         return sgn(self.init_pose[3])
 219
 220     def len(self):
 221         if self.type() == 0:
 222             return ed(self.goal_pose, self.init_pose)
 223         else:
 224             return arc_len(self.init_pose, self.goal_pose)
 225
 226     def pose_at(self, distance):
 227         """Return pose at specific distance.
 228
 229         Well, this does not work well.
 230         """
 231         if self.type() == 0:
 232             h = atan2(
 233                 self.goal_pose[1] - self.init_pose[1],
 234                 self.goal_pose[0] - self.init_pose[0])
 235             return [
 236                 self.init_pose[0] + distance * cos(h),
 237                 self.init_pose[1] + distance * sin(h),
 238                 h, 0, None, None]
 239         else:
 240             x = self.init_pose[0]
 241             y = self.init_pose[1]
 242             h = self.init_pose[2]
 243             h += (
 244                 self.direction() * distance / BicycleCar().wb
 245                 * tan(deg_to_rad(36) * self.type()))
 246             x += self.direction() * distance * cos(h)
 247             y += self.direction() * distance * sin(h)
 248             return [x, y, h, h, None, None]
 249
 250
 251 class BicycleCar:
 252     ctc = 11.2531  # this is guess resulting to max_wa ~= 35.99998076387121
 253     w = 1.983
 254     ft = 1.680
 255     wb = 2.895
 256     df = 2.895 + 0.9  # this is guess
 257     dr = 4.918 - df  # length - df
 258
 259     max_sp = 3 / 3.6  # mps
 260     max_acc = 0.5  # mpss
 261
 262     mtr = ((ctc / 2)**2 - wb**2)**0.5 - ft/2
 263     max_wa = atan(wb / mtr)  # rad
 264     max_wa_rate = deg_to_rad(1)  # rad per sample
 265
 266     def __init__(self, x=0, y=0, h=0):
 267         self.x = x  # x, y is center of rear axle
 268         self.y = y
 269         self.h = h
 270         self.sp = 0  # speed
 271         self.wa = 0  # wheel angle
 272         self.dwa = 0  # desired wa
 273         self.acc = 0
 274         self.braking = False
 275
 276     def pose(self):
 277         return [self.x, self.y, self.h, self.wa, self.sp, self.acc]
 278
 279     def ed(self, pose):
 280         """Euclidean distance from self to pose in 2D."""
 281         return ed(self.pose(), pose)
 282
 283     def ttsp(self, sp):
 284         """Return time to speed.
 285
 286         It is expected that speed is reachable from the current state,
 287         i.e. speed 0 is unreachable from speed 3/3.6 when acc is greater
 288         than zero.
 289         """
 290         sp = check_limits(sp, self.max_sp)
 291         t = (sp - self.sp) / self.acc
 292         assert t >= 0
 293         return t
 294
 295     def bd(self, t=None):
 296         """Return braking distance.
 297
 298         :param t: Time to stop (speed = 0).
 299
 300         See https://en.wikipedia.org/wiki/Braking_distance
 301         See https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction
 302         See https://en.wikipedia.org/wiki/Standard_gravity
 303
 304         But see https://www.nabla.cz/obsah/fyzika/mechanika/rovnomerne-zrychleny-primocary-pohyb.php
 305
 306         s = v_0 * t + 1/2 * a * t**2
 307         """
 308         if t:
 309             return self.sp * t + 1/2 * self.acc * t**2
 310         else:
 311             # The following is a theoretical braking distance.
 312             # mu = 0.6
 313             # g = 9.80665
 314             # return self.sp**2 / 2 / mu / g
 315
 316             # This is braking distance we need:
 317             t = abs(self.sp / self.acc)
 318             return self.sp * t + 1/2 * self.acc * t**2
 319
 320     def ttwa(self, wa):
 321         """Return time to wheel angle."""
 322         wa = check_limits(wa, self.max_wa)
 323         return (wa - self.wa) / self.max_wa_rate * DT
 324
 325     def poses_to(self, pose):
 326         """Return tuple of poses toward pose.
 327
 328         see https://math.stackexchange.com/questions/1794943/how-to-calculate-the-distance-between-two-points-on-a-circle-in-degrees (error in answer, it's theta/2)
 329         see https://math.stackexchange.com/questions/285866/calculating-circle-radius-from-two-points-on-circumference-for-game-movement
 330         see https://en.wikipedia.org/wiki/Arc_length
 331         """
 332         self.wa_to(pose)
 333         s = self.ed(pose)
 334         d = 0
 335         poses = []
 336         while self.sp != 0 and d < s:
 337             opose = self.pose()
 338             self.next()
 339             poses.append(self.pose())
 340             d += self.ed(opose)
 341         return poses
 342
 343     def set_wa_based_on_dwa(self):
 344         if abs(self.dwa - self.wa) > self.max_wa_rate:
 345             if self.dwa > self.wa:
 346                 self.wa += self.max_wa_rate
 347             else:
 348                 self.wa -= self.max_wa_rate
 349         else:
 350             self.wa = self.dwa
 351
 352     def next(self):
 353         self.set_wa_based_on_dwa()
 354         self.sp += self.acc * DT
 355         self.sp = check_limits(self.sp, self.max_sp)
 356         if self.braking:
 357             if self.acc < 0 and self.sp < 0:
 358                 self.acc = 0
 359                 self.sp = 0
 360             elif self.acc > 0 and self.sp > 0:
 361                 self.acc = 0
 362                 self.sp = 0
 363         self.x += DT * self.sp * cos(self.h)
 364         self.y += DT * self.sp * sin(self.h)
 365         self.h += DT * self.sp / self.wb * tan(self.wa)
 366         while self.h > pi:
 367             self.h -= pi
 368         while self.h < -pi:
 369             self.h += pi
 370
 371     def brake_within(self, d):
 372         """Brake within specified distance.
 373
 374         :param d: Distance to brake within.
 375
 376         The computation is based on:
 377
 378         1. time to stop = abs(speed / acc)
 379         2. distance to stop = speed * t + 1/2 * acc * t**2
 380
 381         Now, substitute t in (2) by (1) and get acc from (2).
 382
 383         NOTE: I am not sure why 1/2 is better than 3/2. Probably, I
 384         miscomputed something...
 385         """
 386         bd = sgn(self.sp) * -1
 387         self.acc = check_limits(bd * 1/2 * self.sp**2 / d, self.max_acc)
 388         self.braking = True
 389
 390     def wa_to(self, pose):
 391         if len(pose) > 4:
 392             if pose[4] > 0:
 393                 self.wa = self.max_wa
 394                 # s = self.mtr * asin(self.ed(pose) / 2 / self.mtr)
 395             elif pose[4] < 0:
 396                 self.wa = -self.max_wa
 397                 # s = self.mtr * asin(self.ed(pose) / 2 / self.mtr)
 398             else:
 399                 self.wa = 0
 400
 401
 402 def find_cusps(path):
 403     assert len(path) > 1
 404     cusps_i = []
 405     for i in range(1, len(path) - 1):
 406         if path[i][3] == 0:
 407             if sgn(path[i-1][3]) != sgn(path[i+1][3]):
 408                 cusps_i.append(i)
 409         else:
 410             if sgn(path[i][3]) != sgn(path[i+1][3]) and path[i+1][3] != 0:
 411                 cusps_i.append(i)
 412     return cusps_i
 413
 414
 415 if __name__ == "__main__":
 416     plan = None
 417     if len(sys.argv) == 2:
 418         PLAN_FILE = sys.argv[1]
 419     with open(f"{PLAN_FILE}", "r") as f:
 420         plan = json.load(f)
 421     plan["nnpath"] = list(plan["path"])
 422     plan["path"] = normalize_path(plan["path"])
 423     path = list(plan["path"])
 424     cusps = find_cusps(path)
 425     path[0][3] = path[1][3]  # initial point is part of the first segment
 426     path[0][4] = path[1][4]
 427     path[-1][3] = path[-2][3]  # goal point is part of the last segment
 428     path[-1][4] = path[-2][4]
 429     path_segments = find_segments_of(path)
 430     cusps = find_cusps_in(path_segments)
 431     print(f"path segments ({len(path_segments)}):")
 432     for s in path_segments:
 433         print(f"- {s}")
 434     print(f"cusps ({len(cusps)}):")
 435     for i in range(len(cusps)):
 436         print(f"- {cusps[i]}")
 437     c = BicycleCar(*path[0][:3])
 438     traj = []
 439     # Set wa to the value of init pose.
 440     if path_segments[0].type() == 1:
 441         c.dwa = c.max_wa
 442     elif path_segments[0].type() == -1:
 443         c.dwa = -c.max_wa
 444     else:
 445         c.dwa = 0
 446     for i in range(count_to(c.ttwa(c.dwa))):
 447         c.next()
 448         traj.append(c.pose())
 449     # ---
 450     # Interpolate over segments.
 451     for i in range(len(path_segments)):
 452         s = path_segments[i]
 453         s_is_cusp = False if i + 1 >= len(path_segments) else (
 454             sgn(s.direction()) != sgn(path_segments[i+1].direction()))
 455         # Comment the following three lines to have the single
 456         # BicycleCar model over all the segments without reseting the
 457         # model at the beginning of each segment.
 458         old_sp = c.sp
 459         c = BicycleCar(*s.init_pose[:3])
 460         c.sp = old_sp
 461         # ---
 462         c.acc = set_max(s.direction(), c.max_acc)
 463         c.dwa = set_max(s.type(), c.max_wa)
 464         d = 0
 465         while d < s.len():
 466             if c.braking and sgn(c.sp) == sgn(c.acc):
 467                 break
 468             if (s_is_cusp
 469                     and (max(1, s.len()/2) > abs(s.len() - d))
 470                     and not c.braking):
 471                 c.brake_within(abs(s.len() - d))
 472             op = c.pose()
 473             c.next()
 474             traj.append(c.pose())
 475             if s.type() == 0:
 476                 d += c.ed(op)
 477             else:
 478                 d += arc_len(op, c.pose())
 479         if c.braking and s_is_cusp:
 480             c.sp = 0
 481             c.acc = 0
 482             for _ in range(count_to(3)):
 483                 c.next()
 484                 traj.append(c.pose())
 485             c.acc = set_max(path_segments[i+1].direction(), c.max_acc)
 486             c.dwa = set_max(path_segments[i+1].type(), c.max_wa)
 487             for _ in range(count_to(c.ttwa(c.dwa))):
 488                 c.next()
 489                 traj.append(c.pose())
 490             c.braking = False
 491     # ---
 492     # Perform in slot maneuver.
 493     assert sgn(c.sp) == sgn(c.acc)
 494     lpose = list(plan["ispath"][-1])
 495     lpose.append(0)
 496     ll = 0
 497     if PLAN_FILE.startswith("pa"):
 498         lpose.append(c.max_wa)
 499         ll = arc_len(c.pose(), lpose)
 500         c.dwa = c.max_wa
 501     else:
 502         lpose.append(0)
 503         ll = c.ed(lpose)
 504         c.dwa = 0
 505     c.brake_within(ll)
 506     d = 0
 507     while d < ll:
 508         if c.braking and sgn(c.sp) == sgn(c.acc):
 509             break
 510         op = c.pose()
 511         c.next()
 512         traj.append(c.pose())
 513         if PLAN_FILE.startswith("pa"):
 514             d += arc_len(op, c.pose())
 515         else:
 516             d += c.ed(op)
 517     # ---
 518     plan["traj"] = traj
 519     with open(f"{PLAN_FILE.split('.')[0]}.traj.json", "w") as f:
 520         json.dump(plan, f, indent="\t")
 521     with open(f"{PLAN_FILE.split('.')[0]}.traj.h", "w") as f:
 522         f.write("#ifndef COMMANDER_COMMANDS_H\n")
 523         f.write("#define COMMANDER_COMMANDS_H\n")
 524         f.write(f"#define COMMANDS_COUNT {len(traj)}\n")
 525         f.write("struct command { double wa; double sp; double acc; };\n")
 526         f.write("struct command commands[] = {\n")
 527         for p in traj:
 528             f.write(f"  {{{rad_to_deg(p[3])}, {p[4]}, {p[5]}}},\n")
 529         f.write("};\n")
 530         f.write("#endif // COMMANDER_COMMANDS_H\n")