else:
qd0[i] = -v
qd1[i] = -v
q_list = [q_init, q_final]
qd_list = [qd0, qd1]
T_list = [1.4]
pwp_traj = Trajectory.Interpolate(q_list, qd_list, T_list)
T = sum(T_list)
n_discr = 500
t_step = T / n_discr
traj_1 = pwp_traj.GetSampleTraj(T, t_step)
traj_2 = Trajectory.DynamicShift(robot, traj_1, 7)
spline_traj = Trajectory.SplineInterpolateTrajectory(traj_2.t_vect,
traj_2.q_vect,
k=3,
s=0)
traj = spline_traj.GetSampleTraj(T, t_step)
Trajectory.Execute(robot, traj, 0.01)
xminf = -0.085
xmaxf = 0.15
yminf = -0.16
ymaxf = 0.16
zz = 0.001
p1 = array([xminf, yminf, zz])
p2 = array([xmaxf, yminf, zz])
