pixelated jug working

Learning-and-Intelligent-Systems · Dec 12, 2024 · 8be169c · 8be169c
1 parent 4c5147b
commit 8be169c
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 19 deletions.
diff --git a/predicators/envs/coffee.py b/predicators/envs/coffee.py
@@ -722,7 +722,9 @@ def _get_jug_handle_grasp(cls, state: State,
         # Orient pointing down.
         rot = state.get(jug, "rot") - np.pi / 2
         target_x = state.get(jug, "x") + np.cos(rot) * cls.jug_handle_offset
-        target_y = state.get(jug, "y") + np.sin(rot) * cls.jug_handle_offset
+        target_y = state.get(jug, "y") + np.sin(rot) * cls.jug_handle_offset 
+        if CFG.coffee_use_pixelated_jug:
+            target_y -= 0.02
         target_z = cls.z_lb + cls.jug_handle_height
         return (target_x, target_y, target_z)
 

diff --git a/predicators/envs/pybullet_coffee.py b/predicators/envs/pybullet_coffee.py
@@ -122,7 +122,7 @@ class PyBulletCoffeeEnv(PyBulletEnv, CoffeeEnv):
     # Jug setting
     jug_radius: ClassVar[float] = 0.3 * machine_y_len
     # jug_height: ClassVar[float] = 0.19 * (z_ub - z_lb)  # kettle urdf
-    jug_height: ClassVar[float] = 0.03 * (z_ub - z_lb)  # new cup 
+    jug_height: ClassVar[float] = 0.12#0.1 * (z_ub - z_lb)  # new cup 
     jug_init_x_lb: ClassVar[
         float] = machine_x - machine_x_len / 2 + init_padding
     jug_init_x_ub: ClassVar[
@@ -134,7 +134,7 @@ class PyBulletCoffeeEnv(PyBulletEnv, CoffeeEnv):
         float] = machine_y - machine_y_len - 3 * jug_radius - init_padding
     jug_handle_offset: ClassVar[float] = 3 * jug_radius  # kettle urdf
     # jug_handle_height: ClassVar[float] = jug_height # old kettle
-    jug_handle_height: ClassVar[float] =  0.08 # new jug
+    jug_handle_height: ClassVar[float] =  0.1 # new jug
     jug_init_rot_lb: ClassVar[float] = -2 * np.pi / 3
     jug_init_rot_ub: ClassVar[float] = 2 * np.pi / 3
     # jug_color: ClassVar[Tuple[float, float, float, float]] =\
@@ -793,20 +793,17 @@ def _create_pybullet_liquid_for_cup(self, cup: Object,
                                  physicsClientId=self._physics_client_id)
 
     def _create_pybullet_liquid_for_jug(self) -> Optional[int]:
-        liquid_height = self.jug_height * 0.6
+        liquid_height = self.jug_height * 0.8
         liquid_radius = self.jug_radius * 1.3
-
+        
         collision_id = p.createCollisionShape(
-            p.GEOM_CYLINDER,
-            radius=liquid_radius,
-            height=liquid_height,
+            p.GEOM_BOX,
+            halfExtents=[liquid_radius, liquid_radius, liquid_height / 2],
             physicsClientId=self._physics_client_id)
 
         visual_id = p.createVisualShape(
-            p.GEOM_CYLINDER,
-            radius=liquid_radius,
-            length=liquid_height,
-            # rgbaColor=(0.2 * 1.5, 0.05 * 1.5, 0.0, 1.0), # brown
+            p.GEOM_BOX,
+            halfExtents=[liquid_radius, liquid_radius, liquid_height / 2],
             rgbaColor=(0.35, 0.1, 0.0, 1.0),
             physicsClientId=self._physics_client_id)
 
@@ -1032,11 +1029,11 @@ def _add_pybullet_jug(cls, physics_client_id) -> int:
         jug_orientation = p.getQuaternionFromEuler([0.0, 0.0, rot - np.pi / 2])
 
         # Old jug
-        # jug_id = p.loadURDF(
-        #     utils.get_env_asset_path("urdf/kettle.urdf"),
-        #     globalScaling=0.09,  # enlarged jug
-        #     useFixedBase=False,
-        #     physicsClientId=physics_client_id)
+        jug_id = p.loadURDF(
+            utils.get_env_asset_path("urdf/jug-pixel.urdf"),
+            globalScaling=0.2,  # enlarged jug
+            useFixedBase=False,
+            physicsClientId=physics_client_id)
 
         # p.changeVisualShape(jug_id,
         #                     0,
@@ -1054,8 +1051,6 @@ def _add_pybullet_jug(cls, physics_client_id) -> int:
             physicsClientId=physics_client_id
         )
 
-        # new cup
-        # jug_id = cls.create_cup(physics_client_id)
         p.resetBasePositionAndOrientation(jug_id,
                                           jug_loc,
                                           jug_orientation,

diff --git a/predicators/settings.py b/predicators/settings.py
@@ -328,6 +328,7 @@ class GlobalSettings:
     coffee_simple_tasks = False
     coffee_machine_have_light_bar = True
     coffee_machine_has_plug = False
+    coffee_use_pixelated_jug = False
 
     # satellites env parameters
     satellites_num_sat_train = [2, 3]