PyBullet笛卡尔逆运动学结果不准确，目标姿态被忽略

我参考PyBullet官方示例实现笛卡尔控制仿真，但正运动学验证结果不准确，目标姿态似乎被完全忽略。我简化了示例代码（去掉零空间、阻尼相关逻辑），更换KUKA系列其他机器人模型后问题依旧，以下是我的代码：

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())

请问我哪里操作有误？

内容的提问来源于stack exchange，提问作者FinnSter