Struct JointDynamicState 

pub struct JointDynamicState {
    pub group_timestamp_us: u64,
    pub joint_vel: [f64; 6],
    pub joint_current: [f64; 6],
    pub timestamps: [u64; 6],
    pub valid_mask: u8,
}

关节动态状态（独立帧，但通过缓冲提交保证一致性）

更新频率：500Hz 大小：< 150 字节，Clone 开销低 同步机制：Buffered Commit（收集 6 个关节的速度帧，集齐或超时后一次性提交） 时间同步性：通过 Group Commit 机制，保证 6 个关节数据来自同一 CAN 传输周期

Fields

group_timestamp_us: u64

整个组的大致时间戳（最新一帧的时间，微秒）

注意：存储的是硬件时间戳（来自 PiperFrame.timestamp_us），不是 UNIX 时间戳。 硬件时间戳是设备相对时间，用于帧间时间差计算，不能直接与系统时间戳比较。

joint_vel: [f64; 6]

关节速度（rad/s）[J1, J2, J3, J4, J5, J6]

joint_current: [f64; 6]

关节电流（A）[J1, J2, J3, J4, J5, J6]

注意：扭矩可以通过 get_torque(joint_index) 方法从电流值实时计算得到。

• 关节 1-3 (J1, J2, J3): torque = current * COEFFICIENT_1_3 (1.18125)
• 关节 4-6 (J4, J5, J6): torque = current * COEFFICIENT_4_6 (0.95844)

timestamps: [u64; 6]

每个关节的具体更新时间（用于调试或高阶插值）

valid_mask: u8

有效性掩码（Bit 0-5 对应 Joint 1-6）

• 1 表示本周期内已更新
• 0 表示未更新（可能是丢帧）

Implementations

impl JointDynamicState

pub const COEFFICIENT_1_3: f64 = 1.18125

关节 1-3 的力矩系数（CAN ID: 0x251~0x253）

根据官方参考实现，关节 1、2、3 使用此系数计算力矩。 公式：torque = current * COEFFICIENT_1_3

pub const COEFFICIENT_4_6: f64 = 0.95844

关节 4-6 的力矩系数（CAN ID: 0x254~0x256）

根据官方参考实现，关节 4、5、6 使用此系数计算力矩。 公式：torque = current * COEFFICIENT_4_6

pub fn calculate_torque(joint_index: usize, current: f64) -> f64

根据关节索引和电流值计算扭矩（N·m）

参数

• joint_index: 关节索引（0-5，对应 J1-J6）
• current: 电流值（A）

返回值

计算得到的力矩值（N·m）

示例

// 计算 J1（索引 0）的扭矩，电流为 2.0A
let torque = JointDynamicState::calculate_torque(0, 2.0);
// 结果：2.0 * 1.18125 = 2.3625 N·m

pub fn get_torque(&self, joint_index: usize) -> f64

获取指定关节的扭矩（N·m）

从当前存储的电流值实时计算扭矩，无需额外存储空间。

参数

• joint_index: 关节索引（0-5，对应 J1-J6）

返回值

关节扭矩值（N·m），如果索引超出范围则返回 0.0

示例

let mut state = JointDynamicState::default();
state.joint_current[0] = 2.0; // 设置 J1 的电流为 2.0A
let torque_j1 = state.get_torque(0); // 获取 J1 的扭矩：2.0 * 1.18125 = 2.3625 N·m

pub fn get_all_torques(&self) -> [f64; 6]

获取所有关节的扭矩（N·m）

一次性计算并返回所有6个关节的扭矩值，比多次调用 get_torque() 更高效。

返回值

包含所有关节扭矩的数组 [J1, J2, J3, J4, J5, J6]（N·m）

示例

let mut state = JointDynamicState::default();
state.joint_current = [1.0, 2.0, 0.5, 1.0, 2.0, 0.5];
let all_torques = state.get_all_torques();
// all_torques[0] = 1.0 * 1.18125 = 1.18125 N·m (J1)
// all_torques[3] = 1.0 * 0.95844 = 0.95844 N·m (J4)

pub fn is_complete(&self) -> bool

检查所有关节是否都已更新（valid_mask == 0x3F）

pub fn missing_joints(&self) -> Vec<usize>

获取未更新的关节索引（用于调试）

Trait Implementations

impl Clone for JointDynamicState

fn clone(&self) -> JointDynamicState

Returns a duplicate of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Debug for JointDynamicState

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.

impl Default for JointDynamicState

fn default() -> JointDynamicState

Returns the “default value” for a type.