mani-skill-nightly 2025.5.3.1545__py3-none-any.whl → 2025.5.3.1612__py3-none-any.whl

mani_skill/agents/robots/xarm6/xarm6_robotiq.py

@@ -403,6 +403,18 @@ class XArm6Robotiq(BaseAgent):
         )
         return torch.logical_and(lflag, rflag)
 
+    @staticmethod
+    def build_grasp_pose(approaching, closing, center):
+        """Build a grasp pose ()."""
+        assert np.abs(1 - np.linalg.norm(approaching)) < 1e-3
+        assert np.abs(1 - np.linalg.norm(closing)) < 1e-3
+        assert np.abs(approaching @ closing) <= 1e-3
+        ortho = np.cross(closing, approaching)
+        T = np.eye(4)
+        T[:3, :3] = np.stack([ortho, closing, approaching], axis=1)
+        T[:3, 3] = center
+        return sapien.Pose(T)
+
     def is_static(self, threshold: float = 0.2):
         qvel = self.robot.get_qvel()[..., :-6]
         return torch.max(torch.abs(qvel), 1)[0] <= threshold

mani_skill/envs/tasks/mobile_manipulation/open_cabinet_drawer.py

@@ -131,7 +131,7 @@ class OpenCabinetDrawerEnv(BaseEnv):
         model_ids = self._batched_episode_rng.choice(self.all_model_ids)
         link_ids = self._batched_episode_rng.randint(0, 2**31)
 
-        self._cabinets = []
+        self._cabinets: List[Articulation] = []
         handle_links: List[List[Link]] = []
         handle_links_meshes: List[List[trimesh.Trimesh]] = []
         for i, model_id in enumerate(model_ids):
@@ -163,7 +163,7 @@ class OpenCabinetDrawerEnv(BaseEnv):
                     handle_links[-1].append(link)
                     # save the first mesh in the link object that correspond with a handle
                     handle_links_meshes[-1].append(
-                        link.generate_visual_mesh(
+                        link.generate_mesh(
                             filter=lambda _, render_shape: "handle"
                             in render_shape.name,
                             mesh_name="handle",

mani_skill/envs/tasks/tabletop/plug_charger.py

@@ -183,28 +183,29 @@ class PlugChargerEnv(BaseEnv):
             self.scene_builder.initialize(env_idx)
 
             # Initialize agent
-            qpos = torch.tensor(
-                [
-                    0.0,
-                    np.pi / 8,
-                    0,
-                    -np.pi * 5 / 8,
-                    0,
-                    np.pi * 3 / 4,
-                    np.pi / 4,
-                    0.04,
-                    0.04,
-                ]
-            )
-            qpos = (
-                torch.normal(
-                    0, self.robot_init_qpos_noise, (b, len(qpos)), device=self.device
+            if self.agent.uid == "panda_wristcam":
+                qpos = torch.tensor(
+                    [
+                        0.0,
+                        np.pi / 8,
+                        0,
+                        -np.pi * 5 / 8,
+                        0,
+                        np.pi * 3 / 4,
+                        np.pi / 4,
+                        0.04,
+                        0.04,
+                    ]
                 )
-                + qpos
-            )
-            qpos[:, -2:] = 0.04
-            self.agent.robot.set_qpos(qpos)
-            self.agent.robot.set_pose(sapien.Pose([-0.615, 0, 0]))
+                qpos = (
+                    torch.normal(
+                        0, self.robot_init_qpos_noise, (b, len(qpos)), device=self.device
+                    )
+                    + qpos
+                )
+                qpos[:, -2:] = 0.04
+                self.agent.robot.set_qpos(qpos)
+                self.agent.robot.set_pose(sapien.Pose([-0.615, 0, 0]))
 
             # Initialize charger
             xy = randomization.uniform(

mani_skill/examples/motionplanning/xarm6/motionplanner.py

@@ -0,0 +1,111 @@
+import numpy as np
+import mplib
+from mani_skill.examples.motionplanning.panda.motionplanner import PandaArmMotionPlanningSolver
+from mani_skill.envs.sapien_env import BaseEnv
+import sapien.core as sapien
+
+from mani_skill.utils.structs.pose import to_sapien_pose
+
+
+class XArm6RobotiqMotionPlanningSolver(PandaArmMotionPlanningSolver):
+    CLOSED = 0.81
+    OPEN = 0
+
+    def __init__(self, env: BaseEnv, debug: bool = False, vis: bool = True, base_pose: sapien.Pose = None, visualize_target_grasp_pose: bool = True, print_env_info: bool = True, joint_vel_limits=0.9, joint_acc_limits=0.9):
+        super().__init__(env, debug, vis, base_pose, visualize_target_grasp_pose, print_env_info, joint_vel_limits, joint_acc_limits)
+        self.gripper_state = self.OPEN
+
+    def setup_planner(self):
+        link_names = [link.get_name() for link in self.robot.get_links()]
+        joint_names = [joint.get_name() for joint in self.robot.get_active_joints()]
+        planner = mplib.Planner(
+            urdf=self.env_agent.urdf_path,
+            srdf=self.env_agent.urdf_path.replace(".urdf", ".srdf"),
+            user_link_names=link_names,
+            user_joint_names=joint_names,
+            move_group="eef",
+            joint_vel_limits=np.ones(6) * self.joint_vel_limits,
+            joint_acc_limits=np.ones(6) * self.joint_acc_limits,
+        )
+        planner.set_base_pose(np.hstack([self.base_pose.p, self.base_pose.q]))
+        return planner
+    
+    def open_gripper(self):
+        self.gripper_state = self.OPEN
+        qpos = self.robot.get_qpos()[0, :6].cpu().numpy()
+        for i in range(6):
+            if self.control_mode == "pd_joint_pos":
+                action = np.hstack([qpos, self.gripper_state])
+            else:
+                action = np.hstack([qpos, qpos * 0, self.gripper_state])
+            obs, reward, terminated, truncated, info = self.env.step(action)
+            self.elapsed_steps += 1
+            if self.print_env_info:
+                print(
+                    f"[{self.elapsed_steps:3}] Env Output: reward={reward} info={info}"
+                )
+            if self.vis:
+                self.base_env.render_human()
+        return obs, reward, terminated, truncated, info
+
+    def close_gripper(self, t=6, gripper_state = CLOSED):
+        self.gripper_state = gripper_state
+        qpos = self.robot.get_qpos()[0, :6].cpu().numpy()
+        for i in range(t):
+            if self.control_mode == "pd_joint_pos":
+                action = np.hstack([qpos, self.gripper_state])
+            else:
+                action = np.hstack([qpos, qpos * 0, self.gripper_state])
+            obs, reward, terminated, truncated, info = self.env.step(action)
+            self.elapsed_steps += 1
+            if self.print_env_info:
+                print(
+                    f"[{self.elapsed_steps:3}] Env Output: reward={reward} info={info}"
+                )
+            if self.vis:
+                self.base_env.render_human()
+        return obs, reward, terminated, truncated, info
+    
+    def move_to_pose_with_RRTStar(
+        self, pose: sapien.Pose, dry_run: bool = False, refine_steps: int = 0
+    ):
+        pose = to_sapien_pose(pose)
+        if self.grasp_pose_visual is not None:
+            self.grasp_pose_visual.set_pose(pose)
+        pose = sapien.Pose(p=pose.p, q=pose.q)
+        result = self.planner.plan_qpos_to_pose(
+            np.concatenate([pose.p, pose.q]),
+            self.robot.get_qpos().cpu().numpy()[0],
+            time_step=self.base_env.control_timestep,
+            use_point_cloud=self.use_point_cloud,
+            rrt_range=0.0,
+            planning_time=1,
+            planner_name="RRTstar",
+            wrt_world=True,
+        )
+        if result["status"] != "Success":
+            print(result["status"])
+            self.render_wait()
+            return -1
+        self.render_wait()
+        if dry_run:
+            return result
+        return self.follow_path(result, refine_steps=refine_steps)
+
+
+class XArm6PandaGripperMotionPlanningSolver(PandaArmMotionPlanningSolver):
+    def setup_planner(self):
+        link_names = [link.get_name() for link in self.robot.get_links()]
+        joint_names = [joint.get_name() for joint in self.robot.get_active_joints()]
+        planner = mplib.Planner(
+            urdf=self.env_agent.urdf_path,
+            srdf=self.env_agent.urdf_path.replace(".urdf", ".srdf"),
+            user_link_names=link_names,
+            user_joint_names=joint_names,
+            move_group="panda_hand_tcp",
+            joint_vel_limits=np.ones(6) * self.joint_vel_limits,
+            joint_acc_limits=np.ones(6) * self.joint_acc_limits,
+        )
+        planner.set_base_pose(np.hstack([self.base_pose.p, self.base_pose.q]))
+        return planner
+    

mani_skill/examples/motionplanning/xarm6/run.py

@@ -0,0 +1,148 @@
+import multiprocessing as mp
+import os
+from copy import deepcopy
+import time
+import argparse
+import gymnasium as gym
+import numpy as np
+from tqdm import tqdm
+import os.path as osp
+from mani_skill.utils.wrappers.record import RecordEpisode
+from mani_skill.trajectory.merge_trajectory import merge_trajectories
+from mani_skill.examples.motionplanning.xarm6.solutions import solvePickCube, solvePushCube, solveStackCube, solvePlugCharger
+
+MP_SOLUTIONS = {
+    "PickCube-v1": solvePickCube,
+    "PushCube-v1": solvePushCube,
+    "StackCube-v1": solveStackCube,
+    "PlugCharger-v1": solvePlugCharger,
+}
+
+def parse_args(args=None):
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-e", "--env-id", type=str, default="PickCube-v1", help=f"Environment to run motion planning solver on. Available options are {list(MP_SOLUTIONS.keys())}")
+    parser.add_argument("-o", "--obs-mode", type=str, default="none", help="Observation mode to use. Usually this is kept as 'none' as observations are not necesary to be stored, they can be replayed later via the mani_skill.trajectory.replay_trajectory script.")
+    parser.add_argument("-n", "--num-traj", type=int, default=10, help="Number of trajectories to generate.")
+    parser.add_argument("--only-count-success", action="store_true", help="If true, generates trajectories until num_traj of them are successful and only saves the successful trajectories/videos")
+    parser.add_argument("--reward-mode", type=str)
+    parser.add_argument("-b", "--sim-backend", type=str, default="auto", help="Which simulation backend to use. Can be 'auto', 'cpu', 'gpu'")
+    parser.add_argument("--render-mode", type=str, default="rgb_array", help="can be 'sensors' or 'rgb_array' which only affect what is saved to videos")
+    parser.add_argument("--vis", action="store_true", help="whether or not to open a GUI to visualize the solution live")
+    parser.add_argument("--save-video", action="store_true", help="whether or not to save videos locally")
+    parser.add_argument("--traj-name", type=str, help="The name of the trajectory .h5 file that will be created.")
+    parser.add_argument("--shader", default="default", type=str, help="Change shader used for rendering. Default is 'default' which is very fast. Can also be 'rt' for ray tracing and generating photo-realistic renders. Can also be 'rt-fast' for a faster but lower quality ray-traced renderer")
+    parser.add_argument("--record-dir", type=str, default="demos", help="where to save the recorded trajectories")
+    parser.add_argument("--num-procs", type=int, default=1, help="Number of processes to use to help parallelize the trajectory replay process. This uses CPU multiprocessing and only works with the CPU simulation backend at the moment.")
+    return parser.parse_args()
+
+def _main(args, proc_id: int = 0, start_seed: int = 0) -> str:
+    env_id = args.env_id
+    env = gym.make(
+        env_id,
+        obs_mode=args.obs_mode,
+        control_mode="pd_joint_pos",
+        render_mode=args.render_mode,
+        robot_uids="xarm6_robotiq",
+        reward_mode="dense" if args.reward_mode is None else args.reward_mode,
+        sensor_configs=dict(shader_pack=args.shader),
+        human_render_camera_configs=dict(shader_pack=args.shader),
+        viewer_camera_configs=dict(shader_pack=args.shader),
+        sim_backend=args.sim_backend
+    )
+    if env_id not in MP_SOLUTIONS:
+        raise RuntimeError(f"No already written motion planning solutions for {env_id}. Available options are {list(MP_SOLUTIONS.keys())}")
+    
+    if not args.traj_name:
+        new_traj_name = time.strftime("%Y%m%d_%H%M%S")
+    else:
+        new_traj_name = args.traj_name
+
+    if args.num_procs > 1:
+        new_traj_name = new_traj_name + "." + str(proc_id)
+    env = RecordEpisode(
+        env,
+        output_dir=osp.join(args.record_dir, env_id, "motionplanning"),
+        trajectory_name=new_traj_name, save_video=args.save_video,
+        source_type="motionplanning",
+        source_desc="official motion planning solution from ManiSkill contributors",
+        video_fps=30,
+        save_on_reset=False
+    )
+    output_h5_path = env._h5_file.filename
+    solve = MP_SOLUTIONS[env_id]
+    print(f"Motion Planning Running on {env_id}")
+    pbar = tqdm(range(args.num_traj), desc=f"proc_id: {proc_id}")
+    seed = start_seed
+    successes = []
+    solution_episode_lengths = []
+    failed_motion_plans = 0
+    passed = 0
+    while True:
+        try:
+            res = solve(env, seed=seed, debug=False, vis=True if args.vis else False)
+        except Exception as e:
+            print(f"Cannot find valid solution because of an error in motion planning solution: {e}")
+            res = -1
+
+        if res == -1:
+            success = False
+            failed_motion_plans += 1
+        else:
+            success = res[-1]["success"].item()
+            elapsed_steps = res[-1]["elapsed_steps"].item()
+            solution_episode_lengths.append(elapsed_steps)
+        successes.append(success)
+        if args.only_count_success and not success:
+            seed += 1
+            env.flush_trajectory(save=False)
+            if args.save_video:
+                env.flush_video(save=False)
+            continue
+        else:
+            env.flush_trajectory()
+            if args.save_video:
+                env.flush_video()
+            pbar.update(1)
+            pbar.set_postfix(
+                dict(
+                    success_rate=np.mean(successes),
+                    failed_motion_plan_rate=failed_motion_plans / (seed + 1),
+                    avg_episode_length=np.mean(solution_episode_lengths),
+                    max_episode_length=np.max(solution_episode_lengths),
+                    # min_episode_length=np.min(solution_episode_lengths)
+                )
+            )
+            seed += 1
+            passed += 1
+            if passed == args.num_traj:
+                break
+    env.close()
+    return output_h5_path
+
+def main(args):
+    if args.num_procs > 1 and args.num_procs < args.num_traj:
+        if args.num_traj < args.num_procs:
+            raise ValueError("Number of trajectories should be greater than or equal to number of processes")
+        args.num_traj = args.num_traj // args.num_procs
+        seeds = [*range(0, args.num_procs * args.num_traj, args.num_traj)]
+        pool = mp.Pool(args.num_procs)
+        proc_args = [(deepcopy(args), i, seeds[i]) for i in range(args.num_procs)]
+        res = pool.starmap(_main, proc_args)
+        pool.close()
+        # Merge trajectory files
+        output_path = res[0][: -len("0.h5")] + "h5"
+        merge_trajectories(output_path, res)
+        for h5_path in res:
+            tqdm.write(f"Remove {h5_path}")
+            os.remove(h5_path)
+            json_path = h5_path.replace(".h5", ".json")
+            tqdm.write(f"Remove {json_path}")
+            os.remove(json_path)
+    else:
+        _main(args)
+
+if __name__ == "__main__":
+    # start = time.time()
+    mp.set_start_method("spawn")
+    main(parse_args())
+    # print(f"Total time taken: {time.time() - start}")

mani_skill/examples/motionplanning/xarm6/solutions/__init__.py

@@ -0,0 +1,4 @@
+from .pick_cube import solve as solvePickCube
+from .push_cube import solve as solvePushCube
+from .stack_cube import solve as solveStackCube
+from .plug_charger import solve as solvePlugCharger

mani_skill/examples/motionplanning/xarm6/solutions/pick_cube.py

@@ -0,0 +1,66 @@
+import numpy as np
+import sapien
+
+from mani_skill.envs.tasks import PickCubeEnv
+from mani_skill.examples.motionplanning.xarm6.motionplanner import \
+    XArm6RobotiqMotionPlanningSolver, XArm6PandaGripperMotionPlanningSolver
+from mani_skill.examples.motionplanning.panda.utils import (
+    compute_grasp_info_by_obb, get_actor_obb)
+
+def solve(env: PickCubeEnv, seed=None, debug=False, vis=False):
+    env.reset(seed=seed)
+
+    if env.unwrapped.robot_uids == "xarm6_robotiq":
+        planner_cls = XArm6RobotiqMotionPlanningSolver
+    elif env.unwrapped.robot_uids == "xarm6_pandagripper":
+        planner_cls = XArm6PandaGripperMotionPlanningSolver
+    else:
+        raise ValueError(f"Unsupported robot uid: {env.robot_uid}")
+    planner = planner_cls(
+        env,
+        debug=debug,
+        vis=vis,
+        base_pose=env.unwrapped.agent.robot.pose,
+        visualize_target_grasp_pose=vis,
+        print_env_info=False,
+    )
+
+    FINGER_LENGTH = 0.025
+    env = env.unwrapped
+
+    # retrieves the object oriented bounding box (trimesh box object)
+    obb = get_actor_obb(env.cube)
+
+    approaching = np.array([0, 0, -1])
+    # get transformation matrix of the tcp pose, is default batched and on torch
+    target_closing = env.agent.tcp.pose.to_transformation_matrix()[0, :3, 1].cpu().numpy()
+    # we can build a simple grasp pose using this information
+    grasp_info = compute_grasp_info_by_obb(
+        obb,
+        approaching=approaching,
+        target_closing=target_closing,
+        depth=FINGER_LENGTH,
+    )
+    closing, center = grasp_info["closing"], grasp_info["center"]
+    grasp_pose = env.agent.build_grasp_pose(approaching, closing, env.cube.pose.sp.p)
+
+    # -------------------------------------------------------------------------- #
+    # Reach
+    # -------------------------------------------------------------------------- #
+    reach_pose = grasp_pose * sapien.Pose([0, 0, -0.05])
+    planner.move_to_pose_with_RRTStar(reach_pose)
+
+    # -------------------------------------------------------------------------- #
+    # Grasp
+    # -------------------------------------------------------------------------- #
+    planner.move_to_pose_with_screw(grasp_pose)
+    planner.close_gripper()
+
+    # -------------------------------------------------------------------------- #
+    # Move to goal pose
+    # -------------------------------------------------------------------------- #
+    goal_pose = sapien.Pose(env.goal_site.pose.sp.p, grasp_pose.q)
+    res = planner.move_to_pose_with_screw(goal_pose)
+
+    planner.close()
+    return res

mani_skill/examples/motionplanning/xarm6/solutions/plug_charger.py

@@ -0,0 +1,111 @@
+import gymnasium as gym
+import numpy as np
+import sapien.core as sapien
+import trimesh
+from tqdm import tqdm
+from transforms3d.euler import euler2quat
+
+from mani_skill.envs.tasks import PlugChargerEnv
+from mani_skill.examples.motionplanning.xarm6.motionplanner import \
+    XArm6RobotiqMotionPlanningSolver, XArm6PandaGripperMotionPlanningSolver
+from mani_skill.examples.motionplanning.panda.utils import (
+    compute_grasp_info_by_obb, get_actor_obb)
+
+
+def main():
+    env: PlugChargerEnv = gym.make(
+        "PlugCharger-v1",
+        obs_mode="none",
+        control_mode="pd_joint_pos",
+        render_mode="rgb_array",
+        reward_mode="sparse",
+    )
+    for seed in tqdm(range(100)):
+        res = solve(env, seed=seed, debug=False, vis=True)
+        print(res[-1])
+    env.close()
+
+
+def solve(env: PlugChargerEnv, seed=None, debug=False, vis=False):
+    env.reset(seed=seed)
+    assert env.unwrapped.control_mode in [
+        "pd_joint_pos",
+        "pd_joint_pos_vel",
+    ], env.unwrapped.control_mode
+    if env.unwrapped.robot_uids == "xarm6_robotiq":
+        planner_cls = XArm6RobotiqMotionPlanningSolver
+    elif env.unwrapped.robot_uids == "xarm6_pandagripper":
+        planner_cls = XArm6PandaGripperMotionPlanningSolver
+    else:
+        raise ValueError(f"Unsupported robot uid: {env.robot_uid}")
+    planner = planner_cls(
+        env,
+        debug=debug,
+        vis=vis,
+        base_pose=env.unwrapped.agent.robot.pose,
+        visualize_target_grasp_pose=False,
+        print_env_info=False,
+        joint_vel_limits=0.5,
+        joint_acc_limits=0.5,
+    )
+
+    FINGER_LENGTH = 0.025
+    env = env.unwrapped
+    charger_base_pose = env.charger_base_pose
+    charger_base_size = np.array(env.unwrapped._base_size) * 2
+
+    obb = trimesh.primitives.Box(
+        extents=charger_base_size,
+        transform=charger_base_pose.sp.to_transformation_matrix(),
+    )
+
+    approaching = np.array([0, 0, -1])
+    target_closing = env.agent.tcp.pose.sp.to_transformation_matrix()[:3, 1]
+    grasp_info = compute_grasp_info_by_obb(
+        obb,
+        approaching=approaching,
+        target_closing=target_closing,
+        depth=FINGER_LENGTH,
+    )
+    closing, center = grasp_info["closing"], grasp_info["center"]
+    grasp_pose = env.agent.build_grasp_pose(approaching, closing, center)
+
+    # add a angle to grasp
+    grasp_angle = np.deg2rad(15)
+    grasp_pose = grasp_pose * sapien.Pose(q=euler2quat(0, grasp_angle, 0))
+
+    # -------------------------------------------------------------------------- #
+    # Reach
+    # -------------------------------------------------------------------------- #
+    reach_pose = grasp_pose * sapien.Pose([0, 0, -0.05])
+    planner.move_to_pose_with_RRTStar(reach_pose)
+
+    # -------------------------------------------------------------------------- #
+    # Grasp
+    # -------------------------------------------------------------------------- #
+    planner.move_to_pose_with_screw(grasp_pose)
+    planner.close_gripper()
+
+    # -------------------------------------------------------------------------- #
+    # Align
+    # -------------------------------------------------------------------------- #
+    pre_insert_pose = (
+        env.goal_pose.sp
+        * sapien.Pose([-0.05, 0.0, 0.0])
+        * env.charger.pose.sp.inv()
+        * env.agent.tcp.pose.sp
+    )
+    insert_pose = env.goal_pose.sp * env.charger.pose.sp.inv() * env.agent.tcp.pose.sp * sapien.Pose([0, 0, -0.005])
+    planner.move_to_pose_with_screw(pre_insert_pose, refine_steps=0)
+    planner.move_to_pose_with_screw(pre_insert_pose, refine_steps=5)
+    # -------------------------------------------------------------------------- #
+    # Insert
+    # -------------------------------------------------------------------------- #
+    res = planner.move_to_pose_with_screw(insert_pose)
+
+    planner.close()
+    return res
+
+
+if __name__ == "__main__":
+    main()

mani_skill/examples/motionplanning/xarm6/solutions/push_cube.py

@@ -0,0 +1,38 @@
+import numpy as np
+import sapien
+
+from mani_skill.envs.tasks import PushCubeEnv
+from mani_skill.examples.motionplanning.xarm6.motionplanner import \
+    XArm6RobotiqMotionPlanningSolver, XArm6PandaGripperMotionPlanningSolver
+
+def solve(env: PushCubeEnv, seed=None, debug=False, vis=False):
+    env.reset(seed=seed)
+    if env.unwrapped.robot_uids == "xarm6_robotiq":
+        planner_cls = XArm6RobotiqMotionPlanningSolver
+    elif env.unwrapped.robot_uids == "xarm6_pandagripper":
+        planner_cls = XArm6PandaGripperMotionPlanningSolver
+    else:
+        raise ValueError(f"Unsupported robot uid: {env.robot_uid}")
+    planner = planner_cls(
+        env,
+        debug=debug,
+        vis=vis,
+        base_pose=env.unwrapped.agent.robot.pose,
+        visualize_target_grasp_pose=vis,
+        print_env_info=False,
+    )
+
+    FINGER_LENGTH = 0.025
+    env = env.unwrapped
+    planner.close_gripper()
+    reach_pose = sapien.Pose(p=env.obj.pose.sp.p + np.array([-0.05, 0, 0]), q=env.agent.tcp.pose.sp.q)
+    planner.move_to_pose_with_RRTStar(reach_pose)
+
+    # -------------------------------------------------------------------------- #
+    # Move to goal pose
+    # -------------------------------------------------------------------------- #
+    goal_pose = sapien.Pose(p=env.goal_region.pose.sp.p,q=env.agent.tcp.pose.sp.q)
+    res = planner.move_to_pose_with_screw(goal_pose)
+
+    planner.close()
+    return res

mani_skill/examples/motionplanning/xarm6/solutions/stack_cube.py

@@ -0,0 +1,92 @@
+import argparse
+import gymnasium as gym
+import numpy as np
+import sapien
+from transforms3d.euler import euler2quat
+
+from mani_skill.envs.tasks import StackCubeEnv
+from mani_skill.examples.motionplanning.xarm6.motionplanner import \
+    XArm6RobotiqMotionPlanningSolver, XArm6PandaGripperMotionPlanningSolver
+from mani_skill.examples.motionplanning.panda.utils import (
+    compute_grasp_info_by_obb, get_actor_obb)
+from mani_skill.utils.wrappers.record import RecordEpisode
+
+def solve(env: StackCubeEnv, seed=None, debug=False, vis=False):
+    env.reset(seed=seed)
+    assert env.unwrapped.control_mode in [
+        "pd_joint_pos",
+        "pd_joint_pos_vel",
+    ], env.unwrapped.control_mode
+    if env.unwrapped.robot_uids == "xarm6_robotiq":
+        planner_cls = XArm6RobotiqMotionPlanningSolver
+    elif env.unwrapped.robot_uids == "xarm6_pandagripper":
+        planner_cls = XArm6PandaGripperMotionPlanningSolver
+    else:
+        raise ValueError(f"Unsupported robot uid: {env.robot_uid}")
+    planner = planner_cls(
+        env,
+        debug=debug,
+        vis=vis,
+        base_pose=env.unwrapped.agent.robot.pose,
+        visualize_target_grasp_pose=vis,
+        print_env_info=False,
+    )
+    FINGER_LENGTH = 0.025
+    env = env.unwrapped
+    obb = get_actor_obb(env.cubeA)
+
+    approaching = np.array([0, 0, -1])
+    target_closing = env.agent.tcp.pose.to_transformation_matrix()[0, :3, 1].numpy()
+    grasp_info = compute_grasp_info_by_obb(
+        obb,
+        approaching=approaching,
+        target_closing=target_closing,
+        depth=FINGER_LENGTH,
+    )
+    closing, center = grasp_info["closing"], grasp_info["center"]
+    grasp_pose = env.agent.build_grasp_pose(approaching, closing, center)
+
+    # Search a valid pose
+    angles = np.arange(0, np.pi * 2 / 3, np.pi / 2)
+    angles = np.repeat(angles, 2)
+    angles[1::2] *= -1
+    for angle in angles:
+        delta_pose = sapien.Pose(q=euler2quat(0, 0, angle))
+        grasp_pose2 = grasp_pose * delta_pose
+        res = planner.move_to_pose_with_RRTStar(grasp_pose2, dry_run=True)
+        if res == -1:
+            continue
+        grasp_pose = grasp_pose2
+        break
+    else:
+        print("Fail to find a valid grasp pose")
+
+    # -------------------------------------------------------------------------- #
+    # Reach
+    # -------------------------------------------------------------------------- #
+    reach_pose = grasp_pose * sapien.Pose([0, 0, -0.05])
+    planner.move_to_pose_with_RRTStar(reach_pose)
+
+    # -------------------------------------------------------------------------- #
+    # Grasp
+    # -------------------------------------------------------------------------- #
+    planner.move_to_pose_with_screw(grasp_pose)
+    planner.close_gripper()
+
+    # -------------------------------------------------------------------------- #
+    # Lift
+    # -------------------------------------------------------------------------- #
+    lift_pose = sapien.Pose([0, 0, 0.1]) * grasp_pose
+    planner.move_to_pose_with_screw(lift_pose)
+
+    # -------------------------------------------------------------------------- #
+    # Stack
+    # -------------------------------------------------------------------------- #
+    goal_pose = env.cubeB.pose * sapien.Pose([0, 0, env.cube_half_size[2] * 2])
+    offset = (goal_pose.p - env.cubeA.pose.p).numpy()[0] # remember that all data in ManiSkill is batched and a torch tensor
+    align_pose = sapien.Pose(lift_pose.p + offset, lift_pose.q)
+    planner.move_to_pose_with_RRTStar(align_pose)
+
+    res = planner.open_gripper()
+    planner.close()
+    return res

mani_skill/utils/assets/data.py

@@ -185,6 +185,7 @@ def initialize_data_sources():
     )
     DATA_SOURCES["xarm6"] = DataSource(
         source_type="robot",
+        # TODO: update to new url with .convex.stl assets for xarm6 to support motion planning
         url="https://github.com/haosulab/ManiSkill-XArm6/archive/refs/tags/v0.1.0.zip",
         target_path="robots/xarm6",
     )

mani_skill/utils/structs/actor.py

@@ -242,27 +242,33 @@ class Actor(PhysxRigidDynamicComponentStruct[sapien.Entity]):
                 cg[group] = value
                 cs.set_collision_groups(cg)
 
-    def get_first_collision_mesh(self, to_world_frame: bool = True) -> trimesh.Trimesh:
+    def get_first_collision_mesh(
+        self, to_world_frame: bool = True
+    ) -> Union[trimesh.Trimesh, None]:
         """
         Returns the collision mesh of the first managed actor object. Note results of this are not cached or optimized at the moment
-        so this function can be slow if called too often
+        so this function can be slow if called too often. Some actors have no collision meshes, in which case this function returns None
 
         Args:
             to_world_frame (bool): Whether to transform the collision mesh pose to the world frame
         """
-        return self.get_collision_meshes(to_world_frame=to_world_frame, first_only=True)
+        mesh = self.get_collision_meshes(to_world_frame=to_world_frame, first_only=True)
+        if isinstance(mesh, trimesh.Trimesh):
+            return mesh
+        return None
 
     def get_collision_meshes(
         self, to_world_frame: bool = True, first_only: bool = False
-    ) -> List[trimesh.Trimesh]:
+    ) -> Union[List[trimesh.Trimesh], trimesh.Trimesh]:
         """
         Returns the collision mesh of each managed actor object. Note results of this are not cached or optimized at the moment
-        so this function can be slow if called too often
+        so this function can be slow if called too often. Some actors have no collision meshes, in which case this function returns an empty list.
 
         Args:
             to_world_frame (bool): Whether to transform the collision mesh pose to the world frame
             first_only (bool): Whether to return the collision mesh of just the first actor managed by this object. If True,
-                this also returns a single Trimesh.Mesh object instead of a list
+                this also returns a single Trimesh.Mesh object instead of a list. This can be useful for efficiency reasons if you know
+                ahead of time all of the managed actors have the same collision mesh
         """
         assert (
             not self.merged
@@ -274,9 +280,12 @@ class Actor(PhysxRigidDynamicComponentStruct[sapien.Entity]):
             actor_meshes = []
             for comp in actor.components:
                 if isinstance(comp, physx.PhysxRigidBaseComponent):
-                    actor_meshes.append(merge_meshes(get_component_meshes(comp)))
+                    merged = merge_meshes(get_component_meshes(comp))
+                    if merged is not None:
+                        actor_meshes.append(merged)
             mesh = merge_meshes(actor_meshes)
-            meshes.append(mesh)
+            if mesh is not None:
+                meshes.append(mesh)
             if first_only:
                 break
         if to_world_frame:
@@ -287,6 +296,8 @@ class Actor(PhysxRigidDynamicComponentStruct[sapien.Entity]):
                         mesh.apply_transform(mat[i].sp.to_transformation_matrix())
                     else:
                         mesh.apply_transform(mat.sp.to_transformation_matrix())
+        if len(meshes) == 0:
+            return []
         if first_only:
             return meshes[0]
         return meshes

mani_skill/utils/structs/articulation.py

@@ -320,19 +320,24 @@ class Articulation(BaseStruct[physx.PhysxArticulation]):
     #                 g0, g1, g2, g3 = s.get_collision_groups()
     #                 s.set_collision_groups([g0, g1, g2 | (1 << 29), g3])
 
-    def get_first_collision_mesh(self, to_world_frame: bool = True) -> trimesh.Trimesh:
+    def get_first_collision_mesh(
+        self, to_world_frame: bool = True
+    ) -> Union[trimesh.Trimesh, None]:
         """
         Returns the collision mesh of the first managed articulation object. Note results of this are not cached or optimized at the moment
-        so this function can be slow if called too often
+        so this function can be slow if called too often. Some articulations have no collision meshes, in which case this function returns None
 
         Args:
             to_world_frame (bool): Whether to transform the collision mesh pose to the world frame
         """
-        return self.get_collision_meshes(to_world_frame=to_world_frame, first_only=True)
+        mesh = self.get_collision_meshes(to_world_frame=to_world_frame, first_only=True)
+        if isinstance(mesh, trimesh.Trimesh):
+            return mesh
+        return None
 
     def get_collision_meshes(
         self, to_world_frame: bool = True, first_only: bool = False
-    ) -> List[trimesh.Trimesh]:
+    ) -> Union[List[trimesh.Trimesh], trimesh.Trimesh]:
         """
         Returns the collision mesh of each managed articulation object. Note results of this are not cached or optimized at the moment
         so this function can be slow if called too often
@@ -340,7 +345,8 @@ class Articulation(BaseStruct[physx.PhysxArticulation]):
         Args:
             to_world_frame (bool): Whether to transform the collision mesh pose to the world frame
             first_only (bool): Whether to return the collision mesh of just the first articulation managed by this object. If True,
-                this also returns a single Trimesh.Mesh object instead of a list
+                this also returns a single Trimesh.Mesh object instead of a list. This can be useful for efficiency reasons if you know
+                ahead of time all of the managed actors have the same collision mesh
         """
         assert (
             not self.merged
@@ -365,9 +371,12 @@ class Articulation(BaseStruct[physx.PhysxArticulation]):
                         link_mesh.apply_transform(pose.sp.to_transformation_matrix())
                     art_meshes.append(link_mesh)
             mesh = merge_meshes(art_meshes)
-            meshes.append(mesh)
+            if mesh is not None:
+                meshes.append(mesh)
             if first_only:
                 break
+        if len(meshes) == 0:
+            return []
         if first_only:
             return meshes[0]
         return meshes

{mani_skill_nightly-2025.5.3.1545.dist-info → mani_skill_nightly-2025.5.3.1612.dist-info}/METADATA

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

{mani_skill_nightly-2025.5.3.1545.dist-info → mani_skill_nightly-2025.5.3.1612.dist-info}/RECORD

@@ -65,7 +65,7 @@ mani_skill/agents/robots/xarm/__init__.py,sha256=6Mhn4vV4f9XxcK493U5W9VE6yGGgydP
 mani_skill/agents/robots/xarm/xarm7_ability.py,sha256=yj7CUBQpbGVUiT22qweJKTniJE0DxdEyyKj329vr0HY,6106
 mani_skill/agents/robots/xarm6/__init__.py,sha256=0r19OsKmm1ssKB5Rrie8syWQvpXNooVOv6m-iygrdM0,109
 mani_skill/agents/robots/xarm6/xarm6_nogripper.py,sha256=FPhOpWQw5RPsSHLhZ9JWjYeh25GboO4I5_Hn05Ub84Q,7379
-mani_skill/agents/robots/xarm6/xarm6_robotiq.py,sha256=FIc_BvV7x5Fs9GsE5-m88NaSjPitnDYIGU2ZJbqE-m4,15651
+mani_skill/agents/robots/xarm6/xarm6_robotiq.py,sha256=80DsYdfcPbqL-o1VjZ_KkTWweEH1a9NS9y4lFHCtMSk,16129
 mani_skill/assets/maniskill2-scene-2.glb,sha256=C5om9o9r6B-fWoauzNfUm2WV5sh8Nf7AvZRlYo1-IXQ,4737204
 mani_skill/assets/deformable_manipulation/beaker.glb,sha256=MMaoH6OruVSzO8CKuK2AMyaxA5kjsbbDQXyTVycWsPI,18104
 mani_skill/assets/deformable_manipulation/bottle.glb,sha256=AHWoATBEBeesfbiYNfSB0O0PWhsH0oa2wUBv79w9AVA,36476
@@ -587,7 +587,7 @@ mani_skill/envs/tasks/humanoid/assets/cardboard_box/01.png,sha256=UAzLp_o885YkTu
 mani_skill/envs/tasks/humanoid/assets/cardboard_box/box.mtl,sha256=E7F4ELdSP2Yd62T_rF_-gN1xa6N6Eo3AqAb17R5TSa8,219
 mani_skill/envs/tasks/humanoid/assets/cardboard_box/textured.obj,sha256=XQEt4EeMiW4kx4GhNOYk8TPi7lSuS6XTEBQjWSsxKJ0,12723
 mani_skill/envs/tasks/mobile_manipulation/__init__.py,sha256=GQajIA-t_p_9Hrhkj5-MEEmbGfaIkCYgaDepVUdhJ2Q,98
-mani_skill/envs/tasks/mobile_manipulation/open_cabinet_drawer.py,sha256=RBSjXseWSulU76gMIkDdAAhghZ2Wp38qF3hxb21UI8I,15925
+mani_skill/envs/tasks/mobile_manipulation/open_cabinet_drawer.py,sha256=xokzhopVVnNJJjm0kyw7OlvrdADmIz7rkxyXG6XPj9E,15938
 mani_skill/envs/tasks/mobile_manipulation/robocasa/__init__.py,sha256=mjpK3FNM5GnR4CaWfD1i2gVk5tU4N6tTkDm2pCqsfoo,40
 mani_skill/envs/tasks/mobile_manipulation/robocasa/kitchen.py,sha256=fXLoTIDgk-0eEaH7TrCODYWfVOVkGP6cRzinyfKEkWc,25625
 mani_skill/envs/tasks/quadruped/__init__.py,sha256=_AHS3H9Cj2XT7--5jwW1NMmVWbZxRpdGDNXudT6OrZM,92
@@ -602,7 +602,7 @@ mani_skill/envs/tasks/tabletop/pick_cube.py,sha256=dQC3smYUttMLJHXAFMO31vFZKzQ4N
 mani_skill/envs/tasks/tabletop/pick_cube_cfgs.py,sha256=Zz_F3QI_sSqKNDsngp1mzPgkiyewVu47N2iHz522uOg,2166
 mani_skill/envs/tasks/tabletop/pick_single_ycb.py,sha256=mrqEoOa9UVF34Z5fpsvjcr683diUffsKEjJ9Zh0qfFU,10409
 mani_skill/envs/tasks/tabletop/place_sphere.py,sha256=J3ReBFK7TyZQlleIFspz7Pl1wqAzaYoveGZfNNL5DVM,10101
-mani_skill/envs/tasks/tabletop/plug_charger.py,sha256=0g-rkNf-oo2ovttlcQ58jqUq6So4SKvYMTHORZUOi_0,10571
+mani_skill/envs/tasks/tabletop/plug_charger.py,sha256=jgLD2o0eRj-raAEh3ma8OLZ09es6wg8wX1ckWBdr53o,10710
 mani_skill/envs/tasks/tabletop/poke_cube.py,sha256=KV6mp-Xgm9h4GYUcAUop2AZ4IECTdQKEMRRd9NThyBo,9343
 mani_skill/envs/tasks/tabletop/pull_cube.py,sha256=txtWuIRFzTKxJtWbrygxVEgOPDvm73WlERXldjlUB-I,5546
 mani_skill/envs/tasks/tabletop/pull_cube_tool.py,sha256=NaZpdbYYL4zC41GVY__eq4uRIQpVXthzAqe5oSq8YWU,9951
@@ -669,6 +669,14 @@ mani_skill/examples/motionplanning/panda/solutions/pull_cube.py,sha256=rQkvCRYjV
 mani_skill/examples/motionplanning/panda/solutions/pull_cube_tool.py,sha256=g6adx921V2SOVYYFlh_gLwV5I0tnz70qCLm81oA6YhA,3609
 mani_skill/examples/motionplanning/panda/solutions/push_cube.py,sha256=EynyseBJ_njMP74o9gVxqWOOqoC5j1rBc4XQzFug9EQ,1113
 mani_skill/examples/motionplanning/panda/solutions/stack_cube.py,sha256=QIa280jNOJfJqqgbb5WWEBxErFPE7Mv4-_ZL9TCsRos,3280
+mani_skill/examples/motionplanning/xarm6/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+mani_skill/examples/motionplanning/xarm6/motionplanner.py,sha256=OcJ2OgY09j3kKl7tFa8GcuGGknF999I_bWZGHZF8lps,4751
+mani_skill/examples/motionplanning/xarm6/run.py,sha256=yejnUUasyllXZYWpWD9ksvdjgM8o9TwGVfY24kXQxSI,7053
+mani_skill/examples/motionplanning/xarm6/solutions/__init__.py,sha256=csJg82Tjow5MsYEQEY86ETdPoR59s55KwSg4xaEVyy0,191
+mani_skill/examples/motionplanning/xarm6/solutions/pick_cube.py,sha256=7gKuTgGrQOuSQr32nZDg07IRk2Dp9jIRCl3TZ1xlPsc,2566
+mani_skill/examples/motionplanning/xarm6/solutions/plug_charger.py,sha256=myvHjl9Gc2RM2UYb12cOi56C3QbEbeNmvUBLMiHcHp0,3895
+mani_skill/examples/motionplanning/xarm6/solutions/push_cube.py,sha256=iiZHwvOGGAaxPkzE_sAN6WdTYfcxEMVx0Da3e2rzL48,1419
+mani_skill/examples/motionplanning/xarm6/solutions/stack_cube.py,sha256=zarkqZ2MSfBu-Uv7DNNiRmqyv_JLPiIWcpONGyfbf0Y,3595
 mani_skill/examples/teleoperation/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 mani_skill/examples/teleoperation/interactive_panda.py,sha256=NsGy_ghtXl3HPbwLjKINkizOXqX_rMr30lUfscmhyQ4,10423
 mani_skill/render/__init__.py,sha256=Uy6h1bzammUO8QVPVCDcuCuhnuN3e5votaho45drAGw,118
@@ -698,7 +706,7 @@ mani_skill/utils/registration.py,sha256=u8ftfGvQP4qzlKNqLQjGi3cRF_-h6Rz-28xbLkW_
 mani_skill/utils/sapien_utils.py,sha256=QMV0jRZO51KzIMB5CVW_Ne-4fPw0-mqM4a3yhNZaMYo,16430
 mani_skill/utils/assets/README.md,sha256=5kkmsIiV64ySEGO34HaAlpjXTyrGs1KTV5WnofK46G0,70
 mani_skill/utils/assets/__init__.py,sha256=gQVKwAczcImTXArSltBWKlSUUuguO12sZYO3Jh5KLso,159
-mani_skill/utils/assets/data.py,sha256=YRobBeCxdf23kJxMac9J-dvvJoLSck8mQTbPZGeIAi4,8591
+mani_skill/utils/assets/data.py,sha256=7TsxGBVL05Cg6TLjzx9Nv93uzLby1TaatHs-m15oHew,8686
 mani_skill/utils/building/__init__.py,sha256=quCI5WYGhzGLMVg_NDyYv2G_MxRTBL8R6XD4a6iY8qc,218
 mani_skill/utils/building/_mjcf_loader.py,sha256=SqzSoRootFvItHrzwrDuSHScePxbaPqWb7262M7HzIU,37011
 mani_skill/utils/building/actor_builder.py,sha256=sTnpoThOqWegD-87Q7GyaVvB40fiWUwPOQpYkth-sis,14956
@@ -776,8 +784,8 @@ mani_skill/utils/scene_builder/table/assets/Dining_Table_204_1.glb,sha256=IleHi3
 mani_skill/utils/scene_builder/table/assets/table.glb,sha256=yw69itZDjBFg8JXZAr9VQV-dZD-MaZChhqBSJR_nlRo,3891588
 mani_skill/utils/structs/README.md,sha256=qnYKimp_ZkgNcduURrYQxVTimNmq_usDMKoQ8VtMdCs,286
 mani_skill/utils/structs/__init__.py,sha256=BItR3Xe0z6xCrMHAEaH0AAAVyeonsQ3q-DJUyRUibAA,524
-mani_skill/utils/structs/actor.py,sha256=ImzUZULVnsaBksk5Zu2FD-GgYpfg1u4XZcH1jCtd-vQ,16772
-mani_skill/utils/structs/articulation.py,sha256=6olG0RxQ84J8p7479Mn3bdy6tsMKLpWKMu3NRbbgzfo,34235
+mani_skill/utils/structs/actor.py,sha256=L0p6vkr8rGtJmF22xAq8Q7nhXKnDD5dahzODSAko0bg,17394
+mani_skill/utils/structs/articulation.py,sha256=c7e_Rfkhskg_Rqz1bkesqHCTzNubObLGZQS0BkkFZF0,34691
 mani_skill/utils/structs/articulation_joint.py,sha256=TY6joQ0RpnVlHbQHdtx_QQYqTWgFHLiZ642SSWZUuTw,11736
 mani_skill/utils/structs/base.py,sha256=meGQK5Y4KtHKLnp9VeOZS2gtwg9tE55whuEeqOguBaI,19465
 mani_skill/utils/structs/decorators.py,sha256=Lv6wQ989dOnreo2tB-qopDnkeBp_jsn1pmfUR-OY8VQ,535
@@ -802,8 +810,8 @@ mani_skill/vector/wrappers/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJ
 mani_skill/vector/wrappers/gymnasium.py,sha256=v1MDPIrVACBKCulrpdXBK2jDZQI7LKYFZgGgaCC5avY,7408
 mani_skill/vector/wrappers/sb3.py,sha256=SlXdiEPqcNHYMhJCzA29kBU6zK7DKTe1nc0L6Z3QQtY,4722
 mani_skill/viewer/__init__.py,sha256=srvDBsk4LQU75K2VIttrhiQ68p_ro7PSDqQRls2PY5c,1722
-mani_skill_nightly-2025.5.3.1545.dist-info/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
-mani_skill_nightly-2025.5.3.1545.dist-info/METADATA,sha256=Rg4NFpfjPnBr_gJij40MK3UbXVVocui1dkrKW3xecf0,9291
-mani_skill_nightly-2025.5.3.1545.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
-mani_skill_nightly-2025.5.3.1545.dist-info/top_level.txt,sha256=bkBgOVl_MZMoQx2aRFsSFEYlZLxjWlip5vtJ39FB3jA,11
-mani_skill_nightly-2025.5.3.1545.dist-info/RECORD,,
+mani_skill_nightly-2025.5.3.1612.dist-info/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
+mani_skill_nightly-2025.5.3.1612.dist-info/METADATA,sha256=4nixw1uljNAUYQ0gC6Th21pC-pStemPbqA4SlftUBdU,9291
+mani_skill_nightly-2025.5.3.1612.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
+mani_skill_nightly-2025.5.3.1612.dist-info/top_level.txt,sha256=bkBgOVl_MZMoQx2aRFsSFEYlZLxjWlip5vtJ39FB3jA,11
+mani_skill_nightly-2025.5.3.1612.dist-info/RECORD,,