set_full_state of stage broken

[s-bl]

We noticed that set_full_env_state of the stage is recreating the objects in the scene which will create a different physical state, so running the simulation from a reloaded state yields different results than reloading.

In case interventions should be applied on the fly it might be good to only change properties and not recreate objects all the time.

For our use case we only want to store and set states without changing the structure of the envs (no new objects, etc.). For that reason we tried to use stage.set_full_state/get_full_state pair of function instead of the full_env_state functions. These didn't worked out of the box but we fixed some errors, see the diffs below.

With this changes we getting already smaller deviations between the original environment and a reseted copy. But we still get some deviations for the object and we don't know where they come from.

(In general, the list version of the state should be removed for clarity/bugs reasons)

Index: python/src/causal_world/envs/scene/silhouette.py
===================================================================
--- python/src/causal_world/envs/scene/silhouette.py    (date 1597787231831)
+++ python/src/causal_world/envs/scene/silhouette.py    (date 1597787231831)
@@ -211,11 +211,14 @@
                 self._pybullet_client_ids[0])
             position = np.array(position)
             position[-1] -= WorldConstants.FLOOR_HEIGHT
+            cylindrical_position = cart2cyl(np.array(position))
             for name in self._state_variable_names:
                 if name == 'type':
                     state.append(self._type_id)
                 elif name == 'cartesian_position':
                     state.extend(position)
+                elif name == 'cylindrical_position':
+                    state.extend(cylindrical_position)
                 elif name == 'orientation':
                     state.extend(orientation)
                 elif name == 'size':
@@ -445,7 +448,7 @@
         position[-1] += WorldConstants.FLOOR_HEIGHT
         shape_id = pybullet.createVisualShape(
             shapeType=pybullet.GEOM_BOX,
-            halfExtents=self._size / 2,
+            halfExtents=np.asarray(self._size) / 2,
             rgbaColor=np.append(self._color, self._alpha),
             physicsClientId=pybullet_client_id)
         block_id = pybullet.createMultiBody(baseVisualShapeIndex=shape_id,

Index: python/src/causal_world/envs/scene/objects.py
===================================================================
--- python/src/causal_world/envs/scene/objects.py   (date 1597787843999)
+++ python/src/causal_world/envs/scene/objects.py   (date 1597787843999)
@@ -384,6 +384,7 @@
                 self._pybullet_client_ids[0])
             position = np.array(position)
             position[-1] -= WorldConstants.FLOOR_HEIGHT
+            cylindrical_position = cart2cyl(np.array(position))
             if self.is_not_fixed():
                 linear_velocity, angular_velocity = pybullet.\
                     getBaseVelocity(
@@ -395,6 +396,8 @@
                     state.append(self._type_id)
                 elif name == 'cartesian_position':
                     state.extend(position)
+                elif name == 'cylindrical_position':
+                    state.extend(cylindrical_position)
                 elif name == 'orientation':
                     state.extend(orientation)
                 elif name == 'linear_velocity':
@@ -457,15 +460,12 @@
             position = interventions_dict['cartesian_position']
         if 'orientation' in interventions_dict:
             orientation = interventions_dict['orientation']
-        if 'mass' in interventions_dict:
-            self._mass = interventions_dict['mass']
-        if 'friction' in interventions_dict:
-            self._lateral_friction = interventions_dict['friction']
         if 'size' in interventions_dict:
-            self._size = interventions_dict['size']
-            self._set_volume()
-            self.reinit_object()
-        elif 'cartesian_position' in interventions_dict or 'orientation' in \
+            if not np.isclose(self._size, interventions_dict['size']):
+                self._size = interventions_dict['size']
+                self._set_volume()
+                self.reinit_object()
+        if 'cartesian_position' in interventions_dict or 'orientation' in \
                 interventions_dict:
             for i in range(0, len(self._pybullet_client_ids)):
                 position[-1] += WorldConstants.FLOOR_HEIGHT
@@ -474,14 +474,16 @@
                     position,
                     orientation,
                     physicsClientId=self._pybullet_client_ids[i])
-        elif 'mass' in interventions_dict:
-            for i in range(0, len(self._pybullet_client_ids)):
-                pybullet.changeDynamics(
-                    self._block_ids[i],
-                    -1,
-                    mass=self._mass,
-                    physicsClientId=self._pybullet_client_ids[i])
-        elif 'friction' in interventions_dict:
+        if 'mass' in interventions_dict:
+            if not np.isclose(self._mass, interventions_dict['mass']):
+                self._mass = interventions_dict['mass']
+                for i in range(0, len(self._pybullet_client_ids)):
+                    pybullet.changeDynamics(
+                        self._block_ids[i],
+                        -1,
+                        mass=self._mass,
+                        physicsClientId=self._pybullet_client_ids[i])
+        if 'friction' in interventions_dict:
             self._set_lateral_friction(self._lateral_friction)

         if 'color' in interventions_dict:

[ossamaAhmed]

Thanks for reporting this. Recreating the objects in the stage are needed to ensure a deterministic and reproducible trajectories after resetting the environment and also because we have some interventions which potentially can change the env structure (having more objects or deleting objects and so on). If you are trying to save the state of the environment without changing the environment structure, then this should be supported using get_state and set_state under causal_world. This is not entirely deterministic functions because a potential bug in pybullet (see https://github.com/bulletphysics/bullet3/issues/2982 ). Are you trying to get and set the state across different instances for planning using the true model?

[ossamaAhmed]

The deviations come from the fact that pybullet saves the previous n contact points to calculate the next state, so even if you set everything to be the same it wont yield the same next state since the cached contact points are not the same. We addressed this as a pybullet issue in general since its a limitation we couldn't work around at the moment.

[s-bl]

Hey,

thanks for the quick reply.

Are you trying to get and set the state across different instances for planning using the true model?

Exactly, that's why we can't use the in-memory save and load functionality of pybullet.

We addressed this as a pybullet issue in general since its a limitation we couldn't work around at the moment.

I see, good. We could reduce the deviations already quite a bit with the aforementioned changed. For the moment, we can live with the remaining small deviations.