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/sawyer_plate_slide_back_side_v3.py b/metaworld/envs/sawyer_plate_slide_back_side_v3.py index 017a1879a..5e3067555 100644 --- a/metaworld/envs/sawyer_plate_slide_back_side_v3.py +++ b/metaworld/envs/sawyer_plate_slide_back_side_v3.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/sawyer_plate_slide_back_v3.py b/metaworld/envs/sawyer_plate_slide_back_v3.py index 523cf8fd3..a0bb8e07a 100644 --- a/metaworld/envs/sawyer_plate_slide_back_v3.py +++ b/metaworld/envs/sawyer_plate_slide_back_v3.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/sawyer_plate_slide_side_v3.py b/metaworld/envs/sawyer_plate_slide_side_v3.py index ff50ed5b4..d52737dc4 100644 --- a/metaworld/envs/sawyer_plate_slide_side_v3.py +++ b/metaworld/envs/sawyer_plate_slide_side_v3.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/sawyer_plate_slide_v3.py b/metaworld/envs/sawyer_plate_slide_v3.py index 9a7bc77fa..2d702699f 100644 --- a/metaworld/envs/sawyer_plate_slide_v3.py +++ b/metaworld/envs/sawyer_plate_slide_v3.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/sawyer_reach_v3.py b/metaworld/envs/sawyer_reach_v3.py index 215900efb..036bdc61f 100644 --- a/metaworld/envs/sawyer_reach_v3.py +++ b/metaworld/envs/sawyer_reach_v3.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/sawyer_reach_wall_v3.py b/metaworld/envs/sawyer_reach_wall_v3.py index f8ba44eec..278254943 100644 --- a/metaworld/envs/sawyer_reach_wall_v3.py +++ b/metaworld/envs/sawyer_reach_wall_v3.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/sawyer_soccer_v3.py b/metaworld/envs/sawyer_soccer_v3.py index f276cdfb9..02f97b78b 100644 --- a/metaworld/envs/sawyer_soccer_v3.py +++ b/metaworld/envs/sawyer_soccer_v3.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/sawyer_stick_pull_v3.py b/metaworld/envs/sawyer_stick_pull_v3.py index afa307d53..1ddd6a5cc 100644 --- a/metaworld/envs/sawyer_stick_pull_v3.py +++ b/metaworld/envs/sawyer_stick_pull_v3.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/sawyer_stick_push_v3.py b/metaworld/envs/sawyer_stick_push_v3.py index acafec612..9bcfa547a 100644 --- a/metaworld/envs/sawyer_stick_push_v3.py +++ b/metaworld/envs/sawyer_stick_push_v3.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/sawyer_sweep_into_goal_v3.py b/metaworld/envs/sawyer_sweep_into_goal_v3.py index 9890cf0ec..7382263cc 100644 --- a/metaworld/envs/sawyer_sweep_into_goal_v3.py +++ b/metaworld/envs/sawyer_sweep_into_goal_v3.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/sawyer_sweep_v3.py b/metaworld/envs/sawyer_sweep_v3.py index e819cf436..33c5918da 100644 --- a/metaworld/envs/sawyer_sweep_v3.py +++ b/metaworld/envs/sawyer_sweep_v3.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/sawyer_window_close_v3.py b/metaworld/envs/sawyer_window_close_v3.py index 59eef4544..d4b6f8208 100644 --- a/metaworld/envs/sawyer_window_close_v3.py +++ b/metaworld/envs/sawyer_window_close_v3.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/sawyer_window_open_v3.py b/metaworld/envs/sawyer_window_open_v3.py index b729beadf..d7f3dc4ba 100644 --- a/metaworld/envs/sawyer_window_open_v3.py +++ b/metaworld/envs/sawyer_window_open_v3.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(