Weird behavior of the IK solver

✅ Closed

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?

3 months ago

Activity

Yahav

Seems like this is fixed now.

1 6 days ago Reply

Yahav changed status to ✅ Closed

6 days ago

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 2 months 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 2 months ago Reply

Alban Laflaquiere

@Lars Berscheid
I just tried your suggested solution today, but unfortunately it didn't solve the issue.
More precisely, I set reference_config=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] for each CartesianWaypoint. I think the exact value doesn't matter much in this context, as we only care about making the IK solution deterministic.
Still, even after this change, I got to same false positive collision detection, and still in a stochastic fashion.
So something else is happening here.

0 1 month ago Reply

Alban Laflaquiere

@Lars Berscheid:
Hi Lars,
Do you any new feedback on this issue? We're currently stuck as we never know when the planner will fail or not (and replanning doesn't seem to solve the problem -- it seems the planner tends to bug repeateadly after a first false detection).

0 1 month ago Reply

Lars Berscheid

Sorry for my late reply!

Yeah, I'm also unsure why your script fails on your, but works fine on my side. I just tested it again with > 200 plannings without any problems... I don't think this is because of numerical issues, but honestly I don't have a better answer.

The reference config of [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] is probably not ideal, and might result in the same behavior as above. If the numerical inverse kinematics starting from the reference config fails, we will again reset to a random value, leading to the same stochastic process. That's fairly probable with the all-zeros config, as it might be far away (and is not even a valid config for the Franka robot, as the maximum limit is -0.07 in joint 4). I'll add a warning message in the next releave of the motion library to spot this behavior.

For debugging, could you calculate the IK manually (e.g. using robot.inverse_kinematics) and use these values as the reference position? For the Cartesian waypoints in your script below, these are the IK solutions that I've calculated and should be collision free:

[-0.3274771303922277, 0.3786149725851499, -0.5190916229922914, -2.321379727095345, 2.2633439695559487, 1.7737626399172535, -1.1309722860266402]
[0.06557335751639988, -0.7479832308250878, 0.16553520059137034, -2.3234145666909938, 0.11222719256029653, 1.5826990810699688, 0.9699804952955885]
[-0.05824691871062488, -0.7849908992273619, 0.037656799166794534, -2.3546417772547996, 0.02662631071135914, 1.5704956102517105, 0.7522515344517674]

Does the error still happen then?

Which version of the motion library are you running (e.g. python3 -c "import jacobi; print(jacobi.__version__)")?

0 1 month 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 months 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 months ago Reply

Yahav changed status to 💬 Open

3 months 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 3 months 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 3 months 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 3 months 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 3 months ago Reply

Weird behavior of the IK solver

Activity

One vote