mani-skill-nightly 2025.9.14.2244__py3-none-any.whl → 2025.9.14.2354__py3-none-any.whl

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

@@ -7,7 +7,7 @@ from transforms3d.euler import euler2quat
 from mani_skill.envs.tasks import StackCubeEnv
 from mani_skill.examples.motionplanning.panda.motionplanner import \
     PandaArmMotionPlanningSolver
-from mani_skill.examples.motionplanning.panda.utils import (
+from mani_skill.examples.motionplanning.base_motionplanner.utils import (
     compute_grasp_info_by_obb, get_actor_obb)
 from mani_skill.utils.wrappers.record import RecordEpisode
 

mani_skill/examples/motionplanning/panda/solutions/stack_pyramid.py

@@ -7,7 +7,7 @@ from transforms3d.euler import euler2quat
 from mani_skill.envs.tasks import StackPyramidEnv
 from mani_skill.examples.motionplanning.panda.motionplanner import \
     PandaArmMotionPlanningSolver
-from mani_skill.examples.motionplanning.panda.utils import (
+from mani_skill.examples.motionplanning.base_motionplanner.utils import (
     compute_grasp_info_by_obb, get_actor_obb)
 from mani_skill.utils.wrappers.record import RecordEpisode
 from mani_skill.utils.structs import Pose

mani_skill/examples/motionplanning/so100/motionplanner.py

@@ -0,0 +1,40 @@
+import mplib
+import numpy as np
+import sapien
+
+from mani_skill.envs.sapien_env import BaseEnv
+from mani_skill.utils.structs.pose import to_sapien_pose
+from transforms3d import euler
+from mani_skill.examples.motionplanning.two_finger_gripper.motionplanner import TwoFingerGripperMotionPlanningSolver
+
+
+class SO100ArmMotionPlanningSolver (TwoFingerGripperMotionPlanningSolver):
+    OPEN = 0
+    CLOSED = -0.8
+    MOVE_GROUP = "Fixed_Jaw_tip"
+
+    def __init__(
+        self,
+        env: BaseEnv,
+        debug: bool = False,
+        vis: bool = True,
+        base_pose: sapien.Pose = None,  # TODO mplib doesn't support robot base being anywhere but 0
+        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._so_100_visual_grasp_pose_transform = sapien.Pose(q=euler.euler2quat(0, -np.pi / 2, np.pi / 2))
+
+    @property
+    def _so_100_grasp_pose_tcp_transform(self):
+        return self.base_env.agent.robot.links_map["Fixed_Jaw_tip"].pose.sp * self.base_env.agent.tcp_pose.sp.inv()
+
+    def _update_grasp_visual(self, target: sapien.Pose) -> None:
+        if self.grasp_pose_visual is not None:
+            self.grasp_pose_visual.set_pose(target * self._so_100_visual_grasp_pose_transform)
+
+    def _transform_pose_for_planning(self, target: sapien.Pose) -> sapien.Pose:
+        return sapien.Pose(p=target.p + self._so_100_grasp_pose_tcp_transform.p, q=target.q)
+

mani_skill/examples/motionplanning/so100/run.py

@@ -0,0 +1,142 @@
+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.so100.solutions import solvePickCube
+MP_SOLUTIONS = {
+    "PickCubeSO100-v1": solvePickCube,
+}
+def parse_args(args=None):
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-e", "--env-id", type=str, default="PickCubeSO100-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 = 9) -> 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,
+        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,
+        record_reward=False,
+        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 = [0]
+    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/so100/solutions/__init__.py

@@ -0,0 +1 @@
+from .pick_cube import solve as solvePickCube

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

@@ -0,0 +1,70 @@
+import numpy as np
+import sapien
+from transforms3d.euler import euler2quat
+
+from mani_skill.envs.tasks import PickCubeEnv
+from mani_skill.examples.motionplanning.so100.motionplanner import \
+    SO100ArmMotionPlanningSolver
+from mani_skill.examples.motionplanning.base_motionplanner.utils import (
+    compute_grasp_info_by_obb, get_actor_obb)
+
+def solve(env: PickCubeEnv, seed=None, debug=False, vis=False):
+    env.reset(seed=seed)
+    planner = SO100ArmMotionPlanningSolver(
+        env,
+        debug=False,
+        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])
+
+    # rotate around x-axis to align with the expected frame for computing grasp poses (Z is up/down)
+    tcp_pose = sapien.Pose(q=euler2quat(np.pi / 2, 0, 0)) * env.agent.tcp_pose.sp
+    target_closing = (tcp_pose ).to_transformation_matrix()[:3, 1]
+    # we can build a simple grasp pose using this information for Panda
+    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)
+
+    # due to how SO100 is defined we need to transform the grasp pose back to what is expected by SO100
+    grasp_pose = grasp_pose * sapien.Pose(q=euler2quat(-np.pi/2, 0*np.pi / 2, np.pi / 2))
+
+    # -------------------------------------------------------------------------- #
+    # Reach
+    # -------------------------------------------------------------------------- #
+    planner.gripper_state = 0
+    reach_pose = sapien.Pose([0, 0, 0.03]) * grasp_pose
+    planner.move_to_pose_with_screw(reach_pose)
+    
+    # reach_pose = sapien.Pose([0, 0, -0.02]) * env.agent.tcp_pose.sp
+    # planner.move_to_pose_with_screw(sapien.Pose(reach_pose.p, env.agent.tcp_pose.sp.q))
+
+    # -------------------------------------------------------------------------- #
+    # Grasp
+    # -------------------------------------------------------------------------- #
+    planner.move_to_pose_with_screw(sapien.Pose([0, 0, 0.01]) * grasp_pose)
+    planner.close_gripper(gripper_state=-0.8)
+
+    # -------------------------------------------------------------------------- #
+    # 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)
+    res = planner.move_to_pose_with_screw(goal_pose)
+
+    planner.close()
+    return res

mani_skill/examples/motionplanning/two_finger_gripper/motionplanner.py

@@ -0,0 +1,149 @@
+import mplib
+import numpy as np
+import sapien
+
+from mani_skill.envs.sapien_env import BaseEnv
+from mani_skill.envs.scene import ManiSkillScene
+from mani_skill.examples.motionplanning.base_motionplanner.motionplanner import BaseMotionPlanningSolver
+from transforms3d import quaternions
+
+
+class TwoFingerGripperMotionPlanningSolver(BaseMotionPlanningSolver):
+    OPEN = 1
+    CLOSED = -1
+
+    def __init__(
+        self,
+        env: BaseEnv,
+        debug: bool = False,
+        vis: bool = True,
+        base_pose: sapien.Pose = None,  # TODO mplib doesn't support robot base being anywhere but 0
+        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, print_env_info, joint_vel_limits, joint_acc_limits)
+        self.gripper_state = self.OPEN
+        self.visualize_target_grasp_pose = visualize_target_grasp_pose
+        self.grasp_pose_visual = None
+        if self.vis and self.visualize_target_grasp_pose:
+            if "grasp_pose_visual" not in self.base_env.scene.actors:
+                self.grasp_pose_visual = build_two_finger_gripper_grasp_pose_visual(
+                    self.base_env.scene
+                )
+            else:
+                self.grasp_pose_visual = self.base_env.scene.actors["grasp_pose_visual"]
+            self.grasp_pose_visual.set_pose(self.base_env.agent.tcp_pose)
+
+    def _update_grasp_visual(self, target: sapien.Pose) -> None:
+        if self.grasp_pose_visual is not None:
+            self.grasp_pose_visual.set_pose(target)
+    
+    def follow_path(self, result, refine_steps: int = 0):
+        n_step = result["position"].shape[0]
+        for i in range(n_step + refine_steps):
+            qpos = result["position"][min(i, n_step - 1)]
+            if self.control_mode == "pd_joint_pos_vel":
+                qvel = result["velocity"][min(i, n_step - 1)]
+                action = np.hstack([qpos, qvel, self.gripper_state])
+            else:
+                action = np.hstack([qpos, 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 open_gripper(self, t=6, gripper_state=None):
+        if gripper_state is None:
+            gripper_state = self.OPEN
+        self.gripper_state = gripper_state
+        qpos = self.robot.get_qpos()[0, : len(self.planner.joint_vel_limits)].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 close_gripper(self, t=6, gripper_state=None):
+        if gripper_state is None:
+            gripper_state = self.CLOSED
+        self.gripper_state = gripper_state
+        qpos = self.robot.get_qpos()[0, : len(self.planner.joint_vel_limits)].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 build_two_finger_gripper_grasp_pose_visual(scene: ManiSkillScene):
+    builder = scene.create_actor_builder()
+    grasp_pose_visual_width = 0.01
+    grasp_width = 0.05
+
+    builder.add_sphere_visual(
+        pose=sapien.Pose(p=[0, 0, 0.0]),
+        radius=grasp_pose_visual_width,
+        material=sapien.render.RenderMaterial(base_color=[0.3, 0.4, 0.8, 0.7])
+    )
+
+    builder.add_box_visual(
+        pose=sapien.Pose(p=[0, 0, -0.08]),
+        half_size=[grasp_pose_visual_width, grasp_pose_visual_width, 0.02],
+        material=sapien.render.RenderMaterial(base_color=[0, 1, 0, 0.7]),
+    )
+    builder.add_box_visual(
+        pose=sapien.Pose(p=[0, 0, -0.05]),
+        half_size=[grasp_pose_visual_width, grasp_width, grasp_pose_visual_width],
+        material=sapien.render.RenderMaterial(base_color=[0, 1, 0, 0.7]),
+    )
+    builder.add_box_visual(
+        pose=sapien.Pose(
+            p=[
+                0.03 - grasp_pose_visual_width * 3,
+                grasp_width + grasp_pose_visual_width,
+                0.03 - 0.05,
+            ],
+            q=quaternions.axangle2quat(np.array([0, 1, 0]), theta=np.pi / 2),
+        ),
+        half_size=[0.04, grasp_pose_visual_width, grasp_pose_visual_width],
+        material=sapien.render.RenderMaterial(base_color=[0, 0, 1, 0.7]),
+    )
+    builder.add_box_visual(
+        pose=sapien.Pose(
+            p=[
+                0.03 - grasp_pose_visual_width * 3,
+                -grasp_width - grasp_pose_visual_width,
+                0.03 - 0.05,
+            ],
+            q=quaternions.axangle2quat(np.array([0, 1, 0]), theta=np.pi / 2),
+        ),
+        half_size=[0.04, grasp_pose_visual_width, grasp_pose_visual_width],
+        material=sapien.render.RenderMaterial(base_color=[1, 0, 0, 0.7]),
+    )
+    grasp_pose_visual = builder.build_kinematic(name="grasp_pose_visual")
+    return grasp_pose_visual

mani_skill/examples/motionplanning/xarm6/motionplanner.py

@@ -1,111 +1,22 @@
 import numpy as np
+import sapien
 import mplib
-from mani_skill.examples.motionplanning.panda.motionplanner import PandaArmMotionPlanningSolver
+from mani_skill.examples.motionplanning.two_finger_gripper.motionplanner import TwoFingerGripperMotionPlanningSolver
 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):
+class XArm6RobotiqMotionPlanningSolver(TwoFingerGripperMotionPlanningSolver):
     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
+    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,
     ):
-        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
-    
+        super().__init__(env, debug, vis, base_pose, visualize_target_grasp_pose, print_env_info, joint_vel_limits, joint_acc_limits)

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

@@ -3,8 +3,8 @@ 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 (
+    XArm6RobotiqMotionPlanningSolver
+from mani_skill.examples.motionplanning.base_motionplanner.utils import (
     compute_grasp_info_by_obb, get_actor_obb)
 
 def solve(env: PickCubeEnv, seed=None, debug=False, vis=False):
@@ -12,8 +12,6 @@ def solve(env: PickCubeEnv, seed=None, debug=False, vis=False):
 
     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(

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

@@ -7,8 +7,8 @@ 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 (
+    XArm6RobotiqMotionPlanningSolver
+from mani_skill.examples.motionplanning.base_motionplanner.utils import (
     compute_grasp_info_by_obb, get_actor_obb)
 
 
@@ -34,8 +34,6 @@ def solve(env: PlugChargerEnv, seed=None, debug=False, vis=False):
     ], 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(

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

@@ -3,14 +3,12 @@ import sapien
 
 from mani_skill.envs.tasks import PushCubeEnv
 from mani_skill.examples.motionplanning.xarm6.motionplanner import \
-    XArm6RobotiqMotionPlanningSolver, XArm6PandaGripperMotionPlanningSolver
+    XArm6RobotiqMotionPlanningSolver
 
 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(

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

@@ -6,8 +6,8 @@ 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 (
+    XArm6RobotiqMotionPlanningSolver
+from mani_skill.examples.motionplanning.base_motionplanner.utils import (
     compute_grasp_info_by_obb, get_actor_obb)
 from mani_skill.utils.wrappers.record import RecordEpisode
 
@@ -19,8 +19,6 @@ def solve(env: StackCubeEnv, seed=None, debug=False, vis=False):
     ], 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(

mani_skill/examples/teleoperation/interactive_panda.py

@@ -45,6 +45,7 @@ def main(args: Args):
         control_mode="pd_joint_pos",
         render_mode="rgb_array",
         reward_mode="none",
+        robot_uids=args.robot_uid,
         enable_shadow=True,
         viewer_camera_configs=dict(shader_pack=args.viewer_shader)
     )
@@ -90,6 +91,7 @@ def main(args: Args):
             control_mode="pd_joint_pos",
             render_mode="rgb_array",
             reward_mode="none",
+            robot_uids=args.robot_uid,
             human_render_camera_configs=dict(shader_pack=args.video_saving_shader),
         )
         env = RecordEpisode(