Use xbody rather than body for egocentric computations.

erez-tom · copybara-github · commit f4e5e2336991 · 2022-08-06T15:42:03.000-07:00
This change fixes a bug introduced in [this change](a6d753f), which replaced numpy-based computation of relative frames with native MuJoCo sensors. PiperOrigin-RevId: 465799007 Change-Id: Ifab1b41880e16309e02bc2ec9bcc28b4c5bff489
diff --git a/dm_control/locomotion/walkers/ant.py b/dm_control/locomotion/walkers/ant.py
@@ -159,8 +159,8 @@ def appendages_pos(self):
       self._entity.appendages_sensors.append(
           self._entity.mjcf_model.sensor.add(
               'framepos', name=body.name + '_appendage',
-              objtype='body', objname=body,
-              reftype='body', refname=self._entity.root_body))
+              objtype='xbody', objname=body,
+              reftype='xbody', refname=self._entity.root_body))
     def appendages_ego_pos(physics):
       return np.reshape(
           physics.bind(self._entity.appendages_sensors).sensordata, -1)
@@ -175,8 +175,8 @@ def bodies_quats(self):
       self._entity.bodies_quats_sensors.append(
           self._entity.mjcf_model.sensor.add(
               'framequat', name=body.name + '_ego_body_quat',
-              objtype='body', objname=body,
-              reftype='body', refname=self._entity.root_body))
+              objtype='xbody', objname=body,
+              reftype='xbody', refname=self._entity.root_body))
     def bodies_ego_orientation(physics):
       return np.reshape(
           physics.bind(self._entity.bodies_quats_sensors).sensordata, -1)
@@ -191,8 +191,8 @@ def bodies_pos(self):
       self._entity.bodies_pos_sensors.append(
           self._entity.mjcf_model.sensor.add(
               'framepos', name=body.name + '_ego_body_pos',
-              objtype='body', objname=body,
-              reftype='body', refname=self._entity.root_body))
+              objtype='xbody', objname=body,
+              reftype='xbody', refname=self._entity.root_body))
     def bodies_ego_pos(physics):
       return np.reshape(
           physics.bind(self._entity.bodies_pos_sensors).sensordata, -1)
diff --git a/dm_control/locomotion/walkers/legacy_base.py b/dm_control/locomotion/walkers/legacy_base.py
@@ -223,11 +223,14 @@ def end_effectors_pos(self):
     """Position of end effectors relative to torso, in the egocentric frame."""
     self._entity.end_effectors_pos_sensors[:] = []
     for effector in self._entity.end_effectors:
+      objtype = effector.tag
+      if objtype == 'body':
+        objtype = 'xbody'
       self._entity.end_effectors_pos_sensors.append(
           self._entity.mjcf_model.sensor.add(
               'framepos', name=effector.name + '_end_effector',
-              objtype=effector.tag, objname=effector,
-              reftype='body', refname=self._entity.root_body))
+              objtype=objtype, objname=effector,
+              reftype='xbody', refname=self._entity.root_body))
     def relative_pos_in_egocentric_frame(physics):
       return np.reshape(
           physics.bind(self._entity.end_effectors_pos_sensors).sensordata, -1)
