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
 17import time
 18import torch
 19import numpy as np
 20from copy import deepcopy
 21from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
 22from embodichain.lab.sim.objects import Robot
 23from embodichain.lab.sim.robots import CobotMagicCfg
 24from embodichain.lab.sim.planners.utils import TrajectorySampleMethod
 25from embodichain.lab.sim.planners.motion_generator import MotionGenerator
 26
 27
 28def move_robot_along_trajectory(
 29    robot: Robot, arm_name: str, qpos_list: list[torch.Tensor], delay: float = 0.1
 30):
 31    """
 32    Set the robot joint positions sequentially along the given joint trajectory.
 33    Args:
 34        robot: Robot instance.
 35        arm_name: Name of the robot arm.
 36        qpos_list: List of joint positions (torch.Tensor).
 37        delay: Time delay between each step (seconds).
 38    """
 39    for q in qpos_list:
 40        robot.set_qpos(qpos=q.unsqueeze(0), joint_ids=robot.get_joint_ids(arm_name))
 41        time.sleep(delay)
 42
 43
 44def create_demo_trajectory(
 45    robot: Robot, arm_name: str
 46) -> tuple[list[torch.Tensor], list[np.ndarray]]:
 47    """
 48    Generate a three-point trajectory (start, middle, end) for demonstration.
 49    Args:
 50        robot: Robot instance.
 51        arm_name: Name of the robot arm.
 52    Returns:
 53        qpos_list: List of joint positions (torch.Tensor).
 54        xpos_list: List of end-effector poses (numpy arrays).
 55    """
 56    qpos_fk = torch.tensor(
 57        [[0.0, np.pi / 4, -np.pi / 4, 0.0, np.pi / 4, 0.0]], dtype=torch.float32
 58    )
 59    xpos_begin = robot.compute_fk(name=arm_name, qpos=qpos_fk, to_matrix=True)
 60    xpos_mid = deepcopy(xpos_begin)
 61    xpos_mid[0, 2, 3] -= 0.1  # Move down by 0.1m in Z direction
 62    xpos_final = deepcopy(xpos_mid)
 63    xpos_final[0, 0, 3] += 0.2  # Move forward by 0.2m in X direction
 64
 65    qpos_begin = robot.compute_ik(pose=xpos_begin, name=arm_name)[1][0]
 66    qpos_mid = robot.compute_ik(pose=xpos_mid, name=arm_name)[1][0]
 67    qpos_final = robot.compute_ik(pose=xpos_final, name=arm_name)[1][0]
 68    return [qpos_begin, qpos_mid, qpos_final], [
 69        xpos_begin[0],
 70        xpos_mid[0],
 71        xpos_final[0],
 72    ]
 73
 74
 75def main():
 76    np.set_printoptions(precision=5, suppress=True)
 77    torch.set_printoptions(precision=5, sci_mode=False)
 78
 79    # Initialize simulation
 80    sim = SimulationManager(SimulationManagerCfg(headless=False, sim_device="cpu"))
 81    sim.set_manual_update(False)
 82
 83    # Robot configuration
 84    cfg_dict = {"uid": "CobotMagic"}
 85    robot: Robot = sim.add_robot(cfg=CobotMagicCfg.from_dict(cfg_dict))
 86    arm_name = "left_arm"
 87
 88    # # Generate trajectory points
 89    qpos_list, xpos_list = create_demo_trajectory(robot, arm_name)
 90
 91    from embodichain.lab.sim.planners import (
 92        MotionGenerator,
 93        MotionGenCfg,
 94        MotionGenOptions,
 95        ToppraPlannerCfg,
 96        ToppraPlanOptions,
 97        PlanState,
 98        MoveType,
 99        MovePart,
100    )
101
102    # Initialize motion generator
103    motion_cfg = MotionGenCfg(
104        planner_cfg=ToppraPlannerCfg(
105            robot_uid=robot.uid,
106        )
107    )
108    motion_generator = MotionGenerator(cfg=motion_cfg)
109
110    # Joint space trajectory
111    qpos_list = torch.vstack(qpos_list)
112    options = MotionGenOptions(
113        control_part=arm_name,
114        start_qpos=qpos_list[0],
115        is_interpolate=True,
116        is_linear=False,
117        plan_opts=ToppraPlanOptions(
118            constraints={
119                "velocity": 0.2,
120                "acceleration": 0.5,
121            },
122            sample_method=TrajectorySampleMethod.QUANTITY,
123            sample_interval=20,
124        ),
125    )
126
127    target_states = []
128    for qpos in qpos_list:
129        target_states.append(
130            PlanState(
131                move_type=MoveType.JOINT_MOVE,
132                move_part=MovePart.LEFT,
133                qpos=qpos,
134            )
135        )
136    plan_result = motion_generator.generate(
137        target_states=target_states, options=options
138    )
139    move_robot_along_trajectory(robot, arm_name, plan_result.positions)
140
141    # Cartesian space trajectory
142    options.is_linear = True
143
144    target_states = []
145    for xpos in xpos_list:
146        target_states.append(
147            PlanState(
148                move_type=MoveType.EEF_MOVE,
149                move_part=MovePart.LEFT,
150                xpos=xpos,
151            )
152        )
153    plan_result = motion_generator.generate(
154        target_states=target_states, options=options
155    )
156    sim.reset()
157    move_robot_along_trajectory(robot, arm_name, plan_result.positions)
158
159
160if __name__ == "__main__":
161    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.

Contents