PyBullet 筛选机器人可动关节、校准零位姿态及获取末端执行器位置

示例逻辑：

info = p.getJointInfo(robot_id, i)
if info[2] != p.JOINT_FIXED:
    pybullet_joint_indices.append(i)

1. 遍历每个关节索引 i，获取关节详细信息 info；
2. 通过 info[2] 获取关节类型，排除固定关节（p.JOINT_FIXED）；
3. 将所有可动关节的索引存入 pybullet_joint_indices 列表，为后续关节控制（如力矩控制、位置控制）提供目标关节集合。

三、核心函数 3：p.resetJointState(robot_id, j, 0)

1. 语法格式

p.resetJointState(bodyUniqueId, jointIndex, targetValue, targetVelocity=0.0)

2. 关键参数

3. 功能特性

• 属于状态重置函数，直接强制设置关节的位置和速度，不经过物理仿真迭代（瞬间生效）；
• 不会产生额外的关节力矩，仅用于初始化关节姿态或恢复关节状态（此处用于校准零位姿态）。

示例：

for j in pybullet_joint_indices:
    p.resetJointState(robot_id, j, 0)

遍历所有可动关节，将每个关节的位置强制重置为 0（弧度），使机器人进入零位姿态（机械臂的初始校准姿态，用于后续误差分析和运动规划）。

四、核心函数 4：p.getLinkState(robot_id, end_effector_link_index)

1. 语法格式

link_state_tuple = p.getLinkState(bodyUniqueId, linkIndex, computeForwardKinematics=False)

2. 关键参数

3. 返回值

不可变元组（tuple），核心属性索引说明（代码中用到索引 0 的连杆位置）：

示例：

 ee_state = p.getLinkState(robot_id, end_effector_link_index)
 ee_pos_zero = ee_state[0]

1. p.getLinkState(...)：获取末端执行器连杆的完整状态（位置、姿态等）；
2. ee_state[0]：提取末端执行器在世界坐标系下的三维位置，并赋值给 ee_pos_zero；
3. 打印该位置用于诊断：验证零位姿态下末端执行器的实际位置是否符合预期，为后续校准和误差分析提供参考。

五、辅助语法：p.stepSimulation()

1. 语法格式

p.stepSimulation(physicsClientId=0)

2. 功能说明

• 步进物理仿真函数，执行一次完整的物理计算迭代（包括碰撞检测、力的更新、关节 / 物体姿态更新等）；
• 在 p.resetJointState() 之后调用，用于更新仿真世界的状态，确保 p.getLinkState() 能获取到零位姿态下的最新末端执行器位置（若不调用，可能获取到重置前的旧状态）。

六、相关语法汇总表

总结

1. 核心语法：代码围绕 5 个 PyBullet 函数展开，其中 p.getJointInfo 和 p.getLinkState 是获取机器人结构信息的核心，p.resetJointState 是姿态初始化的关键；
2. 筛选逻辑：通过 p.getJointInfo 的关节类型（索引 2）排除 p.JOINT_FIXED，得到可动关节索引列表；
3. 零位校准：p.resetJointState 重置关节为 0 + p.stepSimulation 刷新状态 + p.getLinkState 读取末端位置，形成完整的零位诊断流程；
4. 参数特性：所有函数均以 robot_id（模型唯一 ID）为首要参数，关节 / 连杆索引为核心定位参数，返回值多为元组类型，需按索引提取目标属性。