差别

这里会显示出您选择的修订版和当前版本之间的差别。

到此差别页面的链接

后一修订版		前一修订版
机器人:仿真:pybullet:pybullet常用代码:pybullet-启动arx5的双臂模拟 [2025/10/22 05:56] – 创建 ctbots机器人:仿真:pybullet:pybullet常用代码:pybullet-启动arx5的双臂模拟 [2025/10/22 06:04] (当前版本) – ctbots
行 1: 行 1:
 ====== 单个python文件实现pybullet在arx5上的双臂仿真 ====== ====== 单个python文件实现pybullet在arx5上的双臂仿真 ======
 +
 +首先看效果图：
 +
 +{{:机器人:仿真:pybullet:pybullet常用代码:arx5.png?800|}}
 +
  
 安装python环境 安装python环境
行 31: 行 36:
 URDF文件参考 URDF文件参考
   - arx5.URDF文件：https://github.com/real-stanford/arx5-sdk/blob/main/models/X5.urdf   - arx5.URDF文件：https://github.com/real-stanford/arx5-sdk/blob/main/models/X5.urdf
-  - apple.URFDF 文件是用AI生成的：+  - apple.URDF 文件是用AI生成的：
  
-<code apple.urdf text>+<code xml apple.urdf>
 <?xml version="1.0"?> <?xml version="1.0"?>
 <robot name="apple"> <robot name="apple">
行 123: 行 128:
 </robot> </robot>
 </code> </code>
 +
 +最终的模拟启动代码如下（纯CPU的）：
 +
 +<code python 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()
 +</code>
 +