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

Typical Usage#

from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg
from embodichain.lab.sim.planners.toppra_planner import ToppraPlanOptions
from embodichain.lab.sim.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(
    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.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(
    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 = MotionGenOptions(),
) -> PlanResult

  • Generates a time-optimal trajectory (joint space), returning a PlanResult data class.

  • 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 = MotionGenOptions(),
) -> 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.