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 ed(self, pose):
 137         """Euclidean distance from self to pose in 2D."""
 138         return ed(self.pose(), pose)
 139
 140     def bd(self, t=None):
 141         """Return braking distance.
 142
 143         :param t: Time to stop (speed = 0).
 144
 145         See https://en.wikipedia.org/wiki/Braking_distance
 146         See https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction
 147         See https://en.wikipedia.org/wiki/Standard_gravity
 148
 149         But see https://www.nabla.cz/obsah/fyzika/mechanika/rovnomerne-zrychleny-primocary-pohyb.php
 150
 151         s = v_0 * t + 1/2 * a * t**2
 152         """
 153         if t:
 154             return self.sp * t + 1/2 * self.acc * t**2
 155         else:
 156             # The following is a theoretical braking distance.
 157             # mu = 0.6
 158             # g = 9.80665
 159             # return self.sp**2 / 2 / mu / g
 160
 161             # This is braking distance we need:
 162             t = abs(self.sp / self.acc)
 163             return self.sp * t + 1/2 * self.acc * t**2
 164
 165     def pose(self):
 166         return [self.x, self.y, self.h, self.wa, self.sp, self.acc]
 167
 168     def poses_to(self, pose):
 169         """Return tuple of poses toward pose.
 170
 171         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)
 172         see https://math.stackexchange.com/questions/285866/calculating-circle-radius-from-two-points-on-circumference-for-game-movement
 173         see https://en.wikipedia.org/wiki/Arc_length
 174         """
 175         self.wa_to(pose)
 176         s = self.ed(pose)
 177         d = 0
 178         poses = []
 179         while self.sp != 0 and d < s:
 180             opose = self.pose()
 181             self.next()
 182             poses.append(self.pose())
 183             d += self.ed(opose)
 184         return poses
 185
 186     def next(self):
 187         self.sp += self.acc * DT
 188         self.sp = check_limits(self.sp, self.max_sp)
 189         if self.braking:
 190             if self.acc < 0 and self.sp < 0:
 191                 self.acc = 0
 192                 self.sp = 0
 193             elif self.acc > 0 and self.sp > 0:
 194                 self.acc = 0
 195                 self.sp = 0
 196         self.h += self.sp / self.wb * tan(self.wa) * DT
 197         while self.h > pi:
 198             self.h -= pi
 199         while self.h < -pi:
 200             self.h += pi
 201         self.x += DT * self.sp * cos(self.h)
 202         self.y += DT * self.sp * sin(self.h)
 203
 204     def brake_within(self, d):
 205         bd = sgn(self.sp)
 206         self.acc = check_limits(-1 * bd * self.sp**2 / 2 / d, self.max_acc)
 207         self.braking = True
 208
 209     def wa_to(self, pose):
 210         if len(pose) > 4:
 211             if pose[4] > 0:
 212                 self.wa = self.max_wa
 213                 # s = self.mtr * asin(self.ed(pose) / 2 / self.mtr)
 214             elif pose[4] < 0:
 215                 self.wa = -self.max_wa
 216                 # s = self.mtr * asin(self.ed(pose) / 2 / self.mtr)
 217             else:
 218                 self.wa = 0
 219
 220
 221 def find_cusps(path):
 222     assert len(path) > 1
 223     cusps_i = []
 224     for i in range(1, len(path) - 1):
 225         if path[i][3] == 0:
 226             if sgn(path[i-1][3]) != sgn(path[i+1][3]):
 227                 cusps_i.append(i)
 228         else:
 229             if sgn(path[i][3]) != sgn(path[i+1][3]) and path[i+1][3] != 0:
 230                 cusps_i.append(i)
 231     return cusps_i
 232
 233
 234 if __name__ == "__main__":
 235     plan = None
 236     with open(f"orig-{PLAN_FILE}.json", "r") as f:
 237         plan = json.load(f)
 238     plan["nnpath"] = list(plan["path"])
 239     plan["path"] = normalize_path(plan["path"])
 240     path = list(plan["path"])
 241     cusps = find_cusps(path)
 242     path[0][3] = path[1][3]  # initial point is part of the first segment
 243     path[0][4] = path[1][4]
 244     path[-1][3] = path[-2][3]  # goal point is part of the last segment
 245     path[-1][4] = path[-2][4]
 246     c = BicycleCar(*path[0][:3])
 247     c.acc = c.max_acc  # start move forward
 248     c.next()
 249     traj = [c.pose()]
 250     i = 0
 251     while len(path) > 0:
 252         i += 1
 253         if i + 3 in cusps:
 254             d = c.ed(path[0])
 255             for j in range(3):
 256                 d += ed(path[j], path[j + 1])
 257             c.brake_within(d)
 258         if len(path) == 1:  # last move
 259             lpose = list(plan["ispath"][-1])
 260             lpose.append(0)
 261             if PLAN_FILE.startswith("pe"):
 262                 lpose.append(0)
 263             else:
 264                 lpose.append(c.wa)
 265             c.brake_within(c.ed(lpose))
 266             traj += c.poses_to(lpose)
 267             traj[-1][5] = -0.1
 268             c.sp = 0
 269             c.acc = -0.1
 270             for j in range(150):
 271                 c.next()
 272                 traj.append(c.pose())
 273                 traj[-1][5] = -0.1
 274             del path[0]
 275             break
 276         traj += c.poses_to(path[0])
 277         if i in cusps:
 278             assert c.acc <= 0
 279             while abs(c.sp - 0) > 1e-3:
 280                 c.next()
 281                 traj.append(c.pose())
 282             c.sp = 0
 283             c.acc = 0
 284             c.wa_to(path[1])
 285             for j in range(150):
 286                 c.next()
 287                 traj.append(c.pose())
 288             c.acc = sgn(path[1][3]) * c.max_acc
 289             c.braking = False
 290             c.next()
 291             traj.append(c.pose())
 292             del cusps[0]
 293         del path[0]
 294     print(f"left {len(path)} waypoints")
 295     plan["traj"] = traj
 296     with open(f"{PLAN_FILE}.json", "w") as f:
 297         json.dump(plan, f, indent="\t")
 298     with open(f"{PLAN_FILE}.h", "w") as f:
 299         f.write("#ifndef COMMANDER_COMMANDS_H\n")
 300         f.write("#define COMMANDER_COMMANDS_H\n")
 301         f.write(f"#define COMMANDS_COUNT {len(traj)}\n")
 302         f.write("struct command { double wa; double sp; double acc; };\n")
 303         f.write("struct command commands[] = {\n")
 304         for p in traj:
 305             f.write(f"  {{{rad_to_deg(p[3])}, {p[4]}, {p[5]}}},\n")
 306         f.write("};\n")
 307         f.write("#endif // COMMANDER_COMMANDS_H\n")