path-to-traj.py

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