CheckStartStateBounds does not support REVOLUTE and NOT Continuous type joints

[chama1176]

trafficstars

Description

CheckStartStateBounds does not support REVOLUTE and NOT Continuous type joints

Your environment

• ROS Distro: Jazzy
• OS Version: e.g. Ubuntu 24.04
• Binary build 2.10.0

Steps to reproduce

• Set the initial posture of the actual robot to a position beyond the urdf limit.
• Press “plan” button in rviz to plan the trajectory.

Expected behaviour

CheckStartStateBounds is not SUCCESS. And fixed to joint limit in case fix_start_state is true.

Actual behaviour

CheckStartStateBounds is SUCCESS.

Details

In https://github.com/moveit/moveit2/pull/2429, CheckStartStateBounds is refactored. The logic has also been changed so that the check is skipped for non-infinite rotation joints, which are used in most arm joints.

https://github.com/moveit/moveit2/blob/a7fe0df4c565db15710581bdcba1d33d70aa0ddb/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp#L105-L120

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[mikeferguson]

Looking at the code - yup, this is definitely an issue - the switch statement logic will basically never hit the default case

[sea-bass]

Does this just involve moving the break; in that block up one indentation level?

[mikeferguson]

The original logic was:

• for each joint model:
  • if REVOLUTE & continuous, call enforceBounds() [OK]
  • else
    • if PLANAR - normalizeRotation [OK]
    • else FLOATING - normalizeRotation [OK]
• read parameters
• for each joint model:
  • apply the satisfiesBounds logic

I think we actually just need to pull the "default" case outside the switch statement and do it for all joints?