deb = time.time()
pb = MinimumTimeZMP.RobotMinimumTime(robot, traj, bounds, tunings, params)
figure(1)
pb.plot_limiting_curves()
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 = spline_traj.ResampleTraj(s_res_u, sdot_res_u, t_step)
print time.time() - deb
zmp = Trajectory.ComputeZMP(traj, params)
zmp2 = Trajectory.ComputeZMP(traj2, params)
#zmp3=Trajectory.ComputeZMP(traj3,params)
com = Trajectory.ComputeCOM(traj, params)
figure(2)
clf()
plot([xmin, xmin, xmax, xmax, xmin], [ymin, ymax, ymax, ymin, ymin],
'k--',
linewidth=2)
plot([xminf, xminf, xmaxf, xmaxf, xminf], [yminf, ymaxf, ymaxf, yminf, yminf],
'k',
linewidth=2)
plot(zmp[0, 0], zmp[1, 0], 'rs', markersize=7)
plot(zmp[0, -1], zmp[1, -1], 'ro', markersize=7)
tunings['sdot_init'] = 1
tunings['sdot_final'] = 1
tunings['threshold_final'] = 1e-2
tunings['threshold_waive'] = 1e-2
pb = MinimumTimeZMP.RobotMinimumTime(robot, traj, bounds, tunings, params)
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.ResampleTraj(s_res_u, sdot_res_u, t_step)
figure(2)
clf()
[xzmp, yzmp] = Trajectory.ComputeZMP(traj, params)
[xzmp2, yzmp2] = Trajectory.ComputeZMP(traj2, params)
[xcom, ycom] = Trajectory.ComputeCOM(traj, params)
plot([xmin, xmin, xmax, xmax, xmin], [ymin, ymax, ymax, ymin, ymin],
'k--',
linewidth=2)
plot(xzmp, yzmp, 'r')
plot(xzmp2, yzmp2, 'b')
plot(xcom, ycom, 'k')
axis('equal')
grid('on')
figure(3)
pb.plot_limiting_curves()
###### Test numerical differentiations
