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]
