`inverse()` returns `None` for a pose generated by `forward()` unless `q0` is close to the original solution

[littelittle]

Hi, thanks for open-sourcing frankik — I ran into a behavior I wanted to clarify.

I can reproduce the following in a minimal example:

import numpy as np
from frankik import FrankaKinematics, RobotType

kinematics = FrankaKinematics(robot_type=RobotType.PANDA)

rest = np.array([0.0, -0.6, 0.0, -2.4, 0.0, 1.9, 0.8], dtype=float)

pose2 = kinematics.forward(
    rest,
    tcp_offset=kinematics.FrankaHandTCPOffset,
)

q2 = kinematics.inverse(
    pose2,
    tcp_offset=kinematics.FrankaHandTCPOffset,
    q0=rest,
)

print(q2)
# array([ 0.00000000e+00, -6.00000000e-01, -2.94505133e-18, -2.40000000e+00,
#         6.21006696e-17,  1.90000000e+00,  8.00000000e-01])

ik_wrapped = np.array([
    0.1230488, -0.33629552, 0.40057753,
   -2.7136159, -2.47291624, 2.26029545, 0.46672463
], dtype=float)

pose_new = kinematics.forward(
    ik_wrapped,
    tcp_offset=kinematics.FrankaHandTCPOffset,
)

print(pose_new)
# array([[-1.67734154e-05, -1.52364659e-05,  1.00000000e+00,
#          4.68353530e-01],
#        [ 1.39756208e-06,  9.99698000e-01,  1.52410922e-05,
#          1.84005480e-01],
#        [-9.99698000e-01,  1.39781772e-06, -1.67784612e-05,
#          3.46557673e-01],
#        [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
#          1.00000000e+00]])

q_test = kinematics.inverse(
    pose_new,
    tcp_offset=kinematics.FrankaHandTCPOffset,
    q0=rest,
)

print(q_test)
# None

q_test = kinematics.inverse(
    pose_new,
    tcp_offset=kinematics.FrankaHandTCPOffset,
    q0=ik_wrapped,
)

print(q_test)
# array([ 0.1230488 , -0.33629552,  0.40057753, -2.7136159 , -2.47291624,
#         2.26029545,  0.46672463])

So for a pose that is directly generated by forward(ik_wrapped, ...), inverse() returns None when q0=rest, but succeeds when q0=ik_wrapped.

Is this expected behavior?

More specifically: is q0 intended to strongly select a branch / local solution family, so that inverse() may return None even for a pose produced by forward() if q0 is on a different branch? Or would this be considered a bug / limitation?

Thanks!