观前提醒：本章主要内容是通过PyBullet仿真kuka机械臂，并控制机械臂末端按照预定轨迹运动
一、什么是运动学？
运动学是物理学的一个分支，专注于物体的运动描述，包括位移、速度、加速度等，而不考虑这些运动是如何产生的（即不考虑力和质量）。运动学可以应用在很多领域，包括机械工程、机器人学、生物力学等。
在机器人学中，运动学特别指的是机器人各个部件之间的运动关系。机器人的运动学可以分为两个主要的部分：
-
正运动学（Forward Kinematics, FK）：这部分处理从给定的关节角度（机器人的配置）到确定机器人手臂末端执行器（例如工具、爪子等）的位置和方向的转换。在正运动学中，你知道每个关节的角度，你要计算的是末端执行器的确切位置。这通常是通过机器人的几何参数来实现的，比如关节的长度和它们之间的固定角度。
-
逆运动学（Inverse Kinematics, IK）：这部分处理的是相反的问题，即确定使机器人的末端执行器达到特定位置和方向所需的关节角度。逆运动学在机器人编程中尤其重要，因为通常我们知道机器人的工具需要在哪个位置执行任务，但我们需要计算出为了达到这个位置，各个关节应该如何移动。逆运动学可能有多个解（即不同的关节配置可以导致末端执行器在相同的位置），也可能没有解（当期望位置超出机器人的工作范围时）。
运动学解析一般需要考虑到机器人的物理结构，例如关节的类型（旋转关节或滑动关节）、关节的排列（串联或并联）以及每个关节的限制等。在复杂的系统中，运动学分析可能需要运用到数学和计算工具，如矩阵代数、三角学和数值方法。
二、如何在PyBullet中实现正逆运动学
正运动学
在PyBullet中，正运动学的计算通常是隐式进行的。当你设置了机器人关节的状态后，PyBullet会自动计算出机器人末端执行器的位置和方向。例如：
resetJointState(bodyUniqueId, jointIndex, targetValue, targetVelocity)
resetJointState函数用于直接设置机器人的某个关节到一个特定的状态。这个函数可以在不运行物理引擎的情况下，直接改变关节的位置和速度。通常，这个函数被用于初始化仿真环境，或者在进行非物理学的运动学研究时设置机器人的起始状态。这个函数不会考虑动力学约束，比如碰撞或者关节之间的相互影响。
在执行上述代码后，PyBullet会根据给定的关节角度desiredJointAngle计算出机器人的每一部分的位置，包括末端执行器。
参数解释：
- bodyUniqueId：机器人或其他物体的唯一ID，这个ID是在创建或加载物体时由PyBullet返回的。
- jointIndex：要设置状态的关节索引。
- targetValue：关节目标位置（通常是角度，对于旋转关节；或者是直线位移，对于滑动关节）。
- targetVelocity：关节目标速度（这是可选参数）。
逆运动学
逆运动学通常比正运动学复杂，因为它可能有多个解，或者在某些情况下没有解。在PyBullet中，可以使用calculateInverseKinematics函数来求解机器人的逆运动学。这个函数通过末端执行器的期望位置和方向（如果给定）来计算每个关节应该达到的角度。并且通常使用数值方法来寻找解，所以说它可以处理复杂的机器人模型，包括那些解析解难以找到或不存在的情况。
jointPoses = p.calculateInverseKinematics(robotId, endEffectorIndex, targetPosition, targetOrientation)
- bodyUniqueId：机器人的唯一ID。
- endEffectorIndex：末端执行器的链条索引，这个索引标识了机器人的哪个部分是末端执行器。
- targetPosition：一个列表或元组，指定了末端执行器期望到达的目标位置的世界坐标（通常是[x, y, z]）。
- targetOrientation：一个列表或元组，指定了末端执行器期望到达的目标方向的四元数。这是可选参数，如果不提供，逆运动学解将不考虑末端执行器的方向。
- …other parameters…：PyBullet 的逆运动学函数还允许设置其他参数，比如关节的上下限、解的逼近度等，可以用来进一步控制逆运动学求解的过程。
calculateInverseKinematics返回的jointAngles是一个列表，包含了所有关节达到目标位置所需的角度值。
可是逆运动学问题会有多个解，这种现象称为逆运动学的多解性（kinematic redundancy），那么calculateInverseKinematics会返回所有的解吗？
答案是：不会！ calculateInverseKinematics函数只返回一个解，这个解是根据内部算法（通常是基于数值优化的方法）所决定的。
三、机械臂模型建立
pybullet预装了一些模型，其中就有一个机械臂模型：“kuka_iiwa/model.urdf”
我们可以使用pybullet_data.getDataPath() 来设置附加的搜索路径，这个路径指向 pybullet 预装的数据目录，目录里面包括了一些基本的URDF模型和环境。
四、仿真程序编写
import pybullet as p
import pybullet_data
import time
import math
# 设置仿真环境
physicsClient = p.connect(p.GUI) # 或者使用p.DIRECT
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # 设置路径以加载模型
p.setGravity(0, 0, -10)
# 加载平面和机械臂模型
planeId = p.loadURDF("plane.urdf")
robotId = p.loadURDF("kuka_iiwa/model.urdf", useFixedBase=1)
# 获取机械臂末端执行器的索引
endEffectorIndex = 6
# 获取机械臂的关节数量
numJoints = p.getNumJoints(robotId)
# 机械臂的初始位置
restingPosition = [0, 0, 0, 1.5708, 0, 1.0367, 0]
for jointNumber in range(numJoints):
p.resetJointState(robotId, jointNumber, restingPosition[jointNumber])
# 设置圆形的参数
circle_radius = 0.3
circle_center = [0, 0]
numPoints = 100
angles = [2 * math.pi * float(i) / numPoints for i in range(numPoints)]
try:
# 开始无限循环
while True:
for angle in angles:
x = circle_center[0] + circle_radius * math.cos(angle)
y = circle_center[1] + circle_radius * math.sin(angle)
z = 0.5
# 使用逆向运动学计算关节角度
jointPoses = p.calculateInverseKinematics(robotId, endEffectorIndex, [x, y, z])
# 移动机械臂
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=robotId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
# 每次移动后等待一小段时间，以便观察
p.stepSimulation()
time.sleep(0.01)
except KeyboardInterrupt:
# 用户中断程序时，退出循环
print("Circle drawing interrupted by user.")
# 断开连接
p.disconnect()
这个代码的效果控制机器臂末端执行器沿着一个圆形轨迹移动
运行程序，仿真效果如下：
下面我们对程序进行详尽的分析
设置仿真环境
physicsClient = p.connect(p.GUI) # 或者使用p.DIRECT
p.connect：这个函数用于连接到PyBullet的服务器。p.GUI作为参数表示以图形用户界面模式启动仿真，如果使用p.DIRECT则不会显示图形界面。
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # 设置路径以加载模型
p.setAdditionalSearchPath：这个函数用于设置PyBullet搜索URDF（统一机器人描述格式）文件的附加路径。
pybullet_data.getDataPath()：获取PyBullet自带数据的路径。
p.setGravity(0, 0, -10)
p.setGravity：设置仿真环境的重力。参数分别对应重力在x、y、z轴上的分量。这里设置为沿着z轴的负方向，大小为10 m/s²，符合地球上的重力加速度。
加载平面和机械臂模型
planeId = p.loadURDF("plane.urdf")
p.loadURDF：这个函数用于加载URDF格式的模型到仿真环境中。
“plane.urdf”：这是平面模型的URDF文件名。
robotId = p.loadURDF("kuka_iiwa/model.urdf", useFixedBase=1)
“kuka_iiwa/model.urdf”：这是KUKA iiwa机械臂的URDF文件名。
useFixedBase=1：这个参数表示机械臂被固定在世界坐标的原点，而不是自由浮动。
获取机械臂末端执行器的索引
endEffectorIndex = 6
这行代码指定了机械臂末端执行器的索引。对于KUKA iiwa，索引6通常指的是末端执行器。
获取机械臂的关节数量
numJoints = p.getNumJoints(robotId)
p.getNumJoints：这个函数返回指定机器人模型的关节数量。
机械臂的初始位置
restingPosition = [0, 0, 0, 1.5708, 0, 1.0367, 0]
for jointNumber in range(numJoints):
p.resetJointState(robotId, jointNumber, restingPosition[jointNumber])
restingPosition：定义了机械臂每个关节的初始位置。
p.resetJointState：这个函数用于设置特定关节的状态（位置和速度）。在这段代码中，它用来将每个关节设置到初始位置。
设置运动轨迹
circle_radius = 0.3
circle_center = [0, 0]
numPoints = 100
angles = [2 * math.pi * float(i) / numPoints for i in range(numPoints)]
设置了用于绘制圆的参数：circle_radius 是圆的半径；circle_center 是圆心的x和y坐标；numPoints 是圆周上点的数量；angles 是用于计算圆上各点坐标的角度的列表。
仿真循环
while True:
for angle in angles:
x = circle_center[0] + circle_radius * math.cos(angle)
y = circle_center[1] + circle_radius * math.sin(angle)
z = 0.5
jointPoses = p.calculateInverseKinematics(robotId, endEffectorIndex, [x, y, z])
这个循环让机械臂的末端执行器按照预设的圆形轨迹移动。
在每个角度，计算圆周上的点坐标 (x, y, z)。
p.calculateInverseKinematics：计算机械臂的逆运动学，以得到末端执行器达到目标位置时各关节的角度。参数包括机器人ID、末端执行器索引和目标位置。
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=robotId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
这个循环设置机械臂的每个关节，以使其移动到逆运动学解算出的位置。
p.setJointMotorControl2：这个函数用来控制机器人的关节。参数包括机器人ID、关节索引、控制模式（这里是位置控制）、目标位置、目标速度、力矩、位置增益和速度增益。
p.stepSimulation()
time.sleep(0.01)
p.stepSimulation()：向前推进物理仿真一步。
time.sleep(0.01)：在每步仿真后暂停0.01秒，让用户可以看到机械臂移动的效果。
异常处理和程序结束
except KeyboardInterrupt:
print("Circle drawing interrupted by user.")
这段代码捕获KeyboardInterrupt异常，当用户按下键盘中断时打印消息。
p.disconnect()
p.disconnect()：断开与仿真环境的连接，清理资源。
评论（0）
您还未登录，请登录后发表或查看评论