diff --git a/docs/_static/metaworld-text.svg b/docs/_static/metaworld-text.svg index 05914e4d5..a9a6497d1 100644 --- a/docs/_static/metaworld-text.svg +++ b/docs/_static/metaworld-text.svg @@ -74,7 +74,7 @@ <<<<<<< HEAD ======= - + >>>>>>> 5186a934e5af1905b684bda17e41836d9cf73287 npt.NDArray[np.float64]: self.model.body("coffee_machine").pos = pos_machine self._target_pos = pos_mug_goal - self.model.site("coffee_goal").pos = self._target_pos + self.model.site("mug_goal").pos = self._target_pos return self._get_obs() def compute_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_back_side_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_back_side_v2.py index 212f54fda..5186fb733 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_back_side_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_back_side_v2.py @@ -130,6 +130,8 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.model.body("puck_goal").pos = self.obj_init_pos self._set_obj_xyz(np.array([-0.15, 0.0])) + self.model.site("goal").pos = self._target_pos + return self._get_obs() def compute_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_back_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_back_v2.py index d4ed8b23f..62f8aac86 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_back_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_back_v2.py @@ -108,6 +108,8 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.data.body("puck_goal").xpos = self._target_pos self._set_obj_xyz(np.array([0, 0.15])) + self.model.site("goal").pos = self._target_pos + return self._get_obs() def compute_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_side_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_side_v2.py index f914d18c8..af5ced17e 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_side_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_side_v2.py @@ -108,6 +108,8 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.data.body("puck_goal").xpos = self._target_pos self._set_obj_xyz(np.zeros(2)) + self.model.site("goal").pos = self._target_pos + return self._get_obs() def compute_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_v2.py index bf40e802a..5834cb183 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_plate_slide_v2.py @@ -112,6 +112,8 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.model.body("puck_goal").pos = self._target_pos self._set_obj_xyz(np.zeros(2)) + self.model.site("goal").pos = self._target_pos + return self._get_obs() def compute_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_reach_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_reach_v2.py index 108cbed0e..9ec8b0fcc 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_reach_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_reach_v2.py @@ -125,7 +125,8 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.obj_init_pos = goal_pos[:3] self._set_obj_xyz(self.obj_init_pos) - self._set_pos_site("goal", self._target_pos) + self.model.site("goal").pos = self._target_pos + return self._get_obs() def compute_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_reach_wall_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_reach_wall_v2.py index 1f16c12c4..f4bf2e50f 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_reach_wall_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_reach_wall_v2.py @@ -114,7 +114,7 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.obj_init_pos = goal_pos[:3] self._set_obj_xyz(self.obj_init_pos) - self._set_pos_site("goal", self._target_pos) + self.model.site("goal").pos = self._target_pos return self._get_obs() def compute_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_soccer_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_soccer_v2.py index 9403a4648..9d9eab599 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_soccer_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_soccer_v2.py @@ -117,7 +117,9 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.maxPushDist = np.linalg.norm( self.obj_init_pos[:2] - np.array(self._target_pos)[:2] ) - self._set_pos_site("goal", self._target_pos) + + self.model.site("goal").pos = self._target_pos + return self._get_obs() def _gripper_caging_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_stick_pull_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_stick_pull_v2.py index c94e0acc9..ead9404cb 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_stick_pull_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_stick_pull_v2.py @@ -157,7 +157,9 @@ def reset_model(self) -> npt.NDArray[np.float64]: self._set_stick_xyz(self.stick_init_pos) self._set_obj_xyz(self.obj_init_qpos) self.obj_init_pos = self.get_body_com("object").copy() - self._set_pos_site("goal", self._target_pos) + + self.model.site("goal").pos = self._target_pos + return self._get_obs() def _stick_is_inserted( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_stick_push_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_stick_push_v2.py index 86169aab4..b26d5d63a 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_stick_push_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_stick_push_v2.py @@ -154,7 +154,9 @@ def reset_model(self) -> npt.NDArray[np.float64]: self._set_stick_xyz(self.stick_init_pos) self._set_obj_xyz(self.obj_init_qpos) self.obj_init_pos = self.get_body_com("object").copy() - self._set_pos_site("goal", self._target_pos) + + self.model.site("goal").pos = self._target_pos + return self._get_obs() def _gripper_caging_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_sweep_into_goal_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_sweep_into_goal_v2.py index 433dbacf5..cac10d341 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_sweep_into_goal_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_sweep_into_goal_v2.py @@ -106,10 +106,7 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.obj_init_pos = np.concatenate([goal_pos[:2], [self.obj_init_pos[-1]]]) self._set_obj_xyz(self.obj_init_pos) - self.maxPushDist = np.linalg.norm( - self.obj_init_pos[:2] - np.array(self._target_pos)[:2] - ) - self._set_pos_site("goal", self._target_pos) + self.model.site("goal").pos = self._target_pos return self._get_obs() def _gripper_caging_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_sweep_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_sweep_v2.py index 1afc2971e..667276212 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_sweep_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_sweep_v2.py @@ -101,11 +101,7 @@ def reset_model(self) -> npt.NDArray[np.float64]: self._target_pos[1] = obj_pos.copy()[1] self._set_obj_xyz(self.obj_init_pos) - self.maxPushDist = np.linalg.norm( - self.get_body_com("obj")[:-1] - self._target_pos[:-1] - ) - self.target_reward = 1000 * self.maxPushDist + 1000 * 2 - self._set_pos_site("goal", self._target_pos) + self.model.site("goal").pos = self._target_pos return self._get_obs() def _gripper_caging_reward( diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_window_close_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_window_close_v2.py index b9f2dc128..86237c047 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_window_close_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_window_close_v2.py @@ -117,7 +117,7 @@ def reset_model(self) -> npt.NDArray[np.float64]: [0.2, 0.0, 0.0] ) self.data.joint("window_slide").qpos = 0.2 - self._set_pos_site("goal", self._target_pos) + self.model.site("goal").pos = self._target_pos return self._get_obs() def _reset_hand(self, steps: int = 50) -> None: diff --git a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_window_open_v2.py b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_window_open_v2.py index 7ed1f00ca..91ab1d0e8 100644 --- a/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_window_open_v2.py +++ b/metaworld/envs/mujoco/sawyer_xyz/v2/sawyer_window_open_v2.py @@ -112,7 +112,9 @@ def reset_model(self) -> npt.NDArray[np.float64]: self.window_handle_pos_init = self._get_pos_objects() self.data.joint("window_slide").qpos = 0.0 assert self._target_pos is not None - self._set_pos_site("goal", self._target_pos) + + self.model.site("goal").pos = self._target_pos + return self._get_obs() def compute_reward(