Get joint values from pose with IK

[Yeesha-R]

Hi, Thank you for providing this module. I am new to python and robotics and was wondering if I could use this module to obtain joint angles from pose values. In the examples provided, I noticed that some joint angles were provided. In my case, I only have the position and orientation of the end-effector of my UR5e robot arm.

If yes, how can I modify the code to get this please?

I tried to modify the code in one of the examples as shown below but could not get any solution for the joint values

import numpy as np
import ur5e_ikfast
import tf.transformations as tf

#Initialize kinematics for UR5 robot arm
ur5e_kin = ur5e_ikfast.PyKinematics()
n_joints = ur5e_kin.getDOF()

#Provide quaternion (x, y, z, w) and position (x, y, z)
quaternion = [0.0, 0.0, 0.0, 1.0]  
position = [0.4, 0.2, 0.5]  

#Convert quaternion and position to a 3x4 transformation matrix
rotation_matrix = tf.quaternion_matrix(quaternion)[:3, :3]
translation_vector = np.array(position).reshape(3, 1)
transformation_matrix = np.hstack((rotation_matrix, translation_vector))

print("\nGenerated transformation matrix from quaternion and position:")
print(transformation_matrix)

#Flatten the transformation matrix for IK solver
ee_pose_flattened = transformation_matrix.reshape(-1).tolist()

#Test inverse kinematics: get joint angles from end effector pose
print("\nTesting inverse kinematics:\n")
joint_configs = ur5e_kin.inverse(ee_pose_flattened)
n_solutions = int(len(joint_configs) / n_joints)
print("%d solutions found:" % n_solutions)
joint_configs = np.asarray(joint_configs).reshape(n_solutions, n_joints)
for joint_config in joint_configs:
    print(joint_config)

#Check cycle-consistency of forward and inverse kinematics with one of the solutions
if n_solutions > 0:
    chosen_joint_angles = joint_configs[0].tolist()
    ee_pose_check = ur5e_kin.forward(chosen_joint_angles)
    ee_pose_check = np.asarray(ee_pose_check).reshape(3, 4)
    print("\nPose from the chosen joint angles:")
    print(ee_pose_check)
    assert np.allclose(transformation_matrix, ee_pose_check, atol=1e-4)
    print("\nTest passed!")
else:
    print("\nNo solutions found.")

[cambel]

Hi @Yeesha-R , Thank you for your interest in my repository.

Only having the pose of the end effector is enough to use this library, the joint angles that you may provide are only for reference so that the library can tell you which inverse kinematic solution is closest to the provided joint angles.

Now, your code is okay, the only problem is that the library won't find a valid solution for every arbitrary pose due to the reach of the arm or singularities. The one that you are testing has no solution, but if you modify that a bit, then it can find a solution

quaternion = [0.01, 0.0, 0.0, 1.0]  
position = [0.4, 0.2, 0.5] 

Then you will get a proper result

Also, I would recommend using the style presented in demo2. That way this library is easier to use. E.g.,

from ur_ikfast import ur_kinematics
import numpy as np

ur3e_arm = ur_kinematics.URKinematics('ur5e')

# Example of a current joint configuration of the robot
joint_angles = [-3.1, -1.6, 1.6, -1.6, -1.6, 0.]  # in radians

#Provide quaternion (x, y, z, w) and position (x, y, z)
quaternion = [0.01, 0.0, 0.0, 1.0]  
position = [0.4, 0.2, 0.5]  

print("inverse() from position + quaternion", ur3e_arm.inverse(np.concatenate([position, quaternion]), all_solutions=False))
print("inverse() from position + quaternion with initial guess", ur3e_arm.inverse(np.concatenate([position, quaternion]), all_solutions=False, q_guess=joint_angles))

[Yeesha-R]

Hi @cambel , Thank you so much for the guidance. I tried it and it works! However, I noticed that the base value is significantly different from the one I read using rostopic info /joint_states. Is there any reason for this?

Kindly note that I changed 'ur3e' in the code above to 'ur5e' cos my robot is ur5e.

Thanks.

[cambel]

Yes, it depends on the frame of reference for the pose that you input. The pose is expected to be that of the tool0 in the frame of reference base_frame. If you input poses of a different frame then it may find a solution but it will not be a valid one.

[Yeesha-R]

I used tool0 and base_frame for my calculations but still got incorrect values for the base joint. I found a calculation that gives me the correct base joint values, so I substitute that into the values I obtain from your code.

Thank you so much for your assistance.