mani-skill-nightly 2025.5.3.813__py3-none-any.whl → 2025.5.3.1548__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/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/trajectory/replay_trajectory.py

@@ -557,6 +557,8 @@ def main(args: Args):
         )
 
     else:
+        env_kwargs["num_envs"] = 1
+        ori_env_kwargs["num_envs"] = 1
         if args.num_envs > 1:
             pool = mp.Pool(args.num_envs)
             proc_args = [

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_nightly-2025.5.3.813.dist-info → mani_skill_nightly-2025.5.3.1548.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: mani-skill-nightly
-Version: 2025.5.3.813
+Version: 2025.5.3.1548
 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.813.dist-info → mani_skill_nightly-2025.5.3.1548.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
@@ -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
@@ -681,7 +689,7 @@ mani_skill/sensors/depth_camera.py,sha256=KCT7DMqQacVag_24MjkKAml87T6FtDqNS0TJFf
 mani_skill/trajectory/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 mani_skill/trajectory/dataset.py,sha256=nrG3jkhdzRqAdjxC_c8Z4FxpkvW3A9XPvUp9-Ux_u38,6351
 mani_skill/trajectory/merge_trajectory.py,sha256=zsjRMTsiIirZGIV4KrtYOM2-zoOAzd7ObZEdWGJzZbE,3685
-mani_skill/trajectory/replay_trajectory.py,sha256=cnqQX9ThXhnTSGyzp4CrQRLESXH9UgDLe-7iJfTm67s,27000
+mani_skill/trajectory/replay_trajectory.py,sha256=AtmsIV4Oj_vh6MDbHV5Kyrlv7J6mXSHvAWwjuFBVMZo,27074
 mani_skill/trajectory/utils/__init__.py,sha256=Nchv09IpXv0FOgpf7Ng1Ekus6ZfAh3kI0KJs-79QOig,1515
 mani_skill/trajectory/utils/actions/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 mani_skill/trajectory/utils/actions/conversion.py,sha256=x88C64ke44gB-HEbqq_gSRFv34L7irSwT_wYttkQUn8,12922
@@ -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
@@ -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.813.dist-info/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
-mani_skill_nightly-2025.5.3.813.dist-info/METADATA,sha256=J-PnBnL_k7V9P90nfNB_WDfe4KTaoIAiOP2NQ7US2hk,9290
-mani_skill_nightly-2025.5.3.813.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
-mani_skill_nightly-2025.5.3.813.dist-info/top_level.txt,sha256=bkBgOVl_MZMoQx2aRFsSFEYlZLxjWlip5vtJ39FB3jA,11
-mani_skill_nightly-2025.5.3.813.dist-info/RECORD,,
+mani_skill_nightly-2025.5.3.1548.dist-info/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
+mani_skill_nightly-2025.5.3.1548.dist-info/METADATA,sha256=MFsr_RXRUCYN__3kPl9e6aAwQ8wkLN_Cwe4dC4thaB4,9291
+mani_skill_nightly-2025.5.3.1548.dist-info/WHEEL,sha256=tZoeGjtWxWRfdplE7E3d45VPlLNQnvbKiYnx7gwAy8A,92
+mani_skill_nightly-2025.5.3.1548.dist-info/top_level.txt,sha256=bkBgOVl_MZMoQx2aRFsSFEYlZLxjWlip5vtJ39FB3jA,11
+mani_skill_nightly-2025.5.3.1548.dist-info/RECORD,,