moveit_servo: Fix segfault when publish_joint_velocities set to false and a joint is close to position limit#497
Conversation
… and a joint is close to position limit
Codecov Report
@@ Coverage Diff @@
## main #497 +/- ##
==========================================
- Coverage 45.18% 45.17% -0.00%
==========================================
Files 169 169
Lines 18732 18734 +2
==========================================
Hits 8462 8462
- Misses 10270 10272 +2
Continue to review full report at Codecov.
|
AndyZe
left a comment
There was a problem hiding this comment.
This makes a lot of sense. Let me test it.
schornakj
left a comment
There was a problem hiding this comment.
I tested this and it does stop Servo from segfaulting when I reach a joint position limit.
However, once I'm at a limit I'm not able to move back in the opposite direction or move any other joint in the robot. I get this warning message:
[moveit_servo.servo_calcs]: servo_server wrist_3_joint close to a position limit. Halting.
Seems like this is related to the conditional that sets halting = true. I think it should check if the commanded direction of rotation is still going towards the limit and still allow rotation if it goes away from the limit. Could that be resolved within the scope of this PR or does that require a lot of extra work?
|
Maybe we should always store joint position/vel/accel internally so we don't have to worry about this type of thing. Then |
|
I will test it too, but seems reasonable
I think the original (seg faulting) code here was checking for this, so I would say the same behavior should be included in this PR |
This also happens to me when the robot reaches collisions or joint limits and once it's there it will be stuck, but I think this's outside the scope of this PR since it requires changing the halting behavior |
| (joint_angle > (limits[0].max_position - parameters_->joint_limit_margin)))) | ||
| const bool near_min_position = | ||
| parameters_->publish_joint_velocities ? | ||
| joint_trajectory.points[0].velocities[joint_idx] < 0 && |
There was a problem hiding this comment.
If you change all the operator[] to at() it should help catch any future occurrences of this too.
However, there is a regression because it could reverse away from joint limits previously. Sorry, but I think we'll have to change the way this PR works. I'll make a pull request against your branch, @JafarAbdi. |
@AndyZe if it is not too late, I think a workable solution is to switch the order |
|
@AdamPettinger do you mind making a PR? That sounds better than the way I did it. |
|
Yep! See JafarAbdi#13 |
AdamPettinger
left a comment
There was a problem hiding this comment.
Does joint limit checking probably the way it should have been all along 😅
Tested, and doesn't seg fault when publish_joint_velocities is false, and also allows backing away from a joint limit
| { | ||
| // Use the most recent robot joint state | ||
| if (original_joint_state_.name[c] == joint->getName()) | ||
| if (joint_state.name[c] == joint->getName()) |
There was a problem hiding this comment.
It makes sense to use Servo's internal joint position here, rather than original_joint_state_. We do the same thing for velocity at L809 👍
… and a joint is close to position limit (moveit#497) * moveit_servo: Fix segfault when publish_joint_velocities set to false and a joint is close to position limit * Enforce joint position limits on internal joint state before making trajectory (#13) * Use the input joint state to check bounds Co-authored-by: AdamPettinger <adam.pettinger@utexas.edu>
Description
moveit_servo is segfaulting when reaching limits if
publish_joint_velocitiesset tofalse. the reason is in ServoCalcs::enforcePositionLimits we do access the velocities despite being an empty vector seeChecklist