DexForce · yuecideng · Mar 19, 2026 · Mar 11, 2026 · Mar 11, 2026 · Mar 12, 2026
diff --git a/AGENTS.md b/AGENTS.md
@@ -73,7 +73,16 @@ Every source file begins with the Apache 2.0 copyright header:
 # Copyright (c) 2021-2026 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.
 # ----------------------------------------------------------------------------
 ```
 

diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.planners.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.planners.rst
@@ -0,0 +1,85 @@
+embodichain.lab.sim.planners
+==========================================
+
+.. automodule:: embodichain.lab.sim.planners
+
+  .. rubric:: Classes
+
+  .. autosummary::
+    BasePlannerCfg
+    BasePlanner
+    ToppraPlannerCfg
+    ToppraPlanner
+    MotionGenCfg
+    MotionGenerator
+    TrajectorySampleMethod
+    MovePart
+    MoveType
+    PlanResult
+    PlanState
+
+.. currentmodule:: embodichain.lab.sim.planners
+
+Base Planner
+------------
+
+.. autoclass:: BasePlannerCfg
+    :members:
+    :exclude-members: __init__, copy, replace, to_dict, validate
+
+.. autoclass:: BasePlanner
+    :members:
+    :inherited-members:
+    :show-inheritance:
+
+Toppra Planner
+--------------
+
+.. autoclass:: ToppraPlannerCfg
+    :members:
+    :exclude-members: __init__, copy, replace, to_dict, validate
+
+.. autoclass:: ToppraPlanner
+    :members:
+    :inherited-members:
+    :show-inheritance:
+
+Motion Generator
+----------------
+
+.. autoclass:: MotionGenCfg
+    :members:
+    :exclude-members: __init__, copy, replace, to_dict, validate
+
+.. autoclass:: MotionGenerator
+    :members:
+    :inherited-members:
+    :show-inheritance:
+
+Utilities
+---------
+
+.. autoclass:: TrajectorySampleMethod
+    :members:
+    :undoc-members:
+    :show-inheritance:
+
+.. autoclass:: MovePart
+    :members:
+    :undoc-members:
+    :show-inheritance:
+
+.. autoclass:: MoveType
+    :members:
+    :undoc-members:
+    :show-inheritance:
+
+.. autoclass:: PlanResult
+    :members:
+    :undoc-members:
+    :show-inheritance:
+
+.. autoclass:: PlanState
+    :members:
+    :undoc-members:
+    :show-inheritance:
diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.rst
@@ -15,6 +15,7 @@
       shapes
       objects
       sensors
+      planners
       solvers
       utility
 
@@ -92,6 +93,14 @@ Solvers
 
    embodichain.lab.sim.solvers
 
+Planners
+--------
+
+.. toctree::
+   :maxdepth: 1
+
+   embodichain.lab.sim.planners
+
 Utility
 -------
 

diff --git a/docs/source/overview/sim/planners/index.rst b/docs/source/overview/sim/planners/index.rst
@@ -31,6 +31,6 @@ See also
 .. toctree::
     :maxdepth: 1
 
-    motion_generator.md
     toppra_planner.md
     trajectory_sample_method.md
+    motion_generator.md
diff --git a/docs/source/overview/sim/planners/motion_generator.md b/docs/source/overview/sim/planners/motion_generator.md
@@ -23,10 +23,12 @@ from embodichain.lab.sim.cfg import (
     JointDrivePropertiesCfg,
 )
 
-from embodichain.lab.sim.planners.motion_generator import MotionGenerator
+from embodichain.lab.sim.planners.motion_generator import MotionGenerator, MotionGenCfg, ToppraPlannerCfg
+from embodichain.lab.sim.planners.toppra_planner import ToppraPlanOptions
 from embodichain.lab.sim.objects.robot import Robot
 from embodichain.lab.sim.solvers.pink_solver import PinkSolverCfg
-from embodichain.lab.sim.planners.utils import TrajectorySampleMethod
+from embodichain.lab.sim.planners.utils import TrajectorySampleMethod, PlanState, MoveType
+from embodichain.lab.sim.planners.motion_generator import MotionGenOptions
 
 # Configure the simulation
 sim_cfg = SimulationManagerCfg(
@@ -66,68 +68,110 @@ robot_cfg = RobotCfg(
 )
 robot = sim.add_robot(cfg=robot_cfg)
 
+# Constraints are now specified in ToppraPlanOptions, not in ToppraPlannerCfg
 motion_gen = MotionGenerator(
-    robot=robot,
-    uid="arm",
-    planner_type="toppra",
-    default_velocity=0.2,
-    default_acceleration=0.5
+    cfg=MotionGenCfg(
+        planner_cfg=ToppraPlannerCfg(
+            robot_uid="UR10_test",
+        )
+    )
 )
-
 ```
 
 ### Trajectory Planning
 
 #### Joint Space Planning
 
 ```python
-current_state = {
-    "position": [0, 0, 0, 0, 0, 0],
-    "velocity": [0, 0, 0, 0, 0, 0],
-    "acceleration": [0, 0, 0, 0, 0, 0]
-}
+# 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",
+    is_interpolate=False,
+)
+
+# Use generate() method instead of plan()
 target_states = [
-    {"position": [1, 1, 1, 1, 1, 1]}
+    PlanState(move_type=MoveType.JOINT_MOVE, qpos=torch.tensor([1, 1, 1, 1, 1, 1]))
 ]
-success, positions, velocities, accelerations, times, duration = motion_gen.plan(
-    current_state=current_state,
+result = motion_gen.generate(
     target_states=target_states,
-    sample_method=TrajectorySampleMethod.TIME,
-    sample_interval=0.01
+    options=motion_opts
 )
 ```
 
-#### Cartesian or Joint Interpolation
+#### Cartesian Space Planning
 
 ```python
-# Using joint configurations (qpos_list)
-qpos_list = [
-    [0, 0, 0, 0, 0, 0],
-    [1, 1, 1, 1, 1, 1]
+import torch
+import numpy as np
+
+# Create options with constraints
+plan_opts = ToppraPlanOptions(
+    constraints={
+        "velocity": 0.2,
+        "acceleration": 0.5,
+    },
+    sample_method=TrajectorySampleMethod.TIME,
+    sample_interval=0.01
+)
+
+# Create motion generation options with interpolation for smoother Cartesian motion
+motion_opts = MotionGenOptions(
+    plan_opts=plan_opts,
+    control_part="arm",
+    is_interpolate=True,  # Enable pre-interpolation for Cartesian moves
+    interpolate_nums=10,   # Number of points between each waypoint
+    is_linear=True,        # Linear interpolation in Cartesian space
+)
+
+# Define target poses as 4x4 transformation matrices
+# Each matrix is [position(3), orientation(3x3)] in row-major order
+target_pose_1 = torch.eye(4)
+target_pose_1[:3, 3] = torch.tensor([0.5, 0.3, 0.4])  # position
+
+target_pose_2 = torch.eye(4)
+target_pose_2[:3, 3] = torch.tensor([0.6, 0.4, 0.3])  # another position
+
+# Use EEF_MOVE for Cartesian space planning
+target_states = [
+    PlanState(move_type=MoveType.EEF_MOVE, xpos=target_pose_1),
+    PlanState(move_type=MoveType.EEF_MOVE, xpos=target_pose_2),
 ]
-out_qpos_list, out_xpos_list = motion_gen.create_discrete_trajectory(
-    qpos_list=qpos_list,
-    is_linear=False,
-    sample_method=TrajectorySampleMethod.QUANTITY,
-    sample_num=20
+
+result = motion_gen.generate(
+    target_states=target_states,
+    options=motion_opts
 )
 ```
 
+
 ### Estimating Trajectory Sample Count
 
 You can estimate the number of sampling points required for a trajectory before generating it:
 
 ```python
 # Estimate based on joint configurations (qpos_list)
-qpos_list = [
+qpos_list = torch.as_tensor([
     [0, 0, 0, 0, 0, 0],
     [0.5, 0.5, 0.5, 0.5, 0.5, 0.5],
     [1, 1, 1, 1, 1, 1]
-]
+])
 sample_count = motion_gen.estimate_trajectory_sample_count(
     qpos_list=qpos_list,  # List of joint positions
     step_size=0.01, # unit: m
     angle_step=0.05, # unit: rad
+    control_part="arm",
 )
 print(f"Estimated sample count: {sample_count}")
 ```

diff --git a/docs/source/overview/sim/planners/toppra_planner.md b/docs/source/overview/sim/planners/toppra_planner.md
@@ -14,44 +14,52 @@
 ### Initialization
 
 ```python
-from embodichain.lab.sim.planners.toppra_planner import ToppraPlanner
-planner = ToppraPlanner(
-    dofs=6,
-    max_constraints={
-        "velocity": [1.0, 1.0, 1.0, 1.0, 1.0, 1.0],
-        "acceleration": [2.0, 2.0, 2.0, 2.0, 2.0, 2.0]
-    }
+from embodichain.lab.sim.planners.toppra_planner import ToppraPlanner, ToppraPlannerCfg
+
+# Configuration - constraints are now specified in ToppraPlanOptions, not here
+cfg = ToppraPlannerCfg(
+    robot_uid="UR5"
 )
+planner = ToppraPlanner(cfg=cfg)
 ```
 
 ### Planning
 
 ```python
-from embodichain.lab.sim.planners.utils import TrajectorySampleMethod
-from embodichain.lab.sim.planners.toppra_planner import ToppraPlanner
-success, positions, velocities, accelerations, times, duration = planner.plan(
-    current_state={
-        "position": [0, 0, 0, 0, 0, 0],
-        "velocity": [0, 0, 0, 0, 0, 0],
-        "acceleration": [0, 0, 0, 0, 0, 0]
+from embodichain.lab.sim.planners.utils import TrajectorySampleMethod, PlanState
+from embodichain.lab.sim.planners.toppra_planner import ToppraPlanOptions
+
+# Create options with constraints and sampling parameters
+options = ToppraPlanOptions(
+    constraints={
+        "velocity": 0.2,          # Joint velocity limit (rad/s) - can also be a list per joint
+        "acceleration": 0.5,      # Joint acceleration limit (rad/s²) - can also be a list per joint
     },
+    sample_method=TrajectorySampleMethod.TIME,  # Or TrajectorySampleMethod.QUANTITY
+    sample_interval=0.01  # Time interval in seconds (if TIME) or number of samples (if QUANTITY)
+)
+
+# Plan trajectory - only target_states needed now (current_state is handled internally)
+result = planner.plan(
     target_states=[
-        {"position": [1, 1, 1, 1, 1, 1]}
+        PlanState(qpos=[1, 1, 1, 1, 1, 1])
     ],
-    sample_method=TrajectorySampleMethod.TIME,
-    sample_interval=0.01
+    options=options
 )
 ```
 
-- `positions`, `velocities`, `accelerations` are arrays of sampled trajectory points.
-- `times` is the array of time stamps.
-- `duration` is the total trajectory time.
+- `result.positions`, `result.velocities`, `result.accelerations` are arrays of sampled trajectory points.
+- `result.dt` is the array of time intervals between each point.
+- `result.duration` is the total trajectory time.
+- `result.success` indicates whether planning succeeded.
 
 ## Notes
 
 - The planner requires the `toppra` library (`pip install toppra==0.6.3`).
 - For dense waypoints, the default spline interpolation is used. For sparse waypoints, you may need to adjust the interpolation method.
 - The number of grid points (`gridpt_min_nb_points`) is important for accurate acceleration constraint handling.
+- Constraints can be specified as a single float (applied to all joints) or as a list of values for each joint.
+- The `sample_method` can be `TrajectorySampleMethod.TIME` for uniform time intervals or `TrajectorySampleMethod.QUANTITY` for a fixed number of samples.
 
 ## References