diff --git a/docs/source/tutorial/motion_gen.rst b/docs/source/tutorial/motion_gen.rst index 712a314..b8d7ee5 100644 --- 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 index dd190f0..b3c9580 100644 --- 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,17 +47,21 @@ 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], @@ -56,11 +69,8 @@ Example: Using PinkSolver [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 index 4557f6d..d767aba 100644 --- 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 new file mode 100644 index 0000000..c09a1ed --- /dev/null +++ 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 new file mode 100644 index 0000000..5d9922b --- /dev/null +++ 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()