updated camera pose in all envs

perezjln · Oct 16, 2024 · 48e38c5 · 48e38c5
1 parent 75fd252
commit 48e38c5
Show file tree

Hide file tree

Showing 7 changed files with 17 additions and 18 deletions.
diff --git a/gym_lowcostrobot/assets/low_cost_robot_6dof/lift_cube.xml b/gym_lowcostrobot/assets/low_cost_robot_6dof/lift_cube.xml
@@ -28,8 +28,8 @@
           <geom friction="0.5" condim="4" pos="0 0 0" size="0.015 0.015 0.015" type="box" name="red_box" rgba="0.5 0 0 1" priority="1"/>
       </body>
 
-	  <camera name="camera_front" pos="0.049 0.888 0.317" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
-	  <camera name="camera_top" pos="0 0 1" euler="0 0 0" mode="fixed"/>
+		<camera name="camera_front" pos="0.049 0.5 0.225" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
+		<camera name="camera_top" pos="0 0.1 0.6" euler="0 0 0" mode="fixed"/>
 	  <camera name="camera_vizu" pos="-0.1 0.6 0.3" quat="-0.15 -0.1 0.6 1"/>
 
     </worldbody>

diff --git a/gym_lowcostrobot/assets/low_cost_robot_6dof/pick_place_cube.xml b/gym_lowcostrobot/assets/low_cost_robot_6dof/pick_place_cube.xml
@@ -29,8 +29,8 @@
           <geom friction="0.5" condim="4" pos="0 0 0" size="0.015 0.015 0.015" type="box" name="red_box" rgba="0.5 0 0 1" priority="1"/>
       </body>
 
-	  <camera name="camera_front" pos="0.049 0.888 0.317" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
-	  <camera name="camera_top" pos="0 0 1" euler="0 0 0" mode="fixed"/>
+		<camera name="camera_front" pos="0.049 0.5 0.225" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
+		<camera name="camera_top" pos="0 0.1 0.6" euler="0 0 0" mode="fixed"/>
 	  <camera name="camera_vizu" pos="-0.1 0.6 0.3" quat="-0.15 -0.1 0.6 1"/>
 
     <geom name="target_region" type="cylinder" pos=".06 .135 0.005" size="0.035 0.01" rgba="0 0 1 0.3"  contype="0" conaffinity="0" />

diff --git a/gym_lowcostrobot/assets/low_cost_robot_6dof/push_cube.xml b/gym_lowcostrobot/assets/low_cost_robot_6dof/push_cube.xml
@@ -28,8 +28,8 @@
           <geom friction="0.5" condim="4" pos="0 0 0" size="0.015 0.015 0.015" type="box" name="red_box" rgba="0.5 0 0 1" priority="1"/>
       </body>
 
-	  <camera name="camera_front" pos="0.049 0.888 0.317" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
-	  <camera name="camera_top" pos="0 0 1" euler="0 0 0" mode="fixed"/>
+		<camera name="camera_front" pos="0.049 0.5 0.225" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
+		<camera name="camera_top" pos="0 0.1 0.6" euler="0 0 0" mode="fixed"/>
 	  <camera name="camera_vizu" pos="-0.1 0.6 0.3" quat="-0.15 -0.1 0.6 1"/>
 
     <geom name="target_region" type="cylinder" pos=".06 .135 0.005" size="0.035 0.01" rgba="0 0 1 0.3"  contype="0" conaffinity="0" />

diff --git a/gym_lowcostrobot/assets/low_cost_robot_6dof/push_cube_loop.xml b/gym_lowcostrobot/assets/low_cost_robot_6dof/push_cube_loop.xml
@@ -30,8 +30,8 @@
 			<geom friction="1.5 1.5 1.5" condim="4" pos="0 0 0" size="0.015 0.015 0.015" type="box" name="cube" rgba="0.5 0 0 1" priority="1"/>
 		</body>
 
-		<camera name="camera_front" pos="0.049 0.888 0.317" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
-		<camera name="camera_top" pos="0 0 1" euler="0 0 0" mode="fixed"/>
+		<camera name="camera_front" pos="0.049 0.5 0.225" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
+		<camera name="camera_top" pos="0 0.1 0.6" euler="0 0 0" mode="fixed"/>
 		<camera name="camera_vizu" pos="-0.1 0.6 0.3" quat="-0.15 -0.1 0.6 1"/>
 
         <!-- Goal Region 1 -->

diff --git a/gym_lowcostrobot/assets/low_cost_robot_6dof/reach_cube.xml b/gym_lowcostrobot/assets/low_cost_robot_6dof/reach_cube.xml
@@ -26,8 +26,8 @@
           <geom friction="0.5" condim="4" pos="0 0 0" size="0.015 0.015 0.015" type="box" name="red_box" rgba="0.5 0 0 1" priority="1"/>
       </body>
 
-      <camera name="camera_front" pos="0.049 0.888 0.317" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
-      <camera name="camera_top" pos="0 0 1" euler="0 0 0" mode="fixed"/>
+		<camera name="camera_front" pos="0.049 0.5 0.225" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
+		<camera name="camera_top" pos="0 0.1 0.6" euler="0 0 0" mode="fixed"/>
       <camera name="camera_vizu" pos="-0.2 0.6 0.3" quat="-0.15 -0.1 0.6 1"/>
 
     </worldbody>

diff --git a/gym_lowcostrobot/assets/low_cost_robot_6dof/stack_two_cubes.xml b/gym_lowcostrobot/assets/low_cost_robot_6dof/stack_two_cubes.xml
@@ -34,8 +34,8 @@
 	    <geom friction="0.5" condim="4" pos="0 0 0" size="0.015 0.015 0.015" type="box" name="cube_blue" rgba="0 0 0.5 1" priority="1"/>
     </body>
 
-	  <camera name="camera_front" pos="0.049 0.888 0.317" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
-	  <camera name="camera_top" pos="0 0 1" euler="0 0 0" mode="fixed"/>
+		<camera name="camera_front" pos="0.049 0.5 0.225" xyaxes="-0.998 0.056 -0.000 -0.019 -0.335 0.942"/>
+		<camera name="camera_top" pos="0 0.1 0.6" euler="0 0 0" mode="fixed"/>
 	  <camera name="camera_vizu" pos="-0.1 0.6 0.3" quat="-0.15 -0.1 0.6 1"/>
 
     </worldbody>

diff --git a/gym_lowcostrobot/envs/stack_two_cubes_env.py b/gym_lowcostrobot/envs/stack_two_cubes_env.py
@@ -219,14 +219,14 @@ def get_observation(self):
             "arm_qpos": self.data.qpos[self.arm_dof_id+self.arm_dof_id+self.nb_dof].astype(np.float32),
             "arm_qvel": self.data.qvel[self.arm_dof_vel_id:self.arm_dof_vel_id+self.nb_dof].astype(np.float32),
         }
+        if self.observation_mode in ["state", "both"]:
+            observation["cube_red_pos"] = self.data.qpos[self.red_cube_dof_id:self.red_cube_dof_id+3].astype(np.float32)
+            observation["cube_blue_pos"] = self.data.qpos[self.blue_cube_dof_id:self.blue_cube_dof_id+3].astype(np.float32)
         if self.observation_mode in ["image", "both"]:
             self.renderer.update_scene(self.data, camera="camera_front")
             observation["image_front"] = self.renderer.render()
             self.renderer.update_scene(self.data, camera="camera_top")
             observation["image_top"] = self.renderer.render()
-        if self.observation_mode in ["state", "both"]:
-            observation["cube_red_pos"] = self.data.qpos[self.red_cube_dof_id:self.red_cube_dof_id+3].astype(np.float32)
-            observation["cube_blue_pos"] = self.data.qpos[self.blue_cube_dof_id:self.blue_cube_dof_id+3].astype(np.float32)
         return observation
 
     def reset(self, seed=None, options=None):
@@ -243,10 +243,9 @@ def reset(self, seed=None, options=None):
         self.data.qpos[self.red_cube_dof_id:self.red_cube_dof_id+7] = np.concatenate([cube_red_pos, cube_red_rot])
         self.data.qpos[self.blue_cube_dof_id:self.blue_cube_dof_id+7] = np.concatenate([cube_blue_pos, cube_blue_rot])
 
-
         # Step the simulation
-        mujoco.mj_forward(self.model, self.data)
-
+        mujoco.mj_forward(self.model, self.data)   
+        
         return self.get_observation(), {}
 
     def step(self, action):