From bff64b0cc942c076f68a54e1f29ed70d78300b7c Mon Sep 17 00:00:00 2001 From: ademiadeniji Date: Thu, 20 Mar 2025 12:29:36 -0400 Subject: [PATCH 1/5] fix left teleop, reskin num mags --- .gitignore | 1 + configs/reskin.yaml | 2 +- configs/teleop.yaml | 1 + frankateach/constants.py | 2 +- frankateach/teleoperator.py | 22 +++++++++++++++++++--- teleop.py | 4 +++- 6 files changed, 26 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index d66c244..9b7b6d4 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ __pycache__/ logs/ extracted_data*/ outputs/ +data/ diff --git a/configs/reskin.yaml b/configs/reskin.yaml index 4fa65a3..a7a266b 100644 --- a/configs/reskin.yaml +++ b/configs/reskin.yaml @@ -1,4 +1,4 @@ reskin_config: port: "/dev/ttyACM0" - num_mags: 10 + num_mags: 5 history: 40 diff --git a/configs/teleop.yaml b/configs/teleop.yaml index 477c924..cd2712f 100644 --- a/configs/teleop.yaml +++ b/configs/teleop.yaml @@ -1,3 +1,4 @@ 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 \ No newline at end of file diff --git a/frankateach/constants.py b/frankateach/constants.py index 6dbe605..65b38e3 100644 --- a/frankateach/constants.py +++ b/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" diff --git a/frankateach/teleoperator.py b/frankateach/teleoperator.py index c81589c..c540ddd 100644 --- a/frankateach/teleoperator.py +++ b/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,7 @@ def __init__( init_gripper_state="open", teleop_mode="robot", home_offset=[0, 0, 0], + deoxys_config_path=None, ) -> None: # Subscribe controller state self._controller_state_subscriber = ZMQKeypointSubscriber( @@ -73,6 +82,7 @@ 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 def _apply_retargeted_angles(self) -> None: self.controller_state = self._controller_state_subscriber.recv_keypoints() @@ -130,7 +140,7 @@ 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)) @@ -153,7 +163,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( diff --git a/teleop.py b/teleop.py index dabc3e1..4acb1e0 100644 --- a/teleop.py +++ b/teleop.py @@ -5,11 +5,12 @@ 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): operator = FrankaOperator( init_gripper_state=init_gripper_state, teleop_mode=teleop_mode, home_offset=home_offset, + deoxys_config_path=deoxys_config_path, ) operator.stream() @@ -27,6 +28,7 @@ def main(cfg): cfg.init_gripper_state, cfg.teleop_mode, cfg.home_offset, + cfg.deoxys_config_path, ), ) oculus_stick_process = Process(target=start_oculus_stick) From fa81fc1ef6beb41fe1d8dfcae0d532e358675709 Mon Sep 17 00:00:00 2001 From: ademiadeniji Date: Tue, 15 Apr 2025 17:46:18 -0400 Subject: [PATCH 2/5] reskin printing / cont action teleop --- configs/teleop.yaml | 3 ++- frankateach/sensors/reskin.py | 9 ++++++++- frankateach/teleoperator.py | 29 ++++++++++++++++++++++++----- teleop.py | 4 +++- 4 files changed, 37 insertions(+), 8 deletions(-) diff --git a/configs/teleop.yaml b/configs/teleop.yaml index cd2712f..599d904 100644 --- a/configs/teleop.yaml +++ b/configs/teleop.yaml @@ -1,4 +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 \ No newline at end of file +deoxys_config_path: "deoxys_right.yml" # deoxys_left.yml, deoxys_right.yml +continuous_gripper: false # true, false \ No newline at end of file diff --git a/frankateach/sensors/reskin.py b/frankateach/sensors/reskin.py index 1e9e7bb..0f145b4 100644 --- a/frankateach/sensors/reskin.py +++ b/frankateach/sensors/reskin.py @@ -34,6 +34,10 @@ def _start_reskin(self): def stream(self): notify_component_start("Reskin sensors") + import numpy as np + counter = 0 + reskin_state = self.sensor_proc.get_data(1)[0] + baseline_magnitude = np.linalg.norm(reskin_state.data[:3]) while True: try: @@ -42,11 +46,14 @@ def stream(self): data_dict = {} data_dict["timestamp"] = reskin_state.time data_dict["sensor_values"] = reskin_state.data + if counter % 100 == 0: + magnitude = np.linalg.norm(reskin_state.data[:3]) + print("magnitude", magnitude - baseline_magnitude) self.history.append(reskin_state.data) data_dict["sensor_history"] = list(self.history) self.reskin_publisher.pub_keypoints(data_dict, topic_name="reskin") self.timer.end_loop() - + counter += 1 except KeyboardInterrupt: break diff --git a/frankateach/teleoperator.py b/frankateach/teleoperator.py index c540ddd..25108b0 100644 --- a/frankateach/teleoperator.py +++ b/frankateach/teleoperator.py @@ -57,6 +57,7 @@ def __init__( teleop_mode="robot", home_offset=[0, 0, 0], deoxys_config_path=None, + continuous_gripper=False, ) -> None: # Subscribe controller state self._controller_state_subscriber = ZMQKeypointSubscriber( @@ -83,7 +84,7 @@ def __init__( 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() @@ -148,10 +149,28 @@ def _apply_retargeted_angles(self) -> None: 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 diff --git a/teleop.py b/teleop.py index 4acb1e0..e9581fc 100644 --- a/teleop.py +++ b/teleop.py @@ -5,12 +5,13 @@ from frankateach.constants import HOST, VR_CONTROLLER_STATE_PORT -def start_teleop(init_gripper_state="open", teleop_mode="robot", home_offset=None, deoxys_config_path=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() @@ -29,6 +30,7 @@ def main(cfg): cfg.teleop_mode, cfg.home_offset, cfg.deoxys_config_path, + cfg.continuous_gripper, ), ) oculus_stick_process = Process(target=start_oculus_stick) From 5e53b78e4de97071ecc42c6d5a73c65819a8fc8c Mon Sep 17 00:00:00 2001 From: Jolia Date: Sat, 19 Apr 2025 17:54:14 -0400 Subject: [PATCH 3/5] update normalized reskin print out --- frankateach/sensors/reskin.py | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/frankateach/sensors/reskin.py b/frankateach/sensors/reskin.py index 0f145b4..4399c7b 100644 --- a/frankateach/sensors/reskin.py +++ b/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: @@ -34,11 +35,9 @@ def _start_reskin(self): def stream(self): notify_component_start("Reskin sensors") - import numpy as np - counter = 0 - reskin_state = self.sensor_proc.get_data(1)[0] - baseline_magnitude = np.linalg.norm(reskin_state.data[:3]) + i = 0 + baseline = None while True: try: self.timer.start_loop() @@ -46,14 +45,26 @@ def stream(self): data_dict = {} data_dict["timestamp"] = reskin_state.time data_dict["sensor_values"] = reskin_state.data - if counter % 100 == 0: - magnitude = np.linalg.norm(reskin_state.data[:3]) - print("magnitude", magnitude - baseline_magnitude) + 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() - counter += 1 + i += 1 except KeyboardInterrupt: break From d75b885f8db4c78fba50e04caae39b531c707ec6 Mon Sep 17 00:00:00 2001 From: Jolia Date: Sat, 19 Apr 2025 20:22:58 -0400 Subject: [PATCH 4/5] update reskin port --- configs/reskin.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/configs/reskin.yaml b/configs/reskin.yaml index a7a266b..c363c4e 100644 --- a/configs/reskin.yaml +++ b/configs/reskin.yaml @@ -2,3 +2,8 @@ reskin_config: port: "/dev/ttyACM0" num_mags: 5 history: 40 + +# reskin_config: +# port: "/dev/ttyACM1" +# num_mags: 5 +# history: 40 \ No newline at end of file From da5115cadbed1528ea74fec0f80f8b5ee95c2937 Mon Sep 17 00:00:00 2001 From: ademiadeniji Date: Fri, 25 Apr 2025 13:57:23 -0400 Subject: [PATCH 5/5] collect reskin by default --- configs/collect_data.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/configs/collect_data.yaml b/configs/collect_data.yaml index ca17a8c..39fd2c7 100644 --- a/configs/collect_data.yaml +++ b/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