Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add joint limiter interface plugins to enforce limits defined in the URDF #1526

Merged
merged 72 commits into from
Jan 4, 2025
Merged
Changes from 1 commit
Commits
Show all changes
72 commits
Select commit Hold shift + click to select a range
011fabe
make JointTrajectoryPoint as an input through the template argument
saikishor Apr 6, 2024
9f34da1
check for node parameter interface before declaring and getting the p…
saikishor Apr 7, 2024
cd7abd0
Added methods to check is the parameter and logging interfaces are se…
saikishor Apr 7, 2024
797076d
add an init method parsing the limits directly in the init method
saikishor Apr 7, 2024
8301b59
update the joint state limiter to use template typename in the header…
saikishor Apr 7, 2024
d80929d
check the size of the joint limits and the joint names in the init me…
saikishor Apr 7, 2024
ea9bda4
added joint range limiter library for individual joints control
saikishor Apr 7, 2024
42fb8c7
added compute_effort_limits method
saikishor Apr 7, 2024
a780465
update the velocity limits based on the position of the joint
saikishor Apr 7, 2024
088fa6d
fix the limits clamping in the methods
saikishor Apr 7, 2024
28f7e3e
Add proper acceleration limits bounding
saikishor Apr 7, 2024
0944328
remove the enforce methods of the double vector as it can be handled …
saikishor Apr 16, 2024
dfda4b2
Added comments to the has_logging_interface and has_parameter_interfa…
saikishor Apr 16, 2024
87714fb
remove the joint range limiter header and reuse it from the joint sat…
saikishor Apr 16, 2024
7a4ce74
rename the plugin names to be more consistent with the types the use …
saikishor Apr 16, 2024
98122ea
Add first version of tests into the joint range limiter
saikishor Apr 17, 2024
02796bf
fix the init check bug
saikishor Apr 17, 2024
55bb040
Apply formatting changes
saikishor Apr 17, 2024
eab6689
update the joint range limiter test with case of only desired positio…
saikishor Apr 17, 2024
7fe2336
Added some minor fixes in the joint range limiter
saikishor Apr 17, 2024
9269842
Fill the data of actual or desired fields into the previous commands
saikishor Apr 17, 2024
f94efc2
reset the prev_data_ on initialization
saikishor Apr 17, 2024
fa90bfa
add test cases of the actual and desired position cases
saikishor Apr 17, 2024
b9918e5
Add tests for the case of no position limits
saikishor Apr 18, 2024
59b697d
add initial test cases for desired velocity only cases
saikishor Apr 18, 2024
372b0d4
Enforce wrt to the position limits only when the actual position valu…
saikishor Apr 18, 2024
560d1bc
change some asserts to expects
saikishor Apr 18, 2024
5b2c0b4
Use lambdas for better testing
saikishor Apr 18, 2024
0dde6a9
If the joint is outside the position range, then don't let the joint …
saikishor Apr 18, 2024
80308b3
extend tests of acceleration limits also with and without actual posi…
saikishor Apr 18, 2024
92d153e
Add tests for the case of actuators with actual position state and ac…
saikishor Apr 18, 2024
a2d7258
remove unused test cases
saikishor Apr 18, 2024
c1e29c9
pass by const reference and parse the optional actual position and ve…
saikishor Apr 18, 2024
3746584
Added tests for the effort case
saikishor Apr 18, 2024
1eb5bad
better conditioning for the acceleration limits enforcement
saikishor Apr 18, 2024
5c86100
Added tests for the desired acceleration case
saikishor Apr 18, 2024
90cd75e
Add tests for the desired jerk test cases
saikishor Apr 18, 2024
71d311d
consider also the acceleration limits when computing the position lim…
saikishor Apr 18, 2024
ffe9aeb
Fix minor bug in the limit enforcement
saikishor Apr 18, 2024
cd60d88
better computation of the position limits
saikishor Apr 18, 2024
bde797a
better combined desired references test
saikishor Apr 18, 2024
1d2faa9
Remove LimitsType template from JointLimiterInterface
adriaroig Apr 22, 2024
4645773
Add SoftJointLimits in JointLimiterInterface
adriaroig Apr 22, 2024
3192bc6
Move joint limits computation in a separate library
adriaroig Apr 25, 2024
c9f36b0
Implement SoftJointLimiter class
adriaroig Apr 25, 2024
1e5dbf0
Export JointInterfacesSoftLimiter in the joint_limiters.xml
adriaroig Apr 25, 2024
02f5760
Fix linking problem in test_joint_range_limiter
adriaroig Apr 25, 2024
6f02f9f
Create test_soft_joint_limiter.cpp
adriaroig Apr 25, 2024
c4ea119
Fix typo in plugins definition
adriaroig Apr 29, 2024
9f31c6a
Rm soft_joint_limiter.hpp
adriaroig Apr 29, 2024
b58bd10
Derive SoiftJointLimiter from JointSaturationLimiter<JointControlInte…
adriaroig Apr 29, 2024
00eb01d
Test SofJointLimiter in position
adriaroig Apr 29, 2024
2c6d1bd
Solve issues in SoftJointLimiter when prev_command is not defined
adriaroig Apr 29, 2024
764d68b
Test SoftJointLimiter in position and effort
adriaroig Apr 29, 2024
f30ce41
Test uses cases for SoftJointLimiter in velocity interface
adriaroig Apr 30, 2024
c8d9eec
Format changes
adriaroig May 3, 2024
7d6ea96
change copyright to PAL
saikishor May 6, 2024
b828b13
add pre-comitting fixes
saikishor May 6, 2024
45328dd
Add intermediate tests to fix the transition bug
saikishor May 7, 2024
b2ee4d8
Fix the undefined behavior when lower bound is greater than upper bound
saikishor May 7, 2024
8d147a1
Add some documentation to the helpers
saikishor May 7, 2024
a4567d4
rename of SoftJointLimiter to JointSoftLimiter and some minor changes
saikishor Jul 19, 2024
a42c972
enable moving out of position limit if velocity is in "right" direction
mamueluth Jul 8, 2024
46395e3
warn everytime we reach the limit
mamueluth Jul 9, 2024
dc64739
update the loguic to check if the actual position is within bounds an…
saikishor Jul 29, 2024
415f535
Remove unnecessary debug prints
saikishor Oct 11, 2024
2a258cf
fix the soft limiting logic and add respective tests
saikishor Oct 11, 2024
226a855
Apply suggestions from code review
saikishor Dec 4, 2024
08f09eb
Rename the joint_limiter_struct.hpp to data_structures.hpp
saikishor Dec 4, 2024
296f992
improve documentation of the helper methods
saikishor Dec 4, 2024
fffd7e4
Merge branch 'master' into add/sw_joint_limiter
saikishor Jan 1, 2025
753133d
Fix other visibility macros and pre-commit fixes
saikishor Jan 1, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Add tests for the case of actuators with actual position state and ac…
…celeration limits
saikishor committed Dec 15, 2024
commit 92d153ebf61c9e31bc91c785fa77ca1975559954
30 changes: 28 additions & 2 deletions joint_limits/test/test_joint_range_limiter.cpp
Original file line number Diff line number Diff line change
@@ -249,15 +249,17 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases)
test_limit_enforcing(-5.0, -3.0, 0.0, true);
test_limit_enforcing(-5.0, -1.0, 0.0, true);
// When the position is out of the limits, then the velocity is saturated to zero
// test_limit_enforcing(-6.0, -1.0, 0.0, true);
test_limit_enforcing(-6.0, -1.0, 0.0, true);
test_limit_enforcing(-6.0, -2.0, 0.0, true);
test_limit_enforcing(-6.0, 1.0, 0.0, true);

// Now remove the position limits and only test with acceleration limits
limits.has_position_limits = false;
limits.has_acceleration_limits = true;
limits.max_acceleration = 0.5;
// When launching init, the prev_command_ within the limiter will be reset
ASSERT_TRUE(Init(limits));
// Now the velocity limits are not saturated by the acceleration limits so in succeeding call it
// Now the velocity limits are now saturated by the acceleration limits so in succeeding call it
// will reach the desired if it is within the max velocity limits. Here, the order of the tests is
// important.
for (auto act_pos :
@@ -281,6 +283,30 @@ TEST_F(JointSaturationLimiterTest, check_desired_velocity_only_cases)
test_limit_enforcing(act_pos, 0.0, 0.5, true);
test_limit_enforcing(act_pos, 0.0, 0.0, false);
}

// Now re-enable the position limits and test with acceleration limits
limits.has_position_limits = true;
limits.has_acceleration_limits = true;
limits.max_acceleration = 0.5;
// When launching init, the prev_command_ within the limiter will be reset
ASSERT_TRUE(Init(limits));
// Now the velocity limits are now saturated by the acceleration limits so in succeeding call it
// will reach the desired if it is within the max velocity limits. Here, the order of the tests is
// important.
test_limit_enforcing(4.5, 0.0, 0.0, false); // Helps to reset th prev_command internally
test_limit_enforcing(4.5, 1.0, 0.5, true);
test_limit_enforcing(4.8, 1.0, 0.2, true);
test_limit_enforcing(4.8, -1.0, -0.3, true);
test_limit_enforcing(4.8, -1.0, -0.8, true);
test_limit_enforcing(4.8, -1.0, -1.0, false);
test_limit_enforcing(-4.8, -1.0, -0.2, true);
test_limit_enforcing(-4.3, -1.0, -0.7, true);
test_limit_enforcing(-4.3, 0.0, -0.2, true);
test_limit_enforcing(-4.3, 0.0, 0.0, false);
test_limit_enforcing(-6.0, 1.0, 0.0, true);
test_limit_enforcing(-6.0, -1.0, 0.0, true);
test_limit_enforcing(6.0, 1.0, 0.0, true);
test_limit_enforcing(6.0, -1.0, 0.0, true);
}

// TEST_F(JointSaturationLimiterTest, when_no_posstate_expect_enforce_false)