def action(self):
station = self.wagon.get_now_station_index()
max_v = self.line.speed_lim[station]
p0 = self.wagon.p_front
v0 = self.wagon.dp
a0 = self.wagon.ddp
self.wagon.trajectory = trajectory.SetVelocity(max_jerk, accel_max,
max_v, p0, v0, a0,
self.t0)
def plot_rdv():
import trajectory
t0 = 0
dt = 0.1
v_tr = 15
p0_tr = 0
f_tr = trajectory.Trajectory(p0=p0_tr, v0=v_tr, a0=0, t0=t0)
v_rdv = v_tr - delta_rv_speed
accel_dist, accel_time = trajectory.get_accel_dist_time(
max_jerk, accel_max, v_rdv)
p0_rdv = p0_tr + v_tr * accel_time + 10
meet_point = p0_rdv + accel_dist + v_rdv * time_at_rv_speed
t_rdv = f_tr.real_roots(p=meet_point)[0]
leave_time = t_rdv - time_at_rv_speed - accel_time
f_rdv = trajectory.Trajectory(p0=p0_rdv, v0=0, a0=0, t0=t0).extend(
trajectory.SetVelocity(max_jerk,
accel_max,
v_rdv,
p0=p0_rdv,
t0=leave_time))
t_end = f_rdv.parts[-1][0]
time = np.arange(t0, t_end + 5, dt)
p_tr = [f_tr(t) for t in time]
v_tr = [f_tr(t, 1) for t in time]
a_tr = [f_tr(t, 2) for t in time]
j_tr = [f_tr(t, 3) for t in time]
p_rdv = [f_rdv(t) for t in time]
v_rdv = [f_rdv(t, 1) for t in time]
a_rdv = [f_rdv(t, 2) for t in time]
j_rdv = [f_rdv(t, 3) for t in time]
plt.subplot(4, 1, 1)
plt.plot(time, p_tr, time, p_rdv)
plt.ylabel('position (m)')
plt.subplot(4, 1, 2)
plt.plot(time, v_tr, time, v_rdv)
plt.ylabel('vitesse (m/s)')
plt.subplot(4, 1, 3)
plt.plot(time, a_tr, time, a_rdv)
plt.ylabel('acceleration (m/s2)')
plt.subplot(4, 1, 4)
plt.plot(time, j_tr, time, j_rdv)
plt.ylabel('jerk (m/s3)')
plt.xlabel('temps (s)')
plt.show()
def plot_SetVelocity():
# Avec saturation
t0 = 0
p0 = 0
v0 = 0
a0 = 0
v = 15
f = trajectory.Trajectory(p0=p0, v0=v0, a0=a0, t0=t0 - 1).extend(
trajectory.SetVelocity(max_jerk,
accel_max,
v,
p0=p0,
v0=v0,
a0=a0,
t0=t0))
plot_pvaj(
f,
"Accélération jusqu'à %02.0f m/s avec saturation de l'accélération" %
v)
# Sans saturation
v = 2
f = trajectory.Trajectory(p0=p0, v0=v0, a0=a0, t0=t0 - 1).extend(
trajectory.SetVelocity(max_jerk,
accel_max,
v,
p0=p0,
v0=v0,
a0=a0,
t0=t0))
plot_pvaj(
f,
"Accélération jusqu'à %01.0f m/s sans saturation de l'accélération" %
v)
def action(self):
station = self.wagon.get_now_station_index()
if station is None:
raise Exception('Wagon seems not to be at a station.')
behind = self.wagon.connected[0].get_behind()
v_lim = self.line.speed_lim[station]
v_rdv = v_lim - delta_rv_speed
accel_dist, accel_time = trajectory.get_accel_dist_time(
max_jerk, accel_max, v_rdv)
meet_point = self.wagon.p_back + accel_dist + v_rdv * time_at_rv_speed
t_rdv = behind.trajectory.real_roots(p=meet_point)[0]
if abs(behind.trajectory(t_rdv, 1) - v_lim) > wrong_velocity_threshold:
raise Exception('Wrong velocity for rendez vous. Station: %i' %
self.station)
leave_time = t_rdv - time_at_rv_speed - accel_time
self.wagon.trajectory = trajectory.Trajectory(
p0=self.wagon.p_front, v0=0, a0=0, t0=self.t0).extend(
trajectory.SetVelocity(max_jerk,
accel_max,
v_rdv,
p0=self.wagon.p_front,
t0=leave_time))
def plot_breakingz():
import trajectory
t0 = 23
t_end = 27
n = 5
dt = 0.01
v_tr = 20
p0_tr = 0
f_tr = trajectory.Trajectory(p0=p0_tr, v0=v_tr, a0=0, t0=0)
v_rdv = v_tr - delta_rv_speed
accel_dist, accel_time = trajectory.get_accel_dist_time(
max_jerk, accel_max, v_rdv)
p0_rdv = p0_tr + v_tr * accel_time + 10
meet_point = p0_rdv + accel_dist + v_rdv * time_at_rv_speed
t_rdv = f_tr.real_roots(p=meet_point)[0]
leave_time = t_rdv - time_at_rv_speed - accel_time
f_rdv = trajectory.Trajectory(p0=p0_rdv, v0=0, a0=0, t0=0).extend(
trajectory.SetVelocity(max_jerk,
accel_max,
v_rdv,
p0=p0_rdv,
t0=leave_time))
start_time = np.linspace(t0, t_end, n)
ps = []
vs = []
ass = []
js = []
ts = []
for st in start_time:
p0 = f_rdv(st)
v0 = f_rdv(st, 1)
a0 = f_rdv(st, 2)
print('p0: %f06.1, v0: %f02.3, a0: %f01.4' % (p0, v0, a0))
f = trajectory.FastestStop(max_jerk,
deccel_max,
p0=p0,
v0=v0,
a0=a0,
t0=st)
time = np.arange(st, f.get_end_time() + 5, dt)
ts.append(time)
ps.append([f(t) for t in time])
vs.append([f(t, 1) for t in time])
ass.append([f(t, 2) for t in time])
js.append([f(t, 3) for t in time])
plt.figure(1)
plt.subplot(4, 1, 1)
for i in range(len(ts)):
plt.plot(ts[i], ps[i])
plt.ylabel('position (m)')
plt.subplot(4, 1, 2)
for i in range(len(ts)):
plt.plot(ts[i], vs[i])
plt.ylabel('vitesse (m/s)')
plt.subplot(4, 1, 3)
for i in range(len(ts)):
plt.plot(ts[i], ass[i])
plt.ylabel('acceleration (m/s2)')
plt.subplot(4, 1, 4)
for i in range(len(ts)):
plt.plot(ts[i], js[i])
plt.ylabel('jerk (m/s3)')
plt.xlabel('temps (s)')
plt.show()
def try_JerkAccelToVelocity():
import trajectory
print(
trajectory.SetVelocity(max_jerk, accel_max, 25, v0=20).get_end_time())
def plot_breaking_point():
import trajectory
t0 = 0
dt = 0.01
v_tr0 = 16
p0_tr = 0
f_tr = trajectory.Trajectory(p0=p0_tr, v0=v_tr0, a0=0, t0=t0)
v_rdv = v_tr0 - delta_rv_speed
accel_dist, accel_time = trajectory.get_accel_dist_time(
max_jerk, accel_max, v_rdv)
p0_rdv = p0_tr + v_tr0 * accel_time + 10
meet_point = p0_rdv + accel_dist + v_rdv * time_at_rv_speed
t_rdv = f_tr.real_roots(p=meet_point)[0]
leave_time = t_rdv - time_at_rv_speed - accel_time
f_rdv = trajectory.Trajectory(p0=p0_rdv, v0=0, a0=0, t0=t0).extend(
trajectory.SetVelocity(max_jerk,
accel_max,
v_rdv,
p0=p0_rdv,
t0=leave_time))
t_end = f_rdv.parts[-1][0]
time = np.arange(t0, t_end + 5, dt)
p_tr = [f_tr(t) for t in time]
v_tr = [f_tr(t, 1) for t in time]
a_tr = [f_tr(t, 2) for t in time]
j_tr = [f_tr(t, 3) for t in time]
p_rdv = [f_rdv(t) for t in time]
v_rdv = [f_rdv(t, 1) for t in time]
a_rdv = [f_rdv(t, 2) for t in time]
j_rdv = [f_rdv(t, 3) for t in time]
brkp_tr = [
trajectory.get_breaking_dist_time(
max_jerk, deccel_max, v0=f_tr(t, 1), a0=f_tr(t, 2))[0] + f_tr(t)
for t in time
]
# print('transit done')
# input()
brkp_rdv = [
trajectory.get_breaking_dist_time(
max_jerk, deccel_nom, v0=f_rdv(t, 1), a0=f_rdv(t, 2))[0] + f_rdv(t)
for t in time
]
# print('rdv done')
# input()
fig = plt.figure(1)
st = fig.suptitle('Cinématique de rendez-vous à %d m/s et point d\'arrêt' %
v_tr0,
fontsize="x-large")
plt.subplot(5, 1, 1)
tr, rv = plt.plot(time, p_tr, time, p_rdv)
plt.ylabel('Position (m)')
plt.subplot(5, 1, 2)
plt.plot(time, v_tr, time, v_rdv)
plt.ylabel('Vitesse (m/s)')
plt.subplot(5, 1, 3)
plt.plot(time, a_tr, time, a_rdv)
plt.ylabel(r'$Accélération (m/s^2)$')
plt.subplot(5, 1, 4)
plt.plot(time, j_tr, time, j_rdv)
plt.ylabel(r'$À-coup (m/s^3)$')
plt.subplot(5, 1, 5)
plt.plot(time, brkp_tr, time, brkp_rdv) #
plt.ylabel('Point d\'arrêt (m)')
plt.xlabel('Temps (s)')
fig.legend((tr, rv), ('Transit', 'Accel'), 'right') # , 'upper left'
st.set_y(0.95)
fig.subplots_adjust(top=0.85)
plt.show()