path2traj.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, sqrt
  12 import json
  13 import sys
  14 from dataclasses import dataclass
  15 from copy import copy
  16
  17 PLAN_FILE = "orig-forward.json"
  18 DT = 0.02  # seconds, sampling period
  19
  20
  21 def sgn(x):
  22     return (0 < x) - (x < 0)
  23
  24
  25 def rad_to_deg(x):
  26     return x * 180 / pi
  27
  28
  29 def deg_to_rad(x):
  30     return x / 180 * pi
  31
  32
  33 def find_segments_of(path):
  34     segments = []
  35     # Direction in the input path is not consistent. The first point
  36     # seems to be correct, the rest can be derived from it.late it ourselves.
  37     direction = sgn(path[0][3])
  38     current_segment = PathSegment(direction)
  39     for p in path:
  40         sp = SegmentPoint(x=p[0], y=p[1], heading=p[2], turn_type=p[4])
  41         current_segment.append(sp)
  42         cusp = p[5]
  43         if cusp and len(current_segment) > 1:
  44             current_segment.precalc_dist()
  45             segments.append(current_segment)
  46             direction *= -1;
  47             current_segment = PathSegment(direction)
  48             current_segment.append(copy(sp))
  49     if len(current_segment) > 1:
  50         current_segment.precalc_dist()
  51         segments.append(current_segment)
  52     return segments
  53
  54
  55 @dataclass
  56 class SegmentPoint:
  57     x: float
  58     y: float
  59     heading: float
  60     turn_type: float
  61
  62     dist_from_seg_start: float = None
  63
  64     def distance_to(self, other):
  65         return sqrt ((other.x - self.x) * (other.x - self.x) +
  66                      (other.y - self.y) * (other.y - self.y))
  67
  68
  69 class PathSegment(list):
  70     def __init__(self, direction: int):
  71         self.direction = direction # ±1
  72         self.last_pose_index = None
  73
  74     def precalc_dist(self):
  75         dist = 0
  76         self[0].dist_from_seg_start = dist
  77         for i in range(1, len(self)):
  78             dist += self[i-1].distance_to(self[i])
  79             self[i].dist_from_seg_start = dist
  80
  81     def total_length(self):
  82         return self[-1].dist_from_seg_start
  83
  84     def pose_at(self, distance):
  85         """Return pose at specific distance from the start of the segment.
  86         """
  87         if distance > self.total_length():
  88             raise Exception("distance to high")
  89
  90         i = self.last_pose_index if self.last_pose_index is not None else 0
  91
  92         while distance < self[i].dist_from_seg_start:
  93             i -= 1
  94         while distance > self[i+1].dist_from_seg_start:
  95             i += 1
  96
  97         ratio = (distance - self[i].dist_from_seg_start) / \
  98             (self[i+1].dist_from_seg_start - self[i].dist_from_seg_start)
  99
 100         pose = self[i+1]
 101         pose.x = self[i].x + ratio * (self[i+1].x - self[i].x)
 102         pose.y = self[i].y + ratio * (self[i+1].y - self[i].y)
 103         pose.heading = self[i].heading + ratio * (self[i+1].heading - self[i].heading)
 104         return pose
 105
 106 class BicycleCar:
 107     ctc = 11.2531  # this is guess resulting to max_wa ~= 35.99998076387121
 108     w = 1.983
 109     ft = 1.680
 110     wb = 2.895
 111     df = 2.895 + 0.9  # this is guess
 112     dr = 4.918 - df  # length - df
 113
 114     max_sp = 2.5 / 3.6  # mps
 115     max_acc = 0.5  # mpss
 116
 117     mtr = ((ctc / 2)**2 - wb**2)**0.5 - ft/2
 118     max_wa = atan(wb / mtr)  # rad
 119     max_wa_rate = deg_to_rad(50)  # rad per second
 120
 121     def __init__(self, x=0, y=0, h=0, forward=True):
 122         self.x = x  # x, y is center of rear axle
 123         self.y = y
 124         self.h = h
 125         self.sp = 0  # speed
 126         self.wa = 0  # wheel angle
 127         self.dwa = 0  # desired wa
 128         self.acc = 0
 129         self.forward = forward     # gear: forward/backward
 130         self.time = 0
 131         self.traj = []
 132         self.traj.append(self.pose()) # Start trajectory with the initial pose
 133
 134     def __repr__(self):
 135         return f"BicycleCar{{x:{self.x:.3f} y:{self.y:.3f} h:{self.h:.3f}}}"
 136
 137     def pose(self):
 138         return [self.x, self.y, self.h, self.wa, self.sp, self.acc, self.forward]
 139
 140
 141     def bd(self, acc, t=None):
 142         """Return braking distance.
 143
 144         :param t: Time to stop (speed = 0).
 145
 146         See https://en.wikipedia.org/wiki/Braking_distance
 147         See https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction
 148         See https://en.wikipedia.org/wiki/Standard_gravity
 149
 150         But see https://www.nabla.cz/obsah/fyzika/mechanika/rovnomerne-zrychleny-primocary-pohyb.php
 151
 152         s = v_0 * t + 1/2 * a * t**2
 153         """
 154         if not t:
 155             t = abs(self.sp / acc)
 156         return abs(self.sp) * t + 1/2 * -1*abs(acc) * t**2
 157
 158     def set_wa_based_on_dwa(self, dt):
 159         if abs(self.dwa - self.wa) > self.max_wa_rate * dt:
 160             if self.dwa > self.wa:
 161                 self.wa += self.max_wa_rate * dt
 162             else:
 163                 self.wa -= self.max_wa_rate * dt
 164         else:
 165             self.wa = self.dwa
 166         if abs(self.wa) > self.max_wa:
 167             self.wa = self.max_wa * sgn(self.wa)
 168
 169     def next(self, dt):
 170         #self.wa = self.dwa
 171         self.set_wa_based_on_dwa(dt)
 172         self.sp += self.acc * dt
 173         if abs(self.sp) > self.max_sp:
 174             self.sp = self.max_sp * sgn(self.sp)
 175             self.acc = 0
 176         self.x += dt * self.sp * cos(self.h)
 177         self.y += dt * self.sp * sin(self.h)
 178         self.h += dt * self.sp / self.wb * tan(self.wa)
 179         while self.h > pi:
 180             self.h -= 2*pi
 181         while self.h < -pi:
 182             self.h += 2*pi
 183         self.time += dt
 184         self.traj.append(self.pose())
 185
 186
 187 if __name__ == "__main__":
 188     plan = None
 189     if len(sys.argv) == 2:
 190         PLAN_FILE = sys.argv[1]
 191     with open(f"{PLAN_FILE}", "r") as f:
 192         plan = json.load(f)
 193     plan["nnpath"] = list(plan["path"])
 194     #plan["path"] = normalize_path(plan["path"])
 195     path = list(plan["path"])
 196     #cusps = find_cusps_in(path)
 197     path_segments = find_segments_of(path)
 198     print(f"path segments ({len(path_segments)}):")
 199     for s in path_segments:
 200         print(f"- dir:{s.direction:+d} len:{s.total_length()} {s}")
 201     p = path_segments[0][0]
 202     c = BicycleCar(p.x, p.y, p.heading, path_segments[0].direction == +1)
 203     # ---
 204     # Interpolate over segments.
 205     for i in range(len(path_segments)):
 206         s = path_segments[i]
 207
 208         # wait 2 second, prepare wheel angle and gear
 209         c.sp = 0
 210         c.acc = 0
 211         c.dwa = s[1].turn_type
 212         c.forward = s.direction == +1
 213         for _ in range(int(2/DT)):
 214             c.next(DT)
 215
 216         dist = 0
 217         t0 = c.time
 218         while dist < s.total_length():
 219             p = s.pose_at(dist)
 220             dist_to_end = s.total_length() - dist
 221             c.acc = s.direction * c.max_acc
 222             if dist_to_end <= c.bd(c.max_acc):
 223                 c.acc *= -1
 224             c.dwa = p.turn_type
 225             sp = c.sp + DT/2 * c.acc # avg. speed in the sample period
 226             c.next(DT)
 227             dist += abs(sp) * DT
 228         print(f"seg{i}: t0:{t0:5.2f}s t_end:{c.time:5.2f}s")
 229         # Add time to change gear and steer to other side
 230         c.sp = 0
 231         c.acc = 0
 232         for _ in range(int(1/DT)): # wait 1 second
 233             c.next(DT)
 234
 235     # ---
 236     plan["traj"] = c.traj
 237     with open(f"{PLAN_FILE.split('.')[0]}.traj.json", "w") as f:
 238         json.dump(plan, f, indent="\t")
 239     with open(f"trajectory.h", "w") as f:
 240         f.write("#pragma once\n")
 241         f.write("#include <stdbool.h>\n")
 242         f.write("struct command { double wa; double sp; double acc; bool forward; };\n")
 243         f.write("extern struct command commands[];\n")
 244         f.write("extern const unsigned commands_count;\n")
 245     with open(f"traj-{PLAN_FILE.split('.')[0]}.cpp", "w") as f:
 246         f.write('#include "trajectory.h"\n')
 247         f.write(f"const unsigned commands_count = {len(c.traj)};\n")
 248         f.write("struct command commands[] = {\n")
 249         for p in c.traj:
 250             f.write(f"  {{{rad_to_deg(p[3])}, {p[4]}, {p[5]}, {'true' if p[6] else 'false'}}},\n")
 251         f.write("};\n")