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