Improvement on Servo enforcePositionLimits()#451
Conversation
Codecov Report
@@ Coverage Diff @@
## main #451 +/- ##
==========================================
- Coverage 51.43% 51.42% -0.01%
==========================================
Files 223 223
Lines 23339 23344 +5
==========================================
Hits 12002 12002
- Misses 11337 11342 +5
Continue to review full report at Codecov.
|
tylerjw
left a comment
There was a problem hiding this comment.
Generally approve; left a couple of minor comments.
| // Check if pending velocity command is moving in the right direction | ||
| auto joint_itr = | ||
| std::find(joint_trajectory.joint_names.begin(), joint_trajectory.joint_names.end(), joint->getName()); | ||
| auto joint_idx = std::distance(joint_trajectory.joint_names.begin(), joint_itr); | ||
|
|
||
| if ((joint_trajectory.points[0].velocities[joint_idx] < 0 && | ||
| (joint_angle < (limits[0].min_position + parameters_->joint_limit_margin))) || | ||
| (current_state_->getJointVelocities(joint)[0] > 0 && | ||
| (joint_trajectory.points[0].velocities[joint_idx] > 0 && | ||
| (joint_angle > (limits[0].max_position - parameters_->joint_limit_margin)))) |
There was a problem hiding this comment.
Is there a current test that tests the behavior of this? If not, could you extend the test to test this?
There was a problem hiding this comment.
@tylerjw please realize you are asking a lot here. The tests are currently commented with this note:
# TODO(henningkayser): Port tests to ROS2
So I effectively need to fix some other tests to add this one.
There was a problem hiding this comment.
Getting #428 merged would unblock this PR. The test we need (TestEnforcePosLimits) is already there
There was a problem hiding this comment.
Let's get this merged and we are working to get TestEnforcePosLimits operational in #428.
tylerjw
left a comment
There was a problem hiding this comment.
thank you for adding the test, this looks good to me 🥇
c6e46bb to
93ee267
Compare
vatanaksoytezer
left a comment
There was a problem hiding this comment.
I've read through the code and checked the failing CI. I left a small comment with a question
93ee267 to
6e5fb0b
Compare
6e5fb0b to
f032b21
Compare
6597a58 to
7c511be
Compare
Previously, the incoming robot state velocity was used to check if the robot would move beyond the joint limit. That was a problem if the robot state did not contain velocity data, i.e. if "copy dynamics" hadn't been turned on.
This PR uses the prospective robot velocity from Servo itself to check if the joint would move beyond the position limit and halt, if so. This velocity should always be available regardless of whether "copy dynamics" is ON.