feat: add tquat_flange to robot observation

Author: juelg

python/rcs/envs/base.py

@@ -65,6 +65,14 @@ class TQuatDictType(RCSpaceType):
             dtype=np.float64,
         ),
     ]
+    tquat_flange: Annotated[
+        Vec7Type,
+        gym.spaces.Box(
+            low=np.array([-0.855, -0.855, -1] + [-1] + [-np.inf] * 3),
+            high=np.array([0.855, 0.855, 1.188] + [1] + [np.inf] * 3),
+            dtype=np.float64,
+        ),
+    ]
 
 
 class LimitedTQuatRelDictType(RCSpaceType):
@@ -346,6 +354,12 @@ def get_robot_obs(self) -> ArmObsType:
             ),
             joints=self.robot.get_joint_position(),
             xyzrpy=self.robot.get_cartesian_position().xyzrpy(),
+            tquat_flange=np.concatenate(
+                [
+                    self.robot.get_cartesian_flange_position().translation(),
+                    self.robot.get_cartesian_flange_position().rotation_q(),
+                ]
+            ),
         )
 
     def action(self, action: dict[str, Any]) -> dict[str, Any]: