Inverse kinematics cartesian result inaccurate

Question

My try to simulate cartesian control with PyBullet with the help of GitHub examples gives an inaccurate result when checking via forward kinematics. It seems target orientation is neglected:

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

I reduced GitHub examples (no null space or dampening) but get same problems. I tried with another robot model from KUKA library. Nothing changed. What am I doing wrong?

Answer 1

The accuracy of the inverse kinematics calculation depends on the current configuration of the robot. The closer the two configurations are, the better the result will be. The PyBullet API exposes options to perform the IK iteratively and hence improve accuracy (see discussion on this GitHub issue). For example, you could try:

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

A higher number of iterations and lower residual threshold should get you closer to the target pose.

Alternatively, you can re-calculate the inverse kinematics after every step of the simulation, apply new motor controls, and it should get better as the robot approaches the target pose.

Unfortunately, I am missing the urdf files in your example so cannot test in your code. However, the orientation works for me as used in the command above.