逆运动学笛卡尔结果不准确

Question

我尝试在 GitHub 示例的帮助下使用 PyBullet 模拟笛卡尔控制，但在通过正向运动学检查时给出了不准确的结果。看来目标导向被忽视了:

import pybullet
import time
import pybullet_data
import numpy as np

x = [400,400,300,300,300,400,400,300,300,300]
y = [100,200,200,100,100,100,200,200,100,100]
z = [800,1500,1500,1500,1600,1500,1500,1500,1500,1600]
a = [0,0,0,0,0,0,0,0,0,0]
b = [0,0,0,0,0,0,0,0,0,0]
c = [0,0,0,0,0,0,0,0,0,0]
SimulateCartesianPositions(x,y,z,a,b,c)


def SimulateCartesianPositions(x=None, y=None, z=None, a=None, b=None, c=None, frame=None):
    cartesianPositions = PointArray = np.column_stack([x, y, z, a, b, c])
    cartesianPositions = np.array(cartesianPositions) / 1000
    physicsClient = pybullet.connect(pybullet.GUI)
    pybullet.resetSimulation()
    pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
    planeID = pybullet.loadURDF("kuka_experimental/plane/plane.urdf")
    robot = pybullet.loadURDF("kuka_experimental/kuka_kr10_support/urdf/kr10r1420.urdf", [0, 0, 0],useFixedBase=1)
    pybullet.resetBasePositionAndOrientation(robot, [0, 0, 0], [0, 0, 0, 1])
    pybullet.setGravity(0, 0, -9.81)
    rp = [0, 0, 0, 0, 0, 1]

    for i in range(6):
        pybullet.resetJointState(robot, i, rp[i])

    endeffectorID = 6
    axisPositions = np.empty([np.shape(cartesianPositions)[0],6])

    for i in range(np.shape(cartesianPositions)[0]):
        axisPositions[i, :] = pybullet.calculateInverseKinematics(robot,
                                                endeffectorID,
                                                targetPosition=cartesianPositions[i,0:3],
                                                targetOrientation=pybullet.getQuaternionFromEuler(cartesianPositions[i,3:6])
                                                )
        pybullet.setJointMotorControlArray(
            robot, range(6), pybullet.POSITION_CONTROL,
            targetPositions=axisPositions[i, :])
        StartStepSimulation(robot, axisPositions[i, :])
        world_position, world_orientation = pybullet.getLinkState(robot, 2)[:2]
        print('Position:', world_position)
        print('Orientation:', world_orientation)


def StartStepSimulation(robot, targetPositions):
    currentangles = np.array([[j[0] for j in pybullet.getJointStates(robot, range(6))]])
    while np.all(np.round(currentangles, decimals=4) != np.round(targetPositions, decimals=4)):
        pybullet.stepSimulation()
        time.sleep(1. / 240.)
        currentangles = np.array([[j[0] for j in pybullet.getJointStates(robot, range(6))]])
    if not pybullet.getContactPoints():
        print('MSG: No Collision.')
    else:
        print('ERR: Collision detected!')
        print('ERR: Contact Points:', pybullet.getContactPoints())

我减少了 GitHub 示例（没有零空间或阻尼），但我遇到了同样的问题。我尝试使用 KUKA 库中的另一个机器人模型。没有改变。我做错了什么？

Answer 1

逆运动学计算的精度取决于机器人当前的配置。两种配置越接近，结果就越好。 PyBullet API公开了迭代执行 IK 的选项，从而提高了准确性（请参阅关于 此 GitHub 问题的讨论）。例如，您可以尝试：

target_joint_positions = bullet_client.calculateInverseKinematics(
    robot.body_id, robot.end_effector_link_id, position, orientation,
    maxNumIterations=100, residualThreshold=0.001)

更高的迭代次数和更低的残差阈值应该会让你更接近目标姿势。

或者，您可以在模拟的每一步之后重新计算逆运动学，应用新的电机控制，随着机器人接近目标位姿，它应该会变得更好。

不幸的是，我缺少您示例中的 urdf 文件，因此无法在您的代码中进行测试。但是，如上面命令中使用的那样，方向对我有用。