clean up and edit comment

serl_robot_infra/franka_env/envs/relative_env.py

@@ -6,7 +6,9 @@
 
 class RelativeFrame(gym.Wrapper):
     """
-    This wrapper makes the observation and action relative to the end-effector frame.
+    This wrapper transforms the observation and action to be expressed in the end-effector frame.
+    Optionally, it can transform the tcp_pose into a relative frame defined as the reset pose.
+    
     This wrapper is expected to be used on top of the base Franka environment, which has the following
     observation space:
     {
@@ -26,7 +28,7 @@ def __init__(self, env: Env, include_relative_pose=True):
 
         self.include_relative_pose = include_relative_pose
         if self.include_relative_pose:
-            # Transformation matrix from reset pose's relative frame to base frame
+            # Homogeneous transformation matrix from reset pose's relative frame to base frame
             self.T_r_o_inv = np.zeros((4, 4))
 
     def step(self, action):
@@ -54,7 +56,7 @@ def reset(self, **kwargs):
         self.adjoint_matrix = self.construct_adjoint_matrix(obs["state"]["tcp_pose"])
         if self.include_relative_pose:
             # Update transformation matrix from the reset pose's relative frame to base frame
-            self.T_r_o_inv = self.construct_T_matrix_inv(obs["state"]["tcp_pose"])
+            self.T_r_o_inv = np.linalg.inv(self.construct_homogeneous_matrix(obs["state"]["tcp_pose"]))
 
         # Transform observation to spatial frame
         return self.transform_observation(obs), info
@@ -65,19 +67,15 @@ def transform_observation(self, obs):
         using the adjoint matrix
         """
         adjoint_inv = np.linalg.inv(self.adjoint_matrix)
-        R_inv = adjoint_inv[:3, :3]
         obs["state"]["tcp_vel"] = adjoint_inv @ obs["state"]["tcp_vel"]
-        # obs['state']['tcp_force'] = R_inv @ obs['state']['tcp_force']
-        # obs['state']['tcp_torque'] = R_inv @ obs['state']['tcp_torque']
 
-        # let the current pose and rotation in base frame be p_b_o and theta_b_o
         if self.include_relative_pose:
-            T_b_o = self.construct_T_matrix(obs["state"]["tcp_pose"])
-            # Transformation matrix from current pose to reset pose's relative frame
+            T_b_o = self.construct_homogeneous_matrix(obs["state"]["tcp_pose"])
             T_b_r = self.T_r_o_inv @ T_b_o
+
+            # Reconstruct transformed tcp_pose vector
             p_b_r = T_b_r[:3, 3]
             theta_b_r = R.from_matrix(T_b_r[:3, :3]).as_quat()
-            # xyz + quat in relative frame
             obs["state"]["tcp_pose"] = np.concatenate((p_b_r, theta_b_r))
 
         return obs
@@ -110,9 +108,9 @@ def construct_adjoint_matrix(self, tcp_pose):
         adjoint_matrix[:3, 3:] = skew_matrix @ rotation
         return adjoint_matrix
 
-    def construct_T_matrix(self, tcp_pose):
+    def construct_homogeneous_matrix(self, tcp_pose):
         """
-        Construct the transformation matrix from relative frame to base frame
+        Construct the homogeneous transformation matrix from given pose.
         """
         rotation = R.from_quat(tcp_pose[3:]).as_matrix()
         translation = np.array(tcp_pose[:3])
@@ -121,15 +119,3 @@ def construct_T_matrix(self, tcp_pose):
         T[:3, 3] = translation
         T[3, 3] = 1
         return T
-
-    def construct_T_matrix_inv(self, tcp_pose):
-        """
-        Construct the inverse of the transformation matrix from relative frame to base frame
-        """
-        rotation = R.from_quat(tcp_pose[3:]).as_matrix()
-        translation = np.array(tcp_pose[:3])
-        T = np.zeros((4, 4))
-        T[:3, :3] = rotation.T
-        T[:3, 3] = -rotation.T @ translation
-        T[3, 3] = 1
-        return T

serl_robot_infra/franka_env/spacemouse/spacemouse_expert.py

@@ -20,7 +20,7 @@ def _read_spacemouse(self):
             with self.state_lock:
                 self.latest_data["action"] = np.array(
                     [-state.y, state.x, state.z, -state.roll, -state.pitch, -state.yaw]
-                )
+                ) # spacemouse axis matched with robot base frame
                 self.latest_data["buttons"] = state.buttons
 
     def get_action(self):

serl_robot_infra/robot_servers/franka_server.py

@@ -358,13 +358,6 @@ def precision_mode():
         reconf_client.update_configuration(impedance_config)
         return "precision Mode"
 
-    # try:
-    #     webapp.run(host="0.0.0.0")
-    # except Exception as e:
-    #     robot_server.stop_impedance()
-    #     robot_server.joint_controller.terminate()
-    #     roscore.terminate()
-    #     raise Exception("robot server errored: ", e)
     webapp.run(host="0.0.0.0")