Change in normalized reskin

.gitignore

@@ -3,3 +3,4 @@ __pycache__/
 logs/
 extracted_data*/
 outputs/
+data/

configs/collect_data.yaml

@@ -7,4 +7,4 @@ demo_num: 0
 collect_img: True
 collect_depth: False
 collect_state: True
-collect_reskin: False
+collect_reskin: True

configs/reskin.yaml

@@ -1,4 +1,9 @@
 reskin_config:
   port: "/dev/ttyACM0"
-  num_mags: 10
+  num_mags: 5
   history: 40
+
+# reskin_config:
+#   port: "/dev/ttyACM1"
+#   num_mags: 5
+#   history: 40

configs/teleop.yaml

@@ -1,3 +1,5 @@
 init_gripper_state: open # open, close
 teleop_mode: robot # robot, human
 home_offset: null
+deoxys_config_path: "deoxys_right.yml" # deoxys_left.yml, deoxys_right.yml
+continuous_gripper: false # true, false

frankateach/constants.py

@@ -14,7 +14,7 @@
 CONTROL_TOPIC = "control"
 
 # VR constants
-VR_TCP_HOST = "10.19.225.15"
+VR_TCP_HOST = "10.19.189.139"
 VR_TCP_PORT = 5555
 VR_CONTROLLER_TOPIC = b"oculus_controller"
 

frankateach/sensors/reskin.py

@@ -5,6 +5,7 @@
 from frankateach.utils import FrequencyTimer, notify_component_start
 
 from reskin_sensor import ReSkinProcess
+import numpy as np
 
 
 class ReskinSensorPublisher:
@@ -35,18 +36,35 @@ def _start_reskin(self):
     def stream(self):
         notify_component_start("Reskin sensors")
 
+        i = 0
+        baseline = None 
         while True:
             try:
                 self.timer.start_loop()
                 reskin_state = self.sensor_proc.get_data(1)[0]
                 data_dict = {}
                 data_dict["timestamp"] = reskin_state.time
                 data_dict["sensor_values"] = reskin_state.data
+
                 self.history.append(reskin_state.data)
+
+                if baseline is None and len(self.history) >= 5:
+                    # Calculate baseline from first 5 samples
+                    history_list = list(self.history)
+                    baseline = np.mean(history_list[:5], axis=0)
+                    print(f"Baseline fixed: {baseline[:3]} (from first 5 samples)")
+
+                current_data = reskin_state.data
+                if baseline is not None:
+                    adjusted_data = current_data - baseline
+                    adjusted_norm = np.linalg.norm(adjusted_data[:3])
+                    if i % 100 == 0:
+                        print(f"Adjusted norm: {adjusted_norm:.4f}")
+
                 data_dict["sensor_history"] = list(self.history)
                 self.reskin_publisher.pub_keypoints(data_dict, topic_name="reskin")
                 self.timer.end_loop()
-
+                i += 1
             except KeyboardInterrupt:
                 break
 

frankateach/teleoperator.py

@@ -27,13 +27,21 @@
 from numpy.linalg import pinv
 
 
-def get_relative_affine(init_affine, current_affine):
+def get_relative_affine(init_affine, current_affine, deoxys_config_path):
+
     H_V_des = pinv(init_affine) @ current_affine
 
     # Transform to robot frame.
     relative_affine_rot = (pinv(H_R_V) @ H_V_des @ H_R_V)[:3, :3]
     relative_affine_trans = (pinv(H_R_V_star) @ H_V_des @ H_R_V_star)[:3, 3]
 
+    # Mirror the x-axis and y-axis for left arm setup
+    if "deoxys_left" in deoxys_config_path:
+        relative_affine_trans[0] *= -1  
+        relative_affine_trans[1] *= -1 
+        relative_affine_rot[:, 0] *= -1  
+        relative_affine_rot[:, 1] *= -1
+
     # Homogeneous coordinates
     relative_affine = np.block(
         [[relative_affine_rot, relative_affine_trans.reshape(3, 1)], [0, 0, 0, 1]]
@@ -48,6 +56,8 @@ def __init__(
         init_gripper_state="open",
         teleop_mode="robot",
         home_offset=[0, 0, 0],
+        deoxys_config_path=None,
+        continuous_gripper=False,   
     ) -> None:
         # Subscribe controller state
         self._controller_state_subscriber = ZMQKeypointSubscriber(
@@ -73,7 +83,8 @@ def __init__(
         self.home_offset = (
             np.array(home_offset) if home_offset is not None else np.zeros(3)
         )
-
+        self.deoxys_config_path = deoxys_config_path
+        self.continuous_gripper = continuous_gripper
     def _apply_retargeted_angles(self) -> None:
         self.controller_state = self._controller_state_subscriber.recv_keypoints()
 
@@ -130,18 +141,36 @@ def _apply_retargeted_angles(self) -> None:
 
         if self.start_teleop and self.teleop_mode == "robot":
             relative_affine = get_relative_affine(
-                self.init_affine, self.controller_state.right_affine
+                self.init_affine, self.controller_state.right_affine, self.deoxys_config_path
             )
         else:
             relative_affine = np.zeros((4, 4))
             relative_affine[3, 3] = 1
 
         gripper_action = None
         if self.teleop_mode == "robot":
-            if self.controller_state.right_index_trigger > 0.5:
-                gripper_action = GRIPPER_CLOSE
-            elif self.controller_state.right_hand_trigger > 0.5:
-                gripper_action = GRIPPER_OPEN
+            if self.continuous_gripper:
+                # Initialize or update last gripper state
+                if not hasattr(self, 'last_gripper_state'):
+                    self.last_gripper_state = self.gripper_state
+
+                # Calculate gripper change based on trigger values
+                gripper_change = 0
+                if self.controller_state.right_index_trigger > 0.5:
+                    # Gradually close (move towards 1)
+                    gripper_change = 0.05 * GRIPPER_CLOSE
+                elif self.controller_state.right_hand_trigger > 0.5:
+                    # Gradually open (move towards -1)
+                    gripper_change = 0.05 * GRIPPER_OPEN
+
+                # Update gripper state with change, clamping between -1 and 1
+                gripper_action = np.clip(self.last_gripper_state + gripper_change, GRIPPER_OPEN, GRIPPER_CLOSE)
+                self.last_gripper_state = gripper_action
+            else:
+                if self.controller_state.right_index_trigger > 0.5:
+                    gripper_action = GRIPPER_CLOSE
+                elif self.controller_state.right_hand_trigger > 0.5:
+                    gripper_action = GRIPPER_OPEN
 
         if gripper_action is not None and gripper_action != self.gripper_state:
             self.gripper_state = gripper_action
@@ -153,7 +182,13 @@ def _apply_retargeted_angles(self) -> None:
             )
 
             target_pos = self.home_pos + relative_pos
-            target_rot = self.home_rot @ relative_rot
+            # Mirror the home rotation for left arm before combining with relative rotation
+            if "deoxys_left" in self.deoxys_config_path:
+                home_rot_mirrored = self.home_rot.copy()
+                home_rot_mirrored[:2, :] *= -1
+                target_rot = home_rot_mirrored @ relative_rot
+            else:
+                target_rot = self.home_rot @ relative_rot
             target_quat = transform_utils.mat2quat(target_rot)
 
             target_pos = np.clip(

teleop.py

@@ -5,11 +5,13 @@
 from frankateach.constants import HOST, VR_CONTROLLER_STATE_PORT
 
 
-def start_teleop(init_gripper_state="open", teleop_mode="robot", home_offset=None):
+def start_teleop(init_gripper_state="open", teleop_mode="robot", home_offset=None, deoxys_config_path=None, continuous_gripper=False):
     operator = FrankaOperator(
         init_gripper_state=init_gripper_state,
         teleop_mode=teleop_mode,
         home_offset=home_offset,
+        deoxys_config_path=deoxys_config_path,
+        continuous_gripper=continuous_gripper,
     )
     operator.stream()
 
@@ -27,6 +29,8 @@ def main(cfg):
             cfg.init_gripper_state,
             cfg.teleop_mode,
             cfg.home_offset,
+            cfg.deoxys_config_path,
+            cfg.continuous_gripper,
         ),
     )
     oculus_stick_process = Process(target=start_oculus_stick)