CheckStartStateBounds missing parameter to specifiy how much the joint can be outside the limits?

[jsupratman13]

Description

I have a hardware where a prismatic joint fails on the CheckStartStateBounds even though the joint is practically within the limits. The issue occurs due to sensor reading limitations, causing the reported position to slightly dip below the lower joint limit by a negligible amount (e.g. less than 0.001 mm)

https://moveit.picknik.ai/main/doc/concepts/motion_planning.html#checkstartstatebounds

The robot may then end up in a configuration where one or more of its joints is slightly outside its joint limits. In this case, the motion planner is unable to plan since it will think that the starting state is outside joint limits. The “CheckStartStateBounds” planning request adapter will “fix” the start state by moving it to the joint limit. However, this is obviously not the right solution every time - e.g. where the joint is really outside its joint limits by a large amount. A parameter for the adapter specifies how much the joint can be outside its limits for it to be “fixable”.

From the document above, I assumed that CheckStartStateBounds have a parameter that "fixes" the joint if it's within this parameter.
ROS1 MoveIt seem to have a parameter that does this if I'm reading correctly.
https://github.com/moveit/moveit/blob/118d219d78563dcafe79c8b69245f00537e9fb9e/moveit_ros/planning/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp#L147

However, I don't see this particularly parameter in MoveIt2.
Was this removed for a specific reason? Or is there a parameter that does this?

ROS Distro

Jazzy

OS and version

Ubuntu 24.04

Source or binary build?

Binary

If binary, which release version?

No response

If source, which branch?

No response

Which RMW are you using?

CycloneDDS

Steps to Reproduce

Plan a robot arm with prismatic joint, with sensor reading slightly below or above the joint limit.

Expected behavior

A parameter that "fixes" the joint if joint value is within this parameter.

Actual behavior

No parameter found.

Backtrace or Console output

No response

[sea-bass]

That's right, the parameter was removed here:

#2429

However, it's because you don't need to specify a tolerance anymore. There is a boolean parameter called fix_start_state which will do the corrections needed:

moveit2/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml

Line 3 in 7284d36

fix_start_state: {

[jsupratman13]

Thanks for the response.
I have a follow-up question.

For my situation (the hardware with a prismatic joint that dips lower than the lower limit by negligible amount), the CheckStartStateBounds considers the start_state out of bounds and therefore, enters the following if-statement which sets the variable is_out_of_bounds to true.

moveit2/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp

Lines 153 to 155 in 7284d36

	if (!start_state.satisfiesBounds(jmodel))
	{
	is_out_of_bounds |= true;

However, it's because you don't need to specify a tolerance anymore. There is a boolean parameter called fix_start_state which will do the corrections needed:
Originally posted by @sea-bass in #3383

May I assume that the fix_start_state boolean parameter is a user-define parameter that will do the corrections if the start state is out of bounds?
Does it also mean that the correction should happen even if is_out_of_bounds is true? If so, doesn't the following code returns failure even if the fix_start_state is true?

moveit2/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp

Line 184 in 7284d36

if (is_out_of_bounds || (!params.fix_start_state && should_fix_state))

[sea-bass]

Ah, it seems like this parameter only fixes normalization/wrapping issues, not out of bounds. I had forgotten that this behavior was modified with respect to moveit 1.

See #3143 (comment)

[jsupratman13]

Oh, I see.

The design to no longer "fix" the joint when out of bounds is intentional. (To avoid small jumps that lead to undesirable behavior)

Thank you for the clarification