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