diff --git a/PathTracking/PurePursuit.py b/PathTracking/PurePursuit.py new file mode 100644 index 00000000..47a1e01a --- /dev/null +++ b/PathTracking/PurePursuit.py @@ -0,0 +1,49 @@ +#! /usr/bin/python +# -*- coding: utf-8 -*- +u""" +author: Atsushi Sakai +""" +import numpy as np +import math +import matplotlib.pyplot as plt +import unicycle_model + + +def main(): + cx = np.arange(0, 50, 0.1) + cy = [math.sin(ix / 5.0) * 5.0 for ix in cx] + + T = 10.0 + dt = 0.1 + time = 0.0 + + state = unicycle_model.State() + + x = [state.x] + y = [state.y] + yaw = [state.yaw] + v = [state.v] + + while T >= time: + ai = 1.0 + di = 0.01 + state = unicycle_model.update(state, ai, di) + + x.append(state.x) + y.append(state.y) + yaw.append(state.yaw) + v.append(state.v) + + time = time + dt + + plt.plot(cx, cy, ".r") + plt.plot(x, y, "-b") + plt.axis("equal") + plt.grid(True) + plt.show() + + pass + + +if __name__ == '__main__': + main() diff --git a/PathTracking/unicycle_model.py b/PathTracking/unicycle_model.py new file mode 100644 index 00000000..27925e3b --- /dev/null +++ b/PathTracking/unicycle_model.py @@ -0,0 +1,68 @@ +#! /usr/bin/python +# -*- coding: utf-8 -*- +""" + + +author Atsushi Sakai +""" + +import math + +dt = 0.1 # [s] +L = 2.9 # [m] + + +class State: + + def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): + self.x = x + self.y = y + self.yaw = yaw + self.v = v + + +def update(state, a, delta): + + state.x = state.x + state.v * math.cos(state.yaw) * dt + state.y = state.y + state.v * math.sin(state.yaw) * dt + state.yaw = state.yaw + state.v / L * math.tan(delta) * dt + state.v = state.v + a * dt + + return state + + +if __name__ == '__main__': + print("start unicycle simulation") + import matplotlib.pyplot as plt + + T = 100 + a = [1.0] * T + delta = [math.radians(1.0)] * T + # print(delta) + # print(a, delta) + + state = State() + + x = [] + y = [] + yaw = [] + v = [] + + for (ai, di) in zip(a, delta): + state = update(state, ai, di) + + x.append(state.x) + y.append(state.y) + yaw.append(state.yaw) + v.append(state.v) + + flg, ax = plt.subplots(1) + plt.plot(x, y) + plt.axis("equal") + plt.grid(True) + + flg, ax = plt.subplots(1) + plt.plot(v) + plt.grid(True) + + plt.show()