Joint 'floor_gripper_joint' from the starting state is outside bounds

[dan9thsense]

I'm posting this as a separate issue because, even if we can fix the JointTrajectory start state issue, this one remains. Once the floor_gripper_joint gets out of bounds, there seems to be no way to move the arm.

[overall.py-1] [WARN] [1710643042.126686186] [moveit_ros.fix_start_state_bounds]: Joint 'floor_gripper_joint' from the starting state is outside bounds by a significant margin: [-0.246719 ] should be in the range [-0.001 ], [0.001 ] but the error above the 'start_state_max_bounds_error' parameter (currently set to 0.050000)
[overall.py-1] [WARN] [1710643042.128532484] [moveit_kinematic_constraints.kinematic_constraints]: Joint floor_gripper_joint is constrained to be below the minimum bounds. Assuming minimum bounds instead.
[overall.py-1] [WARN] [1710643042.128888579] [moveit_kinematic_constraints.kinematic_constraints]: Joint floor_gripper_joint is constrained to be below the minimum bounds. Assuming minimum bounds instead.
[overall.py-1] [WARN] [1710643042.129526483] [ompl]: ./src/ompl/base/src/Planner.cpp:257 - floor_robot/floor_robot: Skipping invalid start state (invalid bounds)

[dan9thsense]

To be fair, you can get around it with a pose move, but that is such a kludge, there has to be a better way.

[dan9thsense]

Argggh, not true, pose moves can fail in this case too. Cannot find a consistent way to recover the arm after that error.

[overall.py-1] [INFO] [1710649432.255785962] [my_move_it]: pose move starting
[overall.py-1] [WARN] [1710649432.257176716] [moveit_ros.fix_start_state_bounds]: Joint 'floor_gripper_joint' from the starting state is outside bounds by a significant margin: [0.302303 ] should be in the range [-0.001 ], [0.001 ] but the error above the 'start_state_max_bounds_error' parameter (currently set to 0.050000)
[overall.py-1] [WARN] [1710649432.258910960] [ompl]: ./src/ompl/base/src/Planner.cpp:257 - floor_robot/floor_robot: Skipping invalid start state (invalid bounds)
[overall.py-1] [ERROR] [1710649432.258990912] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207 - floor_robot/floor_robot: Motion planning start tree could not be initialized!
[overall.py-1] [ERROR] [1710649432.625685975] [my_move_it]: Planning failed in move.plan_and_execute
[overall.py-1] [ERROR] [1710649432.630336468] [my_move_it]: pose move failed

[dan9thsense]

This is not uncommon:

floor_gripper joint is out of bounds (-0.001, 0.001). value is: 0.0010009682970331113

[jaybrecht]

Does this issue happen when the end effector is disturbed due to a contact? The floor gripper joint is a dummy joint. However it cannot be listed as a fixed joint to the fact that gazebo ignores joints that take place after a fixed joint in a chain. It should be an easy fix to increase the bounds of the joint in the URDF but technically it is not actuated so it shouldn't ever move. I will put together a PR to fix this issue this week but it would be nice to know if you know of a way to test that the fix is sufficient.

[jaybrecht]

I opened the above pull request with what should be a fix. I was actually able to fix the issue with it being a fixed joint with an additional tag in the urdf. @dan9thsense are you able to check out this PR locally and see if it fixes your issue?

[dan9thsense]

I'm out of the country at the moment with limited computing resources, but I can check next week when I return. Thanks for addressing the issue.

[dan9thsense]

Tested it this evening and it works great. Thanks for fixing this one.