Can not plan simple motions after applying calibration

[firesurfer]

Feature summary

Hi,

we just finished the mechanical setup of the second version of our machine. Today a read the kinematics calibration from the new arms and applied them to the URDF.
Interestingly especially the PILZ planner (linear in the provided example) is not able the plan motions that it were able to plan before applying the calibration.

I disabled all machine specific parts in RVIZ for the screenshot but I guess the point becomes clear. We want to perform a linear cartesian motion from start_pose to end_pose.

PILZ reports either unable to find IK Solution or the typical Joint velocity limit of ur_top/elbow_joint violated. Set the velocity scaling factor lower! Actual joint velocity is -33.3537, while the limit is 3.14159.

With the default kinematics applied the planning succeeds.

The kinematics read from the arm:

kinematics:
  shoulder:
    x: 0
    y: 0
    z: 0.18056132253822044
    roll: -0
    pitch: 0
    yaw: 2.2439482151408174e-07
  upper_arm:
    x: 0.0001730271946047779
    y: 0
    z: 0
    roll: 1.5703136403087588
    pitch: 0
    yaw: 4.741412193447898e-06
  forearm:
    x: -0.4783046168746562
    y: 0
    z: 0
    roll: 3.1415006505292848
    pitch: 3.1413102504401502
    yaw: 3.1415922567654904
  wrist_1:
    x: -0.36002174741464615
    y: -0.00048803098180374852
    z: 0.17432934076906578
    roll: 0.0027994696976741183
    pitch: -4.9009044730039759e-05
    yaw: -2.9541451840223513e-06
  wrist_2:
    x: 1.8493640688227288e-05
    y: -0.11970324846035348
    z: 0.00019042956250675861
    roll: 1.5692054810633871
    pitch: 0
    yaw: -2.794478333677762e-07
  wrist_3:
    x: 3.9335569007590755e-05
    y: 0.11558991871393763
    z: 0.00010931162624213268
    roll: 1.5717420112877298
    pitch: 3.1415926535897931
    yaw: -3.1415922617259731
  hash: ....

In our setup we have two arms. Whats interesting is that the behavior occurs on both of them. (In our old machine setup we also have two arms and both of them work fine). So this looks somehow systematic.

[firesurfer]

@RobertWilbrandt and @fmauch sorry for already pushing. But this is a real show stopper and I have not clue what to do about. If there is additional information needed I am happy to provide that.

[firesurfer]

It seems to be especially related to a certain configuration of the arm:

In this configuration neither the Pilz linear planner nor Ompl with a cartesian target is able to plan the motion.

[RobertWilbrandt]

Hey, sorry for not getting back to you on this sooner. I'm currently trying to reproduce this. Could you provide some more information on the problem? In particular:

• Just to make sure: We are talking about a ur16e?
• Are you working on humble / iron / rolling? I'm not aware of any recent changes that could be related, but this might help with reproducing the error conditions.
• Could you give me specific joint positions where this error occurs? I just tried using the default and your configuration in poses similar to the second image you showed, but that still worked fine.
• Can you verify that this is not related to your defined collision meshes? That would be an easy explanation for being able to use the cartesian controllers but not being able to plan.
• Can you provide moveit output from the planning, for both the pilz and the ompl case?

I hope that i'm able to reproduce this locally

[firesurfer]

Hey,

• Yes I tried it with the calibration files of 4 different UR16e
• Iron - the UR packages are pulled in from source. I am currently on my own fork an until felix merges the scaled cartesian controller PRs ;)

Joint positions:
[0.2, -0.8, 1.6, 0.0, 0.0, 0.1] - Plan to a cartesian pose ["ur/tool0", xyz=(-0.1,0.1,0.0), rpy=(0.0,0.0,0.0)] Endeffector also "ur/tool0". Pilz linear planer reports:

[winder_moveit_node-14] [ERROR] [1710410153.520596082] [moveit.pilz_industrial_motion_planner.trajectory_functions]: Joint acceleration limit of ur_top/elbow_joint violated. Set the acceleration scaling factor lower! Actual joint acceleration is 25.3806, while the limit is 5. 
[winder_moveit_node-14] [ERROR] [1710410153.520642549] [moveit.pilz_industrial_motion_planner.trajectory_functions]: Inverse kinematics solution at 1.6s violates the joint velocity/acceleration/deceleration limits.
[winder_moveit_node-14] [ERROR] [1710410153.520694916] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Failed to generate valid joint trajectory from the Cartesian path

In this specific case the OMPL Cartesian planner actually manages to plan a trajectory but it is not straight.

Regarding the collision meshes. Please see my email as we do not want to show screenshots of the additional tools online.
Also note: with the default calibration it plans just fine. Also for many, many other motions it also just works fine. It was just by random that I implemented an integration test for our framework which uses this combination.

[firesurfer]

Just as a follow up what I already tried:

1. Use the calibration files of the 4 different UR16e instead of the default calibration file
2. Try the same planning problem with our old machine setup (where we actually manage to implement the whole process without noticing) and with the new machine setup
3. Regenerate ACM after applying the calibration
4. Recheck the generated ACM / srdf files manually as the automatic sampling might not detect certain constellations of adjacent links
5. Recheck the meshes manually in blender (check if there are any vertices "flying around") - Recheck the meshes in our CAD software - meshes do have a few surfaces that are not closed (we exported some PCB parts with the CAD model...)
6. For testing purposes I set the acceleration limits to a very high value. Then the Linear planner actually plans a trajectory but for like 1 position in between there is a huge jump. Afterwards it returns into the "normal" configuration
7. Try different motions. See images in the first and second post. For the motion in the first post the cartesian planner moves in a slight curve.

[RobertWilbrandt]

As a follow-up on our offline discussions: Our current assumption is that the robot with the correction appplied is too close to a singularity (4 axes in parallel) and the planners cannot correctly handle this (even though i'm not quite sure why a configuration-space planner like the ompl-based ones should have problems with this). I did not yet get to reproducing this, but at this point we mainly want to exclude that there is some problem with some collision geometry that plays into the observed problems.

[firesurfer]

@RobertWilbrandt I am pretty sure that the assumption is correct after doing more tests. It really only happens if the two wrist joints are in parallel.

We solved this by turning the component we want to interact with by 90°...

What still puzzles me a bit is that this only happens after applying the calibration from the robot.