robot.SetDOFVelocityLimits(100 * vel_lim) ################# Initialize a trajectory ######################## P0 = poly1d([1, -1, 1, 0.5]) P1 = poly1d([1, -3, 1]) pwp_traj = Trajectory.PieceWisePolyTrajectory([[P0, P1]], [1]) T = 1 n_discr = 500. t_step = T / n_discr a = time.clock() traj = pwp_traj.GetSampleTraj(T, t_step) # Compute the inverse dynamics of the original trajectory tau = Trajectory.ComputeTorques(robot, traj, grav) #mpl.axes.set_default_color_cycle(['r','b']) figure(3) clf() Trajectory.PlotTorques(traj.t_vect, tau, tau_min, tau_max) ################# Compute a dynamically-feasible timing ######################## # Torques limits tau_min = [-30, -30] tau_max = [10, 10] # Some tuning parameters tunings = {'': 0} tunings['i_threshold'] = 10 # threshold in the tangent points search tunings['slope_threshold'] = 1 # threshold in the tangent points search
def Smooth(robot, traj_orig, t1, t2, tunings, grav, tau_min, tau_max, qd_max): global coco deb = time.time() i1 = 0 t_vect = traj_orig.t_vect n_steps = traj_orig.n_steps t_step = traj_orig.t_step q_vect = traj_orig.q_vect qd_vect = traj_orig.qd_vect for i in range(n_steps): if t1 < t_vect[i]: i1 = i break for i in range(i1, n_steps): if t2 < t_vect[i]: i2 = i break print 'Initial time: ' + str(t1) print 'Final time: ' + str(t2) print 'Duration: ' + str(t2 - t1) c_sdot = 1 q_list = [q_vect[:, i1], q_vect[:, i2]] qd_list = [qd_vect[:, i1] / c_sdot, qd_vect[:, i2] / c_sdot] T_list = [t2 - t1] pwp_traj_shortcut = Trajectory.Interpolate(q_list, qd_list, T_list) sample_traj_shortcut = pwp_traj_shortcut.GetSampleTraj( pwp_traj_shortcut.duration, tunings['t_step_sample']) if Trajectory.CheckCollisionTraj(robot, sample_traj_shortcut)[0]: print 'Shortcut collides, returning' print 'Computation time was: ' + str(time.time() - deb) return ['collision', traj_orig] pb = MinimumTime.RobotMinimumTime(robot, sample_traj_shortcut, tunings, grav, tau_min, tau_max, qd_max, c_sdot, c_sdot) if not pb.possible: print 'Shortcut is dynamically imposible, returning' print 'Computation time was: ' + str(time.time() - deb) return ['impossible', traj_orig] s_res = pb.s_res sdot_res = pb.sdot_res t_shortcut = len(s_res) * tunings['t_step_integrate'] if t_shortcut >= t2 - t1: print 'Shortcut time (' + str( t_shortcut) + ') is longer than original, returning' print 'Computation time was: ' + str(time.time() - deb) return ['too_long', traj_orig] if t2 - t1 - t_shortcut < tunings['threshold_waive']: print 'Gain is not significant (' + str(t2 - t1 - t_shortcut) + '), returning' print 'Computation time was: ' + str(time.time() - deb) return ['not_signif', traj_orig] print 'Great! Shortcut time is: ' + str(t_shortcut) + ' (' + str( int(t_shortcut / (t2 - t1) * 100)) + '% of original, saves ' + str(t2 - t1 - t_shortcut) + ' seconds)' print 'Computation time was: ' + str(time.time() - deb) + ' seconds' undersample_coef = t_step / tunings['t_step_integrate'] undersample_coef = int(round(undersample_coef)) s_res_u = s_res[range(1, len(s_res), undersample_coef)] sdot_res_u = sdot_res[range(1, len(s_res), undersample_coef)] if len(s_res_u) < 2: return ['impossible', traj_orig] resampled_traj_u = pwp_traj_shortcut.ResampleTraj(s_res_u, sdot_res_u, t_step) tau_u = Trajectory.ComputeTorques(robot, resampled_traj_u, grav) traj2 = Trajectory.Insert(traj_orig, i1, i2, resampled_traj_u) return ['great', traj2]