使用 PyBullet 搭建仿真环境

参考 Simulation and Control
简洁方式建立环境

from simulation_and_control import pb
sim = pb.SimInterface(conf_file_name, conf_file_path_ext = cur_dir)
# Get active joint names from the simulation
ext_names = sim.getNameActiveJoints()
# Adjust the shape for compatibility
ext_names = np.expand_dims(np.array(ext_names), axis=0)

SimInterface：搭建环境
1. 读入环境参数，如 time_step 和 use_gui 等
2. 建立 Client：BulletClient 是 PyBullet 的封装客户端类，主要用于：
  - 创建和管理 独立的物理仿真环境（Physics Client / Physics Server）
  - 可以让你在同一个 Python 程序里同时开启 多个独立仿真环境，互不干扰
3. 设置环境参数
4. 建立平面：PyBullet 自带一些 内置 URDF 文件，比如：
  - "plane.urdf" → 一个平面地面，用作仿真环境的地板
  - "r2d2.urdf" → 一个示例机器人
  - 这些 URDF 文件通常存放在 pybullet_data 目录下（可通过 pybullet_data.getDataPath() 获取）

self.pybullet_client = bullet_client.BulletClient(connection_mode=connection_mode)
 
self.pybullet_client.setPhysicsEngineParameter(numSolverIterations=30)
self.pybullet_client.setTimeStep(self.time_step)
self.pybullet_client.setGravity(0, 0, -9.81)
self.pybullet_client.setPhysicsEngineParameter(enableConeFriction=0)
self.pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
 
self.ground_body = self.pybullet_client.loadURDF("plane.urdf")

SimInterface：创建 Robot

for i in range(len(conf_file_json['robot_pybullet']['urdf_path'])):
	bot = SimRobot(self.pybullet_client,conf_file_json,i,conf_file_path_ext)
	self.bot.append(bot)

SimInterface：初始化 Robot 姿态
1. self.pybullet_client.setJointMotorControl2
2. self.pybullet_client.resetJointState
3. self.pybullet_client.resetBasePositionAndOrientation
4. 计算每个 robot 在公共坐标系中的位姿（通过 self.pybullet_client.invertTransform）

self.ResetPose()

SimInterface：建立 Observation 队列，并初始化
- collections.deque 是一个双端队列，常见于延迟模拟、滑动窗口处理、强化学习/仿真

self.observation_history=collections.deque(maxlen=100) # 这是一个双端队列
self.ReceiveObservation()

current_state = {
	'motor_angles':self.bot[j].motor_angles,
	'motor_velocities':self.bot[j].motor_velocities,
	'motor_accelerations':self.bot[j].motor_accelerations,
	'base_position': self.bot[j].base_position,
	'base_orientation': self.bot[j].base_orientation,
	'base_lin_vel': self.bot[j].base_lin_vel,
	'base_ang_vel': self.bot[j].base_ang_vel,
	'base_lin_vel_body_frame': self.bot[j].base_lin_vel_body_frame,
	'base_ang_vel_body_frame': self.bot[j].base_ang_vel_body_frame
}
self.bot[j].state_buffer.append(current_state)  # Update the buffer

SimBot：设置参数并加载 URDF 模型 self._LoadPybulletURDF
1. 设置参数
2. 获得 URDF 路径
3. 根据不同设置获得初始位姿，并加载
4. 加载 Joint 的基础设置，包括激活、去除阻尼和摩擦力等 pybullet_client.getJointInfo
5. 开启 FeetJoint 的力传感器

# 示例
self.bot_pybullet = pybullet_client.loadURDF(
	urdf_file,
	self._GetDefaultInitPosition(),
	self._GetDefaultInitOrientation(),
	useFixedBase=True,
	flags=flags)
	
self._BuildJointNameToIdAndActiveJoint(pybullet_client)
self._RemoveDefaultJointDamping(pybullet_client)
self._RemoveURDFJointDampingAndFriction(pybullet_client)
 
self._BuildFeetJointIDAndForceSensors(pybullet_client,index)

SimBot：设置基础参数
1. 机器人足部的 friction 和 restitution
2. 每个 link 的初始位姿
3. 每个 motor 的初始偏移量和方向
  - 物理关节的零位可能不是 URDF 定义的零位；所以要做减法，把角度映射到标准零位：np.asarray(motor_angles) - np.asarray(self.bot[index].motor_offset)
  - 把反向安装的关节翻转成统一正向：乘以 self.bot[index].motor_direction
4. 每个 joint 的初始角度和速度
5. 建立管理所有 motor 的模型

self.servo_motor_model = ServoMotorModel(
	len(self.active_joint_ids), 
	self.conf['robot_pybullet']["servo_pos_gains"][index], 			
	self.conf['robot_pybullet']["servo_vel_gains"][index],
	friction_torque=self.conf['robot_pybullet']['motor_damping'][index],
	friction_coefficient=self.conf['robot_pybullet']['motor_damping_coeff'][index],
	elastic_torque=self.conf['robot_pybullet']['motor_elastic_torque'][index],
	elastic_coefficient=self.conf['robot_pybullet']['motor_elastic_coeff'][index],
	motor_load=self.conf['robot_pybullet']['motor_inertia'][index],
	motor_load_coefficient=self.conf['robot_pybullet']['motor_inertia_coeff'][index])

SimBot：添加 noise 和 delay 等额外设置，建立 state_buffer

self.state_buffer = collections.deque(maxlen=self.delay_steps + 1)

附录：getJointInfo 的返回值，是一个元组

for i in range(pybullet_client.getNumJoints(robot_id)):
    info = pybullet_client.getJointInfo(robot_id, i)
    print(f"Joint {i}: name={info[1].decode('utf-8')}, type={info[2]}, lower={info[8]}, upper={info[9]}")

Tip

序号字段含义
0 jointIndex 关节索引
1 jointName 关节名称（byte string，需要 .decode('utf-8')）
2 jointType 关节类型（0=REVOLUTE/旋转, 1=PRISMATIC/平移, 2=SPHERICAL, 3=PLANAR, 4=FIXED）
3 qIndex 状态向量里关节位置的索引（-1 表示不在状态向量里）
4 uIndex 状态向量里关节速度/力的索引（-1 表示不在状态向量里）
5 flags 关节标志位（bit mask）
6 jointDamping 关节阻尼系数
7 jointFriction 关节摩擦系数
8 jointLowerLimit 关节下限（弧度或米）
9 jointUpperLimit 关节上限（弧度或米）
10 jointMaxForce 关节最大力/扭矩
11 jointMaxVelocity 关节最大速度
12 linkName 关节对应的子连杆名称（byte string）
13 jointAxis 关节轴向向量 [x, y, z]
14 parentFramePos 父连杆坐标系下关节位置 [x, y, z]
15 parentFrameOrn 父连杆坐标系下关节姿态 [x, y, z, w]（四元数）
16 parentIndex 父连杆索引（-1 表示根链接）

具身智能学习笔记

Explorer

使用 PyBullet 搭建仿真环境

Graph View

Backlinks

序号	字段	含义
0	`jointIndex`	关节索引
1	`jointName`	关节名称（byte string，需要 `.decode('utf-8')`）
2	`jointType`	关节类型（0=REVOLUTE/旋转, 1=PRISMATIC/平移, 2=SPHERICAL, 3=PLANAR, 4=FIXED）
3	`qIndex`	状态向量里关节位置的索引（-1 表示不在状态向量里）
4	`uIndex`	状态向量里关节速度/力的索引（-1 表示不在状态向量里）
5	`flags`	关节标志位（bit mask）
6	`jointDamping`	关节阻尼系数
7	`jointFriction`	关节摩擦系数
8	`jointLowerLimit`	关节下限（弧度或米）
9	`jointUpperLimit`	关节上限（弧度或米）
10	`jointMaxForce`	关节最大力/扭矩
11	`jointMaxVelocity`	关节最大速度
12	`linkName`	关节对应的子连杆名称（byte string）
13	`jointAxis`	关节轴向向量 `[x, y, z]`
14	`parentFramePos`	父连杆坐标系下关节位置 `[x, y, z]`
15	`parentFrameOrn`	父连杆坐标系下关节姿态 `[x, y, z, w]`（四元数）
16	`parentIndex`	父连杆索引（-1 表示根链接）