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

# ----------------------------------------------------------------------------
# Copyright (c) 2021-2025 DexForce Technology Co., Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ----------------------------------------------------------------------------

import time
import torch
import numpy as np
from copy import deepcopy
from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
from embodichain.lab.sim.objects import Robot
from embodichain.lab.sim.robots import CobotMagicCfg
from embodichain.lab.sim.planners.utils import TrajectorySampleMethod
from embodichain.lab.sim.planners.motion_generator import MotionGenerator


def move_robot_along_trajectory(
    robot: Robot, arm_name: str, qpos_list: list[torch.Tensor], delay: float = 0.1
):
    """
    Set the robot joint positions sequentially along the given joint trajectory.
    Args:
        robot: Robot instance.
        arm_name: Name of the robot arm.
        qpos_list: List of joint positions (torch.Tensor).
        delay: Time delay between each step (seconds).
    """
    for q in qpos_list:
        robot.set_qpos(qpos=q.unsqueeze(0), joint_ids=robot.get_joint_ids(arm_name))
        time.sleep(delay)


def create_demo_trajectory(
    robot: Robot, arm_name: str
) -> tuple[list[torch.Tensor], list[np.ndarray]]:
    """
    Generate a three-point trajectory (start, middle, end) for demonstration.
    Args:
        robot: Robot instance.
        arm_name: Name of the robot arm.
    Returns:
        qpos_list: List of joint positions (torch.Tensor).
        xpos_list: List of end-effector poses (numpy arrays).
    """
    qpos_fk = torch.tensor(
        [[0.0, np.pi / 4, -np.pi / 4, 0.0, np.pi / 4, 0.0]], dtype=torch.float32
    )
    xpos_begin = robot.compute_fk(name=arm_name, qpos=qpos_fk, to_matrix=True)
    xpos_mid = deepcopy(xpos_begin)
    xpos_mid[0, 2, 3] -= 0.1  # Move down by 0.1m in Z direction
    xpos_final = deepcopy(xpos_mid)
    xpos_final[0, 0, 3] += 0.2  # Move forward by 0.2m in X direction

    qpos_begin = robot.compute_ik(pose=xpos_begin, name=arm_name)[1][0]
    qpos_mid = robot.compute_ik(pose=xpos_mid, name=arm_name)[1][0]
    qpos_final = robot.compute_ik(pose=xpos_final, name=arm_name)[1][0]
    return [qpos_begin, qpos_mid, qpos_final], [
        xpos_begin[0],
        xpos_mid[0],
        xpos_final[0],
    ]


def main():
    np.set_printoptions(precision=5, suppress=True)
    torch.set_printoptions(precision=5, sci_mode=False)

    # Initialize simulation
    sim = SimulationManager(SimulationManagerCfg(headless=False, sim_device="cpu"))
    sim.set_manual_update(False)

    # Robot configuration
    cfg_dict = {"uid": "CobotMagic"}
    robot: Robot = sim.add_robot(cfg=CobotMagicCfg.from_dict(cfg_dict))
    arm_name = "left_arm"

    # # Generate trajectory points
    qpos_list, xpos_list = create_demo_trajectory(robot, arm_name)

    # Initialize motion generator
    motion_generator = MotionGenerator(
        robot=robot,
        uid=arm_name,
        planner_type="toppra",
        default_velocity=0.2,
        default_acceleration=0.5,
    )

    # Joint space trajectory
    out_qpos_list, _ = motion_generator.create_discrete_trajectory(
        qpos_list=[q.numpy() for q in qpos_list],
        is_linear=False,
        sample_method=TrajectorySampleMethod.QUANTITY,
        sample_num=20,
    )
    move_robot_along_trajectory(robot, arm_name, out_qpos_list)

    # Cartesian space trajectory
    out_qpos_list, _ = motion_generator.create_discrete_trajectory(
        xpos_list=[x.numpy() for x in xpos_list],
        is_linear=True,
        sample_method=TrajectorySampleMethod.QUANTITY,
        sample_num=20,
    )
    move_robot_along_trajectory(robot, arm_name, out_qpos_list)


if __name__ == "__main__":
    main()

Typical Usage

from embodichain.lab.sim.planners.motion_generator import MotionGenerator

# Assume you have a robot instance and uid
motion_gen = MotionGenerator(
    robot=robot,
    uid="arm",
    default_velocity=0.2,
    default_acceleration=0.5
)

# Plan a joint-space trajectory
current_state = {"position": [0, 0, 0, 0, 0, 0]}
target_states = [{"position": [0.5, 0.2, 0, 0, 0, 0]}]
success, positions, velocities, accelerations, times, duration = motion_gen.plan(
    current_state=current_state,
    target_states=target_states
)

# Generate a discrete trajectory (joint or Cartesian)
qpos_list, xpos_list = motion_gen.create_discrete_trajectory(
    qpos_list=[[0,0,0,0,0,0],[0.5,0.2,0,0,0,0]],
    sample_num=20
)

API Reference

Initialization

MotionGenerator(
    robot: Robot,
    uid: str,
    sim=None,
    planner_type="toppra",
    default_velocity=0.2,
    default_acceleration=0.5,
    collision_margin=0.01,
    **kwargs
)

• robot: Robot instance, must support get_joint_ids, compute_fk, compute_ik

• uid: Unique robot identifier (e.g., “arm”)

• planner_type: Planner type (default: “toppra”)

• default_velocity, default_acceleration: Default joint constraints

plan

plan(
    current_state: Dict,
    target_states: List[Dict],
    sample_method=TrajectorySampleMethod.TIME,
    sample_interval=0.01,
    **kwargs
) -> Tuple[bool, positions, velocities, accelerations, times, duration]

• Plans a time-optimal trajectory (joint space), returns trajectory arrays and duration.

create_discrete_trajectory

create_discrete_trajectory(
    xpos_list=None,
    qpos_list=None,
    is_use_current_qpos=True,
    is_linear=False,
    sample_method=TrajectorySampleMethod.QUANTITY,
    sample_num=20,
    qpos_seed=None,
    **kwargs
) -> Tuple[List[np.ndarray], List[np.ndarray]]

• Generates a discrete 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,
    **kwargs
) -> int

• 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.