Use previous command to enforce the joint limits on position interfaces

[saikishor]

This PR partially reverts the changes proposed in the PR : #1988 and the corresponding changes introduced into the JointLimits integration PR : 7846f6a

Additionally, if the actual position is outside the joint limits completely, then it will throw an exception to avoid controlling the joint and probably causing self collision

[fmauch]

With the ur_robot_driver the problem with limits mostly seems to be arising from the tracking error. Since the hardware needs a couple of cycles to reach a command and report it through its state interfaces, the value read from the state interfaces as "actual" might be lacking behind what we want to command to the robot. As the soft limiter checks the "actual" position with a maximum step size given the maximum joint velocity multiplied by the cycle time. This effectively reduces the velocity with which a joint can move along. See below screenshot for details:

The joint speed is limited (much below the actual joint limit) due to that.

I had success with effectively swapping saving the last command or the "actual" position into the position in

ros2_control/joint_limits/src/joint_soft_limiter.cpp

Lines 100 to 107 in 65340a4

	if (actual.has_position())
	{
	position = actual.position.value();
	}
	else if (prev_command_.has_position() && std::isfinite(prev_command_.position.value()))
	{
	position = prev_command_.position.value();
	}

, making it default to the last commanded position if available.

Specifically:

if ( prev_command_.has_data() && prev_command_.has_position() && std::isfinite(prev_command_.position.value()))
{
  position = prev_command_.position.value();
}
else if (actual.has_position() && std::isfinite(actual.position.value()))
{
  position = actual.position.value();
}

[fmauch]

Tested things with the latest version of this and that definitively resolves our issues.

[fmauch]

This all seems to make sense to me and fixes things when running with a UR.

[codecov]

Codecov Report

Attention: Patch coverage is 89.65517% with 9 lines in your changes missing coverage. Please review.

Project coverage is 89.10%. Comparing base (65340a4) to head (7222b62).
Report is 2 commits behind head on master.

Files with missing lines	Patch %	Lines
joint_limits/src/joint_limits_helpers.cpp	60.00%	6 Missing ⚠️
joint_limits/src/joint_soft_limiter.cpp	90.00%	2 Missing ⚠️
hardware_interface/src/resource_manager.cpp	75.00%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #2183      +/-   ##
==========================================
- Coverage   89.18%   89.10%   -0.09%     
==========================================
  Files         139      139              
  Lines       16096    16105       +9     
  Branches     1382     1388       +6     
==========================================
- Hits        14356    14350       -6     
- Misses       1216     1225       +9     
- Partials      524      530       +6     

Flag	Coverage Δ
unittests	89.10% <89.65%> (-0.09%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...ace_testing/test/test_components/test_actuator.cpp	88.52% <100.00%> (ø)
...e_interface_testing/test/test_resource_manager.cpp	99.41% <100.00%> (ø)
...t/test_resource_manager_prepare_perform_switch.cpp	100.00% <ø> (ø)
...s/include/joint_limits/joint_limiter_interface.hpp	50.79% <ø> (ø)
.../include/joint_limits/joint_saturation_limiter.hpp	100.00% <100.00%> (ø)
joint_limits/src/joint_range_limiter.cpp	90.90% <100.00%> (+0.13%)	⬆️
joint_limits/src/joint_saturation_limiter.cpp	85.38% <100.00%> (+0.08%)	⬆️
joint_limits/test/test_joint_range_limiter.cpp	100.00% <100.00%> (ø)
joint_limits/test/test_joint_soft_limiter.cpp	99.63% <100.00%> (-0.01%)	⬇️
hardware_interface/src/resource_manager.cpp	76.67% <75.00%> (-0.33%)	⬇️
... and 2 more

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[fmauch]

This is looking good, I just have one question:

What exactly is the mutex for? As I understand, it should avoid that limiting and resetting can happen at the same time.

In reality this should not happen, since ResourceManager::perform_command_mode_switch and on_enforce happen in the same thread, right?

If they would be running in separate threads, the two lock guards would potentially block each other either reset_internals or on_enforce taking longer that it should running on its own. This would be potentially violating cycle-time constraints.

[saikishor]

This is looking good, I just have one question:

What exactly is the mutex for? As I understand, it should avoid that limiting and resetting can happen at the same time.

In reality this should not happen, since ResourceManager::perform_command_mode_switch and on_enforce happen in the same thread, right?

If they would be running in separate threads, the two lock guards would potentially block each other either reset_internals or on_enforce taking longer that it should running on its own. This would be potentially violating cycle-time constraints.

@fmauch I've added this because when you run a controller or a hardware component as asynchronous. This case might happen, where enforce might be called in parallel. This is to protect these cases especially.

The good thing is both the threads are on high priority, so there should be less likely chance or only minimal time blockage

[christophfroehlich]

The idea makes sense for me, and the changes LGTM

[christophfroehlich]

Failing tests due to flaky #2191 or already fixed with #2188