想做一个机械臂仿真软件，以前利用C++ 结合FCL和OSG实现了一个半成品，但是这种方案需要根据机械臂DH参数，解析出机械臂各关节位姿然后利用OSG渲染，并且需要自己实现逆解算法，过于麻烦。而使用pybullet，上述得大部分功能都有现成的API接口。
仿真软件得几个要素：
（1），渲染引擎 ，需要将模型渲染可视化出来。VTK,OSG或者原生OpenGL。
（2），动力学引擎，根据物体的物理属性计算运动、旋转和碰撞。Bullet或者ODE，或者只用于碰撞检测的FCL。
（3），模型描述，负责机器人模型的建模，描述各机器人的连杆和关节，还有质量属性，颜色等。比如常用的URDF文件和SDF文件。
pybullet 具备了上述所有的要素，动力学引擎使用bullet，渲染使用OpenGL，并可以直接加载URDF文件，而且提供了 正逆算法。
实现逻辑：
1，利用URDF文件，加载模型。
2，给出目标，也就是抓取点位置。
3，利用逆解算法，求出各关节角度。
4，设置各关节角度，并判判断否碰撞。
5，理想情况下，将此组角度过滤，选择下一组不碰撞的数据 发送真实的给机械臂。
代码:
import pybullet as p
import pybullet_data
import time
import math
from collections import namedtuple
import numpy as np
# 启动仿真引擎的GUI
p.connect(p.GUI)
# 设置重力加速度
p.setGravity(0, 0, -9.81)
# 加载URDF模型路径
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# 设置视角
p.resetDebugVisualizerCamera(cameraDistance=3,
cameraYaw=120,
cameraPitch=-45,
cameraTargetPosition=[0,0,0])
# 加载平面模型作为地面
planeId = p.loadURDF("plane.urdf")
#托盘
trayId=p.loadURDF("tray/traybox.urdf",basePosition=[0.6,0,0.63])
#桌子
tableId=p.loadURDF("table/table.urdf",basePosition=[0.5,0,0])
# 加载ur5 机械臂
robotId = p.loadURDF("D:\\keiler\\sim\\urdfs\\ur5.urdf",useFixedBase=True ) #basePosition=[0.0,0,0.62]
initial_position = [0, 0, 0.62] # 设置初始位置为 [0, 0, 0.62] 米
initial_orientation = p.getQuaternionFromEuler([0, 0, 0]) # 设置初始方向为欧拉角 [0, 0, 0] 对应的四元数
p.resetBasePositionAndOrientation(robotId, initial_position, initial_orientation)
# 获取机械臂末端执行器的索引
endEffectorIndex =6
# 获取机械臂的关节数量
numJoints = p.getNumJoints(robotId)
print("关节数量:"+str(numJoints))
# 打印每个关节的信息
for joint_index in range(numJoints):
joint_info = p.getJointInfo(robotId, joint_index)
print(f"Joint {joint_index}: {joint_info}")
# 机械臂的初始位置
restingPosition = [0,3.14, -1.57, 1.57, 1.57, 1.57, -1.57, 0]
for jointNumber in range(numJoints):
p.resetJointState(robotId, jointNumber, restingPosition[jointNumber])
# 设置圆形的参数
circle_radius = 3
circle_center = [0, 0]
numPoints = 50
angles = [2 * math.pi * float(i) / numPoints for i in range(numPoints)]
#调参控件
target_x = p.addUserDebugParameter("Target X", -10, 10, 0)
target_y = p.addUserDebugParameter("Target Y", -10, 10, 0)
target_z = p.addUserDebugParameter("Target Z", -10, 10, 0)
#开始按钮
button_start = p.addUserDebugParameter("Start", 1, 0, 1)
# 初始状态变量
button_state_prev = 0
#画个球
sphere_visual = p.createVisualShape(shapeType=p.GEOM_SPHERE, radius=0.03, rgbaColor=[1, 0, 0, 1])
sphere_id = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=-1, baseVisualShapeIndex=sphere_visual, basePosition=[0, 0, 1])
#多组解
def calculate_ik_multiple_solutions(robot_id, end_effector_index, target_position, target_orientation, num_solutions=5):
solutions = []
for i in range(num_solutions):
# 生成一个随机的初始关节状态
random_joint_positions = [np.random.uniform(-np.pi, np.pi) for _ in range(numJoints)]
# 计算逆运动学
ik_solution = p.calculateInverseKinematics(robot_id, end_effector_index, target_position, target_orientation, jointDamping=[0.01]*numJoints, lowerLimits=[-np.pi]*numJoints, upperLimits=[np.pi]*numJoints, jointRanges=[2*np.pi]*numJoints, restPoses=random_joint_positions)
solutions.append(ik_solution)
return solutions
try:
while True:
x = p.readUserDebugParameter(target_x)
y = p.readUserDebugParameter(target_y)
z = p.readUserDebugParameter(target_z)
# 更新球的位置
p.resetBasePositionAndOrientation(sphere_id, [x, y, z], [0, 0, 0, 1])
#判断按钮状态
button_state = p.readUserDebugParameter(button_start)
if button_state != button_state_prev:
print("Button pressed")
# 使用逆向运动学计算关节角度
jointPoses = p.calculateInverseKinematics(robotId, endEffectorIndex, [x, y, z],[0, 0, 0, 1])
# 移动机械臂
p.setJointMotorControl2(bodyIndex=robotId,jointIndex=1,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[0],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
p.setJointMotorControl2(bodyIndex=robotId,jointIndex=2,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[1],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
p.setJointMotorControl2(bodyIndex=robotId,jointIndex=3,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[2],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
p.setJointMotorControl2(bodyIndex=robotId,jointIndex=4,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[3],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
p.setJointMotorControl2(bodyIndex=robotId,jointIndex=5,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[4],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
p.setJointMotorControl2(bodyIndex=robotId,jointIndex=6,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[5],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
button_state_prev = button_state
contact_points = p.getContactPoints(bodyA=trayId,bodyB=robotId)
isColl = bool(contact_points)
if isColl:
print("撞上了！")
p.stepSimulation()
time.sleep(0.01)
except KeyboardInterrupt:
# 用户中断程序时，退出循环
print("Circle drawing interrupted by user.")
# 断开与仿真引擎的连接
p.disconnect()问题：
1，逆解 只能返回一组关节角度，没法根据碰撞，去规划路径。
2，返回的各关节角度 貌似有偏差。
3，界面二次开发有点困难，控件太少，想封装成一个产品需要结合其它界面框架，比如Qt，有个想法是用pybullet当服务器，PyQt+VTK当客户端，因为这套东西可以无UI运行。
效果：
源码：