From ac62f6e959de9ae31afd165659ed9ffefbf866ee Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 12 Feb 2026 21:21:43 +0800 Subject: [PATCH 1/4] wip --- embodichain/lab/sim/sim_manager.py | 4 +- embodichain/lab/sim/utility/gizmo_utils.py | 146 +++++++++++++++++++++ 2 files changed, 149 insertions(+), 1 deletion(-) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 1c97789a..35ae05b5 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -1130,7 +1130,7 @@ def get_robot_uid_list(self) -> List[str]: def enable_gizmo( self, uid: str, control_part: str | None = None, gizmo_cfg: object = None - ) -> None: + ) -> Gizmo: """Enable gizmo control for any simulation object (Robot, RigidObject, Camera, etc.). Args: @@ -1190,6 +1190,8 @@ def enable_gizmo( f"Failed to create gizmo for {object_type} '{uid}' with control_part '{control_part}': {e}" ) + return gizmo + def disable_gizmo(self, uid: str, control_part: str | None = None) -> None: """Disable and remove gizmo for a robot. diff --git a/embodichain/lab/sim/utility/gizmo_utils.py b/embodichain/lab/sim/utility/gizmo_utils.py index 676a93fa..395d685c 100644 --- a/embodichain/lab/sim/utility/gizmo_utils.py +++ b/embodichain/lab/sim/utility/gizmo_utils.py @@ -21,8 +21,12 @@ """ from typing import Callable +from typing import TYPE_CHECKING from dexsim.types import TransformMask +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + def create_gizmo_callback() -> Callable: """Create a standard gizmo transform callback function. @@ -44,3 +48,145 @@ def gizmo_transform_callback(node, translation, rotation, flag): node.set_rotation_rpy(rotation) return gizmo_transform_callback + + +def run_gizmo_robot_control_loop(robot: "Robot", control_part: str = "arm"): + """Run a control loop for testing gizmo controls on a robot. + + This function implements a control loop that allows users to manipulate a robot + using gizmo controls with keyboard input for additional commands. + + Args: + robot (Robot): The robot to control with the gizmo. + control_part (str, optional): The part of the robot to control. Defaults to "arm". + + Keyboard Controls: + Q/ESC: Exit the control loop + P: Print current robot state (joint positions, end-effector pose) + G: Toggle gizmo visibility + R: Reset robot to initial pose + I: Print control information + """ + import select + import sys + import tty + import termios + import time + import numpy as np + + np.set_printoptions(precision=5, suppress=True) + + from embodichain.lab.sim import SimulationManager + from embodichain.lab.sim.objects import Robot + + from embodichain.utils.logger import log_info, log_warning, log_error + + sim = SimulationManager.get_instance() + + # Enter auto-update mode. + sim.set_manual_update(False) + + # Enable gizmo for the robot + gizmo = sim.enable_gizmo(uid=robot.uid, control_part=control_part) + + # Store initial robot configuration + initial_qpos = robot.get_qpos(name=control_part) + + gizmo_visible = True + step_count = 0 + + log_info("\n=== Gizmo Robot Control ===") + log_info("Gizmo Controls:") + log_info(" Use the 3D gizmo to drag and manipulate the robot") + log_info("\nKeyboard Controls:") + log_info(" Q/ESC: Exit control loop") + log_info(" P: Print current robot state") + log_info(" G: Toggle gizmo visibility") + log_info(" R: Reset robot to initial pose") + log_info(" I: Print this information again") + + # Save terminal settings + old_settings = termios.tcgetattr(sys.stdin) + tty.setcbreak(sys.stdin.fileno()) + + def get_key(): + """Non-blocking keyboard input.""" + if select.select([sys.stdin], [], [], 0)[0]: + return sys.stdin.read(1) + return None + + last_time = time.time() + last_step = 0 + + try: + while True: + time.sleep(0.033) # ~30Hz + sim.update_gizmos() + step_count += 1 + + # Check for keyboard input + key = get_key() + + if key: + # Exit controls + if key in ["q", "Q", "\x1b"]: # Q or ESC + log_info("Exiting gizmo control loop...") + sim.disable_gizmo(uid=robot.uid, control_part=control_part) + break + + # Print robot state + elif key in ["p", "P"]: + current_qpos = robot.get_qpos(name=control_part) + eef_pose = robot.compute_fk(name=control_part, qpos=current_qpos) + log_info(f"\n=== Robot State ===") + log_info(f"Control part: {control_part}") + log_info(f"Joint positions: {current_qpos.squeeze().tolist()}") + log_info(f"End-effector pose:\n{eef_pose.squeeze().numpy()}") + + # Toggle gizmo visibility + elif key in ["g", "G"]: + if gizmo_visible: + sim.set_gizmo_visibility( + uid=robot.uid, control_part=control_part, visible=False + ) + log_info("Gizmo hidden") + gizmo_visible = False + else: + sim.set_gizmo_visibility( + uid=robot.uid, control_part=control_part, visible=True + ) + log_info("Gizmo shown") + gizmo_visible = True + + # Reset to initial pose + elif key in ["r", "R"]: + # TODO: Workaround for reset. Gizmo pose should be fixed in the future. + sim.disable_gizmo(uid=robot.uid, control_part=control_part) + robot.clear_dynamics() + robot.set_qpos(qpos=initial_qpos, name=control_part, target=False) + sim.enable_gizmo(uid=robot.uid, control_part=control_part) + log_info("Robot reset to initial pose") + + # Print info + elif key in ["i", "I"]: + log_info("\n=== Gizmo Robot Control ===") + log_info("Gizmo Controls:") + log_info(" Use the 3D gizmo to drag and manipulate the robot") + log_info("\nKeyboard Controls:") + log_info(" Q/ESC: Exit control loop") + log_info(" P: Print current robot state") + log_info(" G: Toggle gizmo visibility") + log_info(" R: Reset robot to initial pose") + log_info(" I: Print this information again") + + except KeyboardInterrupt: + sim.disable_gizmo(uid=robot.uid, control_part=control_part) + log_info("\nControl loop interrupted by user (Ctrl+C)") + + finally: + try: + # Restore terminal settings + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) + except: + pass + log_info("Gizmo control loop terminated") From 57ea75caa71e7a91360b857994c6041886366603 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 13 Feb 2026 14:32:33 +0800 Subject: [PATCH 2/4] wip --- embodichain/lab/sim/utility/gizmo_utils.py | 45 ++++++++++++++++++---- 1 file changed, 38 insertions(+), 7 deletions(-) diff --git a/embodichain/lab/sim/utility/gizmo_utils.py b/embodichain/lab/sim/utility/gizmo_utils.py index 395d685c..b2a2fafc 100644 --- a/embodichain/lab/sim/utility/gizmo_utils.py +++ b/embodichain/lab/sim/utility/gizmo_utils.py @@ -50,7 +50,9 @@ def gizmo_transform_callback(node, translation, rotation, flag): return gizmo_transform_callback -def run_gizmo_robot_control_loop(robot: "Robot", control_part: str = "arm"): +def run_gizmo_robot_control_loop( + robot: "Robot", control_part: str = "arm", end_link_name: str | None = None +): """Run a control loop for testing gizmo controls on a robot. This function implements a control loop that allows users to manipulate a robot @@ -59,6 +61,7 @@ def run_gizmo_robot_control_loop(robot: "Robot", control_part: str = "arm"): Args: robot (Robot): The robot to control with the gizmo. control_part (str, optional): The part of the robot to control. Defaults to "arm". + end_link_name (str | None, optional): The name of the end link for FK calculations. Defaults to None. Keyboard Controls: Q/ESC: Exit the control loop @@ -78,6 +81,7 @@ def run_gizmo_robot_control_loop(robot: "Robot", control_part: str = "arm"): from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.objects import Robot + from embodichain.lab.sim.solvers import PinkSolverCfg from embodichain.utils.logger import log_info, log_warning, log_error @@ -86,6 +90,23 @@ def run_gizmo_robot_control_loop(robot: "Robot", control_part: str = "arm"): # Enter auto-update mode. sim.set_manual_update(False) + # Replace robot's default solver with PinkSolver for gizmo control. + robot_solver = robot.get_solver(name=control_part) + control_part_link_names = robot.get_control_part_link_names(name=control_part) + end_link_name = ( + control_part_link_names[-1] if end_link_name is None else end_link_name + ) + pink_solver_cfg = PinkSolverCfg( + urdf_path=robot.cfg.fpath, + end_link_name=end_link_name, + root_link_name=robot_solver.root_link_name, + pos_eps=1e-2, + rot_eps=5e-2, + max_iterations=300, + dt=0.1, + ) + robot.init_solver(cfg={control_part: pink_solver_cfg}) + # Enable gizmo for the robot gizmo = sim.enable_gizmo(uid=robot.uid, control_part=control_part) @@ -93,7 +114,6 @@ def run_gizmo_robot_control_loop(robot: "Robot", control_part: str = "arm"): initial_qpos = robot.get_qpos(name=control_part) gizmo_visible = True - step_count = 0 log_info("\n=== Gizmo Robot Control ===") log_info("Gizmo Controls:") @@ -115,14 +135,10 @@ def get_key(): return sys.stdin.read(1) return None - last_time = time.time() - last_step = 0 - try: while True: time.sleep(0.033) # ~30Hz sim.update_gizmos() - step_count += 1 # Check for keyboard input key = get_key() @@ -132,6 +148,10 @@ def get_key(): if key in ["q", "Q", "\x1b"]: # Q or ESC log_info("Exiting gizmo control loop...") sim.disable_gizmo(uid=robot.uid, control_part=control_part) + if robot_solver: + robot.init_solver( + cfg={control_part: robot_solver.cfg} + ) # Restore original solver break # Print robot state @@ -143,7 +163,14 @@ def get_key(): log_info(f"Joint positions: {current_qpos.squeeze().tolist()}") log_info(f"End-effector pose:\n{eef_pose.squeeze().numpy()}") - # Toggle gizmo visibility + if eef_pose is None: + log_info( + "End-effector pose unavailable: compute_fk returned None " + f"for control part '{control_part}'." + ) + else: + eef_pose_np = eef_pose.detach().cpu().numpy().squeeze() + log_info(f"End-effector pose:\n{eef_pose_np}") elif key in ["g", "G"]: if gizmo_visible: sim.set_gizmo_visibility( @@ -181,6 +208,10 @@ def get_key(): except KeyboardInterrupt: sim.disable_gizmo(uid=robot.uid, control_part=control_part) + if robot_solver: + robot.init_solver( + cfg={control_part: robot_solver.cfg} + ) # Restore original solver log_info("\nControl loop interrupted by user (Ctrl+C)") finally: From b724029fb68151f9d347d5092ccaad97a0911a5b Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 13 Feb 2026 14:54:15 +0800 Subject: [PATCH 3/4] wip --- examples/sim/demo/open_drawer.py | 349 +++++++++++++++++++++++++++++++ 1 file changed, 349 insertions(+) create mode 100644 examples/sim/demo/open_drawer.py diff --git a/examples/sim/demo/open_drawer.py b/examples/sim/demo/open_drawer.py new file mode 100644 index 00000000..6165ef40 --- /dev/null +++ b/examples/sim/demo/open_drawer.py @@ -0,0 +1,349 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +""" +This script demonstrates the creation and simulation of UR10 robot with gripper, +and performs an open drawer task in a simulated environment. +""" + +import argparse +import os +import numpy as np +import torch +from scipy.spatial.transform import Rotation as R +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.objects import Robot, Articulation +from embodichain.lab.sim.utility.action_utils import interpolate_with_distance_warp +from embodichain.lab.sim.solvers import PytorchSolverCfg +from embodichain.data import get_data_path +from embodichain.utils import logger +from embodichain.lab.sim.cfg import ( + RobotCfg, + LightCfg, + ArticulationCfg, + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, + URDFCfg, +) +import time + + +def parse_arguments(): + """ + Parse command-line arguments to configure the simulation. + + Returns: + argparse.Namespace: Parsed arguments including rendering options and cabinet path. + """ + parser = argparse.ArgumentParser( + description="Create and simulate a robot to open drawer in SimulationManager" + ) + parser.add_argument( + "--enable_rt", action="store_true", help="Enable ray tracing rendering" + ) + parser.add_argument( + "--cabinet_path", type=str, default="", help="Path to cabinet URDF file" + ) + parser.add_argument("--headless", action="store_true", help="Enable headless mode") + return parser.parse_args() + + +def initialize_simulation(args) -> SimulationManager: + """ + Initialize the simulation environment based on the provided arguments. + + Args: + args (argparse.Namespace): Parsed command-line arguments. + + Returns: + SimulationManager: Configured simulation manager instance. + """ + config = SimulationManagerCfg( + headless=args.headless, + sim_device="cpu", + enable_rt=True, + physics_dt=1.0 / 100.0, + ) + sim = SimulationManager(config) + sim.add_light(cfg=LightCfg(uid="main_light", intensity=50.0, init_pos=(0, 0, 2.0))) + return sim + + +def create_robot(sim: SimulationManager) -> Robot: + """ + Create and configure a robot with an arm and a gripper in the simulation. + + Args: + sim (SimulationManager): The simulation manager instance. + + Returns: + Robot: The configured robot instance added to the simulation. + """ + ur10_urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf") + gripper_urdf_path = get_data_path("DH_PGI_140_80/DH_PGI_140_80.urdf") + + gripper_attach_xpos = np.eye(4) + gripper_attach_xpos[:3, :3] = R.from_rotvec([0, 0, 90], degrees=True).as_matrix() + + return sim.add_robot( + cfg=RobotCfg( + uid="UR10_with_gripper", + urdf_cfg=URDFCfg( + components=[ + {"component_type": "arm", "urdf_path": ur10_urdf_path}, + { + "component_type": "hand", + "urdf_path": gripper_urdf_path, + "transform": gripper_attach_xpos, + }, + ] + ), + control_parts={ + "arm": ["JOINT[0-9]"], + "hand": ["GRIPPER_FINGER[1-2]_JOINT_1"], + }, + drive_pros=JointDrivePropertiesCfg( + stiffness={"JOINT[0-9]": 1e4, "GRIPPER_FINGER[1-2]_JOINT_1": 1e4}, + damping={"JOINT[0-9]": 1e3, "GRIPPER_FINGER[1-2]_JOINT_1": 1e3}, + max_effort={"JOINT[0-9]": 1e5, "GRIPPER_FINGER[1-2]_JOINT_1": 1e3}, + drive_type="force", + ), + solver_cfg={ + "arm": PytorchSolverCfg( + end_link_name="ee_link", root_link_name="base_link", tcp=np.eye(4) + ) + }, + init_qpos=[0.0, -np.pi / 2, -np.pi / 2, 0.0, np.pi / 2, 0.0, 0.0, 0.0], + init_pos=[0.0, 0.0, 0.0], + ) + ) + + +def create_cabinet( + sim: SimulationManager, + cabinet_urdf_path: str, + cabinet_posi: list[float], + cabinet_rota: list[float], +) -> Articulation: + """ + Create a cabinet articulated object in the simulation. + + Args: + sim (SimulationManager): The simulation manager instance. + cabinet_path (str, optional): Path to cabinet URDF file. If None, uses default path. + + Returns: + Articulation: The cabinet object added to the simulation. + """ + cabinet = sim.add_articulation( + cfg=ArticulationCfg( + uid="cabinet", + fpath=cabinet_urdf_path, + init_pos=cabinet_posi, + init_rot=cabinet_rota, + init_qpos=[0, 0, 0], + drive_pros=JointDrivePropertiesCfg( + stiffness=1e-2, damping=1e-1, max_effort=1e-1, drive_type="force" + ), + fix_base=True, + body_scale=[0.75, 0.75, 0.75], + ) + ) + + return cabinet + + +def open_drawer( + sim: SimulationManager, + robot: Robot, + cabinet: Articulation, + grasp_relative_posi: list[float], +): + """ + Execute the trajectory to drive the robot to open the cabinet drawer. + + Args: + sim (SimulationManager): The simulation manager instance. + robot (Robot): The robot instance. + cabinet (Articulation): The cabinet object. + """ + arm_ids = robot.get_joint_ids("arm") + gripper_ids = robot.get_joint_ids("hand") + arm_start_qpos = robot.get_qpos()[:, arm_ids] + + joint_names = cabinet.joint_names + drawer_joint_id = next( + (i for i, name in enumerate(joint_names) if "drawer" in name.lower()), + len(joint_names) - 1, + ) + + if drawer_joint_id is not None: + cabinet_qpos = cabinet.get_qpos() + cabinet_qpos[:, drawer_joint_id] = 0.0 + cabinet.set_qpos(cabinet_qpos, target=False) + sim.update(step=20) + + cabinet_position = cabinet.get_local_pose(to_matrix=True)[:, :3, 3] + arm_start_xpos = robot.compute_fk(arm_start_qpos, name="arm", to_matrix=True) + + # grasp drawer handle waypoint generation + grasp_xpos = arm_start_xpos.clone() + grasp_posi = cabinet_position + torch.tensor(grasp_relative_posi, device=sim.device) + grasp_xpos[:, :3, 3] = grasp_posi + approach_xpos = grasp_xpos.clone() + approach_xpos[:, 0, 3] -= 0.3 + pull_xpos = grasp_xpos.clone() + pull_xpos[:, 0, 3] -= 0.5 + open_qpos = torch.tensor( + [ + [0.0, 0.0], + ], + device=sim.device, + ) + close_qpos = torch.tensor( + [ + [0.08, 0.08], + ], + device=sim.device, + ) + + # compute ik for each waypoint + _, approach_qpos = robot.compute_ik( + approach_xpos, joint_seed=arm_start_qpos, name="arm" + ) + _, grasp_qpos = robot.compute_ik(grasp_xpos, joint_seed=approach_qpos, name="arm") + _, pull_qpos = robot.compute_ik(pull_xpos, joint_seed=grasp_qpos, name="arm") + + arm_traj = torch.concatenate( + [ + arm_start_qpos, + approach_qpos, + grasp_qpos, + grasp_qpos, + approach_qpos, + approach_qpos, + pull_qpos, + ] + ) + + hand_traj = torch.concatenate( + [ + open_qpos, + open_qpos, + open_qpos, + close_qpos, + close_qpos, + open_qpos, + open_qpos, + ] + ) + + all_trajectory = torch.hstack([arm_traj, hand_traj]) + + n_interp = 400 + interp_trajectory = interpolate_with_distance_warp( + trajectory=all_trajectory[None, :, :], + interp_num=n_interp, + device=sim.device, + ) + # from IPython import embed; embed() + for i in range(n_interp): + robot.set_qpos(interp_trajectory[:, i, :]), + sim.update(step=5) + time.sleep(0.01) + + from IPython import embed + + embed() + + +def move_circular(robot: Robot, sim: SimulationManager): + + qpos_start = robot.get_qpos() + qpos_end = qpos_start.clone() + qpos_end[:, 0] -= 0.5 + + all_trajectory = torch.concatenate([qpos_start, qpos_end]) + + n_interp = 400 + interp_trajectory = interpolate_with_distance_warp( + trajectory=all_trajectory[None, :, :], + interp_num=n_interp, + device=sim.device, + ) + # from IPython import embed; embed() + for i in range(n_interp): + robot.set_qpos(interp_trajectory[:, i, :]), + sim.update(step=5) + time.sleep(0.01) + + +if __name__ == "__main__": + + # table 0 + cabinet_posi = [1.4, 0.0, 0.7] + cabinet_rpy = [90, -93, 0] + grasp_relative_posi = [-0.59, 0.00, -0.015] + cabinet_path = ( + "/home/dex/Downloads/demo_assets/cabinet2/obj.urdf" + ) + + # # table 1 + # cabinet_posi = [1.55, 0.2, 0.55] + # cabinet_rpy = [90, -90, 0] + # grasp_relative_posi = [-0.68, -0.6, 0.055] + # cabinet_path = ( + # "/home/dex/Downloads/demo_assets/table/obj.urdf" + # ) + + # # # table 2 + # cabinet_posi = [1.6, -0.2, 0.55] + # cabinet_rpy = [90, -90, 0] + # grasp_relative_posi = [-0.65, 0.0, 0.12] + # cabinet_path = ( + # "/home/dex/Downloads/demo_assets/cabinet/obj.urdf" + # ) + + # cabinet_posi = [1.4, -0.2, 0.85] + # cabinet_rpy = [90, -65, 0] + # grasp_relative_posi = [-0.65, 0.0, 0.12] + # cabinet_path = "/home/dex/Downloads/demo_assets/drawer/obj.urdf" + + args = parse_arguments() + sim = initialize_simulation(args) + robot = create_robot(sim) + cabinet = create_cabinet(sim, cabinet_path, cabinet_posi, cabinet_rpy) + # cabinet._entities[0].create_physical_visible_node(np.array([0.9, 0.1, 0.1, 0.9])) + + if not args.headless: + sim.open_window() + + # cabinet.set_qpos(torch.tensor([[0.3, 0]])) + + # sim.update(step=5000) + from IPython import embed + + embed() + # move_circular(robot, sim) + sim.update(step=100) + open_drawer(sim, robot, cabinet, grasp_relative_posi) + + logger.log_info("\n Press Ctrl+C to exit simulation loop.") + try: + while True: + sim.update(step=10) + except KeyboardInterrupt: + logger.log_info("\n Exit") From b75065bfcb2a0e6db872635d6b4ba8feb3df97e9 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Fri, 13 Feb 2026 14:55:25 +0800 Subject: [PATCH 4/4] wip --- examples/sim/demo/open_drawer.py | 349 ------------------------------- 1 file changed, 349 deletions(-) delete mode 100644 examples/sim/demo/open_drawer.py diff --git a/examples/sim/demo/open_drawer.py b/examples/sim/demo/open_drawer.py deleted file mode 100644 index 6165ef40..00000000 --- a/examples/sim/demo/open_drawer.py +++ /dev/null @@ -1,349 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -""" -This script demonstrates the creation and simulation of UR10 robot with gripper, -and performs an open drawer task in a simulated environment. -""" - -import argparse -import os -import numpy as np -import torch -from scipy.spatial.transform import Rotation as R -from embodichain.lab.sim import SimulationManager, SimulationManagerCfg -from embodichain.lab.sim.objects import Robot, Articulation -from embodichain.lab.sim.utility.action_utils import interpolate_with_distance_warp -from embodichain.lab.sim.solvers import PytorchSolverCfg -from embodichain.data import get_data_path -from embodichain.utils import logger -from embodichain.lab.sim.cfg import ( - RobotCfg, - LightCfg, - ArticulationCfg, - JointDrivePropertiesCfg, - RigidBodyAttributesCfg, - URDFCfg, -) -import time - - -def parse_arguments(): - """ - Parse command-line arguments to configure the simulation. - - Returns: - argparse.Namespace: Parsed arguments including rendering options and cabinet path. - """ - parser = argparse.ArgumentParser( - description="Create and simulate a robot to open drawer in SimulationManager" - ) - parser.add_argument( - "--enable_rt", action="store_true", help="Enable ray tracing rendering" - ) - parser.add_argument( - "--cabinet_path", type=str, default="", help="Path to cabinet URDF file" - ) - parser.add_argument("--headless", action="store_true", help="Enable headless mode") - return parser.parse_args() - - -def initialize_simulation(args) -> SimulationManager: - """ - Initialize the simulation environment based on the provided arguments. - - Args: - args (argparse.Namespace): Parsed command-line arguments. - - Returns: - SimulationManager: Configured simulation manager instance. - """ - config = SimulationManagerCfg( - headless=args.headless, - sim_device="cpu", - enable_rt=True, - physics_dt=1.0 / 100.0, - ) - sim = SimulationManager(config) - sim.add_light(cfg=LightCfg(uid="main_light", intensity=50.0, init_pos=(0, 0, 2.0))) - return sim - - -def create_robot(sim: SimulationManager) -> Robot: - """ - Create and configure a robot with an arm and a gripper in the simulation. - - Args: - sim (SimulationManager): The simulation manager instance. - - Returns: - Robot: The configured robot instance added to the simulation. - """ - ur10_urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf") - gripper_urdf_path = get_data_path("DH_PGI_140_80/DH_PGI_140_80.urdf") - - gripper_attach_xpos = np.eye(4) - gripper_attach_xpos[:3, :3] = R.from_rotvec([0, 0, 90], degrees=True).as_matrix() - - return sim.add_robot( - cfg=RobotCfg( - uid="UR10_with_gripper", - urdf_cfg=URDFCfg( - components=[ - {"component_type": "arm", "urdf_path": ur10_urdf_path}, - { - "component_type": "hand", - "urdf_path": gripper_urdf_path, - "transform": gripper_attach_xpos, - }, - ] - ), - control_parts={ - "arm": ["JOINT[0-9]"], - "hand": ["GRIPPER_FINGER[1-2]_JOINT_1"], - }, - drive_pros=JointDrivePropertiesCfg( - stiffness={"JOINT[0-9]": 1e4, "GRIPPER_FINGER[1-2]_JOINT_1": 1e4}, - damping={"JOINT[0-9]": 1e3, "GRIPPER_FINGER[1-2]_JOINT_1": 1e3}, - max_effort={"JOINT[0-9]": 1e5, "GRIPPER_FINGER[1-2]_JOINT_1": 1e3}, - drive_type="force", - ), - solver_cfg={ - "arm": PytorchSolverCfg( - end_link_name="ee_link", root_link_name="base_link", tcp=np.eye(4) - ) - }, - init_qpos=[0.0, -np.pi / 2, -np.pi / 2, 0.0, np.pi / 2, 0.0, 0.0, 0.0], - init_pos=[0.0, 0.0, 0.0], - ) - ) - - -def create_cabinet( - sim: SimulationManager, - cabinet_urdf_path: str, - cabinet_posi: list[float], - cabinet_rota: list[float], -) -> Articulation: - """ - Create a cabinet articulated object in the simulation. - - Args: - sim (SimulationManager): The simulation manager instance. - cabinet_path (str, optional): Path to cabinet URDF file. If None, uses default path. - - Returns: - Articulation: The cabinet object added to the simulation. - """ - cabinet = sim.add_articulation( - cfg=ArticulationCfg( - uid="cabinet", - fpath=cabinet_urdf_path, - init_pos=cabinet_posi, - init_rot=cabinet_rota, - init_qpos=[0, 0, 0], - drive_pros=JointDrivePropertiesCfg( - stiffness=1e-2, damping=1e-1, max_effort=1e-1, drive_type="force" - ), - fix_base=True, - body_scale=[0.75, 0.75, 0.75], - ) - ) - - return cabinet - - -def open_drawer( - sim: SimulationManager, - robot: Robot, - cabinet: Articulation, - grasp_relative_posi: list[float], -): - """ - Execute the trajectory to drive the robot to open the cabinet drawer. - - Args: - sim (SimulationManager): The simulation manager instance. - robot (Robot): The robot instance. - cabinet (Articulation): The cabinet object. - """ - arm_ids = robot.get_joint_ids("arm") - gripper_ids = robot.get_joint_ids("hand") - arm_start_qpos = robot.get_qpos()[:, arm_ids] - - joint_names = cabinet.joint_names - drawer_joint_id = next( - (i for i, name in enumerate(joint_names) if "drawer" in name.lower()), - len(joint_names) - 1, - ) - - if drawer_joint_id is not None: - cabinet_qpos = cabinet.get_qpos() - cabinet_qpos[:, drawer_joint_id] = 0.0 - cabinet.set_qpos(cabinet_qpos, target=False) - sim.update(step=20) - - cabinet_position = cabinet.get_local_pose(to_matrix=True)[:, :3, 3] - arm_start_xpos = robot.compute_fk(arm_start_qpos, name="arm", to_matrix=True) - - # grasp drawer handle waypoint generation - grasp_xpos = arm_start_xpos.clone() - grasp_posi = cabinet_position + torch.tensor(grasp_relative_posi, device=sim.device) - grasp_xpos[:, :3, 3] = grasp_posi - approach_xpos = grasp_xpos.clone() - approach_xpos[:, 0, 3] -= 0.3 - pull_xpos = grasp_xpos.clone() - pull_xpos[:, 0, 3] -= 0.5 - open_qpos = torch.tensor( - [ - [0.0, 0.0], - ], - device=sim.device, - ) - close_qpos = torch.tensor( - [ - [0.08, 0.08], - ], - device=sim.device, - ) - - # compute ik for each waypoint - _, approach_qpos = robot.compute_ik( - approach_xpos, joint_seed=arm_start_qpos, name="arm" - ) - _, grasp_qpos = robot.compute_ik(grasp_xpos, joint_seed=approach_qpos, name="arm") - _, pull_qpos = robot.compute_ik(pull_xpos, joint_seed=grasp_qpos, name="arm") - - arm_traj = torch.concatenate( - [ - arm_start_qpos, - approach_qpos, - grasp_qpos, - grasp_qpos, - approach_qpos, - approach_qpos, - pull_qpos, - ] - ) - - hand_traj = torch.concatenate( - [ - open_qpos, - open_qpos, - open_qpos, - close_qpos, - close_qpos, - open_qpos, - open_qpos, - ] - ) - - all_trajectory = torch.hstack([arm_traj, hand_traj]) - - n_interp = 400 - interp_trajectory = interpolate_with_distance_warp( - trajectory=all_trajectory[None, :, :], - interp_num=n_interp, - device=sim.device, - ) - # from IPython import embed; embed() - for i in range(n_interp): - robot.set_qpos(interp_trajectory[:, i, :]), - sim.update(step=5) - time.sleep(0.01) - - from IPython import embed - - embed() - - -def move_circular(robot: Robot, sim: SimulationManager): - - qpos_start = robot.get_qpos() - qpos_end = qpos_start.clone() - qpos_end[:, 0] -= 0.5 - - all_trajectory = torch.concatenate([qpos_start, qpos_end]) - - n_interp = 400 - interp_trajectory = interpolate_with_distance_warp( - trajectory=all_trajectory[None, :, :], - interp_num=n_interp, - device=sim.device, - ) - # from IPython import embed; embed() - for i in range(n_interp): - robot.set_qpos(interp_trajectory[:, i, :]), - sim.update(step=5) - time.sleep(0.01) - - -if __name__ == "__main__": - - # table 0 - cabinet_posi = [1.4, 0.0, 0.7] - cabinet_rpy = [90, -93, 0] - grasp_relative_posi = [-0.59, 0.00, -0.015] - cabinet_path = ( - "/home/dex/Downloads/demo_assets/cabinet2/obj.urdf" - ) - - # # table 1 - # cabinet_posi = [1.55, 0.2, 0.55] - # cabinet_rpy = [90, -90, 0] - # grasp_relative_posi = [-0.68, -0.6, 0.055] - # cabinet_path = ( - # "/home/dex/Downloads/demo_assets/table/obj.urdf" - # ) - - # # # table 2 - # cabinet_posi = [1.6, -0.2, 0.55] - # cabinet_rpy = [90, -90, 0] - # grasp_relative_posi = [-0.65, 0.0, 0.12] - # cabinet_path = ( - # "/home/dex/Downloads/demo_assets/cabinet/obj.urdf" - # ) - - # cabinet_posi = [1.4, -0.2, 0.85] - # cabinet_rpy = [90, -65, 0] - # grasp_relative_posi = [-0.65, 0.0, 0.12] - # cabinet_path = "/home/dex/Downloads/demo_assets/drawer/obj.urdf" - - args = parse_arguments() - sim = initialize_simulation(args) - robot = create_robot(sim) - cabinet = create_cabinet(sim, cabinet_path, cabinet_posi, cabinet_rpy) - # cabinet._entities[0].create_physical_visible_node(np.array([0.9, 0.1, 0.1, 0.9])) - - if not args.headless: - sim.open_window() - - # cabinet.set_qpos(torch.tensor([[0.3, 0]])) - - # sim.update(step=5000) - from IPython import embed - - embed() - # move_circular(robot, sim) - sim.update(step=100) - open_drawer(sim, robot, cabinet, grasp_relative_posi) - - logger.log_info("\n Press Ctrl+C to exit simulation loop.") - try: - while True: - sim.update(step=10) - except KeyboardInterrupt: - logger.log_info("\n Exit")