Gradient of `PoseError` becomes INF

[JYChen18]

If it’s not a bug, please use discussions: https://github.com/NVlabs/curobo/discussions

Please provide the below information in addition to your issue:

1. cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]): python
2. python version: 3.8.13
3. Isaac Sim version (if using): No

---

Issue Details

The gradient of PoseError (in src/curobo/curobolib/geom.py) becomes INF if the current_position is far from goal_position during optimization, e.g. when the L2 norm of current_position - goal_position is close to 2. The reason might be an overflow caused by the large loss (or the gradient).

I know this situation is rare, but it happens when I use a large robot, i.e. UR10e. Once it happens, the optimization will fail without any warning or error.

[balakumar-s]

1. Do you see this during motion_gen or IK?
2. Can you provide a start joint configuration and goal pose for the UR10 where this would be true so that we can add a unit test?

Yes, I agree this can happen. We can provide a web page on how to tune the weights for a robot if that's helpful.

[JYChen18]

Thanks for your reply.

In my situation, I used the function plan_batch in motion_gen for a shadow hand mounted on a UR10e. The success rate was often lower than 80% even if no world_model was used. I tried to debug and found this gradient issue.

To reproduce the INF issue in the gradient of PoseError on UR10e, a possible start joint configuration and goal pose can be [0.5787, 0.0443, -1.4419, -0.1732, -1.5708, 2.1495] and Pose.from_list([ -1., 0., 0., 0., 1., 0., 0.], respectively.

However, when I use UR10e without the end-effector, I find that motion_gen can always plan successfully, although there are still INFs in the gradient. Therefore, the failure of my robot may not be caused by this gradient issue, and the true reason can be somewhere else, e.g. the energy weights, since the shadow hand has a lot of joints.

Yes, I would appreciate it if you could provide a web page on how to tune the weights, especially for a robot with too many joints.

[balakumar-s]

For shadow hand, do you have multiple end-effector links for pose targets?

[JYChen18]

Yes, I do have.

After further debugging, I found that the goal pose of the shadow hand had some self-collision, which led to the failure. The default weight now works well after I fixed the self-collision problems in my goal pose.

Anyway, this is out of the scope of this issue. I will close this issue since this gradient issue does not affect the success rate of motion generation.

Thanks again for your great work!