Retime a path subject to kinematic constraintsΒΆ

In this example, we will see how can we retime a generic spline-based path subject to kinematic constraints. This is very simple to do with toppra, as we shall see below. First import the library.

10 import toppra as ta
11 import toppra.constraint as constraint
12 import toppra.algorithm as algo
13 import numpy as np
14 import matplotlib.pyplot as plt
15 import time
16
17 ta.setup_logging("INFO")

We generate a path with some random waypoints.

22 def generate_new_problem(seed=9):
23     # Parameters
24     N_samples = 5
25     dof = 7
26     np.random.seed(seed)
27     way_pts = np.random.randn(N_samples, dof)
28     return (
29         np.linspace(0, 1, 5),
30         way_pts,
31         10 + np.random.rand(dof) * 20,
32         10 + np.random.rand(dof) * 2,
33     )
34 ss, way_pts, vlims, alims = generate_new_problem()

Define the geometric path and two constraints.

38 path = ta.SplineInterpolator(ss, way_pts)
39 pc_vel = constraint.JointVelocityConstraint(vlims)
40 pc_acc = constraint.JointAccelerationConstraint(alims)

We solve the parametrization problem using the ParametrizeConstAccel parametrizer. This parametrizer is the classical solution, guarantee constraint and boundary conditions satisfaction.

47 instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel")
48 jnt_traj = instance.compute_trajectory()

Out:

INFO [algorithm.py : 104] No gridpoint specified. Automatically choose a gridpoint with 290 points
INFO [reachability_algorithm.py : 65] Solver wrapper not supplied. Choose solver wrapper automatically!
INFO [reachability_algorithm.py : 75] Select solver seidel
INFO [algorithm.py : 191] Successfully parametrize path. Duration: 3.525, previously 1.000)
INFO [algorithm.py : 193] Finish parametrization in 0.013 secs

The output trajectory is an instance of toppra.interpolator.AbstractGeometricPath.

53 ts_sample = np.linspace(0, jnt_traj.duration, 100)
54 qs_sample = jnt_traj(ts_sample)
55 qds_sample = jnt_traj(ts_sample, 1)
56 qdds_sample = jnt_traj(ts_sample, 2)
57 fig, axs = plt.subplots(3, 1, sharex=True)
58 for i in range(path.dof):
59     # plot the i-th joint trajectory
60     axs[0].plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i))
61     axs[1].plot(ts_sample, qds_sample[:, i], c="C{:d}".format(i))
62     axs[2].plot(ts_sample, qdds_sample[:, i], c="C{:d}".format(i))
63 axs[2].set_xlabel("Time (s)")
64 axs[0].set_ylabel("Position (rad)")
65 axs[1].set_ylabel("Velocity (rad/s)")
66 axs[2].set_ylabel("Acceleration (rad/s2)")
67 plt.show()
plot kinematics

Optionally, we can inspect the output.

72 instance.compute_feasible_sets()
73 instance.inspect()
Path-position path-velocity plot

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

Gallery generated by Sphinx-Gallery