单个python文件实现pybullet在arx5上的双臂仿真
首先看效果图：
安装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文件参考：
URDF文件参考
- apple.URDF 文件是用AI生成的：
- apple.urdf
<?xml version="1.0"?> <robot name="apple"> <!-- 苹果主体（球体近似） --> <link name="apple_body"> <visual> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <sphere radius="0.08"/> <!-- 苹果直径约16cm，半径0.08m --> </geometry> <material name="red_green"> <color rgba="0.8 0.2 0.2 1.0"/> <!-- 红色为主色 --> </material> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <sphere radius="0.08"/> </geometry> </collision> <inertial> <origin xyz="0 0 0" rpy="0 0 0"/> <mass value="0.2"/> <!-- 苹果重量约200g --> <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/> </inertial> </link> <!-- 苹果柄（圆柱体） --> <link name="apple_stem"> <visual> <origin xyz="0 0 0.08" rpy="1.5708 0 0"/> <!-- 沿Y轴旋转90度 --> <geometry> <cylinder length="0.03" radius="0.005"/> <!-- 柄长3cm，半径0.5cm --> </geometry> <material name="brown"> <color rgba="0.5 0.25 0.1 1.0"/> <!-- 棕色 --> </material> </visual> <collision> <origin xyz="0 0 0.08" rpy="1.5708 0 0"/> <geometry> <cylinder length="0.03" radius="0.005"/> </geometry> </collision> <inertial> <origin xyz="0 0 0.08" rpy="0 0 0"/> <mass value="0.01"/> <!-- 柄重量约10g --> <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/> </inertial> </link> <!-- 苹果叶子（平面近似） --> <link name="apple_leaf"> <visual> <origin xyz="0.02 0 0.1" rpy="0 0 0"/> <!-- 稍微偏移以贴近柄 --> <geometry> <box size="0.04 0.02 0.001"/> <!-- 叶子宽4cm，长2cm，厚0.1cm --> </geometry> <material name="green"> <color rgba="0.0 0.5 0.0 1.0"/> <!-- 绿色 --> </material> </visual> <collision> <origin xyz="0.02 0 0.1" rpy="0 0 0"/> <geometry> <box size="0.04 0.02 0.001"/> </geometry> </collision> <inertial> <origin xyz="0.02 0 0.1" rpy="0 0 0"/> <mass value="0.005"/> <!-- 叶子重量约5g --> <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/> </inertial> </link> <!-- 连接苹果主体与柄 --> <joint name="stem_to_body" type="fixed"> <parent link="apple_body"/> <child link="apple_stem"/> <origin xyz="0 0 0.08" rpy="0 0 0"/> </joint> <!-- 连接柄与叶子 --> <joint name="leaf_to_stem" type="fixed"> <parent link="apple_stem"/> <child link="apple_leaf"/> <origin xyz="0.02 0 0.03" rpy="0 0 0"/> <!-- 叶子连接到柄末端 --> </joint> </robot>
最终的模拟启动代码如下（纯CPU的）：
- run-sim.py
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()