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

一、 核心函数 1：p.getNumJoints(robot_id)

1. 语法格式

num_joints = p.getNumJoints(bodyUniqueId)

2. 关键参数

参数名	类型	说明
bodyUniqueId（此处为robot_id）	int	必选参数，机器人模型的唯一 ID（由p.loadURDF()加载模型时返回），用于指定要查询的目标机器人

3. 返回值

整数类型（int），表示目标机器人的总关节数量（包含固定关节、旋转关节、移动关节等所有类型的关节，基座不被计入关节数量）。

4. 代码中的作用

num_joints = p.getNumJoints(robot_id)

获取robot_id对应的机器人总关节数，为后续遍历所有关节（for i in range(num_joints)）提供循环范围，确保不会遗漏或超出关节索引范围。

二、 核心函数 2：p.getJointInfo(robot_id, i)

1. 语法格式

joint_info_tuple = p.getJointInfo(bodyUniqueId, jointIndex)

2. 关键参数

参数名	类型	说明
bodyUniqueId（robot_id）	int	必选参数，机器人模型唯一 ID，指定查询对象
jointIndex（此处为i）	int	必选参数，关节索引（范围：0 ~ 总关节数-1），指定要查询的具体关节

3. 返回值

不可变元组（tuple），包含约 20 项关节属性。详见pybullet.getJointInfo初识-CSDN博客

通过 info[2] != p.JOINT_FIXED 筛选可动关节，关键常量说明：

常量名	数值	含义	是否可动
p.JOINT_FIXED	4	固定关节（连杆间刚性连接，无相对运动）	否（需过滤）
p.JOINT_REVOLUTE	0	旋转关节（绕单轴旋转，如机械臂关节）	是（保留）
p.JOINT_PRISMATIC	1	移动关节（沿单轴平移，如直线滑台）	是（保留）
p.JOINT_SPHERICAL	2	球关节（三维旋转，如肩关节）	是（保留）

eg

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. 关键参数

参数名	类型	说明
bodyUniqueId（robot_id）	int	必选参数，机器人模型唯一 ID
jointIndex（此处为j）	int	必选参数，要重置状态的关节索引（此处为可动关节索引）
targetValue（此处为 0）	float	必选参数，关节目标位置（旋转关节：单位为弧度；移动关节：单位为米）
targetVelocity	float	可选参数，关节目标速度，默认值为 0.0

3. 功能特性

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

eg

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. 关键参数

参数名	类型	说明
bodyUniqueId（robot_id）	int	必选参数，机器人模型唯一 ID
linkIndex（end_effector_link_index）	int	必选参数，连杆索引（范围：-1 ~ 总连杆数-1；-1对应基座，非负整数对应具体连杆）
computeForwardKinematics	bool	可选参数，是否强制计算正运动学，默认值为 False（优先使用缓存数据，提升效率）

3. 返回值

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

元组索引	属性含义	类型	代码中的使用
0	连杆世界坐标系位置	tuple	ee_state[0]：获取末端执行器的三维坐标（x, y, z）
1	连杆世界坐标系姿态（四元数）	tuple	未使用，可用于获取末端执行器的旋转姿态
2	连杆局部坐标系位置（相对于基座）	tuple	未使用
3	连杆局部坐标系姿态（相对于基座）	tuple	未使用
4+	连杆速度等进阶属性	-	未使用

eg

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()能获取到零位姿态下的最新末端执行器位置（若不调用，可能获取到重置前的旧状态）。

六、 相关语法汇总表

函数名	核心功能	必选参数	返回值	代码中的用途
p.getNumJoints(robot_id)	获取机器人总关节数	robot_id（模型 ID）	整数（总关节数）	为关节遍历提供循环范围
p.getJointInfo(robot_id, i)	获取单个关节详细信息	robot_id（模型 ID）、i（关节索引）	元组（关节属性）	筛选非固定关节，收集可动关节索引
p.resetJointState(robot_id, j, 0)	重置关节位置 / 速度	robot_id（模型 ID）、j（关节索引）、0（目标位置）	无返回值	将可动关节重置为 0，初始化零位姿态
p.getLinkState(robot_id, ee_link_idx)	获取连杆位置 / 姿态	robot_id（模型 ID）、ee_link_idx（连杆索引）	元组（连杆状态）	读取零位姿态下末端执行器的世界坐标
p.stepSimulation()	步进物理仿真	无（默认仿真客户端 ID=0）	无返回值	刷新仿真状态，确保零位姿态生效

总结

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）为首要参数，关节 / 连杆索引为核心定位参数，返回值多为元组类型，需按索引提取目标属性。