Trajectory planning diagnostics#
The motion-planning tutorial scripts provide small, reproducible examples for
the trapezoidal and Double-S time laws added to EmbodiChain. They complement
the Motion Generator tutorial: the diagnostic scripts exercise the planner
directly, while motion_generator.py shows planner integration with a robot.
Scalar profile#
trapezoidal_profile.py does not start a simulator. It plots position,
velocity, acceleration, and sampled jerk for one scalar move:
1# ----------------------------------------------------------------------------
2# Copyright (c) 2021-2026 DexForce Technology Co., Ltd.
3#
4# Licensed under the Apache License, Version 2.0 (the "License");
5# you may not use this file except in compliance with the License.
6# You may obtain a copy of the License at
7#
8# http://www.apache.org/licenses/LICENSE-2.0
9#
10# Unless required by applicable law or agreed to in writing, software
11# distributed under the License is distributed on an "AS IS" BASIS,
12# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13# See the License for the specific language governing permissions and
14# limitations under the License.
15# ----------------------------------------------------------------------------
16
17"""Plot a minimal scalar trapezoidal or Double-S trajectory example."""
18
19from __future__ import annotations
20
21import argparse
22import os
23
24os.environ.setdefault("MPLCONFIGDIR", "/tmp/embodichain-matplotlib")
25
26import matplotlib.pyplot as plt
27import torch
28
29from embodichain.lab.sim.motion.planners.trapezoidal_planner import (
30 TrapezoidalPlanOptions,
31 _plan_linear_profiles,
32)
33
34
35def configure_plot_fonts() -> None:
36 """Apply lightweight readable font defaults for this example."""
37 plt.rcParams.update(
38 {
39 "font.sans-serif": ["Noto Sans CJK SC", "DejaVu Sans", "sans-serif"],
40 "font.size": 11.0,
41 "axes.titlesize": 13.0,
42 "legend.fontsize": 9.0,
43 "axes.unicode_minus": False,
44 }
45 )
46
47
48def positive_float(value: str) -> float:
49 """Parse a finite positive floating-point argument."""
50 parsed = float(value)
51 if not torch.isfinite(torch.tensor(parsed)) or parsed <= 0.0:
52 raise argparse.ArgumentTypeError("value must be finite and positive")
53 return parsed
54
55
56def parse_args() -> argparse.Namespace:
57 """Parse example options."""
58 parser = argparse.ArgumentParser(description=__doc__)
59 parser.add_argument(
60 "--profile",
61 choices=("velocity_trapezoidal", "acceleration_trapezoidal"),
62 default="acceleration_trapezoidal",
63 )
64 parser.add_argument("--distance", type=positive_float, default=0.1)
65 parser.add_argument("--velocity", type=positive_float, default=0.15)
66 parser.add_argument("--acceleration", type=positive_float, default=0.3)
67 parser.add_argument("--jerk", type=positive_float, default=1.0)
68 parser.add_argument("--samples", type=int, default=501)
69 parser.add_argument(
70 "--show-plot",
71 action=argparse.BooleanOptionalAction,
72 default=True,
73 help="Display the diagnostic figure (default: enabled).",
74 )
75 return parser.parse_args()
76
77
78def main() -> None:
79 """Plan the scalar move and show its derivatives."""
80 args = parse_args()
81 configure_plot_fonts()
82 if args.samples < 3:
83 raise SystemExit("--samples must be at least 3")
84 planner_profile = (
85 "trapezoidal" if args.profile == "velocity_trapezoidal" else "double_s"
86 )
87 waypoints = torch.tensor([[[0.0], [args.distance]]], dtype=torch.float64)
88 result = _plan_linear_profiles(
89 waypoints,
90 TrapezoidalPlanOptions(
91 profile=planner_profile,
92 constraints={
93 "velocity": args.velocity,
94 "acceleration": args.acceleration,
95 "jerk": args.jerk,
96 },
97 sample_interval=args.samples,
98 backend="torch",
99 ),
100 )
101 time = result.dt[0].cumsum(dim=0)
102 position = result.positions[0, :, 0]
103 velocity = result.velocities[0, :, 0]
104 acceleration = result.accelerations[0, :, 0]
105 jerk = torch.gradient(acceleration, spacing=(time,), edge_order=2)[0]
106
107 print(
108 f"[INFO] profile={args.profile}, duration={result.duration.item():.12f} s, "
109 f"max_velocity={velocity.abs().max().item():.6f}, "
110 f"max_acceleration={acceleration.abs().max().item():.6f}, "
111 f"max_sampled_jerk={jerk.abs().max().item():.6f}"
112 )
113
114 figure, axes = plt.subplots(4, 1, figsize=(11, 10), sharex=True)
115 for axis, values, title, ylabel in (
116 (axes[0], position, "Position", "q"),
117 (axes[1], velocity, "Velocity", "dq/dt"),
118 (axes[2], acceleration, "Acceleration", "d²q/dt²"),
119 (axes[3], jerk, "Jerk", "d³q/dt³"),
120 ):
121 axis.plot(time.numpy(), values.numpy(), linewidth=2.0)
122 axis.set_title(title)
123 axis.set_ylabel(ylabel)
124 axis.grid(True, alpha=0.25)
125 axes[-1].set_xlabel("time [s]")
126 figure.suptitle(args.profile.replace("_", " ").title())
127 figure.tight_layout()
128 if args.show_plot:
129 plt.show()
130 plt.close(figure)
131
132
133if __name__ == "__main__":
134 main()
Run it from the repository root (--no-show-plot is useful on headless
machines):
python scripts/tutorials/sim/planner/trapezoidal_profile.py \
--profile acceleration_trapezoidal \
--distance 0.1 \
--samples 501 \
--no-show-plot
The accepted profile names are velocity_trapezoidal and
acceleration_trapezoidal. The latter is the jerk-limited Double-S profile.
Planner and Cartesian path#
trapezoidal_planner.py runs the same time laws through
MotionGenerator. Its --path option selects joint, cartesian,
or both; --profile selects a profile (or both). It uses the
standard environment launcher options, including --headless,
--num-envs, and --device.
python scripts/tutorials/sim/planner/trapezoidal_planner.py \
--path both \
--profile acceleration_trapezoidal \
--headless \
--no-show-plot
Use --plot-output outputs/trajectory.png to save the diagnostic figure.
The script’s complete option list is kept in the executable source:
def parse_args() -> argparse.Namespace:
"""Parse tutorial arguments."""
parser = argparse.ArgumentParser(description=__doc__, conflict_handler="resolve")
add_sim_args_to_parser(parser)
parser.add_argument(
"--profile",
choices=(*PROFILE_SPECS, "both"),
default="acceleration_trapezoidal",
help=(
"Diagnostic to run: trapezoidal velocity, jerk-limited "
"trapezoidal acceleration, or both."
),
)
parser.add_argument(
"--samples",
type=sample_count,
default=DEFAULT_SAMPLES,
help="Number of output trajectory samples.",
)
parser.add_argument(
"--path",
choices=("joint", "cartesian", "both"),
default="cartesian",
help="Plan a synchronized multi-joint path, a straight EEF path, or both.",
)
parser.add_argument(
"--cartesian-distance",
type=positive_float,
default=DEFAULT_CARTESIAN_DISTANCE,
help="Length in metres of the diagonal straight EEF demo path.",
)
parser.add_argument(
"--cartesian-path",
choices=("bezier", "line"),
default="line",
help="Cartesian geometric path (default: line).",
)
parser.add_argument(
"--cartesian-step",
type=positive_float,
default=DEFAULT_CARTESIAN_STEP,
help="Cartesian interpolation spacing in metres before IK.",
)
parser.add_argument(
"--cartesian-velocity",
type=positive_float,
default=DEFAULT_CARTESIAN_VELOCITY,
help="Maximum straight-line EEF speed in m/s.",
)
parser.add_argument(
"--cartesian-acceleration",
type=positive_float,
default=DEFAULT_CARTESIAN_ACCELERATION,
help="Maximum straight-line EEF acceleration in m/s².",
)
parser.add_argument(
"--cartesian-jerk",
type=positive_float,
default=DEFAULT_CARTESIAN_JERK,
help="Maximum straight-line EEF jerk in m/s³.",
)
parser.add_argument(
"--backend",
choices=("auto", "torch", "warp"),
default="auto",
help="Backend used to compose sampled joint states.",
)
parser.add_argument(
"--blend-tolerance",
type=nonnegative_float,
default=0.0,
help="Quintic blend tolerance for the multi-waypoint joint path.",
)
parser.add_argument(
"--minimum-duration",
type=positive_float,
default=None,
help="Optional minimum duration in seconds for every planned path.",
)
parser.add_argument(
"--replay-speed",
type=positive_float,
default=DEFAULT_REPLAY_SPEED,
help="Trajectory playback speed multiplier in the simulation window.",
)
parser.add_argument(
"--plot-env",
type=int,
default=0,
help="Batch environment index shown in the diagnostic plot.",
)
parser.add_argument(
"--plot-output",
type=Path,
default=None,
help="Optional PNG path. By default the figure is not saved.",
)
parser.add_argument(
"--show-plot",
action=argparse.BooleanOptionalAction,
default=True,
help="Display the plot interactively (default: enabled).",
)
return parser.parse_args()
Regression checks and plots#
The lightweight checks do not create a simulator. Run one scenario at a time;
valid choices are bezier, trapezoidal, double-s, blend,
minimum-duration, batch, se3, and backend:
python scripts/tutorials/sim/planner/trajectory_pr_checks.py backend
trajectory_pr_plots.py writes a PNG for one of bezier, trapezoidal,
double-s, blend, minimum-duration, or se3. Add --show only
when a graphical display is available:
python scripts/tutorials/sim/planner/trajectory_pr_plots.py \
double-s \
--output outputs/trajectory_plots/double-s.png
SCENARIOS: dict[str, Callable[[], None]] = {
"bezier": run_bezier,
"trapezoidal": run_trapezoidal,
"double-s": run_double_s,
"blend": run_blend,
"minimum-duration": run_minimum_duration,
"batch": run_batch,
"se3": run_se3,
"backend": run_backend,
}
def main() -> None:
"""Run the selected diagnostic scenario."""
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("scenario", choices=SCENARIOS)
args = parser.parse_args()
torch.set_printoptions(precision=9, linewidth=140)
SCENARIOS[args.scenario]()
print(f"\n[PASS] {args.scenario}")
if __name__ == "__main__":