Forward Kinematics Issue

[AlfaKeNTAvR]

Description

While working on my Forward Kinematics (FK) solution for the Kinova Gen3 7DoF robot using screw-axis theory, I used the default FK service call and the base_feedback topic (toolpose(xyz)) provided by the kortex_driver as a ground truth.

I noticed a misalignment between my solution and the default one. At the home position, with all joint angles at 0, the default FK outputs _tool_pose_x: -0.007, tool_pose_y: -0.0134, tool_posez: 1.308. The _tool_posex and _tool_posey values showed a significant difference from my solution, so I checked the tf_tree and the transitions between frames but couldn't find any transition that could lead to the solution produced by the default FK.

I implemented the FK solution provided in the KINOVA® Gen3 Ultra lightweight robot User Guide on Page 157 (transformation matrices), which gave me a solution similar to the default FK solution, but without the Robotiq gripper transition. After carefully examining each transformation matrix and comparing them to the drawing on Page 158, I noticed that in the Frame 3 to 4 matrix, the sign for the Y component in the translation vector (0.0064) was inverted: it was -0.0064 but should have been positive (0.0064) according to the drawing. After correcting the sign and running the code again, my FK solution matched the tf transition from the _baselink to the _braceletlink.

I also reviewed the Robotiq 2F-85 gripper manual and am puzzled about the origin of the tool_pose_x value. I assume that it should be 0, not -0.007, suggesting there might be an error in this aspect as well. However, since the transformation matrix for this part is not provided, I cannot confirm my suspicion.

Version

ROS distribution : Noetic

Branch and commit you are using :

noetic-devel 373cacd4

Steps to reproduce

Code example (if necessary)

!/usr/bin/env python

import rospy
import tf
import numpy as np

# Initialize the ROS node
rospy.init_node('my_tf_listener')

# Create a TF listener object
listener = tf.TransformListener()

space_frame = 'base_link'
ee_frame = 'bracelet_link'

try:
    # Wait for the TF between /base_link and /shoulder to become available
    listener.waitForTransform(
        space_frame,
        ee_frame,
        rospy.Time(0),
        rospy.Duration(4.0),
    )

    # Once available, lookup the transform
    (trans, rot) = listener.lookupTransform(
        space_frame,
        ee_frame,
        rospy.Time(0),
    )

    # Print the translation component of the transform
    print(f'\n{ee_frame} -> {space_frame}: { np.array(trans).round(3)}\n')

except (
    tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException
) as e:
    print(e)

#   /my_gen3/base_feeback topic:
#   commanded_tool_pose_x: -0.007018944248557091
#   commanded_tool_pose_y: -0.013408052735030651
#   commanded_tool_pose_z: 1.3085328340530396#

import numpy as np
from numpy import (
    sin as s,
    cos as c,
)

def kinova_forward_kinematics(joint_angles):
    """

    """

    q = joint_angles
    T = np.zeros([4, 4, 7])

    T[:, :, 0] = np.array(
        [
            [c(q[0]), -s(q[0]), 0, 0],
            [-s(q[0]), -c(q[0]), 0, 0],
            [0, 0, -1, 0.1564],
            [0, 0, 0, 1],
        ]
    )

    T[:, :, 1] = np.array(
        [
            [c(q[1]), -s(q[1]), 0, 0],
            [0, 0, -1, 0.0054],
            [s(q[1]), c(q[1]), 0, -0.1284],
            [0, 0, 0, 1],
        ]
    )

    T[:, :, 2] = np.array(
        [
            [c(q[2]), -s(q[2]), 0, 0],
            [0, 0, 1, -0.2104],
            [-s(q[2]), -c(q[2]), 0, -0.0064],
            [0, 0, 0, 1],
        ]
    )

    # Mismatch between the drawing and the FK solution.
    T[:, :, 3] = np.array(
        [
            [c(q[3]), -s(q[3]), 0, 0],
            # In FK is -0.0064, should be 0.0064 based on the drawing.
            [0, 0, -1, 0.0064],
            [s(q[3]), c(q[3]), 0, -0.2104],
            [0, 0, 0, 1],
        ]
    )

    T[:, :, 4] = np.array(
        [
            [c(q[4]), -s(q[4]), 0, 0],
            [0, 0, 1, -0.2084],
            [-s(q[4]), -c(q[4]), 0, -0.0064],
            [0, 0, 0, 1],
        ]
    )

    T[:, :, 5] = np.array(
        [
            [c(q[5]), -s(q[5]), 0, 0],
            [0, 0, -1, 0],
            [s(q[5]), c(q[5]), 0, -0.1059],
            [0, 0, 0, 1],
        ]
    )
    T[:, :, 6] = np.array(
        [
            [c(q[6]), -s(q[6]), 0, 0],
            [0, 0, 1, -0.1059],
            [-s(q[6]), -c(q[6]), 0, 0],
            [0, 0, 0, 1],
        ]
    )

    forward_kinematics = T[:, :, 0]
    print(f'\nB to 1: {forward_kinematics[:, 3][0:3]}')

    for i in range(1, len(joint_angles)):
        forward_kinematics = forward_kinematics @ T[:, :, i]
        print(f'B to {i+1}: {forward_kinematics[:, 3][0:3]}')

    return forward_kinematics

joint_angles = np.array([0, 0, 0, 0, 0, 0, 0])

print(
    f'\nForward kinematics: \n{kinova_forward_kinematics(joint_angles).round(3)}'
)

Expected behavior

From what I have experienced and examined based on the drawing and the tf_tree, the FK for the home position translation vector (base_link to bracelet_link) should be x: 0, y: -0.025, z: 1.126.

Any other information

Any other information you believe the maintainers need to know.

[martinleroux]

Hi AlfaKeNTAvR,

Thank you for notifying us. You are correct, there is indeed a sign error in the transformation matrix in our documentation. This has actually been already noted and will be fixed in the User Guide for the next release.

I would also add that trying to match the output of your own kinematics calculation to the values output by a physical robot will never work perfectly. All robot units are calibrated to identify any minor defect they may have due to tolerances on parts or assembly problems. The reality of things is such that these will never be zero, so in order to have accurate and repeatable robot, we have to apply corrections that are unique to each arm. If you are interested in getting the calibration parameters for your robot to obtain more accurate results, please reach out to us at support@kinova.ca and provide us with the serial number of your arm - usually printed on a sticker at the base.

[AlfaKeNTAvR]

Hey Martin,

Thank you for the reply! I just tested the FK output (commanded_tool_pose) from the latest version from the noetic-devel (Release v2.5.2) and it has the same calculation error, so I wanted to clarify that it's not just a documentation issue, but the code contains the same problem.

I agree, will reach out, thank you!

Best wishes, Nikita

[martinleroux]

Hi @AlfaKeNTAvR ,

The code running the kinematics in the robot loads its model from the exact same URDF file you can find here. As you can see, the sign is positive (which does contradict the documentation, but as I said, it is a known issue with the doc). You can also be assured that our robots pass calibration, accuracy and repeatability tests to avoid this kind of mistake. Although based on the information you provided I can't tell you what is wrong with your test, I can guarantee there is no such issue in the firmware of the robot.

Cheers