moveit_servo: Fix segfault when publish_joint_velocities set to false and a joint is close to position limit #497

JafarAbdi · 2021-06-14T21:31:08Z

Description

moveit_servo is segfaulting when reaching limits if publish_joint_velocities set to false. the reason is in ServoCalcs::enforcePositionLimits we do access the velocities despite being an empty vector see

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

… and a joint is close to position limit

codecov · 2021-06-14T22:26:18Z

Codecov Report

Merging #497 (b7a1d8f) into main (eb2c8b9) will decrease coverage by 0.01%.
The diff coverage is 0.00%.

@@            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

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo_calcs.cpp	`0.00% <0.00%> (ø)`

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update eb2c8b9...b7a1d8f. Read the comment docs.

AndyZe

This makes a lot of sense. Let me test it.

schornakj

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?

AndyZe · 2021-06-15T13:54:05Z

Maybe we should always store joint position/vel/accel internally so we don't have to worry about this type of thing. Then publish_joint_velocities / publish_joint_positions / publish_joint_accelerations would only be applied to the outgoing message. Does that seem like a good path forward?

AdamPettinger · 2021-06-15T13:57:00Z

I will test it too, but seems reasonable

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?

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

JafarAbdi · 2021-06-15T14:00:05Z

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?

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

tylerjw · 2021-06-15T14:00:17Z

moveit_ros/moveit_servo/src/servo_calcs.cpp

-             (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 &&


If you change all the operator[] to at() it should help catch any future occurrences of this too.

AndyZe · 2021-06-15T14:10:19Z

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

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.

AdamPettinger · 2021-06-15T15:33:24Z

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

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 composeJointTrajMessage and enforcePositionLimits is called so that enforcePositionLimits works on a JointState and uses internal_joint_state_ which holds position and velocity information regardless of what should be published

AndyZe · 2021-06-15T15:53:48Z

@AdamPettinger do you mind making a PR? That sounds better than the way I did it.

AdamPettinger · 2021-06-16T14:00:43Z

Yep! See JafarAbdi#13

…rajectory (#13)

AdamPettinger

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

nbbrooks

+1 switch to at()

moveit_ros/moveit_servo/src/servo_calcs.cpp

AndyZe · 2021-06-24T13:30:48Z

moveit_ros/moveit_servo/src/servo_calcs.cpp

    {
      // Use the most recent robot joint state
-      if (original_joint_state_.name[c] == joint->getName())
+      if (joint_state.name[c] == joint->getName())


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>

moveit_servo: Fix segfault when publish_joint_velocities set to false…

4f01213

… and a joint is close to position limit

JafarAbdi requested review from AndyZe and AdamPettinger June 14, 2021 21:31

AndyZe reviewed Jun 15, 2021

View reviewed changes

schornakj reviewed Jun 15, 2021

View reviewed changes

tylerjw reviewed Jun 15, 2021

View reviewed changes

AdamPettinger and others added 3 commits June 21, 2021 21:54

Enforce joint position limits on internal joint state before making t…

94f2e5d

…rajectory (#13)

Merge branch 'main' into pr-fix_servo_bug

7b4e912

Merge branch 'main' into pr-fix_servo_bug

d386574

AdamPettinger approved these changes Jun 21, 2021

View reviewed changes

nbbrooks reviewed Jun 23, 2021

View reviewed changes

moveit_ros/moveit_servo/src/servo_calcs.cpp Show resolved Hide resolved

Use the input joint state to check bounds

b7a1d8f

AndyZe reviewed Jun 24, 2021

View reviewed changes

AndyZe approved these changes Jun 24, 2021

View reviewed changes

JafarAbdi merged commit fb94e78 into moveit:main Jun 25, 2021

JafarAbdi added a commit to JafarAbdi/moveit2 that referenced this pull request Jun 28, 2021

Re-add moveit#497

f334195

MikeWrock pushed a commit to MikeWrock/moveit2 that referenced this pull request Aug 15, 2022

New calibration tutorial (moveit#497)

7a547f8

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

moveit_servo: Fix segfault when publish_joint_velocities set to false and a joint is close to position limit #497

moveit_servo: Fix segfault when publish_joint_velocities set to false and a joint is close to position limit #497

JafarAbdi commented Jun 14, 2021

codecov bot commented Jun 14, 2021 •

edited

AndyZe left a comment

schornakj left a comment

AndyZe commented Jun 15, 2021 •

edited

AdamPettinger commented Jun 15, 2021

JafarAbdi commented Jun 15, 2021

tylerjw Jun 15, 2021

AndyZe commented Jun 15, 2021

AdamPettinger commented Jun 15, 2021

AndyZe commented Jun 15, 2021

AdamPettinger commented Jun 16, 2021

AdamPettinger left a comment

nbbrooks left a comment

AndyZe Jun 24, 2021 •

edited

moveit_servo: Fix segfault when publish_joint_velocities set to false and a joint is close to position limit #497

moveit_servo: Fix segfault when publish_joint_velocities set to false and a joint is close to position limit #497

Conversation

JafarAbdi commented Jun 14, 2021

Description

Checklist

codecov bot commented Jun 14, 2021 • edited

Codecov Report

AndyZe left a comment

Choose a reason for hiding this comment

schornakj left a comment

Choose a reason for hiding this comment

AndyZe commented Jun 15, 2021 • edited

AdamPettinger commented Jun 15, 2021

JafarAbdi commented Jun 15, 2021

tylerjw Jun 15, 2021

Choose a reason for hiding this comment

AndyZe commented Jun 15, 2021

AdamPettinger commented Jun 15, 2021

AndyZe commented Jun 15, 2021

AdamPettinger commented Jun 16, 2021

AdamPettinger left a comment

Choose a reason for hiding this comment

nbbrooks left a comment

Choose a reason for hiding this comment

AndyZe Jun 24, 2021 • edited

Choose a reason for hiding this comment

codecov bot commented Jun 14, 2021 •

edited

AndyZe commented Jun 15, 2021 •

edited

AndyZe Jun 24, 2021 •

edited