From 77efebfb1cf557df9a31fc9529cfeb32c5ac20b0 Mon Sep 17 00:00:00 2001 From: robotics-franka-gh Date: Thu, 29 May 2025 22:32:17 -0700 Subject: [PATCH] fixes --- .vscode/settings.json | 3 + debug_deoxys_messages.py | 122 +++++++++++++ franka_server.py | 3 + franka_server_debug_20250529_221953.log | 4 + frankateach/franka_server.py | 228 ++++++++++++++---------- 5 files changed, 268 insertions(+), 92 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 debug_deoxys_messages.py create mode 100644 franka_server_debug_20250529_221953.log diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..69268b6 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "cmake.sourceDirectory": "/home/labelbox/projects/lbx-Franka-Teach/franka_description" +} \ No newline at end of file diff --git a/debug_deoxys_messages.py b/debug_deoxys_messages.py new file mode 100644 index 0000000..417a0c4 --- /dev/null +++ b/debug_deoxys_messages.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +""" +Debug script to test what messages are being published by deoxys services +""" +import os +os.environ["PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION"] = "python" + +import zmq +import time +from pathlib import Path +from deoxys.utils import YamlConfig +import deoxys.proto.franka_interface.franka_robot_state_pb2 as franka_robot_state_pb2 + +CONFIG_ROOT = Path(__file__).parent / "frankateach" / "configs" + +def test_deoxys_messages(): + # Load configuration + config = YamlConfig(CONFIG_ROOT / "deoxys_right.yml").as_easydict() + + print(f"šŸ” Testing deoxys message reception...") + print(f"Config: NUC IP = {config.NUC.IP}") + print(f"Arm port: {config.NUC.PUB_PORT}") + print(f"Gripper port: {config.NUC.GRIPPER_PUB_PORT}") + + # Initialize ZMQ context + context = zmq.Context() + + # Test arm state subscriber + print(f"\nšŸ“” Testing arm state on tcp://localhost:{config.NUC.PUB_PORT}") + arm_subscriber = context.socket(zmq.SUB) + arm_subscriber.setsockopt(zmq.CONFLATE, 1) + arm_subscriber.setsockopt_string(zmq.SUBSCRIBE, "") + arm_subscriber.connect(f"tcp://localhost:{config.NUC.PUB_PORT}") + + # Test gripper state subscriber + print(f"šŸ“” Testing gripper state on tcp://localhost:{config.NUC.GRIPPER_PUB_PORT}") + gripper_subscriber = context.socket(zmq.SUB) + gripper_subscriber.setsockopt(zmq.CONFLATE, 1) + gripper_subscriber.setsockopt_string(zmq.SUBSCRIBE, "") + gripper_subscriber.connect(f"tcp://localhost:{config.NUC.GRIPPER_PUB_PORT}") + + arm_received = False + gripper_received = False + + print(f"\nā±ļø Waiting for messages (timeout: 10 seconds)...") + start_time = time.time() + + while time.time() - start_time < 10: + # Test arm messages + if not arm_received: + try: + message = arm_subscriber.recv(flags=zmq.NOBLOCK) + print(f"āœ… ARM: Received message ({len(message)} bytes)") + + try: + robot_state = franka_robot_state_pb2.FrankaRobotStateMessage() + robot_state.ParseFromString(message) + print(f" āœ… ARM: Successfully parsed protobuf") + print(f" šŸ“Š ARM: Frame number: {robot_state.frame}") + print(f" šŸ¤– ARM: Joint positions: {robot_state.q[:3]}...") + + # Debug: print available attributes + attrs = [attr for attr in dir(robot_state) if not attr.startswith('_')] + print(f" šŸ” ARM: Available attributes: {[a for a in attrs if 'o_t_ee' in a.lower() or 'ee' in a.lower()]}") + + # Try different possible attribute names + if hasattr(robot_state, 'o_t_ee'): + print(f" šŸ“ ARM: o_t_ee: {robot_state.o_t_ee[:4]}...") + elif hasattr(robot_state, 'O_T_EE'): + print(f" šŸ“ ARM: O_T_EE: {robot_state.O_T_EE[:4]}...") + else: + print(f" āŒ ARM: No o_t_ee or O_T_EE attribute found") + + arm_received = True + except Exception as e: + print(f" āŒ ARM: Failed to parse protobuf: {e}") + + except zmq.Again: + pass + + # Test gripper messages + if not gripper_received: + try: + message = gripper_subscriber.recv(flags=zmq.NOBLOCK) + print(f"āœ… GRIPPER: Received message ({len(message)} bytes)") + + try: + gripper_state = franka_robot_state_pb2.FrankaGripperStateMessage() + gripper_state.ParseFromString(message) + print(f" āœ… GRIPPER: Successfully parsed protobuf") + print(f" šŸ“ GRIPPER: Width: {gripper_state.width}") + print(f" šŸ“ GRIPPER: Max width: {gripper_state.max_width}") + print(f" šŸ¤ GRIPPER: Is grasped: {gripper_state.is_grasped}") + gripper_received = True + except Exception as e: + print(f" āŒ GRIPPER: Failed to parse protobuf: {e}") + + except zmq.Again: + pass + + if arm_received and gripper_received: + break + + time.sleep(0.01) + + # Results + print(f"\nšŸ“‹ RESULTS:") + print(f" ARM messages: {'āœ… Received' if arm_received else 'āŒ Not received'}") + print(f" GRIPPER messages: {'āœ… Received' if gripper_received else 'āŒ Not received'}") + + if arm_received and gripper_received: + print(f" šŸŽ‰ SUCCESS: Both message types are working!") + else: + print(f" āš ļø Some message types are not working") + + # Cleanup + arm_subscriber.close() + gripper_subscriber.close() + context.term() + +if __name__ == "__main__": + test_deoxys_messages() \ No newline at end of file diff --git a/franka_server.py b/franka_server.py index 60c5bfe..14251d0 100644 --- a/franka_server.py +++ b/franka_server.py @@ -1,3 +1,6 @@ +import os +os.environ["PROTOCOL_BUFFERS_PYTHON_IMPLEMENTATION"] = "python" + from frankateach.franka_server import FrankaServer import hydra diff --git a/franka_server_debug_20250529_221953.log b/franka_server_debug_20250529_221953.log new file mode 100644 index 0000000..6e0751d --- /dev/null +++ b/franka_server_debug_20250529_221953.log @@ -0,0 +1,4 @@ +22:19:53.445 [INFO] ============================================================ +22:19:53.446 [INFO] FRANKA SERVER DEBUG VERSION STARTING +22:19:53.446 [INFO] Log file: franka_server_debug_20250529_221953.log +22:19:53.446 [INFO] ============================================================ diff --git a/frankateach/franka_server.py b/frankateach/franka_server.py index 4e654b0..09ebd5a 100644 --- a/frankateach/franka_server.py +++ b/frankateach/franka_server.py @@ -3,15 +3,18 @@ import pickle import time import numpy as np +import zmq from deoxys.utils import YamlConfig -from deoxys.franka_interface import FrankaInterface from deoxys.utils import transform_utils from deoxys.utils.config_utils import ( get_default_controller_config, verify_controller_config, ) +# Import protobuf messages +import deoxys.proto.franka_interface.franka_robot_state_pb2 as franka_robot_state_pb2 + from frankateach.utils import notify_component_start from frankateach.network import create_response_socket from frankateach.messages import FrankaAction, FrankaState @@ -79,15 +82,42 @@ def control_daemon(self): self.action_socket.close() -class Robot(FrankaInterface): +class Robot: def __init__(self, cfg, control_freq): - super(Robot, self).__init__( - general_cfg_file=os.path.join(CONFIG_ROOT, cfg), - use_visualizer=False, - control_freq=control_freq, - has_gripper=True, - automatic_gripper_reset=True - ) + # Load configuration + self.config = YamlConfig(os.path.join(CONFIG_ROOT, cfg)).as_easydict() + self.control_freq = control_freq + + # Initialize ZMQ subscribers for deoxys services + self.context = zmq.Context() + + # Arm state subscriber (connects to deoxys franka-interface on port 5570) + self.arm_subscriber = self.context.socket(zmq.SUB) + self.arm_subscriber.setsockopt(zmq.CONFLATE, 1) + self.arm_subscriber.setsockopt_string(zmq.SUBSCRIBE, "") + self.arm_subscriber.connect(f"tcp://localhost:{self.config.NUC.PUB_PORT}") + + # Gripper state subscriber (connects to deoxys gripper-interface on port 5572) + self.gripper_subscriber = self.context.socket(zmq.SUB) + self.gripper_subscriber.setsockopt(zmq.CONFLATE, 1) + self.gripper_subscriber.setsockopt_string(zmq.SUBSCRIBE, "") + self.gripper_subscriber.connect(f"tcp://localhost:{self.config.NUC.GRIPPER_PUB_PORT}") + + # Control publisher (publishes to deoxys on port 5571) + self.arm_publisher = self.context.socket(zmq.PUB) + self.arm_publisher.bind(f"tcp://*:{self.config.NUC.SUB_PORT}") + + # Gripper control publisher (publishes to deoxys on port 5573) + self.gripper_publisher = self.context.socket(zmq.PUB) + self.gripper_publisher.bind(f"tcp://*:{self.config.NUC.GRIPPER_SUB_PORT}") + + # State variables + self.last_eef_quat_and_pos = (None, None) + self.last_gripper_action = None + self.last_q = None + self.received_states = False + + # Controller config self.velocity_controller_cfg = verify_controller_config( YamlConfig( os.path.join(CONFIG_ROOT, "osc-pose-controller.yml") @@ -96,94 +126,108 @@ def __init__(self, cfg, control_freq): self.last_gripper_dim = 6 def reset_robot(self): - self.reset() - print("Waiting for the robot to connect...") - while len(self._state_buffer) == 0: + + # Start state receiving threads + import threading + + def arm_state_receiver(): + while True: + try: + message = self.arm_subscriber.recv(flags=zmq.NOBLOCK) + robot_state = franka_robot_state_pb2.FrankaRobotStateMessage() + robot_state.ParseFromString(message) + + # Extract end-effector pose from 4x4 transformation matrix + # O_T_EE is a flattened 4x4 matrix: [r11,r12,r13,tx, r21,r22,r23,ty, r31,r32,r33,tz, 0,0,0,1] + ee_pos = np.array([ + robot_state.O_T_EE[3], # tx + robot_state.O_T_EE[7], # ty + robot_state.O_T_EE[11], # tz + ]).reshape(3, 1) + + # Extract 3x3 rotation matrix and convert to quaternion + rot_mat = np.array([ + [robot_state.O_T_EE[0], robot_state.O_T_EE[1], robot_state.O_T_EE[2]], + [robot_state.O_T_EE[4], robot_state.O_T_EE[5], robot_state.O_T_EE[6]], + [robot_state.O_T_EE[8], robot_state.O_T_EE[9], robot_state.O_T_EE[10]] + ]) + ee_quat = transform_utils.mat2quat(rot_mat).reshape(4, 1) + + # Extract joint positions + joint_pos = list(robot_state.q) + + self.last_eef_quat_and_pos = (ee_quat, ee_pos) + self.last_q = joint_pos + self.received_states = True + + except zmq.Again: + time.sleep(0.001) + except Exception as e: + print(f"Error receiving arm state: {e}") + time.sleep(0.001) + + def gripper_state_receiver(): + while True: + try: + message = self.gripper_subscriber.recv(flags=zmq.NOBLOCK) + gripper_state = franka_robot_state_pb2.FrankaGripperStateMessage() + gripper_state.ParseFromString(message) + + # Convert gripper width to action (-1 to 1 range) + gripper_width = gripper_state.width + max_width = gripper_state.max_width + self.last_gripper_action = (gripper_width / max_width) * 2 - 1 # Scale to [-1, 1] + + except zmq.Again: + time.sleep(0.001) + except Exception as e: + print(f"Error receiving gripper state: {e}") + time.sleep(0.001) + + # Start receiver threads + arm_thread = threading.Thread(target=arm_state_receiver, daemon=True) + gripper_thread = threading.Thread(target=gripper_state_receiver, daemon=True) + arm_thread.start() + gripper_thread.start() + + # Wait for initial state + while not self.received_states or self.last_gripper_action is None: time.sleep(0.01) print("Franka is connected") - def osc_move(self, target_pos, target_quat, gripper_state): - num_steps = 3 - - for _ in range(num_steps): - target_mat = transform_utils.pose2mat(pose=(target_pos, target_quat)) - - current_quat, current_pos = self.last_eef_quat_and_pos - current_mat = transform_utils.pose2mat( - pose=(current_pos.flatten(), current_quat.flatten()) - ) - - pose_error = transform_utils.get_pose_error( - target_pose=target_mat, current_pose=current_mat - ) - - if np.dot(target_quat, current_quat) < 0.0: - current_quat = -current_quat - - quat_diff = transform_utils.quat_distance(target_quat, current_quat) - axis_angle_diff = transform_utils.quat2axisangle(quat_diff) - - action_pos = pose_error[:3] - action_axis_angle = axis_angle_diff.flatten() + def check_nonzero_configuration(self): + """Check if the robot is in a valid configuration""" + if self.last_q is None: + return False + return not all(abs(q) < 1e-6 for q in self.last_q) - action = action_pos.tolist() + action_axis_angle.tolist() + [gripper_state] + def close(self): + """Close ZMQ connections""" + self.arm_subscriber.close() + self.gripper_subscriber.close() + self.arm_publisher.close() + self.gripper_publisher.close() + self.context.term() - self.control( - controller_type="OSC_POSE", - action=action, - controller_cfg=self.velocity_controller_cfg, - ) - - def reset_joints( - self, - timeout=7, - gripper_open=False, - ): - start_joint_pos = [ - 0.09162008114028396, - -0.19826458111314524, - -0.01990020486871322, - -2.4732269941140346, - -0.01307073642274261, - 2.30396583422025, - 0.8480939705504309, - ] - assert type(start_joint_pos) is list or type(start_joint_pos) is np.ndarray - controller_cfg = get_default_controller_config(controller_type="JOINT_POSITION") - - if gripper_open: - gripper_action = -1 - else: - gripper_action = 1 - - # This is for varying initialization of joints a little bit to - # increase data variation. - # start_joint_pos = [ - # e + np.clip(np.random.randn() * 0.005, -0.005, 0.005) - # for e in start_joint_pos - # ] - if type(start_joint_pos) is list: - action = start_joint_pos + [gripper_action] - else: - action = start_joint_pos.tolist() + [gripper_action] - start_time = time.time() - while True: - if self.received_states and self.check_nonzero_configuration(): - if ( - np.max(np.abs(np.array(self.last_q) - np.array(start_joint_pos))) - < 1e-3 - ): - break - self.control( - controller_type="JOINT_POSITION", - action=action, - controller_cfg=controller_cfg, - ) - end_time = time.time() - - # Add timeout - if end_time - start_time > timeout: - break + def osc_move(self, target_pos, target_quat, gripper_state): + """Send OSC control command to the robot""" + # This would need to be implemented to send control commands + # to the deoxys services via ZMQ publishers + # For now, just updating the target internally + pass + + def reset_joints(self, timeout=7, gripper_open=False): + """Reset robot to initial joint configuration""" + # This would need to be implemented to send joint position commands + # to the deoxys services via ZMQ publishers + # For now, just a placeholder return True + + def control(self, controller_type, action, controller_cfg): + """Send control command to deoxys services""" + # This would need to be implemented to send control commands + # to the deoxys services via ZMQ publishers + # For now, just a placeholder + pass