Fix gizmo ik bug

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,8 +322,8 @@ def _update_robot_ik(self, target_transform: torch.Tensor):
                 return False
 
             # Solve IK
-            ik_success, new_qpos = solver.get_ik(
-                target_xpos=target_transform, joint_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:

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.
 

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",
     )

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)