Retime a straight pathΒΆ

 5 import toppra as ta
 6 import toppra.constraint as constraint
 7 import toppra.algorithm as algo
 8 import numpy as np
 9 import matplotlib.pyplot as plt
10 import time
11
12 time.sleep(0.1)
15 way_pts, vel_limits, accel_limits = np.array([[0, 0, 1], [0.2, 0.3, 0]]), np.array([0.1, 0.2, 0.3]), np.r_[1.0,2,3]
16 path_scalars = np.linspace(0, 1, len(way_pts))
17 path = ta.SplineInterpolator(path_scalars, way_pts)
18
19 ss = np.linspace(0, 1, 100)
20 qs = path(np.linspace(0, 1, 100))
21 for i in range(way_pts.shape[1]):
22     plt.plot(ss, qs[:, i])
23 plt.show()
plot straight line

Create velocity bounds, then velocity constraint object

27 vlim = np.vstack((-vel_limits, vel_limits)).T
28 # Create acceleration bounds, then acceleration constraint object
29 alim = np.vstack((-accel_limits, accel_limits)).T
30 pc_vel = constraint.JointVelocityConstraint(vlim)
31 pc_acc = constraint.JointAccelerationConstraint(
32     alim, discretization_scheme=constraint.DiscretizationType.Interpolation)
33
34 # Setup a parametrization instance. The keyword arguments are
35 # optional.
36 instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel')
37 jnt_traj = instance.compute_trajectory(0, 0)
41 ts_sample = np.linspace(0, jnt_traj.get_duration(), 100)
42 qs_sample = jnt_traj.eval(ts_sample)  # sampled joint positions
43 qds_sample = jnt_traj.evald(ts_sample)  # sampled joint velocities
44 qdds_sample = jnt_traj.evaldd(ts_sample)  # sampled joint accelerations
45
46 for i in range(jnt_traj.dof):
47     # plot the i-th joint trajectory
48     plt.plot(ts_sample, qds_sample[:, i], c="C{:d}".format(i))
49     # plot the i-th joint waypoints
50     # plt.plot(data['t_waypts'], way_pts[:, i], 'x', c="C{:d}".format(i))
51 plt.xlabel("Time (s)")
52 plt.ylabel("Joint velocity (rad/s^2)")
53 plt.show()
plot straight line

Out:

/home/circleci/repo/toppra/utils.py:23: DeprecationWarning: Call to deprecated function get_duration in module toppra.interpolator.
  warnings.warn(

Total running time of the script: ( 0 minutes 0.399 seconds)

Gallery generated by Sphinx-Gallery