From 39e529b72eab7e1ecafa83f3c083f180d29b0c86 Mon Sep 17 00:00:00 2001 From: matafela Date: Tue, 16 Jun 2026 20:33:57 +0800 Subject: [PATCH 1/3] fix gizmo --- embodichain/lab/sim/objects/gizmo.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/embodichain/lab/sim/objects/gizmo.py b/embodichain/lab/sim/objects/gizmo.py index dc7fea00..ab6e7b4f 100644 --- a/embodichain/lab/sim/objects/gizmo.py +++ b/embodichain/lab/sim/objects/gizmo.py @@ -328,8 +328,15 @@ def _update_robot_ik(self, target_transform: torch.Tensor): return False # Solve IK + base_pose = self.target.get_link_pose( + link_name=solver.root_link_name, to_matrix=True + ) + target_transform_root = torch.bmm( + torch.inverse(base_pose), target_transform + ) + ik_success, new_qpos = solver.get_ik( - target_xpos=target_transform, joint_seed=joint_seed + target_xpos=target_transform_root, joint_seed=joint_seed ) if ik_success: From c852babe7d69f2b1c6a4f4102cf83cb9f7051e82 Mon Sep 17 00:00:00 2001 From: matafela Date: Wed, 17 Jun 2026 11:05:38 +0800 Subject: [PATCH 2/3] fix --- embodichain/lab/sim/objects/gizmo.py | 3 +- embodichain/lab/sim/solvers/base_solver.py | 8 ++--- examples/sim/gizmo/gizmo_robot.py | 37 +++++++++++++++------- 3 files changed, 30 insertions(+), 18 deletions(-) diff --git a/embodichain/lab/sim/objects/gizmo.py b/embodichain/lab/sim/objects/gizmo.py index ab6e7b4f..0121407d 100644 --- a/embodichain/lab/sim/objects/gizmo.py +++ b/embodichain/lab/sim/objects/gizmo.py @@ -334,9 +334,8 @@ def _update_robot_ik(self, target_transform: torch.Tensor): target_transform_root = torch.bmm( torch.inverse(base_pose), target_transform ) - ik_success, new_qpos = solver.get_ik( - target_xpos=target_transform_root, joint_seed=joint_seed + target_xpos=target_transform_root, qpos_seed=joint_seed ) if ik_success: diff --git a/embodichain/lab/sim/solvers/base_solver.py b/embodichain/lab/sim/solvers/base_solver.py index ed69d9c5..8640ce02 100644 --- a/embodichain/lab/sim/solvers/base_solver.py +++ b/embodichain/lab/sim/solvers/base_solver.py @@ -397,19 +397,19 @@ def get_tcp(self) -> np.ndarray: def get_ik( self, target_pose: torch.Tensor, - joint_seed: torch.Tensor | None = None, + qpos_seed: torch.Tensor | None = None, num_samples: int | None = None, **kwargs, ) -> Tuple[torch.Tensor, torch.Tensor]: r"""Computes the inverse kinematics for a given target pose. This method generates random joint configurations within the specified limits, - including the provided joint_seed, and attempts to find valid inverse kinematics solutions. - It then identifies the joint position that is closest to the joint_seed. + including the provided qpos_seed, and attempts to find valid inverse kinematics solutions. + It then identifies the joint position that is closest to the qpos_seed. Args: target_pose (torch.Tensor): The target pose represented as a 4x4 transformation matrix. - joint_seed (torch.Tensor | None): The initial joint positions used as a seed. + qpos_seed (torch.Tensor | None): The initial joint positions used as a seed. num_samples (int | None): The number of random joint seeds to generate. **kwargs: Additional keyword arguments for customization. diff --git a/examples/sim/gizmo/gizmo_robot.py b/examples/sim/gizmo/gizmo_robot.py index 40f0d0c1..6b8b9eff 100644 --- a/examples/sim/gizmo/gizmo_robot.py +++ b/examples/sim/gizmo/gizmo_robot.py @@ -23,6 +23,7 @@ import argparse from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.solvers import PytorchSolverCfg from embodichain.lab.sim.cfg import ( RenderCfg, RobotCfg, @@ -58,36 +59,48 @@ def main(): sim.set_manual_update(False) # Get UR10 URDF path - urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf") + ur10_urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf") + gripper_urdf_path = get_data_path("DH_PGC_140_50_M/DH_PGC_140_50_M.urdf") # Create UR10 robot robot_cfg = RobotCfg( uid="ur10_gizmo_test", urdf_cfg=URDFCfg( - components=[{"component_type": "arm", "urdf_path": urdf_path}] + components=[ + {"component_type": "arm", "urdf_path": ur10_urdf_path}, + {"component_type": "hand", "urdf_path": gripper_urdf_path}, + ] ), - control_parts={"arm": ["Joint[1-6]"]}, + control_parts={ + "arm": ["JOINT[0-9]"], + "hand": ["FINGER[1-2]"], + }, solver_cfg={ - "arm": PinkSolverCfg( - urdf_path=urdf_path, + "arm": PytorchSolverCfg( end_link_name="ee_link", root_link_name="base_link", - pos_eps=1e-2, - rot_eps=5e-2, - max_iterations=300, - dt=0.1, + tcp=[ + [0.0, 1.0, 0.0, 0.0], + [-1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.12], + [0.0, 0.0, 0.0, 1.0], + ], + num_samples=30, ) }, drive_pros=JointDrivePropertiesCfg( - stiffness={"Joint[1-6]": 1e4}, - damping={"Joint[1-6]": 1e3}, + stiffness={"JOINT[0-9]": 1e4, "FINGER[1-2]": 1e2}, + damping={"JOINT[0-9]": 1e3, "FINGER[1-2]": 1e1}, + max_effort={"JOINT[0-9]": 1e5, "FINGER[1-2]": 1e3}, + drive_type="force", ), + init_qpos=[0.0, -np.pi / 2, -np.pi / 2, np.pi / 2, -np.pi / 2, 0.0, 0.0, 0.0], ) robot = sim.add_robot(cfg=robot_cfg) # Set initial joint positions initial_qpos = torch.tensor( - [[0, -np.pi / 2, np.pi / 2, 0.0, np.pi / 2, 0.0]], + [[0.0, -np.pi / 2, -np.pi / 2, np.pi / 2, -np.pi / 2, 0.0]], dtype=torch.float32, device="cpu", ) From 46d14c2bb68a3ea4029c7f4f253c1a4c426937a5 Mon Sep 17 00:00:00 2001 From: matafela Date: Wed, 17 Jun 2026 11:23:08 +0800 Subject: [PATCH 3/3] update --- embodichain/lab/sim/objects/gizmo.py | 16 +---- examples/sim/gizmo/gizmo_w1.py | 101 +++++++++++++++++---------- 2 files changed, 66 insertions(+), 51 deletions(-) diff --git a/embodichain/lab/sim/objects/gizmo.py b/embodichain/lab/sim/objects/gizmo.py index 0121407d..9fb370c8 100644 --- a/embodichain/lab/sim/objects/gizmo.py +++ b/embodichain/lab/sim/objects/gizmo.py @@ -301,12 +301,6 @@ def _setup_robot_gizmo(self): def _update_robot_ik(self, target_transform: torch.Tensor): """Update robot joints using IK to reach target transform""" try: - # Get robot solver for the arm - solver = self.target.get_solver(self._robot_arm_name) - if solver is None: - logger.log_warning(f"No solver found for arm: {self._robot_arm_name}") - return False - # Get current joint positions as seed using proprioception proprioception = self.target.get_proprioception() current_qpos_full = proprioception["qpos"] # Full joint positions @@ -328,14 +322,8 @@ def _update_robot_ik(self, target_transform: torch.Tensor): return False # Solve IK - base_pose = self.target.get_link_pose( - link_name=solver.root_link_name, to_matrix=True - ) - target_transform_root = torch.bmm( - torch.inverse(base_pose), target_transform - ) - ik_success, new_qpos = solver.get_ik( - target_xpos=target_transform_root, qpos_seed=joint_seed + ik_success, new_qpos = self.target.compute_ik( + pose=target_transform, name=self._robot_arm_name, joint_seed=joint_seed ) if ik_success: diff --git a/examples/sim/gizmo/gizmo_w1.py b/examples/sim/gizmo/gizmo_w1.py index 09779c84..6c06c467 100644 --- a/examples/sim/gizmo/gizmo_w1.py +++ b/examples/sim/gizmo/gizmo_w1.py @@ -33,6 +33,7 @@ from embodichain.lab.sim.solvers import PinkSolverCfg from embodichain.data import get_data_path from embodichain.utils import logger +from embodichain.lab.sim.robots.dexforce_w1.cfg import DexforceW1Cfg def main(): @@ -58,45 +59,71 @@ def main(): sim = SimulationManager(sim_cfg) sim.set_manual_update(False) - # Get DexForce W1 URDF path - urdf_path = get_data_path( - "DexforceW1V021_INDUSTRIAL_DH_PGC_GRIPPER_M/DexforceW1V021.urdf" + cfg = DexforceW1Cfg.from_dict( + { + "uid": "w1_gizmo_test", + } ) - - # Create DexForce W1 robot - robot_cfg = RobotCfg( - uid="w1_gizmo_test", - urdf_cfg=URDFCfg( - components=[{"component_type": "humanoid", "urdf_path": urdf_path}] - ), - control_parts={"left_arm": ["LEFT_J[1-7]"], "right_arm": ["RIGHT_J[1-7]"]}, - solver_cfg={ - "left_arm": PinkSolverCfg( - urdf_path=urdf_path, - end_link_name="left_ee", - root_link_name="left_arm_base", - pos_eps=1e-2, - rot_eps=5e-2, - max_iterations=300, - dt=0.1, - ), - "right_arm": PinkSolverCfg( - urdf_path=urdf_path, - end_link_name="right_ee", - root_link_name="right_arm_base", - pos_eps=1e-2, - rot_eps=5e-2, - max_iterations=300, - dt=0.1, - ), - }, - drive_pros=JointDrivePropertiesCfg( - stiffness={"(LEFT|RIGHT)_J[1-7]": 1e4, "(ANKLE|KNEE|BUTTOCK|WAIST)": 1e7}, - damping={"(LEFT|RIGHT)_J[1-7]": 1e3, "(ANKLE|KNEE|BUTTOCK|WAIST)": 1e4}, - max_effort={"(LEFT|RIGHT)_J[1-7]": 1e5, "(ANKLE|KNEE|BUTTOCK|WAIST)": 1e10}, - ), + cfg.solver_cfg["left_arm"].tcp = np.array( + [ + [1.0, 0.0, 0.0, 0.012], + [0.0, 1.0, 0.0, 0.04], + [0.0, 0.0, 1.0, 0.11], + [0.0, 0.0, 0.0, 1.0], + ] ) - robot = sim.add_robot(cfg=robot_cfg) + cfg.solver_cfg["right_arm"].tcp = np.array( + [ + [1.0, 0.0, 0.0, 0.012], + [0.0, 1.0, 0.0, -0.04], + [0.0, 0.0, 1.0, 0.11], + [0.0, 0.0, 0.0, 1.0], + ] + ) + + cfg.init_qpos = [ + 1.0000e00, + -2.0000e00, + 1.0000e00, + 0.0000e00, + -2.6921e-05, + -2.6514e-03, + -1.5708e00, + 1.4575e00, + -7.8540e-01, + 1.2834e-01, + 1.5708e00, + -2.2310e00, + -7.8540e-01, + 1.4461e00, + -1.5708e00, + 1.6716e00, + 7.8540e-01, + 7.6745e-01, + 0.0000e00, + 3.8108e-01, + 0.0000e00, + 0.0000e00, + 0.0000e00, + 0.0000e00, + 1.5000e00, + 0.0000e00, + 0.0000e00, + 0.0000e00, + 0.0000e00, + 1.5000e00, + 6.9974e-02, + 7.3950e-02, + 6.6574e-02, + 6.0923e-02, + 0.0000e00, + 6.7342e-02, + 7.0862e-02, + 6.3684e-02, + 5.7822e-02, + 0.0000e00, + ] + robot = sim.add_robot(cfg=cfg) # Set initial joint positions for both arms # Left arm: 8 joints (WAIST + 7 LEFT_J), Right arm: 8 joints (WAIST + 7 RIGHT_J)