update switchOn calculations by incorporating scale

Learning-and-Intelligent-Systems · Jan 5, 2025 · 0b41c48 · 0b41c48
1 parent 66241a5
commit 0b41c48
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/predicators/envs/pybullet_circuit.py b/predicators/envs/pybullet_circuit.py
@@ -221,6 +221,7 @@ def _store_pybullet_bodies(self, pybullet_bodies: Dict[str, Any]) -> None:
         """Store references to PyBullet IDs for environment assets."""
         self._battery.id = pybullet_bodies["battery_id"]
         self._battery.joint_id = self._get_joint_id(self._battery.id, "joint_0")
+        self._battery.joint_scale = 0.1
         self._light.id = pybullet_bodies["light_id"]
         self._wire1.id = pybullet_bodies["wire_ids"][0]
         self._wire2.id = pybullet_bodies["wire_ids"][1]
@@ -352,6 +353,7 @@ def step(self, action: Action, render_obs: bool = False) -> State:
         """
         next_state = super().step(action, render_obs=render_obs)
 
+        _ = self._SwitchOn_holds(next_state, [self._battery])
         # Check if the CircuitClosed predicate is satisfied => turn the light on
         if self._CircuitClosed_holds(next_state, [self._light, self._battery])\
             and self._SwitchOn_holds(next_state, [self._battery]):
@@ -449,11 +451,12 @@ def _SwitchOn_holds(self, state: State, objects: Sequence[Object]) -> bool:
         del state  # unused
         battery, = objects
         joint_state = p.getJointState(battery.id, battery.joint_id, 
-                                    physicsClientId=self._physics_client_id)[0]
+                                physicsClientId=self._physics_client_id)[0] /\
+                                    self._battery.joint_scale
         joint_min = p.getJointInfo(battery.id, battery.joint_id, 
-                                   physicsClientId=self._physics_client_id)[8]
+                                physicsClientId=self._physics_client_id)[8]
         joint_max = p.getJointInfo(battery.id, battery.joint_id, 
-                                   physicsClientId=self._physics_client_id)[9]
+                                physicsClientId=self._physics_client_id)[9]
         joint_state = np.clip((joint_state - joint_min) / 
                               (joint_max - joint_min), 0, 1)
         return bool(joint_state > 0.5)