[doc]Improve kinematic example
-"""
-Retime a path subject to kinematic constraints
-==============================================
+"""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.
+
"""
import toppra as ta
@@ -12,6 +16,8 @@ import time
ta.setup_logging("INFO")
+################################################################################
+# We generate a path with some random waypoints.
def generate_new_problem(seed=9):
# Parameters
@@ -25,27 +31,42 @@ def generate_new_problem(seed=9):
10 + np.random.rand(dof) * 20,
10 + np.random.rand(dof) * 2,
)
-
-
ss, way_pts, vlims, alims = generate_new_problem()
+
+################################################################################
+# Define the geometric path and two constraints.
path = ta.SplineInterpolator(ss, way_pts)
pc_vel = constraint.JointVelocityConstraint(vlims)
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.
instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel")
jnt_traj = instance.compute_trajectory()
-
################################################################################
+# The output trajectory is an instance of
+# :class:`toppra.interpolator.AbstractGeometricPath`.
ts_sample = np.linspace(0, jnt_traj.duration, 100)
qs_sample = jnt_traj(ts_sample)
+qds_sample = jnt_traj(ts_sample, 1)
+qdds_sample = jnt_traj(ts_sample, 2)
+fig, axs = plt.subplots(3, 1, sharex=True)
for i in range(path.dof):
# plot the i-th joint trajectory
- plt.plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i))
-plt.xlabel("Time (s)")
-plt.ylabel("Joint position (rad/s^2)")
+ axs[0].plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i))
+ axs[1].plot(ts_sample, qds_sample[:, i], c="C{:d}".format(i))
+ axs[2].plot(ts_sample, qdds_sample[:, i], c="C{:d}".format(i))
+axs[2].set_xlabel("Time (s)")
+axs[0].set_ylabel("Position (rad)")
+axs[1].set_ylabel("Velocity (rad/s)")
+axs[2].set_ylabel("Acceleration (rad/s2)")
plt.show()
-
################################################################################
+# Optionally, we can inspect the output.
instance.compute_feasible_sets()
instance.inspect()
