add additional observation for gripper quat

MujocoManip/environments/sawyer.py

@@ -3,6 +3,7 @@
 from MujocoManip.environments.base import MujocoEnv
 from MujocoManip.models import SawyerRobot, gripper_factory
 import MujocoManip.miscellaneous.utils as U
+import MujocoManip.miscellaneous.transformations as T
 
 
 class SawyerEnv(MujocoEnv):
@@ -21,7 +22,7 @@ def __init__(self,
         # setup mocap stuff if necessary
         if self.use_eef_ctrl:
             self._setup_mocap()
-        
+
     def _setup_mocap(self):
         U.mjpy_reset_mocap_welds(self.sim)
         self.sim.forward()
@@ -196,6 +197,13 @@ def _right_hand_pose(self):
         """
         return self.pose_in_base_from_name('right_hand')
 
+    @property
+    def _right_hand_quat(self):
+        """
+        Returns eef pose in base frame of robot.
+        """
+        return T.quaternion_from_matrix(self._right_hand_pose)
+
     @property
     def _right_hand_total_velocity(self):
         """

MujocoManip/environments/sawyer_stack.py

@@ -204,11 +204,15 @@ def _get_observation(self):
             di['cubeB_pos'] = cubeB_pos
             di['cubeB_quat'] = cubeB_quat
 
+            # relative positions between gripper and cubes
             gripper_site_pos = np.array(self.sim.data.site_xpos[self.eef_site_id])
             di['gripper_to_cubeA'] = gripper_site_pos - cubeA_pos
             di['gripper_to_cubeB'] = gripper_site_pos - cubeB_pos
             di['cubeA_to_cubeB'] = cubeA_pos - cubeB_pos
 
+            # gripper orientation
+            di['gripper_quat'] = self._right_hand_quat
+
         # proprioception
         di['proprio'] = np.concatenate([
             np.sin(di['joint_pos']),