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(1, len(path) - 1):
 127         if (path[i][4] != last_segment.type()
 128                 #or sgn(path[i][3]) != last_segment.direction()
 129                 or path[i - 1][5]):
 130             last_segment.set_goal(i - 1, path[i - 1])
 131             segments.append(last_segment)
 132             last_segment = PathSegment(i - 1, path[i - 1])
 133             last_segment.init_pose[3] = path[i][3]
 134             last_segment.init_pose[4] = path[i][4]
 135     last_segment.set_goal(len(path) - 1, path[-1])
 136     segments.append(last_segment)
 137     # Fix speed when the segment direction is 0.
 138     for i in range(1, len(segments) - 1):
 139         ls = segments[i-1]
 140         cs = segments[i]
 141         ns = segments[i+1]
 142         if cs.direction() == 0 and ls.direction() == ns.direction():
 143             cs.init_pose[3] = ls.init_pose[3]
 144         elif cs.direction() == 0:
 145             cs.init_pose[3] = ns.init_pose[3]
 146     return segments
 147
 148
 149 def find_cusps_in(path):
 150     """Cusp pose is the pose where the direction changed."""
 151     cusps = []
 152     for i in range(len(path)):
 153         if path[i][5]:
 154             cusps.append(i)
 155     return cusps
 156
 157
 158 class PathSegment:
 159     """Class describing path segment.
 160
 161     :param init_index: Initial index
 162     :param init_pose: Initial pose
 163     :param goal_index: Goal index
 164     :param goal_pose: Goal pose
 165     """
 166     def __init__(self, ii, ip):
 167         """Create new PathSegment object.
 168
 169         :param ii: Initial index
 170         :param ip: Initial pose
 171         """
 172         if ip[4] not in [0, 1, -1]:
 173             raise ValueError(f"Unrecognized PathSegment type '{ip[4]}'")
 174         self.init_index = ii
 175         self.init_pose = list(ip)
 176         self.goal_index = None
 177         self.goal_pose = None
 178
 179     def __repr__(self):
 180         st = self.type()
 181         if st == 1:
 182             st = "left"
 183         elif st == -1:
 184             st = "right"
 185         else:
 186             st = "straight"
 187         di = self.direction()
 188         if di > 0:
 189             di = "forward"
 190         elif di < 0:
 191             di = "backward"
 192         else:
 193             di = "stopped"
 194         sl = self.len()
 195         return f"[{self.init_index}, {self.goal_index}] {st} {di} {sl}"
 196
 197     def set_goal(self, gi, gp):
 198         self.goal_index = gi
 199         self.goal_pose = gp
 200
 201     def type(self):
 202         """Return segment type.
 203
 204         Segment type can be:
 205
 206         - 1 ~ turning left
 207         - -1 ~ turning right
 208         - 0 ~ straight
 209         """
 210         return self.init_pose[4]
 211
 212     def direction(self):
 213         """Return segment direction.
 214
 215         Segment direction can be:
 216
 217         - 1 ~ forward
 218         - -1 ~ backward
 219         - 0 ~ stopped
 220         """
 221         return sgn(self.init_pose[3])
 222
 223     def len(self):
 224         if self.type() == 0:
 225             return ed(self.goal_pose, self.init_pose)
 226         else:
 227             return arc_len(self.init_pose, self.goal_pose)
 228
 229     def pose_at(self, distance):
 230         """Return pose at specific distance.
 231
 232         Well, this does not work well.
 233         """
 234         if self.type() == 0:
 235             h = atan2(
 236                 self.goal_pose[1] - self.init_pose[1],
 237                 self.goal_pose[0] - self.init_pose[0])
 238             return [
 239                 self.init_pose[0] + distance * cos(h),
 240                 self.init_pose[1] + distance * sin(h),
 241                 h, 0, None, None]
 242         else:
 243             x = self.init_pose[0]
 244             y = self.init_pose[1]
 245             h = self.init_pose[2]
 246             h += (
 247                 self.direction() * distance / BicycleCar().wb
 248                 * tan(deg_to_rad(36) * self.type()))
 249             x += self.direction() * distance * cos(h)
 250             y += self.direction() * distance * sin(h)
 251             return [x, y, h, h, None, None]
 252
 253
 254 class BicycleCar:
 255     ctc = 11.2531  # this is guess resulting to max_wa ~= 35.99998076387121
 256     w = 1.983
 257     ft = 1.680
 258     wb = 2.895
 259     df = 2.895 + 0.9  # this is guess
 260     dr = 4.918 - df  # length - df
 261
 262     max_sp = 3 / 3.6  # mps
 263     max_acc = 0.5  # mpss
 264
 265     mtr = ((ctc / 2)**2 - wb**2)**0.5 - ft/2
 266     max_wa = atan(wb / mtr)  # rad
 267     max_wa_rate = deg_to_rad(1)  # rad per sample
 268
 269     def __init__(self, x=0, y=0, h=0):
 270         self.x = x  # x, y is center of rear axle
 271         self.y = y
 272         self.h = h
 273         self.sp = 0  # speed
 274         self.wa = 0  # wheel angle
 275         self.dwa = 0  # desired wa
 276         self.acc = 0
 277         self.braking = False
 278
 279     def pose(self):
 280         return [self.x, self.y, self.h, self.wa, self.sp, self.acc]
 281
 282     def ed(self, pose):
 283         """Euclidean distance from self to pose in 2D."""
 284         return ed(self.pose(), pose)
 285
 286     def ttsp(self, sp):
 287         """Return time to speed.
 288
 289         It is expected that speed is reachable from the current state,
 290         i.e. speed 0 is unreachable from speed 3/3.6 when acc is greater
 291         than zero.
 292         """
 293         sp = check_limits(sp, self.max_sp)
 294         t = (sp - self.sp) / self.acc
 295         assert t >= 0
 296         return t
 297
 298     def bd(self, t=None):
 299         """Return braking distance.
 300
 301         :param t: Time to stop (speed = 0).
 302
 303         See https://en.wikipedia.org/wiki/Braking_distance
 304         See https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction
 305         See https://en.wikipedia.org/wiki/Standard_gravity
 306
 307         But see https://www.nabla.cz/obsah/fyzika/mechanika/rovnomerne-zrychleny-primocary-pohyb.php
 308
 309         s = v_0 * t + 1/2 * a * t**2
 310         """
 311         if t:
 312             return self.sp * t + 1/2 * self.acc * t**2
 313         else:
 314             # The following is a theoretical braking distance.
 315             # mu = 0.6
 316             # g = 9.80665
 317             # return self.sp**2 / 2 / mu / g
 318
 319             # This is braking distance we need:
 320             t = abs(self.sp / self.acc)
 321             return self.sp * t + 1/2 * self.acc * t**2
 322
 323     def ttwa(self, wa):
 324         """Return time to wheel angle."""
 325         wa = check_limits(wa, self.max_wa)
 326         return (wa - self.wa) / self.max_wa_rate * DT
 327
 328     def poses_to(self, pose):
 329         """Return tuple of poses toward pose.
 330
 331         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)
 332         see https://math.stackexchange.com/questions/285866/calculating-circle-radius-from-two-points-on-circumference-for-game-movement
 333         see https://en.wikipedia.org/wiki/Arc_length
 334         """
 335         self.wa_to(pose)
 336         s = self.ed(pose)
 337         d = 0
 338         poses = []
 339         while self.sp != 0 and d < s:
 340             opose = self.pose()
 341             self.next()
 342             poses.append(self.pose())
 343             d += self.ed(opose)
 344         return poses
 345
 346     def set_wa_based_on_dwa(self):
 347         if abs(self.dwa - self.wa) > self.max_wa_rate:
 348             if self.dwa > self.wa:
 349                 self.wa += self.max_wa_rate
 350             else:
 351                 self.wa -= self.max_wa_rate
 352         else:
 353             self.wa = self.dwa
 354
 355     def next(self):
 356         self.set_wa_based_on_dwa()
 357         self.sp += self.acc * DT
 358         self.sp = check_limits(self.sp, self.max_sp)
 359         if self.braking:
 360             if self.acc < 0 and self.sp < 0:
 361                 self.acc = 0
 362                 self.sp = 0
 363             elif self.acc > 0 and self.sp > 0:
 364                 self.acc = 0
 365                 self.sp = 0
 366         self.x += DT * self.sp * cos(self.h)
 367         self.y += DT * self.sp * sin(self.h)
 368         self.h += DT * self.sp / self.wb * tan(self.wa)
 369         while self.h > pi:
 370             self.h -= pi
 371         while self.h < -pi:
 372             self.h += pi
 373
 374     def brake_within(self, d):
 375         """Brake within specified distance.
 376
 377         :param d: Distance to brake within.
 378
 379         The computation is based on:
 380
 381         1. time to stop = abs(speed / acc)
 382         2. distance to stop = speed * t + 1/2 * acc * t**2
 383
 384         Now, substitute t in (2) by (1) and get acc from (2).
 385
 386         NOTE: I am not sure why 1/2 is better than 3/2. Probably, I
 387         miscomputed something...
 388         """
 389         bd = sgn(self.sp) * -1
 390         self.acc = check_limits(bd * 1/2 * self.sp**2 / d, self.max_acc)
 391         self.braking = True
 392
 393     def wa_to(self, pose):
 394         if len(pose) > 4:
 395             if pose[4] > 0:
 396                 self.wa = self.max_wa
 397                 # s = self.mtr * asin(self.ed(pose) / 2 / self.mtr)
 398             elif pose[4] < 0:
 399                 self.wa = -self.max_wa
 400                 # s = self.mtr * asin(self.ed(pose) / 2 / self.mtr)
 401             else:
 402                 self.wa = 0
 403
 404
 405 if __name__ == "__main__":
 406     plan = None
 407     if len(sys.argv) == 2:
 408         PLAN_FILE = sys.argv[1]
 409     with open(f"{PLAN_FILE}", "r") as f:
 410         plan = json.load(f)
 411     plan["nnpath"] = list(plan["path"])
 412     plan["path"] = normalize_path(plan["path"])
 413     path = list(plan["path"])
 414     cusps = find_cusps_in(path)
 415     path_segments = find_segments_of(path)
 416     print(f"path segments ({len(path_segments)}):")
 417     for s in path_segments:
 418         print(f"- {s}")
 419     print(f"cusps ({len(cusps)}):")
 420     for i in range(len(cusps)):
 421         print(f"- {cusps[i]}")
 422     c = BicycleCar(*path[0][:3])
 423     traj = []
 424     # Set wa to the value of init pose.
 425     if path_segments[0].type() == 1:
 426         c.dwa = c.max_wa
 427     elif path_segments[0].type() == -1:
 428         c.dwa = -c.max_wa
 429     else:
 430         c.dwa = 0
 431     for i in range(count_to(c.ttwa(c.dwa))):
 432         c.next()
 433         traj.append(c.pose())
 434     # ---
 435     # Interpolate over segments.
 436     for i in range(len(path_segments)):
 437         s = path_segments[i]
 438         s_is_cusp = s.goal_index in cusps
 439         c.acc = set_max(s.direction(), c.max_acc)
 440         c.dwa = set_max(s.type(), c.max_wa)
 441         d = 0
 442         while d < s.len():
 443             if c.braking and sgn(c.sp) == sgn(c.acc):
 444                 break
 445             if (s_is_cusp
 446                     and (max(1, s.len()/2) > abs(s.len() - d))
 447                     and not c.braking):
 448                 c.brake_within(abs(s.len() - d))
 449             op = c.pose()
 450             c.next()
 451             traj.append(c.pose())
 452             if s.type() == 0:
 453                 d += c.ed(op)
 454             else:
 455                 d += arc_len(op, c.pose())
 456         if c.braking and s_is_cusp:
 457             c.sp = 0
 458             c.acc = 0
 459             for _ in range(count_to(3)):
 460                 c.next()
 461                 traj.append(c.pose())
 462             c.acc = set_max(path_segments[i+1].direction(), c.max_acc)
 463             c.dwa = set_max(path_segments[i+1].type(), c.max_wa)
 464             for _ in range(count_to(c.ttwa(c.dwa))):
 465                 c.next()
 466                 traj.append(c.pose())
 467             c.braking = False
 468     # ---
 469     # Perform in slot maneuver.
 470     assert sgn(c.sp) == sgn(c.acc)
 471     lpose = list(plan["ispath"][-1])
 472     lpose.append(0)
 473     ll = 0
 474     if PLAN_FILE.startswith("pa"):
 475         lpose.append(c.max_wa)
 476         ll = arc_len(c.pose(), lpose)
 477         c.dwa = c.max_wa
 478     else:
 479         lpose.append(0)
 480         ll = c.ed(lpose)
 481         c.dwa = 0
 482     c.brake_within(ll)
 483     d = 0
 484     while d < ll:
 485         if c.braking and sgn(c.sp) == sgn(c.acc):
 486             break
 487         op = c.pose()
 488         c.next()
 489         traj.append(c.pose())
 490         if PLAN_FILE.startswith("pa"):
 491             d += arc_len(op, c.pose())
 492         else:
 493             d += c.ed(op)
 494     # ---
 495     plan["traj"] = traj
 496     with open(f"{PLAN_FILE.split('.')[0]}.traj.json", "w") as f:
 497         json.dump(plan, f, indent="\t")
 498     with open(f"{PLAN_FILE.split('.')[0]}.traj.h", "w") as f:
 499         f.write("#ifndef COMMANDER_COMMANDS_H\n")
 500         f.write("#define COMMANDER_COMMANDS_H\n")
 501         f.write(f"#define COMMANDS_COUNT {len(traj)}\n")
 502         f.write("struct command { double wa; double sp; double acc; };\n")
 503         f.write("struct command commands[] = {\n")
 504         for p in traj:
 505             f.write(f"  {{{rad_to_deg(p[3])}, {p[4]}, {p[5]}}},\n")
 506         f.write("};\n")
 507         f.write("#endif // COMMANDER_COMMANDS_H\n")