====== 单个python文件实现pybullet在arx5上的双臂仿真 ======
首先看效果图：
{{:机器人:仿真:pybullet:pybullet常用代码:arx5.png?800|}}
安装python环境
''pip install pybullet numpy''
准备代码目录结构如下：
.
├── arx5
│ ├── apple.urdf
│ ├── arx5.urdf
│ ├── meshes
│ │ ├── X5
│ │ │ ├── link1.STL
│ │ │ ├── link2.STL
│ │ │ ├── link3.STL
│ │ │ ├── link4.STL
│ │ │ ├── link5.STL
│ │ │ └── link6.STL
│ │ └── base_link.STL
│ └── run-sim.py
└── main.py
其中STL文件参考：
- link文件 https://github.com/real-stanford/arx5-sdk/blob/main/models/meshes/X5/link1.STL
- base_link文件：https://github.com/real-stanford/arx5-sdk/blob/main/models/meshes/X5/base_link.STL
URDF文件参考
- arx5.URDF文件：https://github.com/real-stanford/arx5-sdk/blob/main/models/X5.urdf
- apple.URDF 文件是用AI生成的：
最终的模拟启动代码如下（纯CPU的）：
import pybullet as p
import pybullet_data
import time
import random
import numpy as np
import os
current_py_cwd = os.path.join(os.path.dirname(__file__))
class GraspingController:
def __init__(self, robot_id, gripper_link_index):
self.robot_id = robot_id
self.gripper_link_index = gripper_link_index
self.constraints = []
def grasp_object(self, object_id):
gripper_state = p.getLinkState(self.robot_id, self.gripper_link_index)
gripper_pos, gripper_orn = gripper_state[0], gripper_state[1]
obj_pos, obj_orn = p.getBasePositionAndOrientation(object_id)
constraint_id = p.createConstraint(
parentBodyUniqueId=self.robot_id,
parentLinkIndex=self.gripper_link_index,
childBodyUniqueId=object_id,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0]
)
self.constraints.append(constraint_id)
return constraint_id
def release_object(self, constraint_id=None):
if constraint_id:
p.removeConstraint(constraint_id)
if constraint_id in self.constraints:
self.constraints.remove(constraint_id)
else:
for constraint in self.constraints:
p.removeConstraint(constraint)
self.constraints = []
class ARX5Simulation:
def __init__(self):
try:
p.connect(p.GUI, options="--enable_gpu")
except:
print("警告：无法启用GPU加速，PyBullet将使用CPU进行物理计算")
print("错误：需要GPU加速才能运行此仿真。程序退出。")
exit(1)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)
try:
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0)
p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=45, cameraPitch=-30, cameraTargetPosition=[0,0,0.5])
except:
print("某些渲染选项不可用，继续使用默认设置")
p.setGravity(0, 0, -9.81)
p.setRealTimeSimulation(0)
self.plane_id = p.loadURDF("plane.urdf")
self.object_urdfs = [os.path.join(current_py_cwd, "apple.urdf")]
self.object_id = p.loadURDF(random.choice(self.object_urdfs), basePosition=[0, 0, 0.5])
self.robot_id1 = p.loadURDF(os.path.join(current_py_cwd, "arx5.urdf"), basePosition=[-0.5, 0, 0], useFixedBase=True)
self.robot_id2 = p.loadURDF(os.path.join(current_py_cwd, "arx5.urdf"), basePosition=[0.5, 0, 0], useFixedBase=True)
self.num_joints1 = p.getNumJoints(self.robot_id1)
self.num_joints2 = p.getNumJoints(self.robot_id2)
print(f"ARX5 左臂有 {self.num_joints1} 个关节")
print(f"ARX5 右臂有 {self.num_joints2} 个关节")
self.ee_index1 = 7
self.ee_index2 = 7
self.gripper1 = GraspingController(self.robot_id1, self.ee_index1)
self.gripper2 = GraspingController(self.robot_id2, self.ee_index2)
light_position = [random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(2, 3)]
p.addUserDebugParameter("lightPosX", -2, 2, light_position[0])
p.addUserDebugParameter("lightPosY", -2, 2, light_position[1])
p.addUserDebugParameter("lightPosZ", 1, 3, light_position[2])
self.target_x1 = p.addUserDebugParameter("Target X1", -1, 1, -0.3)
self.target_y1 = p.addUserDebugParameter("Target Y1", -1, 1, 0)
self.target_z1 = p.addUserDebugParameter("Target Z1", 0.1, 1.5, 0.8)
self.target_x2 = p.addUserDebugParameter("Target X2", -1, 1, 0.3)
self.target_y2 = p.addUserDebugParameter("Target Y2", -1, 1, 0)
self.target_z2 = p.addUserDebugParameter("Target Z2", 0.1, 1.5, 0.8)
self.grasp_mode1 = p.addUserDebugParameter("Grasp Arm1", 0, 1, 0)
self.grasp_mode2 = p.addUserDebugParameter("Grasp Arm2", 0, 1, 0)
self.joint1_params = []
self.joint2_params = []
for i in range(6):
joint_info = p.getJointInfo(self.robot_id1, i)
joint_lower_limit = joint_info[8]
joint_upper_limit = joint_info[9]
lower = joint_lower_limit if joint_lower_limit != 0.0 else -3.14
upper = joint_upper_limit if joint_upper_limit != 0.0 else 3.14
param_id = p.addUserDebugParameter(f"Joint1_{i+1}", lower, upper, 0.0)
self.joint1_params.append(param_id)
for i in range(6):
joint_info = p.getJointInfo(self.robot_id2, i)
joint_lower_limit = joint_info[8]
joint_upper_limit = joint_info[9]
lower = joint_lower_limit if joint_lower_limit != 0.0 else -3.14
upper = joint_upper_limit if joint_upper_limit != 0.0 else 3.14
param_id = p.addUserDebugParameter(f"Joint2_{i+1}", lower, upper, 0.0)
self.joint2_params.append(param_id)
self.arm1_constraint = None
self.arm2_constraint = None
self.current_joint_positions1 = [0] * self.num_joints1
self.current_joint_positions2 = [0] * self.num_joints2
def compute_ik(self, robot_id, ee_index, target_pos, target_orn=None):
if target_orn is None:
target_orn = p.getQuaternionFromEuler([0, 0, 0])
try:
joint_poses = p.calculateInverseKinematics(
robot_id,
ee_index,
target_pos,
target_orn
)
return joint_poses
except:
current_poses = []
for i in range(ee_index):
joint_info = p.getJointState(robot_id, i)
current_poses.append(joint_info[0])
return current_poses
def move_arm_to_position(self, robot_id, target_pos, ee_index, max_iterations=100):
target_orn = p.getQuaternionFromEuler([0, 0, 0])
joint_poses = self.compute_ik(robot_id, ee_index, target_pos, target_orn)
for i in range(min(len(joint_poses), ee_index)):
p.setJointMotorControl2(
bodyUniqueId=robot_id,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=joint_poses[i],
force=500
)
def check_grasp_proximity(self, robot_id, ee_index, object_id, threshold=0.1):
gripper_state = p.getLinkState(robot_id, ee_index)
gripper_pos = gripper_state[0]
obj_pos, _ = p.getBasePositionAndOrientation(object_id)
distance = np.linalg.norm(np.array(gripper_pos) - np.array(obj_pos))
return distance < threshold
def run_simulation(self):
print("Starting simulation. Use the sliders to control the arms and grasp objects.")
prev_grasp1 = 0
prev_grasp2 = 0
while True:
target_pos1 = [
p.readUserDebugParameter(self.target_x1),
p.readUserDebugParameter(self.target_y1),
p.readUserDebugParameter(self.target_z1)
]
target_pos2 = [
p.readUserDebugParameter(self.target_x2),
p.readUserDebugParameter(self.target_y2),
p.readUserDebugParameter(self.target_z2)
]
use_position_control = True
for i in range(6):
joint_val1 = p.readUserDebugParameter(self.joint1_params[i])
joint_val2 = p.readUserDebugParameter(self.joint2_params[i])
if abs(joint_val1) > 0.01 or abs(joint_val2) > 0.01:
use_position_control = False
break
if use_position_control:
self.move_arm_to_position(self.robot_id1, target_pos1, self.ee_index1)
self.move_arm_to_position(self.robot_id2, target_pos2, self.ee_index2)
else:
for i in range(6):
joint_angle1 = p.readUserDebugParameter(self.joint1_params[i])
p.setJointMotorControl2(
bodyUniqueId=self.robot_id1,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=joint_angle1,
force=500
)
joint_angle2 = p.readUserDebugParameter(self.joint2_params[i])
p.setJointMotorControl2(
bodyUniqueId=self.robot_id2,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=joint_angle2,
force=500
)
grasp1 = p.readUserDebugParameter(self.grasp_mode1)
grasp2 = p.readUserDebugParameter(self.grasp_mode2)
if grasp1 > 0.5 and prev_grasp1 < 0.5:
if self.check_grasp_proximity(self.robot_id1, self.ee_index1, self.object_id):
if self.arm1_constraint is None:
self.arm1_constraint = self.gripper1.grasp_object(self.object_id)
print("Arm 1: Grasped object")
else:
print("Arm 1: Too far from object to grasp")
elif grasp1 < 0.5 and prev_grasp1 > 0.5:
if self.arm1_constraint is not None:
self.gripper1.release_object(self.arm1_constraint)
self.arm1_constraint = None
print("Arm 1: Released object")
if grasp2 > 0.5 and prev_grasp2 < 0.5:
if self.check_grasp_proximity(self.robot_id2, self.ee_index2, self.object_id):
if self.arm2_constraint is None:
self.arm2_constraint = self.gripper2.grasp_object(self.object_id)
print("Arm 2: Grasped object")
else:
print("Arm 2: Too far from object to grasp")
elif grasp2 < 0.5 and prev_grasp2 > 0.5:
if self.arm2_constraint is not None:
self.gripper2.release_object(self.arm2_constraint)
self.arm2_constraint = None
print("Arm 2: Released object")
prev_grasp1 = grasp1
prev_grasp2 = grasp2
p.stepSimulation()
time.sleep(1./240.)
def shutdown(self):
p.disconnect()
def main():
sim = ARX5Simulation()
try:
sim.run_simulation()
except KeyboardInterrupt:
print("Simulation interrupted by user.")
finally:
sim.shutdown()
if __name__ == "__main__":
main()