Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[BugFix] Fix bug where motion planning example code kept trying to recreate grasp pose visual objects #428

Merged
merged 1 commit into from
Jul 16, 2024
+19 −18
Merged

[BugFix] Fix bug where motion planning example code kept trying to recreate grasp pose visual objects #428

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 19 additions & 18 deletions mani_skill/examples/motionplanning/panda/motionplanner.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,15 +25,16 @@ def __init__(
joint_acc_limits=0.9,
):
self.env = env
self.env_agent: BaseAgent = self.env.unwrapped.agent
self.base_env: BaseEnv = env.unwrapped
self.env_agent: BaseAgent = self.base_env.agent
self.robot = self.env_agent.robot
self.joint_vel_limits = joint_vel_limits
self.joint_acc_limits = joint_acc_limits

self.base_pose = to_sapien_pose(base_pose)

self.planner = self.setup_planner()
self.control_mode = self.env.unwrapped.control_mode
self.control_mode = self.base_env.control_mode

self.debug = debug
self.vis = vis
Expand All @@ -42,10 +43,13 @@ def __init__(
self.gripper_state = OPEN
self.grasp_pose_visual = None
if self.vis and self.visualize_target_grasp_pose:
self.grasp_pose_visual = build_panda_gripper_grasp_pose_visual(
env.unwrapped.scene
)
self.grasp_pose_visual.set_pose(env.unwrapped.agent.tcp.pose)
if "grasp_pose_visual" not in self.base_env.scene.actors:
self.grasp_pose_visual = build_panda_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)
self.elapsed_steps = 0

self.use_point_cloud = False
Expand All @@ -56,11 +60,11 @@ def render_wait(self):
if not self.vis or not self.debug:
return
print("Press [c] to continue")
viewer = self.env.unwrapped.render_human()
viewer = self.base_env.render_human()
while True:
if viewer.window.key_down("c"):
break
self.env.unwrapped.render_human()
self.base_env.render_human()

def setup_planner(self):
link_names = [link.get_name() for link in self.robot.get_links()]
Expand Down Expand Up @@ -93,7 +97,7 @@ def follow_path(self, result, refine_steps: int = 0):
f"[{self.elapsed_steps:3}] Env Output: reward={reward} info={info}"
)
if self.vis:
self.env.unwrapped.render_human()
self.base_env.render_human()
return obs, reward, terminated, truncated, info

def move_to_pose_with_RRTConnect(
Expand All @@ -105,7 +109,7 @@ def move_to_pose_with_RRTConnect(
result = self.planner.plan_qpos_to_pose(
np.concatenate([pose.p, pose.q]),
self.robot.get_qpos().cpu().numpy()[0],
time_step=self.env.unwrapped.control_timestep,
time_step=self.base_env.control_timestep,
use_point_cloud=self.use_point_cloud,
wrt_world=True,
)
Expand All @@ -129,14 +133,14 @@ def move_to_pose_with_screw(
result = self.planner.plan_screw(
np.concatenate([pose.p, pose.q]),
self.robot.get_qpos().cpu().numpy()[0],
time_step=self.env.unwrapped.control_timestep,
time_step=self.base_env.control_timestep,
use_point_cloud=self.use_point_cloud,
)
if result["status"] != "Success":
result = self.planner.plan_screw(
np.concatenate([pose.p, pose.q]),
self.robot.get_qpos().cpu().numpy()[0],
time_step=self.env.unwrapped.control_timestep,
time_step=self.base_env.control_timestep,
use_point_cloud=self.use_point_cloud,
)
if result["status"] != "Success":
Expand All @@ -163,7 +167,7 @@ def open_gripper(self):
f"[{self.elapsed_steps:3}] Env Output: reward={reward} info={info}"
)
if self.vis:
self.env.unwrapped.render_human()
self.base_env.render_human()
return obs, reward, terminated, truncated, info

def close_gripper(self, t=6):
Expand All @@ -181,7 +185,7 @@ def close_gripper(self, t=6):
f"[{self.elapsed_steps:3}] Env Output: reward={reward} info={info}"
)
if self.vis:
self.env.unwrapped.render_human()
self.base_env.render_human()
return obs, reward, terminated, truncated, info

def add_box_collision(self, extents: np.ndarray, pose: sapien.Pose):
Expand All @@ -206,10 +210,7 @@ def clear_collisions(self):
self.use_point_cloud = False

def close(self):
if self.grasp_pose_visual is not None:
if not physx.is_gpu_enabled():
self.grasp_pose_visual.remove_from_scene()

pass

from transforms3d import quaternions

Expand Down