search

usnistgov / ARIAC

Repository for ARIAC (Agile Robotics for Industrial Automation Competition), consisting of kit building and assembly in a simulated warehouse
https://pages.nist.gov/ARIAC_docs/en/latest/index.html
Other
110 stars 61 forks source link

Joint 'floor_gripper_joint' from the starting state is outside bounds #314

Closed dan9thsense closed 6 months ago

dan9thsense commented 6 months ago

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 commented 6 months ago

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 commented 6 months ago

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.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 commented 6 months ago

This is not uncommon:

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

jaybrecht commented 6 months ago

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 commented 6 months ago

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 commented 6 months ago

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 commented 6 months ago

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