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.
|
| // 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.
AndyZe
force-pushed
the
andyz/joint_limits2
branch
from
c6e46bb
to
93ee267
Compare
AndyZe
force-pushed
the
andyz/joint_limits2
branch
from
93ee267
to
6e5fb0b
Compare
AndyZe
force-pushed
the
andyz/joint_limits2
branch
from
6e5fb0b
to
f032b21
Compare
AndyZe
force-pushed
the
andyz/joint_limits2
branch
from
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.