Weird behavior of the IK solver

πŸ’¬ Open
Alban Laflaquiere

Hi,

I have questions about the IK solution generated by the planner when providing a CartesianWaypoint goal, as I experience some inconsistencies.

1) Is the IK solution deterministic?
Running the same script multiple times with the same setup and the same eef goal sometimes results in valid IK solutions and valid plans, and sometimes results in jacobi complaining that the goal pose is in (self?) collision; like for instance:

[jacobi.exception.planner]
        Goal pose at [0.05952, -0.407, -0.2488, -1.928, 0.2708, 1.068, 0.7126] is in collision.

2) Are the attached gripper collision shape + the safety margin properly taken into account when generating the IK solution?
Having visualized the joint state deemed in collision in Studio, it doesn't appear that any obstacle is close enough to the robot to trigger any collision (see attached image). My best guess is that the primitive shape I attached as gripper is (self)-colliding with another link of the robot.
I cannot confirm this hypothesis because I cannot visualize collision shapes in Studio, unfortunately. However, if correct, this would mean that the gripper collision shape and/or its safety margin is not properly handled when generating the IK solution.

Do you have any insight into what is happening?

1 month ago

Activity
Lars Berscheid

Thanks for the script! While running the script works fine for me even for >500 tries (I have no idea why...), I think I have more insight now about what probably happens.

Description

The Franka robot has 7 degrees of freedom, and this means that we use (as Franka itself) a numerical approach for the inverse kinematics. There are also infinite solutions for the inverse kinematics in general, and the calculation will find a solution in the null-space of the robot stochastically. To find a collision-free inverse kinematics solution, we therefore repeatedly try to calculate a solution until we find a collision-free one, and stop after a maximum number of attempts.

Fix

  • In the next release, we'll increase the number of attempts to calculate a valid inverse kinematics. This should significantly reduce the occurrence of this issue.
  • As an immediate, and probably better fix, you could add a reference_configuration to the CartesianWaypoint. Then, we will use this joint position as the starting point for the inverse kinematics, and this makes the process deterministic and faster.

0    1 week ago    Reply

Alban Laflaquiere

Thanks for the feedback.
I'll try the fix you suggest as soon as I find the time.

I wonder if this is the full story though. Because if I "manually" move the robot to the configuration the planner complains about (for instance [-0.7466, 0.3352, -0.09242, -2.307, 2.192, 1.881, -1.202] in my previous example), it doesn't seem to be in collision.
So, apparently the IK solver finds a solution that is indeed collision-free, but something else later complains about an unknown collision.

I'm also surprised you didn't get any issue on your machine. On my side, it's currently very improbable that I can get through a whole benchmark (~15 motins) without any error.
Could the numerical behavior of the library depend on the hardware?

0    1 week ago    Reply

Lars Berscheid

Do you have a small script to reproduce this issue? I'm trying to reproduce this based on your description, but without luck so far.

0    2 weeks ago    Reply

Alban Laflaquiere

Hi Lars,

Here is a minimum script in which I was able to reproduce the error.
I picked start and goal configurations that failed in my own benchmark, hence the weird values.

import jacobi

jacobi_robot = jacobi.robots.FrankaPanda()
jacobi_robot.set_base(jacobi.Frame.from_quaternion(*[-0.44, 0.0, 0.4, 1.0, 0.0, 0.0, 0.0]))

gripper_shape = jacobi.Box(*[0.12, 0.15, 0.27])
gripper_origin = jacobi.Frame.from_quaternion(*[-0.005, 0.0, 0.13, 1.0, 0.0, 0.0, 0.0])
jacobi_robot.end_effector_obstacle = jacobi.Obstacle(gripper_shape, gripper_origin)

jacobi_robot.flange_to_tcp = jacobi.Frame.from_quaternion(*[0.0484, 0.0, 0.25275, 0.70711, -0.0, 0.0, -0.70711])
jacobi_robot.name = "my_panda"

scene = jacobi.Environment(robot=jacobi_robot, safety_margin=0.02)

table = jacobi.Box(1.5, 1.5, 0.8)
table_origin = jacobi.Frame.from_quaternion(*[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
scene.add_obstacle("table", object=table, origin=table_origin)

wall = jacobi.Box(0.5, 0.1, 0.4)
wall_origin = jacobi.Frame.from_quaternion(*[0.16, 0.0, 0.48, 1.0, 0.0, 0.0, 0.0])
scene.add_obstacle("wall", object=wall, origin=wall_origin)

ceiling = jacobi.Box(0.6, 0.6, 0.05)
ceiling_origin = jacobi.Frame.from_quaternion(*[0.23, -0.52, 0.95, 1.0, 0.0, 0.0, 0.0])
scene.add_obstacle("ceiling", object=ceiling, origin=ceiling_origin)

planner = jacobi.Planner(scene, delta_time=0.002)

studio = jacobi.Studio()
studio.reset()

table_s = jacobi.Obstacle(table, table_origin, "66C866")
studio.add_obstacle(table_s)
wall_s = jacobi.Obstacle(wall, wall_origin, "66C866")
studio.add_obstacle(wall_s)
ceiling_s = jacobi.Obstacle(ceiling, ceiling_origin, "66C866")
studio.add_obstacle(ceiling_s)

for k in range(100):

    print(k)

    ## Custom motion 1
    custom_start = jacobi.Waypoint(
        position=[
            0.4756129373849231,
            0.6626189531304539,
            -0.0910173716447656,
            -1.330902773863534,
            0.047343616924051334,
            1.953964180876374,
            -1.1934853259515075,
        ]
    )
    custom_goal = jacobi.CartesianWaypoint(
        position=jacobi.Frame.from_quaternion(
            0.21549999999999997,
            -0.39092,
            0.62049,
            0.0,
            0.7150275827210154,
            0.036801419642144576,
            0.6981269307657917,
        )
    )
    desired_motion = jacobi.Motion("my_motion", jacobi_robot, start=custom_start, goal=custom_goal)
    trajectory_jacobi = planner.plan(desired_motion)

    ## Custom motion 2
    custom_start = jacobi.Waypoint(
        position=[
            0.00023669647774717697,
            -0.6849015790946424,
            -0.00013218181745670515,
            -2.0264241040254856,
            -8.863886091960174e-05,
            1.3420122592007742,
            0.7839475242930395,
        ]
    )
    custom_goal = jacobi.CartesianWaypoint(
        position=jacobi.Frame.from_quaternion(
            -0.09835409462741207,
            0.06582933816320591,
            0.7378914453132221,
            9.524733086889479e-05,
            0.9235779698166707,
            0.3834105808448634,
            0.00022603864171043131,
        )
    )
    desired_motion = jacobi.Motion("my_motion", jacobi_robot, start=custom_start, goal=custom_goal)
    trajectory_jacobi = planner.plan(desired_motion)

    ## Custom motion 3
    custom_start = jacobi.Waypoint(
        position=[
            0.00023676828377959167,
            -0.6848711944802702,
            -0.0001322219388653608,
            -2.0263240981332857,
            -8.866597497352599e-05,
            1.3419427752689022,
            0.7839472049265633,
        ]
    )
    custom_goal = jacobi.CartesianWaypoint(
        position=jacobi.Frame.from_quaternion(
            -0.09835409462741207,
            -0.0341706618367941,
            0.7378914453132221,
            9.524733086889479e-05,
            0.9235779698166707,
            0.3834105808448634,
            0.00022603864171043131,
        )
    )
    desired_motion = jacobi.Motion("my_motion", jacobi_robot, start=custom_start, goal=custom_goal)
    trajectory_jacobi = planner.plan(desired_motion)

The script repeatedly loops over three motions, as the failure seems to be stochastic. In my own runs, I had to wait at most 40 iterations for an error to occur.

The error message is of the type:

Exception has occurred: JacobiError

[jacobi.exception.planner]
    Goal pose at [-0.7466, 0.3352, -0.09242, -2.307, 2.192, 1.881, -1.202] is in collision.
  File "/home/ros/noetic/src/project/temp_custom_script_for_jacobi.py", line 67, in <module>
    trajectory_jacobi = planner.plan(desired_motion)
jacobi.JacobiError: 
[jacobi.exception.planner]
    Goal pose at [-0.7466, 0.3352, -0.09242, -2.307, 2.192, 1.881, -1.202] is in collision.

Note: The script assumes Studio is open and active (for visualization of the obstacles). If you run the script multiple times in a raw, it also illustrates a bug mentioned in another report: studio.reset() does not remove all obstacles from the scene in studio.

1    2 weeks ago    Reply

Yahav changed status to πŸ’¬ Open

1 month ago

Alban Laflaquiere

After more tests with a UR5 instead of a Panda, I can confirm that I experience a weird behavior of the planner.

I now have a benchmark consisting in a set of motions to perform in an empty scene (i.e. no other obstacles than the robot itself). Then:

  • Not loading any gripper collision mesh is fine, and all motions complete successfully.
  • Loading a box gripper collision shape that doesn't appear in collision (similar to the previously attached image) however leads to inconsistent results. Sometimes the planner successfully plans a motion as expected. But most of the times, it complains that the goal pose is in collision, when it actually doesn't appear to be when I manually set the robot to the joint configuration the planner complains about. Setting a safety margin (my first guess) of 0 does not appear to change anything.

(Additional piece of information: All the poses the planner complain about seem to have a wrist_3_link for the UR5 below -pi. It could be a pure coincidence, or maybe revealing an issue with the IK solver and joint limits. I have now seen a configuration estimated in collision while still having wrist_3_link higher than -pi; so it's probably not related.)

I'm really stuck on this bug, so I would really appreciate if you could guide me to a solution.

0    1 month ago    Reply

Alban Laflaquiere

Hi Lars,

Thank you forn the feedback.

I realized my message might have been confusing.
I actually never call robot.inverse_kinematics() myself. Instead I think I do something similar to what you described:

start = jacobi.Waypoint(position=<current joint state>)
goal = jacobi.CartesianWaypoint(position=jacobi.Frame.from_quaternion(*<my target eef pose>)
desired_motion = jacobi.Motion("my_motion", robot, start=start, goal=goal)
traj = planner.plan(desired_motion)

The problem I described above thus happens when using the planner directly.

0    1 month ago    Reply

Lars Berscheid

Hi Alban,

to answer your questions:

(1) For a robot with more than 6 degrees of freedom like the Franka, the IK solution is not deterministic in general. The numerical optimization sometimes fails to find a solution, and then we restart the optimization from a random joint position. This brings randomness into the IK.

To reduce / remove this randomness, you can pass a reference_configuration to the IK method. For the Franka, this is the joint position that is used for starting the numerical optimization. In general, the IK will find a solution closest to the reference configuration.

(2) The collision model (both for environment <-> robot and self collisions) is not used in the public robot.inverse_kinematics(...) methods. Using our planner, you usually have a robot, an environment, and a planner object that have a hierarchy: The robot object does not know about the environment or the planner, and the environment does not know about the planner.
However, if you plan complete motions, e.g. from a Cartesian waypoint to another Cartesian waypoint, the planner will find an IK solution that is collision free. This only works if you pass the Cartesian waypoint directly to the plan method.

We’ll think about a way to calculate the IK considering the environment’s collision model without planning a motion.

Thank you!
Lars

0    1 month ago    Reply

Alban Laflaquiere

As additional datapoint:

I ran some tests with targets for which the planner often complains the goal is in collision. Except this time I didn't load any collision mesh for the gripper.

The result was that the planner never complained about the goal being in collision anymore.
This seems to confirm that 1) the collision that is detected is between the mounted gripper shape and something, and 2) the IK solver sometimes returns solutions for which the gripper collision shape is actually in collision.

1    1 month ago    Reply

One vote