Kinematics example outdated

[Pascal-So]

First of all, thank you for keeping the library alive through this fork!

I noticed that the kinematics example uses a vector() method on the Affine type:

q_new = Kinematics.inverse(x_new.vector(), q, null_space)

But when I try to run this example, I get the following error:

AttributeError: 'franky._franky.Affine' object has no attribute 'vector'

It appears that the reason for this is that the Affine type is now a type alias to Eigen and thus no longer uses the affx library where this method is defined.

Should it still be possible to run this example, am I doing something wrong?

Note that I'm using franky-panda 0.4.4 because I'm currently using a Panda instead of an FR3, but it seems to me that the situation for the latest version is the same.

[TimSchneider42]

Hi Pascal,

thanks for pointing this out! It seems like Kinematics.inverse is expecting a concatenated vector of translation and Euler angles. You could generate this vector yourself from x_new.translation and x_new.quaternion:

from scipy.spatial.transform import Rotation
import numpy as np
def affine_to_vector(affine):
    euler_angles = Rotation.from_quat(affine.quaternion).as_euler("ZYX")
    angles_equal = np.array([euler_angles[0] - np.pi, np.pi - euler_angles[1], euler_angles[2] - np.pi])
    if angles_equal[1] > np.pi:
        angles_equal[1] -= 2 * np.pi
    if angles_equal[2] > np.pi:
        angles_equal[2] += 2 * np.pi
    if np.linalg.norm(euler_angles) >= np.linalg.norm(angles_equal):
        euler_angles = angles_equal
    return np.concatenate([affine.translation, euler_angles])

However, looking at the implementation of Kinematics.inverse, I am not sure whether the Euler angles are actually handled correctly. It seems like Kinematics.inverse is directly optimizing a squared distance of the Euler angles to the target Euler angles, which is usually not a good idea since two different vectors of Euler angles can describe the same rotation. Hence, I am not sure whether this function is guaranteed to converge to a good solution.

I currently cannot test this solution properly, could you tell me whether it works?

Best, Tim

[Pascal-So]

Hi Tim, thank you for looking into it.

I think you're right, the output does not look ideal. Here's one example where I tried to compute the joint angles for a new pose that is only slightly offset from the current pose:

current_q = robot.current_joint_positions
# current_q: [ 0.03030206 -1.00635211 -0.01769905 -2.74210735  0.03016681  1.80323964  -0.09011917]

current_pose = robot.current_pose
# current_pose: (ee_pose=(t=[0.264455 0.00570296 0.489286], q=[0.997822 0.0521772 0.0344239 -0.0210753]), elbow=0.0176954)

new_pose = Affine([0.261053, -0.0181754, 0.48689], [0.999125, -0.0371177, -0.0187841, 0.00441501])

null_space = NullSpaceHandling(2, 0)
new_q = Kinematics.inverse(affine_to_vector(new_pose), current_q, null_space)
# new_q: [ 14.08153308  -9.94684578  -3.53028546  -4.1281186  -12.38642597  15.64145943   2.55128499]
# wrapping this to the [-pi +pi] range: [1.5151624656408273, 2.619524834359172, 2.7528998471795862, 2.1550667071795866, 0.179944644359173, 3.075088815640827, 2.55128499]

new_pose_fk = Kinematics.forward(new_q)
# new_pose_fk: (t=[0 0 0], q=[0.672725 -0.251197 -0.42058 0.554485])

For this example, the robot does not have an end-effector attached and the EE frame is equal to the flange frame. I assume that's what the computations in this library assume.

Joint angles of over 14 radian seem odd to me, and we can also see that at least either the inverse or the forward computation is broken since new_pose_fk does not match new_pose at all.

My usecase through which I even discovered the frankx and franky libraries was that I wanted to check the reachability of certain poses without having to go through the pain of setting up the entirety of ROS and going through moveit or similar tools. The rest of my code is in C++ and runs directly via libfranka, which doesn't check for reachability before starting a movement, and just stops when it encounters a limit. That's not a huge issue for my usecase, but the alternative would be a bit prettier. Either way, from my side it's a nice-to-have feature but I completely understand if you don't want to maintain a big bunch of custom FK and IK equations in the library, especially if it's not clear if the method even converges.

[TimSchneider42]

Hi Pascal,

that confirms my suspicion that the IK implementation is not working properly. If I find some time, I will take a closer look, but I cannot promise anything. Until then, it is probably best to use an IK solver like pykin.

Best, Tim