Motion Generator

Motion Generator#

This tutorial creates one CobotMagic arm, plans joint and Cartesian waypoint paths with TOPPRA, and replays them in simulation. For backend selection, minimal API examples, and result shapes, see MotionGenerator.

Run the tutorial#

Run from the repository root with the simulation runtime and assets installed:

python scripts/tutorials/sim/motion_generator.py --headless --device cpu

Use --num-envs N for a batched scene, --physics newton for the Newton backend, or --viser to publish the scene in a browser. Headless runs record a whole-scene MP4 by default; use --disable-record to skip recording or --record-save-path PATH to choose its destination.

Follow the planning workflow#

  1. Create the simulation and robot, including an IK solver for the arm.

  2. Configure MotionGenerator with ToppraPlannerCfg and place velocity, acceleration, and sampling settings in ToppraPlanOptions.

  3. Read the arm’s starting joint positions and construct batched PlanState waypoints. Cartesian pre-interpolation uses the configured IK solver.

  4. Call generate() and check the result’s success flags.

  5. Replay the timed joint trajectory using play_joint_trajectory() on a fixed control grid. The helper keeps the physics period unchanged and recomputes velocity targets for the executed grid.

Complete tutorial script
  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
 17from __future__ import annotations
 18
 19import argparse
 20import time
 21from collections.abc import Sequence
 22
 23from embodichain.cli.sim import add_sim_args_to_parser
 24
 25DEFAULT_ARENA_SPACE = 3.0
 26
 27
 28def build_parser() -> argparse.ArgumentParser:
 29    """Build CLI options without initializing simulation resources."""
 30    parser = argparse.ArgumentParser(
 31        description="Generate and replay MotionGenerator trajectories for one or more environments."
 32    )
 33    add_sim_args_to_parser(parser)
 34    parser.set_defaults(arena_space=DEFAULT_ARENA_SPACE)
 35    parser.add_argument(
 36        "--step-delay",
 37        type=float,
 38        default=0.1,
 39        help="Seconds to wait between trajectory waypoints during playback.",
 40    )
 41    parser.add_argument(
 42        "--record-fps",
 43        type=int,
 44        default=20,
 45        help="Output video FPS for headless recording.",
 46    )
 47    parser.add_argument(
 48        "--record-save-path",
 49        type=str,
 50        default=None,
 51        help="Optional mp4 output path for headless recording.",
 52    )
 53    parser.add_argument(
 54        "--disable-record",
 55        action="store_true",
 56        help="Disable automatic whole-scene recording in headless mode.",
 57    )
 58    return parser
 59
 60
 61def parse_args(argv: list[str] | None = None) -> argparse.Namespace:
 62    """Parse command line arguments for the motion-generator tutorial."""
 63    parser = build_parser()
 64    return parser.parse_args() if argv is None else parser.parse_args(argv)
 65
 66
 67if __name__ == "__main__":
 68    # Parse before importing optional simulation/planning dependencies.
 69    _cli_args = parse_args()
 70
 71
 72import numpy as np
 73import torch
 74
 75from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
 76from embodichain.lab.sim.cfg import RenderCfg, physics_cfg_for_backend
 77from embodichain.lab.visualization import visualization_cfg_from_args
 78from embodichain.lab.sim.objects import Robot
 79from embodichain.lab.sim.motion.motion_generator import (
 80    MotionGenCfg,
 81    MotionGenOptions,
 82    MotionGenerator,
 83)
 84from embodichain.lab.sim.motion.execution import (
 85    JointTrajectoryPlaybackCfg,
 86    play_joint_trajectory,
 87)
 88from embodichain.lab.sim.motion.planners import (
 89    PlanState,
 90    ToppraPlanOptions,
 91    ToppraPlannerCfg,
 92)
 93from embodichain.lab.sim.motion.planners.utils import TrajectorySampleMethod
 94from embodichain_robots import CobotMagicCfg
 95
 96RECORD_WIDTH = 1920
 97RECORD_HEIGHT = 1080
 98DEFAULT_RECORD_TARGET_Z = 0.95
 99DEFAULT_RECORD_MAX_MEMORY = 2048
100
101
102def compute_record_look_at(
103    num_envs: int,
104    arena_space: float,
105) -> tuple[
106    tuple[float, float, float], tuple[float, float, float], tuple[float, float, float]
107]:
108    """Return a fixed camera pose that frames the full replicated arena grid."""
109    if num_envs <= 0:
110        raise ValueError(f"num_envs must be positive, got {num_envs}.")
111
112    scene_grid_length = int(np.ceil(np.sqrt(num_envs)))
113    scene_grid_rows = int(np.ceil(num_envs / scene_grid_length))
114    span_x = float(max(scene_grid_length - 1, 0) * arena_space)
115    span_y = float(max(scene_grid_rows - 1, 0) * arena_space)
116    scene_extent = 0.5 * max(span_x, span_y)
117
118    target = (
119        0.5 * span_x,
120        0.5 * span_y,
121        DEFAULT_RECORD_TARGET_Z,
122    )
123    eye = (
124        2.6 + target[0] + scene_extent,
125        -2.2 - scene_extent,
126        1.6 + 0.4 * scene_extent,
127    )
128    return eye, target, (0.0, 0.0, 1.0)
129
130
131def move_robot_along_trajectory(
132    sim: SimulationManager,
133    robot: Robot,
134    arm_name: str,
135    qpos_trajectory: torch.Tensor | Sequence[torch.Tensor],
136    qvel_trajectory: torch.Tensor | None,
137    dt: torch.Tensor,
138) -> None:
139    """Play back a planned joint trajectory for one or more environments.
140
141    This function assumes the simulation is in manual-update mode and calls
142    :meth:`SimulationManager.update` after each waypoint so physics advances.
143
144    Args:
145        sim: Simulation manager instance.
146        robot: Robot instance.
147        arm_name: Name of the robot arm to control.
148        qpos_trajectory: Joint positions shaped ``(B, N, DOF)``, ``(N, DOF)``,
149            or a sequence of waypoint tensors.
150        qvel_trajectory: Matching native planner velocities, when available.
151            Playback validates this shape but recomputes velocities after
152            retiming to the executed control grid.
153        dt: Per-waypoint arrival intervals shaped ``(B, N)``.
154    """
155    if isinstance(qpos_trajectory, Sequence):
156        qpos_steps = list(qpos_trajectory)
157        if not qpos_steps:
158            return
159        if qpos_steps[0].dim() == 1:
160            qpos_trajectory = torch.stack(qpos_steps, dim=0).unsqueeze(0)
161        else:
162            qpos_trajectory = torch.stack(qpos_steps, dim=1)
163    if qpos_trajectory.dim() == 2:
164        qpos_trajectory = qpos_trajectory.unsqueeze(0)
165    if qpos_trajectory.dim() != 3:
166        raise ValueError(
167            "qpos_trajectory must have shape (B, N, DOF) or (N, DOF), "
168            f"got {tuple(qpos_trajectory.shape)}."
169        )
170    if qvel_trajectory is not None and qvel_trajectory.shape != qpos_trajectory.shape:
171        raise ValueError("qvel_trajectory must have the same shape as qpos_trajectory.")
172    play_joint_trajectory(
173        sim,
174        robot,
175        positions=qpos_trajectory,
176        dt=dt,
177        joint_ids=robot.get_joint_ids(arm_name),
178        cfg=JointTrajectoryPlaybackCfg(joint_command_mode="position_velocity"),
179    )
180
181
182def create_demo_trajectory(
183    robot: Robot,
184    arm_name: str,
185    num_envs: int = 1,
186) -> tuple[list[torch.Tensor], list[torch.Tensor]]:
187    """Generate a three-point batched demo trajectory for the requested env count."""
188    if num_envs <= 0:
189        raise ValueError(f"num_envs must be positive, got {num_envs}.")
190
191    tensor_kwargs = {"dtype": torch.float32}
192    robot_device = getattr(robot, "device", None)
193    if isinstance(robot_device, (str, torch.device)):
194        tensor_kwargs["device"] = robot_device
195
196    qpos_fk = torch.tensor(
197        [[0.0, np.pi / 4, -np.pi / 4, 0.0, np.pi / 4, 0.0]],
198        **tensor_kwargs,
199    ).repeat(num_envs, 1)
200    xpos_begin = robot.compute_fk(name=arm_name, qpos=qpos_fk, to_matrix=True)
201    xpos_mid = xpos_begin.clone()
202    xpos_mid[:, 2, 3] -= 0.1
203    xpos_final = xpos_mid.clone()
204    xpos_final[:, 0, 3] += 0.2
205
206    qpos_begin = robot.compute_ik(pose=xpos_begin, name=arm_name)[1]
207    qpos_mid = robot.compute_ik(pose=xpos_mid, name=arm_name)[1]
208    qpos_final = robot.compute_ik(pose=xpos_final, name=arm_name)[1]
209    return [qpos_begin, qpos_mid, qpos_final], [xpos_begin, xpos_mid, xpos_final]
210
211
212def start_headless_recording(
213    sim: SimulationManager,
214    args: argparse.Namespace,
215) -> bool:
216    """Start headless viewer recording with a whole-scene camera."""
217    if not args.headless or args.disable_record:
218        return False
219
220    look_at = compute_record_look_at(
221        num_envs=sim.num_envs,
222        arena_space=sim.sim_config.arena_space,
223    )
224    if not sim.start_window_record(
225        save_path=args.record_save_path,
226        fps=args.record_fps,
227        max_memory=DEFAULT_RECORD_MAX_MEMORY,
228        video_prefix="motion_generator_headless",
229        look_at=look_at,
230        use_sim_time=False,
231    ):
232        raise RuntimeError("Failed to start headless recording")
233
234    print("[INFO]: Headless recording enabled.")
235    print(
236        "[INFO]: The output path is reported by `SimulationManager.start_window_record()`."
237    )
238    return True
239
240
241def main(args: argparse.Namespace | None = None) -> None:
242    """Run the motion-generator tutorial."""
243    args = parse_args() if args is None else args
244
245    np.set_printoptions(precision=5, suppress=True)
246    torch.set_printoptions(precision=5, sci_mode=False)
247
248    sim = SimulationManager(
249        SimulationManagerCfg(
250            width=RECORD_WIDTH,
251            height=RECORD_HEIGHT,
252            headless=True,
253            physics_dt=1.0 / 100.0,
254            device=args.device,
255            physics_cfg=physics_cfg_for_backend(args.physics),
256            render_cfg=RenderCfg(renderer=args.renderer),
257            num_envs=args.num_envs,
258            arena_space=args.arena_space,
259            visualization=visualization_cfg_from_args(args),
260        )
261    )
262
263    robot: Robot = sim.add_robot(cfg=CobotMagicCfg.from_dict({"uid": "CobotMagic"}))
264    arm_name = "left_arm"
265
266    sim.prepare()
267
268    if not args.headless:
269        sim.open_window()
270
271    print(
272        f"[INFO]: Running motion generator tutorial with {sim.num_envs} environment(s)"
273    )
274
275    recording_started = start_headless_recording(sim, args)
276    try:
277        qpos_list, xpos_list = create_demo_trajectory(
278            robot=robot,
279            arm_name=arm_name,
280            num_envs=sim.num_envs,
281        )
282
283        motion_generator = MotionGenerator(
284            cfg=MotionGenCfg(
285                planner_cfg=ToppraPlannerCfg(
286                    robot_uid=robot.uid,
287                )
288            )
289        )
290
291        options = MotionGenOptions(
292            strategy="motion_gen",
293            control_part=arm_name,
294            start_qpos=qpos_list[0],
295            is_interpolate=True,
296            is_linear=False,
297            plan_opts=ToppraPlanOptions(
298                constraints={
299                    "velocity": 0.2,
300                    "acceleration": 0.5,
301                },
302                sample_method=TrajectorySampleMethod.QUANTITY,
303                sample_interval=20,
304            ),
305        )
306
307        joint_plan = motion_generator.generate(
308            target_states=[PlanState.from_qpos(qpos) for qpos in qpos_list],
309            options=options,
310        )
311        if joint_plan.positions is None:
312            raise RuntimeError("Joint-space planning did not produce any positions.")
313        move_robot_along_trajectory(
314            sim=sim,
315            robot=robot,
316            arm_name=arm_name,
317            qpos_trajectory=joint_plan.positions,
318            qvel_trajectory=joint_plan.velocities,
319            dt=joint_plan.dt,
320        )
321
322        options.is_linear = True
323        cartesian_plan = motion_generator.generate(
324            target_states=[PlanState.from_xpos(xpos) for xpos in xpos_list],
325            options=options,
326        )
327        if cartesian_plan.positions is None:
328            raise RuntimeError(
329                "Cartesian-space planning did not produce any positions."
330            )
331        sim.reset()
332        move_robot_along_trajectory(
333            sim=sim,
334            robot=robot,
335            arm_name=arm_name,
336            qpos_trajectory=cartesian_plan.positions,
337            qvel_trajectory=cartesian_plan.velocities,
338            dt=cartesian_plan.dt,
339        )
340    finally:
341        if sim.is_window_recording():
342            sim.stop_window_record()
343            sim.wait_window_record_saves()
344        sim.destroy()
345
346
347if __name__ == "__main__":
348    main(_cli_args)

Compare physical tracking#

To compare position-only and position-plus-velocity commands with the same initial state, gains, reference, and cadence, run:

python examples/sim/motion/trajectory_velocity_tracking.py --headless \
    --device cpu --output-dir trajectory_velocity_results

The script restores the configured initial state before each trial. The position-only trial explicitly writes zero target velocity. Outputs include tracking.csv with measurement timestamps, tracking.png with joint errors, and printed RMSE, P95, and maximum absolute error. These measurements apply to the selected robot, drive gains, physics backend, and cadence.

Next steps#

Contents