Runtime Error on simple Cartesian motion

[JKBehrens]

Hi @JeanElsner,

thanks for the great work here. I came across an edge case in the Cartesian motion generation. Executing the following code ...

T_0 = panda_py.fk(constants.JOINT_POSITION_START)
T_1 = T_0.copy()
T_1[0, 3] += 0.35
T_1[2, 3] -= 0.2
# T_1[1, 3] -= 0.2

panda.move_to_joint_position(constants.JOINT_POSITION_START)

panda.move_to_pose(T_1,
                   speed_factor=0.05,
                   stiffness=2 * np.array([600, 600, 600, 600, 250, 150, 50]))

produces the following error:

INFO:panda:Connected to robot (192.168.88.140).
INFO:panda:Panda class destructor invoked (192.168.88.140).
INFO:panda:Initializing motion generation (moveToJointPosition).
INFO:motion:Computed joint trajectory: 1 waypoint, duration 2.10 seconds.
INFO:panda:Starting new controller (Trajectory).
INFO:panda:Stopping active controller (Trajectory).
INFO:panda:Initializing motion generation (moveToPose).
INFO:motion:Computed Cartesian trajectory: 1 waypoint, duration 4.24 seconds.
INFO:panda:Starting new controller (Trajectory).
ERROR:motion:IK failed at time 3.11, prior to waypoint 1. Goal may be outside of workspace.
---------------------------------------------------------------------------
RuntimeError                              Traceback (most recent call last)
Cell In[77], line 10
      6 # T_1[1, 3] -= 0.2
      8 panda.move_to_joint_position(constants.JOINT_POSITION_START)
---> 10 panda.move_to_pose(T_1,
     11                    speed_factor=0.05,
     12                    stiffness=2 * np.array([600, 600, 600, 600, 250, 150, 50]))

RuntimeError: IK produced NaN.

When I shift the goal pose in y (see line 5 in the code above), the motion works but the first joint turns around very quickly. Let me know, if you need more data or how we can debug this.

Best,
Jan

[JeanElsner]

Hi Jan,

thanks for the report! The Cartesian motion generator creates a trajectory based on linear paths between the provided end-effector poses. If the IK fails, as suggested by the error message, there actually is no solution (the IK is analytical and looks for case-consistent solutions). Alternatively, you could try using joint motion generation by converting the Cartesian pose to joint positions using forward kinematics. Except for collisions (including self-collisions) there is always a path in joint state connecting two poses.

Maybe what you're looking for is motion or path planning that actually takes joint limits and collisions into account. This is not supported by panda-py directly yet, but I'm looking into solutions (either by integrating it directly in the C++ backend or building a Python model on top of panda-py).

To make sure this is not a bug, can you provide me with the initial pose you use to run the code? Is it the standard starting pose?

[JKBehrens]

Thanks for your response! It was indeed the standard starting pose. So you can execute the code after unlocking your panda.

The motion should totally be feasible. And the issue happens exactly when the robot is passing the singularity of joint 0 and joint 2 being aligned.

I made the script more precise for you.

from panda_py import constants
import panda_py
panda = panda_py.Panda(hostname)

print()
print("constants.JOINT_POSITION_START: ")
print(constants.JOINT_POSITION_START)
print()


T_0 = panda_py.fk(constants.JOINT_POSITION_START)
T_1 = T_0.copy()
T_1[0, 3] += 0.35
T_1[2, 3] -= 0.2
# T_1[1, 3] -= 0.2

panda.move_to_joint_position(constants.JOINT_POSITION_START)

try:
    panda.move_to_pose(T_1,
                   speed_factor=0.05,
                   stiffness=2 * np.array([600, 600, 600, 600, 250, 150, 50]))
except RuntimeError as e:
    print(e)
    

print()
print("JointState close to failure")
print(panda.q)

results in

INFO:panda:Connected to robot (192.168.88.xxx).
INFO:panda:Panda class destructor invoked (192.168.88.xxx).
INFO:panda:Stopping active controller (Trajectory).
INFO:panda:Initializing motion generation (moveToJointPosition).
INFO:motion:Computed joint trajectory: 1 waypoint, duration 2.09 seconds.
INFO:panda:Starting new controller (Trajectory).

constants.JOINT_POSITION_START: 
[ 0.         -0.78539816  0.         -2.35619449  0.          1.57079633
  0.78539816]

INFO:panda:Stopping active controller (Trajectory).
INFO:panda:Initializing motion generation (moveToPose).
INFO:motion:Computed Cartesian trajectory: 1 waypoint, duration 4.24 seconds.
INFO:panda:Starting new controller (Trajectory).
ERROR:motion:IK failed at time 3.10, prior to waypoint 1. Goal may be outside of workspace.
IK produced NaN.

JointState close to failure
[ 4.07380508e-02 -1.10497193e-03 -3.99322398e-02 -1.98261358e+00
 -3.44854521e-03  2.02047282e+00  7.80826435e-01]

You see that the first three joints are very close to zero.

[JKBehrens]

And here is a video: https://youtu.be/YmotsV75smE?si=sfE6ZL3pELoFK1YH

[JKBehrens]

And here is the result of ik_full on the critical joint state