Motion Generator

Motion Generator#

The MotionGenerator class in EmbodiChain provides a unified and extensible interface for robot trajectory planning. It supports time-optimal trajectory generation (currently via TOPPRA), joint/Cartesian interpolation, and is designed for easy integration with RL, imitation learning, and classical control scenarios.

Key Features#

  • Unified API: One interface for multiple planning strategies (time-optimal, interpolation, etc.)

  • Constraint Support: Velocity/acceleration constraints configurable per joint

  • Flexible Input: Supports both joint space and Cartesian space waypoints

  • Extensible: Easy to add new planners (RRT, PRM, etc.)

  • Integration Ready: Can be used in RL, imitation learning, or classical pipelines

The Code#

The tutorial corresponds to the motion_generator.py script in the scripts/tutorials/sim directory.

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

Typical Usage#

from embodichain.lab.sim.motion.motion_generator import MotionGenerator, MotionGenCfg
from embodichain.lab.sim.motion.planners import ToppraPlannerCfg
from embodichain.lab.sim.motion.planners.toppra_planner import ToppraPlanOptions
from embodichain.lab.sim.motion.planners.utils import PlanState, TrajectorySampleMethod, MoveType

# Assume you have a robot instance and arm_name
# Constraints are now specified in ToppraPlanOptions, not in ToppraPlannerCfg
motion_cfg = MotionGenCfg(
    planner_cfg=ToppraPlannerCfg(
        robot_uid=robot.uid,
    )
)
motion_gen = MotionGenerator(cfg=motion_cfg)

# Create options with constraints and planning parameters
plan_opts = ToppraPlanOptions(
    constraints={
        "velocity": 0.2,
        "acceleration": 0.5,
    },
    sample_method=TrajectorySampleMethod.TIME,
    sample_interval=0.01
)

# Create motion generation options
motion_opts = MotionGenOptions(
    strategy="motion_gen",
    plan_opts=plan_opts,
    control_part=arm_name,
    is_interpolate=True,
    interpolate_nums=10,
)

# Plan a joint-space trajectory (use generate() method instead of plan())
target_states = [
    PlanState(move_type=MoveType.JOINT_MOVE, qpos=torch.tensor([0.5, 0.2, 0., 0., 0., 0.]))
]
plan_result = motion_gen.generate(
    target_states=target_states,
    options=motion_opts
)
success = plan_result.success
positions = plan_result.positions
velocities = plan_result.velocities
accelerations = plan_result.accelerations
duration = plan_result.duration

API Reference#

Initialization

from embodichain.lab.sim.motion.planners.toppra_planner import ToppraPlanOptions

motion_cfg = MotionGenCfg(
    planner_cfg=ToppraPlannerCfg(
        robot_uid=robot.uid,
    )
)
MotionGenerator(cfg=motion_cfg)

  • cfg: MotionGenCfg instance, containing the specific planner’s configuration (like ToppraPlannerCfg)

  • robot_uid: Robot unique identifier

  • constraints: Now specified in ToppraPlanOptions (passed via MotionGenOptions.plan_opts)

MotionGenOptions

motion_opts = MotionGenOptions(
    strategy="motion_gen",               # "motion_gen" or "ik_interp"
    sample_count=None,                    # Optional normalized output length
    interpolation_dt=None,                # Required for deterministic interpolation
    plan_opts=ToppraPlanOptions(...),  # Options for the underlying planner
    control_part=arm_name,              # Robot part to control (e.g., 'left_arm')
    is_interpolate=False,               # Whether to pre-interpolate trajectory
    interpolate_nums=10,                # Number of interpolation points between waypoints
    is_linear=False,                    # Use Cartesian linear interpolation if True, else joint space
    interpolate_position_step=0.002,    # Step size for Cartesian interpolation (meters)
    interpolate_angle_step=np.pi/90,   # Step size for joint interpolation (radians)
    start_qpos=torch.tensor([...]),     # Optional starting joint configuration
)

generate (formerly plan)

generate(
    target_states: list[PlanState],
    options: MotionGenOptions | None = None,
) -> PlanResult

  • strategy="motion_gen" delegates to the configured backend; strategy="ik_interp" performs deterministic waypoint IK and joint interpolation and requires interpolation_dt.

  • Returns a normalized, environment-batched PlanResult with explicit dt and derived duration whenever positions are present. Missing timing raises immediately.

  • Uses target_states (list of PlanState) and options (MotionGenOptions) instead of individual parameters.

interpolate_trajectory

interpolate_trajectory(
    control_part: str | None = None,
    xpos_list: torch.Tensor | None = None,
    qpos_list: torch.Tensor | None = None,
    options: MotionGenOptions | None = None,
) -> Tuple[torch.Tensor, torch.Tensor | None]

  • Interpolates trajectory between waypoints (joint or Cartesian), auto-handles FK/IK.

estimate_trajectory_sample_count

estimate_trajectory_sample_count(
    xpos_list=None,
    qpos_list=None,
    step_size=0.01,
    angle_step=np.pi/90,
    control_part=None,
) -> torch.Tensor

  • Estimates the number of samples needed for a trajectory.

plan_with_collision

plan_with_collision(...)

  • (Reserved) Plan trajectory with collision checking (not yet implemented).

Notes & Best Practices#

  • Only collision-free planning is currently supported; collision checking is a placeholder.

  • Input/outputs are numpy arrays or torch tensors; ensure type consistency.

  • Robot instance must implement get_joint_ids, compute_fk, compute_ik, get_proprioception, etc.

  • For custom planners, extend the PlannerType Enum and _create_planner methods.

  • Constraints (velocity, acceleration) are now specified in ToppraPlanOptions, not in ToppraPlannerCfg.

  • Use PlanState.qpos (not position) for joint positions.

Contents