Different orientation values ​​between the web app and the orientation calculated using the transformation matrices

[felipeadsm]

Description

Hello.

I'm trying to convert a joint value (j1, j2, j3...) to a transformation matrix, as indicated in the gen3 lite rev3 manual, and then convert this matrix into position and orientation values ​​(x, y, z, theta_x, theta_y, theta_z). At the end of the conversion, I have a -180 degree offset in the theta_y value provided by the web app.

The transformations I'm using as a base are these:

Do you have any suspicions about this?

Version

At a minimum, provide the Kortex API and Kortex-enabled device versions.

KortexAPI : 2.3.0

Kortex Device : Gen3 Lite

Code example

import numpy as np
from math import pi
from scipy.spatial.transform import Rotation as R

def gripper_to_base(theta_1: float, theta_2: float, theta_3: float, theta_4: float, theta_5: float, theta_6: float):
    theta_1 = np.radians(theta_1)  # [rad]
    theta_2 = np.radians(theta_2)  # [rad]
    theta_3 = np.radians(theta_3)  # [rad]
    theta_4 = np.radians(theta_4)  # [rad]
    theta_5 = np.radians(theta_5)  # [rad]
    theta_6 = np.radians(theta_6)  # [rad]
    theta_7 = -pi / 2  # [rad]

    m_b_1 = np.array([[np.cos(theta_1), -np.sin(theta_1),  0,         0],
                      [np.sin(theta_1),  np.cos(theta_1),  0,         0],
                      [0,                0,                1,         0.1283],
                      [0,                0,                0,         1]])

    m_1_2 = np.array([[np.cos(theta_2), -np.sin(theta_2),  0,         0],
                      [0,               0,                -1,        -0.0300],
                      [np.sin(theta_2), np.cos(theta_2),   0,         0.1150],
                      [0,               0,                 0,         1]])

    m_2_3 = np.array([[np.cos(theta_3),  -np.sin(theta_3), 0,         0],
                      [-np.sin(theta_3), -np.cos(theta_3), 0,         0.2800],
                      [0,                 0,              -1,         0],
                      [0,                 0,               0,         1]])

    m_3_4 = np.array([[np.cos(theta_4), -np.sin(theta_4),  0,         0],
                      [0,                0,               -1,        -0.1400],
                      [np.sin(theta_4),  np.cos(theta_4),  0,         0.0200],
                      [0,                0,                0,         1]])

    m_4_5 = np.array([[0,                0,                1,         0.0285],
                      [np.sin(theta_5),  np.cos(theta_5),  0,         0],
                      [-np.cos(theta_5), np.sin(theta_5),  0,         0.1050],
                      [0,                0,                0,         1]])

    m_5_6 = np.array([[0,                0,               -1,         -0.1050],
                      [np.sin(theta_6),  np.cos(theta_6),  0,          0],
                      [np.cos(theta_6), -np.sin(theta_6),  0,          0.0285],
                      [0,                0,                0,          1]])

    m_6_ee = np.array([[np.cos(theta_7), -np.sin(theta_7), 0,          0],
                       [np.sin(theta_7),  np.cos(theta_7), 0,          0],
                       [0,                0,               1,          0.1300],
                       [0,                0,               0,          1]])

    b_g_transformation_matrix = m_b_1 @ m_1_2 @ m_2_3 @ m_3_4 @ m_4_5 @ m_5_6 @ m_6_ee

    return b_g_transformation_matrix

# 279.593, 296.949, 50.406, 302.016, 27.767, 62.108 - Joints
# 0.0, -0.7, 0.247, -90.582, -178.79, 175.306 - Pose

transformation_matrix = gripper_to_base(279.593, 296.949, 50.406, 302.016, 27.767, 62.108)
rotation_matrix = transformation_matrix[:3, :3]
rotation_matrix_r = R.from_matrix(rotation_matrix)
orientation = rotation_matrix_r.as_euler('xyz', degrees=True)
position = transformation_matrix[:3, 3]

# The expected value for the orientation is: -90.582, -178.79, 175.306
# The returned value is: -89.41, 1.21, 175.305

Expected behavior

The expected behavior is that the conversion returns me a value equal to that of the web app.

Any other information

My work context is the transformation of points from a fiducial marker to the base of the robotic arm. I'm having difficulty in the final orientation of the point, having a considerable (almost constant) offset in the final values.

[martinleroux]

Hi @felipeadsm ,

I am not familiar with the scipy library. Looking at its documentation, it seems that the second angle will always be within [-90, 90].

Note that this is not the same as the standard used by the robot, which limits theta_Z instead of theta_Y to a 180 deg range.

I also notice that you have a value for theta_7 = -pi/2 which I don't know the signification of, since the robot only has 6 DoF.

Finally, another thing worth looking into is switching your argument for as_euler from 'xyz' to 'zyx' - I have seen enough inconsistencies in libraries over the years between intrinsic and extrinsic notations that I consider it always worth checking.

[felipeadsm]

Hi @martinleroux, thanks for the quick response.

I also noticed this in the documentation. Do you have any suggestions for a library to work with Euler angles?

I am not familiar with the scipy library. Looking at its documentation, it seems that the second angle will always be within [-90, 90].

I think this angle was used to compose the m_6_ee transformation (as) described in the code. In the documentation I don't have the 6Tee matrix with sines and cosines, like in the other transformations. In the documentation, on page 133, it says that the angles used to represent the matrix are all equal to zero.

I also notice that you have a value for theta_7 = -pi/2 which I don't know the signification of, since the robot only has 6 DoF.

Does this transformation make sense to you? What form of matrix should I use?

I changed this argument during my tests because I saw in the documentation, on page 77, that the orientations are defined using an z-y-x Tait-Bryan extrinsic convention. The values ​​reversed the order and the offset of -180 degrees remained.

Finally, another thing worth looking into is switching your argument for as_euler from 'xyz' to 'zyx' - I have seen enough inconsistencies in libraries over the years between intrinsic and extrinsic notations that I consider it always worth checking.

Note that the calculated values ​​were quite close, with only this offset of -180°, which may be due to the scipy conversion.

In issue #101 it is explained in a somewhat confusing way what is used as the default, so I think 'xyz' is correct.

For a fixed referential (Extrinsic), rotations are applied on the x,y,z axes of the base in this order : X then Y then Z. For a mobile referential (Intrinsic), rotations are applied on the x,y,z axes of the mobile frame in this order : Z then Y' then X''

So what do I need to do to get the same result as the web app using this approach?

[hermattt]

Hi @felipeadsm,

Following: https://easyspin.org/documentation/eulerangles.html "Equivalent sets of Euler angles" it is shown that there are a lot of equivalent Euler angles. The best solution for me was calculating orientation in quaternions (harder to understand, but easier to compare).

[felipeadsm]

Hello @hermattt

I looked into Euler angles and also reviewed issue #101 several times and I'm not sure what pattern Kinova uses in Gen3 Lite. I can't seem to get the same values ​​returned by the web app and the Python API.

Hello @martinleroux

I redid my calculations, using a different value for theta_7. I tried 0 and I tried pi/2 and I didn't get any consistent results.

I'm using some specific joint values ​​for testing besides (0, 0, 0, 0, 0, 0) and the orientation results don't make sense. I changed the library, reversed the order of the extrinsic angles, but nothing worked or was minimally consistent. Below is the code used to test using the homogeneous transformation matrices, as indicated in the manual.

import numpy as np
from pytransform3d import rotations as pr

from hand_eye_test.my_hand_eye.utils import gripper_to_base

T_griper2base = gripper_to_base(262.64, 314.61, 131.18, 87.38, 272.51, 271.4)

Translation_griper2base = T_griper2base[:3, 3]

Rotation_griper2base = T_griper2base[:3, :3]
Orientation_griper2base_rad = pr.extrinsic_euler_xyz_from_active_matrix(Rotation_griper2base)
Orientation_griper2base = np.degrees(Orientation_griper2base_rad)

print(f'Real Pose: 0.0 -0.45 0.199 -90.9 -178.7 175.3')

print(f'Pose: {Translation_griper2base[0].round(2)} {Translation_griper2base[1].round(2)} {Translation_griper2base[2].round(2)} '
      f'{Orientation_griper2base[0].round(2)} {Orientation_griper2base[1].round(2)} {Orientation_griper2base[2].round(2)}')

Since the values ​​kept getting wrong in the orientation, I decided to plot all the transformations starting from the homogeneous transformation matrices in the manual. Note the rotations and note my result.

The code used to generate these results is this:

import numpy as np
from math import pi

import matplotlib.pyplot as plt

from pytransform3d.transformations import concat
from pytransform3d.transform_manager import TransformManager

joint_values = 0, 0, 0, 0, 0, 0  # Rad

base_2_J1 = np.array([[np.cos(joint_values[0]), -np.sin(joint_values[0]), 0, 0],
                      [np.sin(joint_values[0]), np.cos(joint_values[0]), 0, 0],
                      [0, 0, 1, 0.1283],
                      [0, 0, 0, 1]])

j1_2_j2 = np.array([[np.cos(joint_values[1]), -np.sin(joint_values[1]), 0, 0],
                    [0, 0, -1, -0.0300],
                    [np.sin(joint_values[1]), np.cos(joint_values[1]), 0, 0.1150],
                    [0, 0, 0, 1]])

j2_2_j3 = np.array([[np.cos(joint_values[2]), -np.sin(joint_values[2]), 0, 0],
                    [-np.sin(joint_values[2]), -np.cos(joint_values[2]), 0, 0.2800],
                    [0, 0, -1, 0],
                    [0, 0, 0, 1]])

j3_2_j4 = np.array([[np.cos(joint_values[3]), -np.sin(joint_values[3]), 0, 0],
                    [0, 0, -1, -0.1400],
                    [np.sin(joint_values[3]), np.cos(joint_values[3]), 0, 0.0200],
                    [0, 0, 0, 1]])

j4_2_j5 = np.array([[0, 0, 1, 0.0285],
                    [np.sin(joint_values[4]), np.cos(joint_values[4]), 0, 0],
                    [-np.cos(joint_values[4]), np.sin(joint_values[4]), 0, 0.1050],
                    [0, 0, 0, 1]])

j5_2_j6 = np.array([[0, 0, -1, -0.1050],
                    [np.sin(joint_values[5]), np.cos(joint_values[5]), 0, 0],
                    [np.cos(joint_values[5]), -np.sin(joint_values[5]), 0, 0.0285],
                    [0, 0, 0, 1]])

a = 0
j6_2_ee = np.array([[np.cos(a), -np.sin(a), 0, 0],
                    [np.sin(a), np.cos(a), 0, 0],
                    [0, 0, 1, 0.1300],
                    [0, 0, 0, 1]])

base_to_j2 = concat(base_2_J1, j1_2_j2)
base_to_j3 = concat(base_to_j2, j2_2_j3)
base_to_j4 = concat(base_to_j3, j3_2_j4)
base_to_j5 = concat(base_to_j4, j4_2_j5)
base_to_j6 = concat(base_to_j5, j5_2_j6)
base_to_ee = concat(base_to_j6, j6_2_ee)

tm = TransformManager()
tm.add_transform("J1", "base", base_2_J1)
tm.add_transform("J2", "J1", j1_2_j2)
tm.add_transform("J3", "J2", j2_2_j3)
tm.add_transform("J4", "J3", j3_2_j4)
tm.add_transform("J5", "J4", j4_2_j5)
tm.add_transform("J6", "J5", j5_2_j6)
tm.add_transform("EE", "J6", j6_2_ee)

ee2object = tm.get_transform("EE", "base")

ax = tm.plot_frames_in("base", s=0.1)
ax.set_xlim((-0.25, 0.75))
ax.set_ylim((-0.5, 0.5))
ax.set_zlim((0.0, 1.0))
plt.show()

The results obtained were these:

The results obtained are different from those presented in the documentation, especially the last transformation.

The results only match when I use the last angle as a pi/2.

Using the DH parameters, I got a result that is also not consistent. The example code:

import numpy as np
from pytransform3d import rotations as pr

def calculate_classical_dh(alpha_i, a_i, d_i, theta_i):
    classical_dh = np.array([
        [np.cos(theta_i), -np.cos(alpha_i) * np.sin(theta_i), np.sin(alpha_i) * np.sin(theta_i), a_i * np.cos(theta_i)],
        [np.sin(theta_i), np.cos(alpha_i) * np.cos(theta_i), -np.sin(alpha_i) * np.cos(theta_i), a_i * np.sin(theta_i)],
        [0, np.sin(alpha_i), np.cos(alpha_i), d_i],
        [0, 0, 0, 1]
    ])

    return classical_dh

joint_values = [262.651, 314.606, 131.178, 87.424, 272.503, 271.421]

j1 = np.radians(joint_values[0])
j2 = np.radians(joint_values[1])
j3 = np.radians(joint_values[2])
j4 = np.radians(joint_values[3])
j5 = np.radians(joint_values[4])
j6 = np.radians(joint_values[5])

# q1 = j1, q2 = j2, q3 = j3, q4 = j4, q5 = j5, q6 = j6
actuator_1 = calculate_classical_dh(np.pi / 2, 0, 0.1283 + 0.1150, j1)
actuator_2 = calculate_classical_dh(np.pi, 0.2800, 0.0300, j2 + np.pi / 2)
actuator_3 = calculate_classical_dh(np.pi / 2, 0, 0.0200, j3 + np.pi / 2)
actuator_4 = calculate_classical_dh(np.pi / 2, 0, 0.1400 + 0.1050, j4 + np.pi / 2)
actuator_5 = calculate_classical_dh(np.pi / 2, 0, 0.0285 + 0.0285, j5 + np.pi)
actuator_6 = calculate_classical_dh(0, 0, 0.1050 + 0.1300, j6 + np.pi / 2)

T_griper2base = actuator_1 @ actuator_2 @ actuator_3 @ actuator_4 @ actuator_5 @ actuator_6

Translation_griper2base = T_griper2base[:3, 3]

Rotation_griper2base = T_griper2base[:3, :3]
Orientation_griper2base_rad = pr.extrinsic_euler_xyz_from_active_matrix(Rotation_griper2base)
Orientation_griper2base = np.degrees(Orientation_griper2base_rad)

print(f'Real Pose: 0.0 -0.45 0.199 -90.9 -178.7 175.3')

print(f'Esti Pose: {Translation_griper2base[0].round(2)} {Translation_griper2base[1].round(2)} '
      f'{Translation_griper2base[2].round(2)} {Orientation_griper2base[0].round(2)} '
      f'{Orientation_griper2base[1].round(2)} {Orientation_griper2base[2].round(2)}')

Could you check if the transformation matrices are correct?

Once and for all, please help me understand which convention I should follow regarding extrinsic angles.

I would also like to understand how I can get the same results from the web app and the API.

[hermattt]

Expected Euler values -> [-90.582, -178.79, 175.306]

Using code from initial comment.

Using theta_7 = 0 (expected angle looking at gen3 lite manual) orientation -> [ 64.33302954, 88.65738856, -30.36790407] Using quaternions there is 89.9999 deg difference between orientation and expected values.

Using theta_7 = pi/2 (expected 90 deg from previous calculations) Orientation -> [89.41837538, -1.21011098, -4.69479126] Using quaternions there is 8.7566e-04 deg difference between orientation and expected values. So, [89.41837538, -1.21011098, -4.69479126] and [-90.582, -178.79, 175.306] are equivalent Euler angles.

rotation_matrix_r.as_quat() ->[ 0.70257175, -0.03631149, -0.02168408, 0.71035499] (R.from_euler("xyz",[-90.582, -178.79, 175.306],degrees=True)).as_quat() -> [ 0.70256971, -0.03630588, -0.02167998, 0.71035742]

rotation_matrix_r.as_euler('ZYX', degrees=True) # intrinsic should the same as rotation_matrix_r.as_euler('xyz', degrees=True) # extrinsic

I don't know if documentation is wrong or code inside Kinova, but using pi/2 seem to resolve your problem.

[martinleroux]

Hello @felipeadsm ,

@hermattt above is correct. I validated directly in the source code and the matrices are in the xyz/ZYX order.

The pi/2 thing is my mistake, it is used to reorient the Y axis properly. It does belong there.

The results above do confirm that our documentation is correct.

[felipeadsm]

Hi @hermattt,

Use pi/2 solve my problem. Thanks for the explanation about equivalent Euler angles. I managed to make the values ​​match.

Hello @martinleroux ,

Regarding the transformation matrices, the documentation seems to be correct.

However, regarding the DH parameters, I was unable to obtain the same results.

Do you have any suggestions or tips regarding this?

Thank you both for your responses.

[martinleroux]

@felipeadsm Apologies for the delayed response, this issue got lost in my emails when I returned from vacation.

Since the transformation matrices are correct, I will be closing this issue. Regarding the DH parameters, this is most likely a math implementation error. Since this particular topic is outside of the scope of the repo, if you need further assistance with this please contact us a support@kinova.ca