add solver and motion generator docs (#31)

yhnsu · yuanhaonan · web-flow · commit d07dbda9d888 · 2025-12-05T19:47:02.000+08:00
Co-authored-by: yuanhaonan &lt;yuanhaonan@dexforce.top&gt;
diff --git a/docs/source/tutorial/motion_gen.rst b/docs/source/tutorial/motion_gen.rst
@@ -6,9 +6,6 @@ Motion Generator
 
 .. currentmodule:: embodichain.lab.sim.planners.motion_generator
 
-Overview
-~~~~~~~~
-
 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
@@ -20,6 +17,19 @@ Key Features
 - **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.
+
+.. dropdown:: Code for motion_generator.py
+    :icon: code
+
+    .. literalinclude:: ../../../scripts/tutorials/sim/motion_generator.py
+        :language: python
+        :linenos:
+
 Typical Usage
 ~~~~~~~~~~~~~
 
diff --git a/docs/source/tutorial/solver.rst b/docs/source/tutorial/solver.rst
@@ -19,17 +19,26 @@ Key Features
 - **Batch and Single Query**: Supports both batch and single FK/IK/Jacobian queries.
 - **Extensible**: New solvers can be added by subclassing `BaseSolver` and implementing required methods.
 
-Example: Using PinkSolver
-~~~~~~~~~~~~~~~~~~~~~~~~~
+The Code
+~~~~~~~~
 
+The tutorial corresponds to the ``srs_solver.py`` script in the ``scripts/tutorials/sim`` directory.
 
-.. code-block:: python
+.. dropdown:: Code for srs_solver.py
+    :icon: code
+
+    .. literalinclude:: ../../../scripts/tutorials/sim/srs_solver.py
+        :language: python
+        :linenos:
 
-   from embodichain.lab.sim.solvers import PinkSolverCfg
-   from embodichain.lab.sim.objects.robot import Robot
+Typical Usage
+~~~~~~~~~~~~~~~~~~~~~~~~
 
-   # 1. Configure PinkSolver
-   pink_cfg = PinkSolverCfg(
+**Step 1: Configure solver**
+
+.. code-block:: python
+
+   srs_cfg = SrsSolverCfg(
        urdf_path="/path/to/robot.urdf",
        joint_names=[
            "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
@@ -38,29 +47,30 @@ Example: Using PinkSolver
        end_link_name="ee_link",
        root_link_name="base_link"
    )
-   # 2. Assign solver config to robot config
-   robot_cfg.solver_cfg = pink_cfg
-   # 3. Instantiate robot (solver will be initialized automatically)
+
+**Step 2: Instantiate the robot with solver**
+
+.. code-block:: python
+
+   robot_cfg.solver_cfg = srs_cfg
    robot = Robot(cfg=robot_cfg, entities=[], device="cpu")
 
-   # 4. Use FK/IK/Jacobian
-   qpos = [0.0, -1.57, 1.57, 0.0, 1.57, 0.0]  # 6-DOF joint angles (radians)
-   ee_pose = robot.compute_fk(qpos)  # Forward kinematics, returns 4x4 matrix
-   print("End-effector pose (FK):\n", ee_pose)
+**Step 3: Use FK/IK/Jacobian**
+
+.. code-block:: python
+
+   qpos = [0.0, -1.57, 1.57, 0.0, 1.57, 0.0]
+   ee_pose = robot.compute_fk(qpos)
 
-   import numpy as np
    target_pose = np.array([
        [0, -1, 0, 0.5],
        [1,  0, 0, 0.2],
        [0,  0, 1, 0.3],
        [0,  0, 0, 1.0]
    ])
    success, qpos_sol = robot.compute_ik(target_pose, joint_seed=qpos)
-   print("IK success:", success)
-   print("IK solution:", qpos_sol)
-
    J = robot.get_solver().get_jacobian(qpos)
-   print("Jacobian:\n", J)
+
 
 **Note**
 
@@ -88,12 +98,6 @@ API Reference
 - **set_position_limits / get_position_limits**: Set or get joint position limits.
 - **set_tcp / get_tcp**: Set or get the tool center point (TCP) transformation.
 
-**PinkSolver**
-
-- Implements all BaseSolver methods using the Pink library.
-- Supports custom task lists, solver type selection, and joint limit handling.
-- See PinkSolverCfg for all configuration options.
-
 Configuration
 ~~~~~~~~~~~~~
 
diff --git a/examples/sim/planners/motion_generator.py b/examples/sim/planners/motion_generator.py
@@ -1,3 +1,19 @@
+# ----------------------------------------------------------------------------
+# 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
diff --git a/scripts/tutorials/sim/motion_generator.py b/scripts/tutorials/sim/motion_generator.py
@@ -0,0 +1,121 @@
+# ----------------------------------------------------------------------------
+# 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.build_multiple_arenas(1)
+    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()
diff --git a/scripts/tutorials/sim/srs_solver.py b/scripts/tutorials/sim/srs_solver.py
@@ -0,0 +1,85 @@
+# ----------------------------------------------------------------------------
+# 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 numpy as np
+import torch
+
+from IPython import embed
+
+from embodichain.lab.sim.objects import Robot
+from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
+from embodichain.lab.sim.robots import DexforceW1Cfg
+
+
+def main():
+    # Set print options for better readability
+    np.set_printoptions(precision=5, suppress=True)
+    torch.set_printoptions(precision=5, sci_mode=False)
+
+    # Initialize simulation
+    sim_device = "cpu"
+    sim = SimulationManager(
+        SimulationManagerCfg(
+            headless=False, sim_device=sim_device, width=2200, height=1200
+        )
+    )
+
+    sim.build_multiple_arenas(1)
+    sim.set_manual_update(False)
+
+    robot: Robot = sim.add_robot(cfg=DexforceW1Cfg.from_dict({"uid": "dexforce_w1"}))
+    arm_name = "left_arm"
+    # Set initial joint positions for left arm
+    qpos_fk_list = [
+        torch.tensor([[0.0, 0.0, 0.0, -np.pi / 2, 0.0, 0.0, 0.0]], dtype=torch.float32),
+    ]
+    robot.set_qpos(qpos_fk_list[0], joint_ids=robot.get_joint_ids(arm_name))
+
+    time.sleep(0.5)
+
+    fk_xpos_batch = torch.cat(qpos_fk_list, dim=0)
+
+    fk_xpos_list = robot.compute_fk(qpos=fk_xpos_batch, name=arm_name, to_matrix=True)
+
+    start_time = time.time()
+    res, ik_qpos = robot.compute_ik(
+        pose=fk_xpos_list,
+        name=arm_name,
+        # joint_seed=qpos_fk_list[0],
+        return_all_solutions=True,
+    )
+    end_time = time.time()
+    print(
+        f"Batch IK computation time for {len(fk_xpos_list)} poses: {end_time - start_time:.6f} seconds"
+    )
+
+    if ik_qpos.dim() == 3:
+        first_solutions = ik_qpos[:, 0, :]
+    else:
+        first_solutions = ik_qpos
+    robot.set_qpos(first_solutions, joint_ids=robot.get_joint_ids(arm_name))
+
+    ik_xpos_list = robot.compute_fk(qpos=first_solutions, name=arm_name, to_matrix=True)
+
+    print("fk_xpos_list: ", fk_xpos_list)
+    print("ik_xpos_list: ", ik_xpos_list)
+
+    embed(header="Test SRSSolver example. Press Ctrl-D to exit.")
+
+
+if __name__ == "__main__":
+    main()
