mani-skill-nightly 2025.8.19.1758__py3-none-any.whl → 2025.8.19.2202__py3-none-any.whl

mani_skill/agents/controllers/pd_ee_pose.py

@@ -1,4 +1,4 @@
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from typing import Literal, Optional, Sequence, Union
 
 import numpy as np
@@ -9,9 +9,11 @@ from mani_skill.agents.controllers.utils.kinematics import Kinematics
 from mani_skill.utils import gym_utils, sapien_utils
 from mani_skill.utils.geometry.rotation_conversions import (
     euler_angles_to_matrix,
+    matrix_to_euler_angles,
     matrix_to_quaternion,
     quaternion_apply,
     quaternion_multiply,
+    quaternion_to_matrix,
 )
 from mani_skill.utils.structs import Pose
 from mani_skill.utils.structs.types import Array, DriveMode
@@ -72,13 +74,14 @@ class PDEEPosController(PDJointPosController):
 
     def reset(self):
         super().reset()
-        if self._target_pose is None:
-            self._target_pose = self.ee_pose_at_base
-        else:
-            # TODO (stao): this is a strange way to mask setting individual batched pose parts
-            self._target_pose.raw_pose[self.scene._reset_mask] = (
-                self.ee_pose_at_base.raw_pose[self.scene._reset_mask]
-            )
+        if self.config.use_target:
+            if self._target_pose is None:
+                self._target_pose = self.ee_pose_at_base
+            else:
+                # TODO (stao): this is a strange way to mask setting individual batched pose parts
+                self._target_pose.raw_pose[
+                    self.scene._reset_mask
+                ] = self.ee_pose_at_base.raw_pose[self.scene._reset_mask]
 
     def compute_target_pose(self, prev_ee_pose_at_base, action):
         # Keep the current rotation and change the position
@@ -105,14 +108,22 @@ class PDEEPosController(PDJointPosController):
         else:
             prev_ee_pose_at_base = self.ee_pose_at_base
 
-        self._target_pose = self.compute_target_pose(prev_ee_pose_at_base, action)
+        # we only need to use the target pose for CPU sim or if a virtual target is enabled
+        # if we have no virtual target and using the gpu sim we can directly use the given action without
+        # having to recompute the new target pose based on the action delta.
+        ik_via_target_pose = self.config.use_target or not self.scene.gpu_sim_enabled
+        if ik_via_target_pose:
+            self._target_pose = self.compute_target_pose(prev_ee_pose_at_base, action)
         pos_only = type(self.config) == PDEEPosControllerConfig
+        if pos_only:
+            action = torch.hstack([action, torch.zeros(action.shape[0], 3, device=self.device)])
+
         self._target_qpos = self.kinematics.compute_ik(
-            self._target_pose,
-            self.articulation.get_qpos(),
-            pos_only=pos_only,
-            action=action,
-            use_delta_ik_solver=self.config.use_delta and not self.config.use_target,
+            pose=self._target_pose if ik_via_target_pose else action,
+            q0=self.articulation.get_qpos(),
+            is_delta_pose=not ik_via_target_pose and self.config.use_delta,
+            current_pose=self.ee_pose_at_base,
+            solver_config=self.config.delta_solver_config,
         )
         if self._target_qpos is None:
             self._target_qpos = self._start_qpos
@@ -175,6 +186,10 @@ class PDEEPosControllerConfig(ControllerConfig):
     interpolate: bool = False
     normalize_action: bool = True
     """Whether to normalize each action dimension into a range of [-1, 1]. Normally for most machine learning workflows this is recommended to be kept true."""
+    delta_solver_config: dict = field(
+        default_factory=lambda: dict(type="levenberg_marquardt", alpha=1.0)
+    )
+    """Configuration for the delta IK solver. Default is `dict(type="levenberg_marquardt", alpha=1.0)`. type can be one of "levenberg_marquardt" or "pseudo_inverse". alpha is a scaling term applied to the delta joint positions generated by the solver. Generally levenberg_marquardt is faster and more accurate than pseudo_inverse and is the recommended option, see https://github.com/haosulab/ManiSkill/issues/955#issuecomment-2742253342 for some analysis on performance."""
     drive_mode: Union[Sequence[DriveMode], DriveMode] = "force"
     controller_cls = PDEEPosController
 

mani_skill/agents/controllers/utils/kinematics.py

@@ -4,7 +4,9 @@ Code for kinematics utilities on CPU/GPU
 
 from contextlib import contextmanager, redirect_stderr, redirect_stdout
 from os import devnull
-from typing import List
+from typing import List, Optional, Union
+
+from mani_skill.utils.geometry import rotation_conversions
 
 try:
     import pytorch_kinematics as pk
@@ -182,62 +184,69 @@ class Kinematics:
 
     def compute_ik(
         self,
-        target_pose: Pose,
+        pose: Union[Pose, torch.Tensor],
         q0: torch.Tensor,
-        pos_only: bool = False,
-        action=None,
-        use_delta_ik_solver: bool = False,
+        is_delta_pose: bool = False,
+        current_pose: Optional[Pose] = None,
+        solver_config: dict = dict(type="levenberg_marquardt", solver_iterations=1, alpha=1.0),
     ):
-        """Given a target pose, via inverse kinematics compute the target joint positions that will achieve the target pose
+        """Given a target pose, initial joint positions, this computes the target joint positions that will achieve the target pose.
+        For optimization you can also provide the delta pose instead and specify is_delta_pose=True.
 
         Args:
-            target_pose (Pose): target pose of the end effector in the world frame. note this is not relative to the robot base frame!
-            q0 (torch.Tensor): initial joint positions of every active joint in the articulation
-            pos_only (bool): if True, only the position of the end link is considered in the IK computation
-            action (torch.Tensor): delta action to be applied to the articulation. Used for fast delta IK solutions on the GPU.
-            use_delta_ik_solver (bool): If true, returns the target joint positions that correspond with a delta IK solution. This is specifically
-                used for GPU simulation to determine which GPU IK algorithm to use.
+            pose (Pose): target pose in the world frame. Note this is not relative to the robot base frame!
+            q0 (torch.Tensor): initial joint positions of each active joint in the articulation. Note that this function will mask out the joints that are not kinematically relevant.
+            is_delta_pose (bool): if True, the `pose` parameter should be a delta pose. It can also be a tensor of shape (N, 6) with the first 3 channels for translation and the last 3 channels for rotation in the euler angle format.
+            current_pose (Optional[Pose]): current pose of the controlled link in the world frame. This is used to optimize the function by avoiding computing the current pose from q0 to compute the delta pose. If is_delta_pose is False, this is not used.
+            solver_config (dict): configuration for the IK solver. Default is `dict(type="levenberg_marquardt", alpha=1.0)`. type can be one of "levenberg_marquardt" or "pseudo_inverse". alpha is a scaling term applied to the delta joint positions generated by the solver.
         """
+        assert isinstance(pose, Pose) or pose.shape[1] == 6, "pose must be a Pose or a tensor with shape (N, 6)"
         if self.use_gpu_ik:
+            B = pose.shape[0]
             q0 = q0[:, self.active_ancestor_joint_idxs]
-            if not use_delta_ik_solver:
-                tf = pk.Transform3d(
-                    pos=target_pose.p,
-                    rot=target_pose.q,
-                    device=self.device,
-                )
-                self.pik.initial_config = q0  # shape (num_retries, active_ancestor_dof)
-                result = self.pik.solve(
-                    tf
-                )  # produce solutions in shape (B, num_retries/initial_configs, active_ancestor_dof)
-                # TODO return mask for invalid solutions. CPU returns None at the moment
-                return result.solutions[:, 0, :]
+            if not is_delta_pose:
+                if current_pose is None:
+                    current_pose = self.pk_chain.forward_kinematics(q0).get_matrix()
+                    current_pose = Pose.create_from_pq(current_pose[:, :3, 3], rotation_conversions.matrix_to_quaternion(current_pose[:, :3, :3]))
+                if isinstance(pose, torch.Tensor):
+                    target_pos, target_rot = pose[:, 0:3], pose[:, 3:6]
+                    target_quat = rotation_conversions.matrix_to_quaternion(
+                        rotation_conversions.euler_angles_to_matrix(target_rot, "XYZ")
+                    )
+                    pose = Pose.create_from_pq(target_pos, target_quat)
+                if isinstance(pose, Pose):
+                    # the following assumes root_translation:root_aligned_body_rotation control frame
+                    translation = pose.p - current_pose.p
+                    quaternion = rotation_conversions.quaternion_multiply(pose.q, rotation_conversions.quaternion_invert(current_pose.q))
+                    pose = Pose.create_from_pq(translation, quaternion)
+            if isinstance(pose, Pose):
+                delta_pose = torch.zeros((B, 6), device=self.device, dtype=torch.float32)
+                delta_pose[:, 0:3] = pose.p
+                delta_pose[:, 3:6] = rotation_conversions.matrix_to_euler_angles(rotation_conversions.quaternion_to_matrix(pose.q), "XYZ")
             else:
-                jacobian = self.pk_chain.jacobian(q0)
-                # code commented out below is the fast kinematics method
-                # jacobian = (
-                #     self.fast_kinematics_model.jacobian_mixed_frame_pytorch(
-                #         self.articulation.get_qpos()[:, self.active_ancestor_joint_idxs]
-                #     )
-                #     .view(-1, len(self.active_ancestor_joints), 6)
-                #     .permute(0, 2, 1)
-                # )
-                # jacobian = jacobian[:, :, self.qmask]
-                if pos_only:
-                    jacobian = jacobian[:, 0:3]
-
-                # NOTE (arth): use only the parts of the jacobian that correspond to the active joints
-                jacobian = jacobian[:, :, self.qmask]
+                delta_pose = pose
+            jacobian = self.pk_chain.jacobian(q0)[:, :, self.qmask]
+            if solver_config["type"] == "levenberg_marquardt":
+                lambd = 0.0001 # Regularization parameter to ensure J^T * J is non-singular.
+                J_T = jacobian.transpose(1, 2)
+                lfs_A = torch.bmm(J_T, jacobian) + lambd * torch.eye(
+                    len(self.qmask), device=self.device
+                )
+                rhs_B = torch.bmm(J_T, delta_pose.unsqueeze(-1))
+                delta_joint_pos = torch.linalg.solve(lfs_A, rhs_B)
 
+            elif solver_config["type"] == "pseudo_inverse":
                 # NOTE (stao): this method of IK is from https://mathweb.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf by Samuel R. Buss
-                delta_joint_pos = torch.linalg.pinv(jacobian) @ action.unsqueeze(-1)
+                delta_joint_pos = torch.linalg.pinv(jacobian) @ delta_pose.unsqueeze(-1)
 
-                return q0[:, self.qmask] + delta_joint_pos.squeeze(-1)
+            return q0[:, self.qmask] + solver_config[
+                "alpha"
+            ] * delta_joint_pos.squeeze(-1)
         else:
             q0 = q0[:, self.pmodel_active_joint_indices]
             result, success, error = self.pmodel.compute_inverse_kinematics(
                 self.end_link_idx,
-                target_pose.sp,
+                pose.sp,
                 initial_qpos=q0.cpu().numpy()[0],
                 active_qmask=self.qmask,
                 max_iterations=100,

{mani_skill_nightly-2025.8.19.1758.dist-info → mani_skill_nightly-2025.8.19.2202.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: mani-skill-nightly
-Version: 2025.8.19.1758
+Version: 2025.8.19.2202
 Summary: ManiSkill3: A Unified Benchmark for Generalizable Manipulation Skills
 Home-page: https://github.com/haosulab/ManiSkill
 Author: ManiSkill contributors

{mani_skill_nightly-2025.8.19.1758.dist-info → mani_skill_nightly-2025.8.19.2202.dist-info}/RECORD

@@ -9,12 +9,12 @@ mani_skill/agents/controllers/__init__.py,sha256=tsd27dW6brQBm563CddUGtFMBNAxe_M
 mani_skill/agents/controllers/base_controller.py,sha256=JcKdQF7VN5YWzFOPwdYKShptfHVnxts78L9yPh4-Kto,13337
 mani_skill/agents/controllers/passive_controller.py,sha256=1cZp758AVuABvgWOzCeguBbbB8wJ6hF_cqN-k_vx5Kw,1322
 mani_skill/agents/controllers/pd_base_vel.py,sha256=-3ZzLZ9nRDMGHWnFE7c4reEbC3jHtQJCOx3LmArz95g,2868
-mani_skill/agents/controllers/pd_ee_pose.py,sha256=iCqK8q1ds7VTH4tkQleyryiBTIts70yS4SEQ0jzpMYI,12134
+mani_skill/agents/controllers/pd_ee_pose.py,sha256=alYkN02OVYDbj7i1OqC6hC87o_FZ85SxHXNmBFXR-lw,13447
 mani_skill/agents/controllers/pd_joint_pos.py,sha256=dSdOu1Q-sGyG3rJnWYNNmzXW8U6_zU3zcIY4wgHRvI8,11108
 mani_skill/agents/controllers/pd_joint_pos_vel.py,sha256=wgiXmenTVIao1Tm1vtdJWTZRFJMB_yQBNd5vQaP8WXw,2489
 mani_skill/agents/controllers/pd_joint_vel.py,sha256=VZF06ISCkdKBX_fUHxb7mdl9GN1Lob5dhrFGlwCx16Q,1957
 mani_skill/agents/controllers/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
-mani_skill/agents/controllers/utils/kinematics.py,sha256=FiSE1nfZejShEqvua9buzA_SCBuEP9OiAd7LpLbSp44,10415
+mani_skill/agents/controllers/utils/kinematics.py,sha256=FxREZaBRjUfc7S-jf0cLhN3HivTPxhq3AnO5zHE2nyI,11920
 mani_skill/agents/robots/__init__.py,sha256=lvW8t2bn2UjuWZ7PsYiKqMnHaMLvGVw6itBhxwHaauk,737
 mani_skill/agents/robots/allegro_hand/__init__.py,sha256=PWH6MMv2i5g1xi76_E-YJLtKFLVKxkJgQ96GD_YeQN4,104
 mani_skill/agents/robots/allegro_hand/allegro.py,sha256=MnU04fCcvppTGJHdhgvs2OqUKToQtNMXbs2bFM5sXiU,4896
@@ -826,8 +826,8 @@ mani_skill/vector/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU
 mani_skill/vector/wrappers/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 mani_skill/vector/wrappers/gymnasium.py,sha256=voHNmYg5Jyy-laMSC2Fd8VggQvhXw3NnfYLbD9QDXAc,7305
 mani_skill/vector/wrappers/sb3.py,sha256=SlXdiEPqcNHYMhJCzA29kBU6zK7DKTe1nc0L6Z3QQtY,4722
-mani_skill_nightly-2025.8.19.1758.dist-info/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
-mani_skill_nightly-2025.8.19.1758.dist-info/METADATA,sha256=Ewkt0h5OQrqfFT7bTTCIMC-8adHb1eP5i0KF0giZxPU,9316
-mani_skill_nightly-2025.8.19.1758.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
-mani_skill_nightly-2025.8.19.1758.dist-info/top_level.txt,sha256=bkBgOVl_MZMoQx2aRFsSFEYlZLxjWlip5vtJ39FB3jA,11
-mani_skill_nightly-2025.8.19.1758.dist-info/RECORD,,
+mani_skill_nightly-2025.8.19.2202.dist-info/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
+mani_skill_nightly-2025.8.19.2202.dist-info/METADATA,sha256=AUKAKHdJL1CFsXi4rUeZLyCvD966GO51Pu6e-fg-r70,9316
+mani_skill_nightly-2025.8.19.2202.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
+mani_skill_nightly-2025.8.19.2202.dist-info/top_level.txt,sha256=bkBgOVl_MZMoQx2aRFsSFEYlZLxjWlip5vtJ39FB3jA,11
+mani_skill_nightly-2025.8.19.2202.dist-info/RECORD,,