From 05ba5f7ca2d97897592ab6b4179bcc9a83511c09 Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Fri, 10 Apr 2026 15:01:53 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E4=B8=A2=E5=BC=83=E6=95=B0?= =?UTF-8?q?=E6=8D=AE=E8=A7=A3=E6=9E=90=E7=9A=84bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/joint_sensor_zero_offsets.yaml | 46 +++++++++---------- .../openarm_joint_sensor/joint_sensor_can.py | 34 ++++++++------ 2 files changed, 44 insertions(+), 36 deletions(-) diff --git a/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml b/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml index 64bb58c..ce53806 100644 --- a/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml +++ b/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml @@ -6,43 +6,43 @@ joint_sensor_node: right_joint_states_topic: /right_joint_sensor_states left_joint_states_topic: /left_joint_sensor_states right_zero_offsets: - - 2.7995149378912956 - - 2.5233983960718795 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 - - 0.0 + - 0.023009711818284373 + - -2.304039143404233 + - 1.9236119080085938 + - 0.09357282806102418 + - 1.6321555583103224 + - -0.9848156658225804 + - -2.06934008285773 + - 1.3744467859455343 right_sensor_directions: - -1.0 - 1.0 - -1.0 - - -1.0 - - -1.0 - - 1.0 - 1.0 - -1.0 - right_auto_zero_on_first_sample: true + - 1.0 + - -1.0 + - 1.0 + right_auto_zero_on_first_sample: false left_zero_offsets: - - -1.1550875332778876 - - 0.7838641826095625 - - -2.710544052193928 - - 0.12118448224296552 - - 3.0265440944983695 - - 2.2196702000705226 - - 1.0921943209745777 - - 1.3729128051576485 + - 2.492718780314167 + - -0.7577865092155065 + - -0.09817477042468115 + - -1.7257283863713466 + - 1.7119225592803753 + - -1.5201749607946704 + - -3.100175172316881 + - 1.2977477465512521 left_sensor_directions: - -1.0 - 1.0 - -1.0 - - 1.0 + - -1.0 - -1.0 - 1.0 - - -1.0 - 1.0 - left_auto_zero_on_first_sample: true + - -1.0 + left_auto_zero_on_first_sample: false right_calibrate_now: false left_calibrate_now: false calibration_yaml_path: /home/shen/openarm_test/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py index c132903..0f5149b 100644 --- a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py +++ b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py @@ -505,14 +505,19 @@ class JointSensorNode(Node): return False since_last = now_sec - state['last_raw_reject_log_sec'][joint_index] + raw_data = (msg.data[1] << 8) | msg.data[2] + deg = (raw_data / 4096.0) * 360.0 + rad = math.radians(deg) + rad = self.wrap_to_pi(rad) if since_last >= self.raw_jump_log_min_interval_sec: self.get_logger().warning( f"[{self.arm_labels[arm_name]}] Joint{joint_index + 1} RAW frame dropped: prev_raw={prev:.6f} curr_raw={raw_sensor_angle:.6f} " f"delta={delta:.6f} rad threshold={self.raw_jump_reject_threshold_rad:.6f} " - f"can_id=0x{msg.arbitration_id:X} data={msg.data.hex()}" + f"can_id=0x{msg.arbitration_id:X} data={msg.data.hex()} " + f"angle={rad:.6f} rad" ) state['last_raw_reject_log_sec'][joint_index] = now_sec - + state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle return True def extract_raw_sensor_angle_rad(self, data: bytes): @@ -524,14 +529,17 @@ class JointSensorNode(Node): if len(data) < 3: return None raw = (data[1] << 8) | data[2] - return (raw / 4096.0) * (2.0 * math.pi) + deg = (raw / 4096.0) * 360.0 + rad = math.radians(deg) + rad = self.wrap_to_pi(rad) + return rad if len(data) < 2: return None # 通用格式：前2字节角度，默认 centi-rad - raw = int.from_bytes(data[0:2], byteorder='big', signed=True) - return self.raw_to_rad(raw) + # raw = int.from_bytes(data[0:2], byteorder='big', signed=True) + # return self.raw_to_rad(raw) def wrap_to_pi(self, angle_rad: float) -> float: # 归一化到 [-pi, pi)，解决 0~2pi 到 ±pi 的过零映射问题 @@ -566,7 +574,7 @@ class JointSensorNode(Node): # 特殊格式：0xAA + 16bit角度 + 16bit速度 if data[0] == 0xAA: - if len(data) < 5: + if len(data) < 8: return 0.0, 0.0 raw_deg = (data[1] << 8) | data[2] raw_vel = (data[3] << 8) | data[4] @@ -576,17 +584,17 @@ class JointSensorNode(Node): vel_rad = math.radians(vel_deg) return rad, vel_rad - if len(data) < 4: + if len(data) < 8: return 0.0, 0.0 - # 通用格式：前2字节为角度，后2字节为速度，默认按 centi-rad 解析 - raw_deg = int.from_bytes(data[0:2], byteorder='big', signed=True) - rad = self.wrap_to_pi(self.raw_to_rad(raw_deg)) + # # 通用格式：前2字节为角度，后2字节为速度，默认按 centi-rad 解析 + # raw_deg = int.from_bytes(data[0:2], byteorder='big', signed=True) + # rad = self.wrap_to_pi(self.raw_to_rad(raw_deg)) - raw_vel = int.from_bytes(data[2:4], byteorder='big', signed=True) - vel_rad = self.raw_to_rad(raw_vel) + # raw_vel = int.from_bytes(data[2:4], byteorder='big', signed=True) + # vel_rad = self.raw_to_rad(raw_vel) - return rad, vel_rad + # return rad, vel_rad def main():