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
  12 import json
  13
  14 PLAN_FILE = "forward"
  15 DT = 0.02  # seconds
  16
  17
  18 def sgn(x):
  19     return (0 < x) - (x < 0)
  20
  21
  22 def ed(p1, p2):
  23     return ((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2)**0.5
  24
  25
  26 def rad_to_deg(x):
  27     return x * 180 / pi
  28
  29
  30 def deg_to_rad(x):
  31     return x / 180 * pi
  32
  33
  34 def count_to(seconds):
  35     return abs(int(seconds / DT)) + 1
  36
  37
  38 def normalize_path(path):
  39     x = path[0][0]
  40     y = path[0][1]
  41     for p in path:
  42         p[0] -= x
  43         p[1] -= y
  44     while path[0] == path[1]:
  45         del path[0]
  46     return path
  47
  48
  49 def check_limits(x, max_x):
  50     if x > max_x:
  51         return max_x
  52     elif x < -max_x:
  53         return -max_x
  54     else:
  55         return x
  56
  57
  58 def set_max(x, max_x):
  59     if x >= 0:
  60         return max_x
  61     else:
  62         return -max_x
  63
  64
  65 def lines_intersect(li1, li2):
  66     x1 = li1[0][0]
  67     y1 = li1[0][1]
  68     x2 = li1[1][0]
  69     y2 = li1[1][1]
  70     x3 = li2[0][0]
  71     y3 = li2[0][1]
  72     x4 = li2[1][0]
  73     y4 = li2[1][1]
  74     deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
  75     if deno == 0:
  76         return False
  77     t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)
  78     t /= deno
  79     u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)
  80     u *= -1
  81     u /= deno
  82     if t < 0 or t > 1 or u < 0 or u > 1:
  83         return False
  84     return x1 + t * (x2 - x1), y1 + t * (y2 - y1)
  85
  86
  87 def point_on_right_side_of(p, li):
  88     x1 = li[0][0]
  89     y1 = li[0][1]
  90     x2 = li[1][0]
  91     y2 = li[1][1]
  92     x3 = p[0]
  93     y3 = p[1]
  94     if sgn((x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1)) < 0:
  95         return False
  96     else:
  97         return True
  98
  99
 100 def min_angle_between(p1, pp, p2):
 101     d1x = p1[0] - pp[0]
 102     d1y = p1[1] - pp[1]
 103     d2x = p2[0] - p1[0]
 104     d2y = p2[1] - p1[1]
 105     dot = d1x * d2x + d1y * d2y
 106     d1 = (d1x * d1x + d1y * d1y)**0.5
 107     d2 = (d2x * d2x + d2y * d2y)**0.5
 108     delta = acos(dot / (d1 * d2))
 109     return min(delta, pi - delta)
 110
 111
 112 class BicycleCar:
 113     ctc = 11.2531  # this is guess resulting to max_wa ~= 35.99998076387121
 114     w = 1.983
 115     ft = 1.680
 116     wb = 2.895
 117     df = 2.895 + 0.9  # this is guess
 118     dr = 4.918 - df  # length - df
 119
 120     max_sp = 3 / 3.6  # mps
 121     max_acc = 0.5  # mpss
 122
 123     mtr = ((ctc / 2)**2 - wb**2)**0.5 - ft/2
 124     max_wa = atan(wb / mtr)  # rad
 125     max_wa_rate = deg_to_rad(1)  # rad per sample
 126
 127     def __init__(self, x=0, y=0, h=0):
 128         self.x = x  # x, y is center of rear axle
 129         self.y = y
 130         self.h = h
 131         self.sp = 0  # speed
 132         self.wa = 0  # wheel angle
 133         self.acc = 0
 134         self.braking = False
 135
 136     def pose(self):
 137         return [self.x, self.y, self.h, self.wa, self.sp, self.acc]
 138
 139     def ed(self, pose):
 140         """Euclidean distance from self to pose in 2D."""
 141         return ed(self.pose(), pose)
 142
 143     def bd(self, t=None):
 144         """Return braking distance.
 145
 146         :param t: Time to stop (speed = 0).
 147
 148         See https://en.wikipedia.org/wiki/Braking_distance
 149         See https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction
 150         See https://en.wikipedia.org/wiki/Standard_gravity
 151
 152         But see https://www.nabla.cz/obsah/fyzika/mechanika/rovnomerne-zrychleny-primocary-pohyb.php
 153
 154         s = v_0 * t + 1/2 * a * t**2
 155         """
 156         if t:
 157             return self.sp * t + 1/2 * self.acc * t**2
 158         else:
 159             # The following is a theoretical braking distance.
 160             # mu = 0.6
 161             # g = 9.80665
 162             # return self.sp**2 / 2 / mu / g
 163
 164             # This is braking distance we need:
 165             t = abs(self.sp / self.acc)
 166             return self.sp * t + 1/2 * self.acc * t**2
 167
 168
 169     def poses_to(self, pose):
 170         """Return tuple of poses toward pose.
 171
 172         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)
 173         see https://math.stackexchange.com/questions/285866/calculating-circle-radius-from-two-points-on-circumference-for-game-movement
 174         see https://en.wikipedia.org/wiki/Arc_length
 175         """
 176         self.wa_to(pose)
 177         s = self.ed(pose)
 178         d = 0
 179         poses = []
 180         while self.sp != 0 and d < s:
 181             opose = self.pose()
 182             self.next()
 183             poses.append(self.pose())
 184             d += self.ed(opose)
 185         return poses
 186
 187     def next(self):
 188         self.sp += self.acc * DT
 189         self.sp = check_limits(self.sp, self.max_sp)
 190         if self.braking:
 191             if self.acc < 0 and self.sp < 0:
 192                 self.acc = 0
 193                 self.sp = 0
 194             elif self.acc > 0 and self.sp > 0:
 195                 self.acc = 0
 196                 self.sp = 0
 197         self.h += self.sp / self.wb * tan(self.wa) * DT
 198         while self.h > pi:
 199             self.h -= pi
 200         while self.h < -pi:
 201             self.h += pi
 202         self.x += DT * self.sp * cos(self.h)
 203         self.y += DT * self.sp * sin(self.h)
 204
 205     def brake_within(self, d):
 206         bd = sgn(self.sp)
 207         self.acc = check_limits(-1 * bd * self.sp**2 / 2 / d, self.max_acc)
 208         self.braking = True
 209
 210     def wa_to(self, pose):
 211         if len(pose) > 4:
 212             if pose[4] > 0:
 213                 self.wa = self.max_wa
 214                 # s = self.mtr * asin(self.ed(pose) / 2 / self.mtr)
 215             elif pose[4] < 0:
 216                 self.wa = -self.max_wa
 217                 # s = self.mtr * asin(self.ed(pose) / 2 / self.mtr)
 218             else:
 219                 self.wa = 0
 220
 221
 222 def find_cusps(path):
 223     assert len(path) > 1
 224     cusps_i = []
 225     for i in range(1, len(path) - 1):
 226         if path[i][3] == 0:
 227             if sgn(path[i-1][3]) != sgn(path[i+1][3]):
 228                 cusps_i.append(i)
 229         else:
 230             if sgn(path[i][3]) != sgn(path[i+1][3]) and path[i+1][3] != 0:
 231                 cusps_i.append(i)
 232     return cusps_i
 233
 234
 235 if __name__ == "__main__":
 236     plan = None
 237     with open(f"orig-{PLAN_FILE}.json", "r") as f:
 238         plan = json.load(f)
 239     plan["nnpath"] = list(plan["path"])
 240     plan["path"] = normalize_path(plan["path"])
 241     path = list(plan["path"])
 242     cusps = find_cusps(path)
 243     path[0][3] = path[1][3]  # initial point is part of the first segment
 244     path[0][4] = path[1][4]
 245     path[-1][3] = path[-2][3]  # goal point is part of the last segment
 246     path[-1][4] = path[-2][4]
 247     c = BicycleCar(*path[0][:3])
 248     c.acc = c.max_acc  # start move forward
 249     c.next()
 250     traj = [c.pose()]
 251     i = 0
 252     while len(path) > 0:
 253         i += 1
 254         if i + 3 in cusps:
 255             d = c.ed(path[0])
 256             for j in range(3):
 257                 d += ed(path[j], path[j + 1])
 258             c.brake_within(d)
 259         if len(path) == 1:  # last move
 260             lpose = list(plan["ispath"][-1])
 261             lpose.append(0)
 262             if PLAN_FILE.startswith("pe"):
 263                 lpose.append(0)
 264             else:
 265                 lpose.append(c.wa)
 266             c.brake_within(c.ed(lpose))
 267             traj += c.poses_to(lpose)
 268             traj[-1][5] = -0.1
 269             c.sp = 0
 270             c.acc = -0.1
 271             for j in range(150):
 272                 c.next()
 273                 traj.append(c.pose())
 274                 traj[-1][5] = -0.1
 275             del path[0]
 276             break
 277         traj += c.poses_to(path[0])
 278         if i in cusps:
 279             assert c.acc <= 0
 280             while abs(c.sp - 0) > 1e-3:
 281                 c.next()
 282                 traj.append(c.pose())
 283             c.sp = 0
 284             c.acc = 0
 285             c.wa_to(path[1])
 286             for j in range(150):
 287                 c.next()
 288                 traj.append(c.pose())
 289             c.acc = sgn(path[1][3]) * c.max_acc
 290             c.braking = False
 291             c.next()
 292             traj.append(c.pose())
 293             del cusps[0]
 294         del path[0]
 295     print(f"left {len(path)} waypoints")
 296     plan["traj"] = traj
 297     with open(f"{PLAN_FILE}.json", "w") as f:
 298         json.dump(plan, f, indent="\t")
 299     with open(f"{PLAN_FILE}.h", "w") as f:
 300         f.write("#ifndef COMMANDER_COMMANDS_H\n")
 301         f.write("#define COMMANDER_COMMANDS_H\n")
 302         f.write(f"#define COMMANDS_COUNT {len(traj)}\n")
 303         f.write("struct command { double wa; double sp; double acc; };\n")
 304         f.write("struct command commands[] = {\n")
 305         for p in traj:
 306             f.write(f"  {{{rad_to_deg(p[3])}, {p[4]}, {p[5]}}},\n")
 307         f.write("};\n")
 308         f.write("#endif // COMMANDER_COMMANDS_H\n")