waypt_times = []
waypt_times.append(t_start)
for seg in range(n_segs):
rospy.logdebug("\n\n>> seg_numer: {}".format(seg))
min_sync_time, phase_dur_jt, phase_jrk_jt = traj.segment_synchronization(
path[seg], path[seg + 1], estimated_vel[seg], estimated_vel[seg + 1],
abs_max_pos, abs_max_vel, abs_max_acc, abs_max_jrk)
waypt_times.append(waypt_times[-1] + min_sync_time)
while abs_t <= waypt_times[-1]:
for jt in range(n_jts):
p_start = path[seg][jt]
v_start = estimated_vel[seg][jt]
phases_dur = phase_dur_jt[jt]
phases_jrk = phase_jrk_jt[jt]
pos, vel, acc, jrk = traj.sample_segment(abs_t, waypt_times[-2],
p_start, v_start,
phases_jrk, phases_dur)
traj_pos[jt].append(pos)
traj_vel[jt].append(vel)
traj_acc[jt].append(acc)
traj_jrk[jt].append(jrk)
traj_time.append(abs_t)
abs_t = abs_t + 1 / frq
# plot pos, vel, acc, jrk. plot waypoints and estimated velocity as well to check if there is any difference
fig, axes = plt.subplots(4, sharex=True)
for jt in range(0, n_jts):
axes[0].plot(traj_time, traj_pos[jt])
axes[1].plot(traj_time, traj_vel[jt])
axes[2].plot(traj_time, traj_acc[jt])
axes[3].plot(traj_time, traj_jrk[jt])
#convert duration and jrk to 1d list
jrk_dur_arr = np.array(jrk_dur)
phases_durs = jrk_dur_arr[:, 1].tolist()
phases_jrks = jrk_dur_arr[:, 0].tolist()
# print "\n>>>> seg {}: tot_seg_time = {}, [ph_jrk, ph_dur]\n{}".format(seg, sum(T), jrk_dur_arr)
t_seg_durs.append(phases_durs)
j_seg_durs.append(phases_jrks)
t_jt_seg_dur.append(sum(phases_durs))
#T_seg: contains the start/end time of each segment
seg_times.append(sum(phases_durs) + seg_times[seg])
t_start = seg_times[seg]
t = t_start
while t < seg_times[seg + 1]:
ps, vl, ac, jk = traj.sample_segment(t, t_start, path[jt][seg],
estimated_vel[jt][seg],
phases_jrks, phases_durs)
pos.append(ps)
vel.append(vl)
acc.append(ac)
jrk.append(jk)
jt_times.append(t)
t = t + (1.0 / frq)
traj_pos.append(pos)
traj_vel.append(vel)
traj_acc.append(acc)
traj_jrk.append(jrk)
traj_time.append(jt_times)
jt_seg_times.append(seg_times)
frq = 125.0
t_start = 0.0
t = t_start
traj_pos = [[] for jt in range(n_jts)]
traj_vel = [[] for jt in range(n_jts)]
traj_acc = [[] for jt in range(n_jts)]
traj_jrk = [[] for jt in range(n_jts)]
traj_time = []
while t <= syn_t:
for jt in range(n_jts):
p_start = pos_start[jt]
v_start = vel_start[jt]
phases_dur = jt_dur[jt]
phases_jrk = jt_jrk[jt]
pos, vel, acc, jrk = traj.sample_segment(t, t_start, p_start, v_start,
phases_jrk, phases_dur)
traj_pos[jt].append(pos)
traj_vel[jt].append(vel)
traj_acc[jt].append(acc)
traj_jrk[jt].append(jrk)
traj_time.append(t)
t = t + 1 / frq
# plot pos, vel, acc, jrk. plot waypoints and estimated velocity as well to check if there is any difference
t_start_vec = [traj_time[0] for jt in range(n_jts)]
t_end_vec = [traj_time[-1] for jt in range(n_jts)]
fig, axes = plt.subplots(4, sharex=True)
for jt in range(0, n_jts):
axes[0].plot(traj_time, traj_pos[jt])
axes[1].plot(traj_time, traj_vel[jt])
axes[2].plot(traj_time, traj_acc[jt])