pieces_list.append(
poly1d([(a[i_dof] - a_prev[i_dof]) / T, a_prev[i_dof]]))
pwp_traj_linear = Trajectory.PieceWisePolyTrajectory([pieces_list], [T])
traj = pwp_traj_linear.GetSampleTraj(T, t_step)
pb = MinimumTime.RobotMinimumTime(robot, traj, tunings, grav, tau_min,
tau_max, qd_max, 1e-2, 1e-2)
pb_list.append(pb)
s_res = pb.s_res
sdot_res = pb.sdot_res
undersample_coef = int(round(t_step / tunings['t_step_integrate']))
s_res_u = s_res[range(1, len(s_res), undersample_coef)]
sdot_res_u = sdot_res[range(1, len(s_res), undersample_coef)]
traj2 = pwp_traj_linear.ResampleTraj(s_res_u, sdot_res_u, t_step)
traj_list.append(traj2)
traj2 = Trajectory.Concat(traj_list)
# Inverse dynamics of the retimed trajectory and plot
tau2 = Trajectory.ComputeTorques(robot, traj2, grav)
mpl.axes.set_default_color_cycle(['r', 'b', 'g', 'm'])
figure(7)
Trajectory.PlotTorques(traj2.t_vect, tau2, tau_min, tau_max)
figure(8)
clf()
plot(transpose(traj2.qd_vect))
################# Run the shortcutting algorithm ########################
# Tuning parameters for the shortcutting algorithm
tunings['t_step_sample'] = 0.001 # time step to sample the shortcut
tunings[
