add warning if interpolation parameter is out of range [0, 1] #2664

JStech · 2021-05-12T19:50:43Z

Description

Added warnings if the interpolation parameter t is outside the range [0, 1] or NaN. Would have helped in debugging something recently.

Maybe it would be good to also clamp the value to [0, 1] (and make a NaN a 0 or something)?

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

jliukkonen-pn

Very nice improvement!

jliukkonen-pn · 2021-05-12T19:57:14Z

moveit_core/robot_model/src/robot_model.cpp

@@ -1290,6 +1290,8 @@ double RobotModel::distance(const double* state1, const double* state2) const

 void RobotModel::interpolate(const double* from, const double* to, double t, double* state) const
 {
+  ROS_WARN_STREAM_COND_NAMED(std::isnan(t) || t < 0. || t > 1., LOGNAME,


Would it make sense to add std::isinf check too? Also, typo in word parameter.

Actually, should this whole function just fail if the fraction t is not reasonable? Print error and return instead of giving a warning? After all, there was a crash when t was NaN. 🤔

I believe infinite values will be caught by t < 0. || t > 1.. Actually, I think a NaN would also, but I'm not sure that's true across all platforms, etc. Thanks for the spelling catch.

I was on the fence about how strongly it should react to an incorrect value. Since it's a void return type, failure isn't really an option, just complaining and doing nothing. (Or, maybe, throwing an exception?)

They should probably throw an exception. Interpolating from NaNs and infs sounds dangerous. What about an assert?

Values smaller than 0 and larger than 1 are perfectly fine: You just extrapolate beyond the point. However, a warning in this case is probably useful. nan and inf could throw.

Values smaller than 0 and larger than 1 are perfectly fine: You just extrapolate beyond the point. However, a warning in this case is probably useful. nan and inf could throw.

Thumbs up for this approach.

codecov · 2021-05-12T20:48:49Z

Codecov Report

Merging #2664 (256114c) into master (01e594e) will decrease coverage by 0.01%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##           master    #2664      +/-   ##
==========================================
- Coverage   60.49%   60.48%   -0.00%     
==========================================
  Files         402      402              
  Lines       29637    28983     -654     
==========================================
- Hits        17925    17528     -397     
+ Misses      11712    11455     -257

Impacted Files	Coverage Δ
...bot_model/include/moveit/robot_model/robot_model.h	`68.43% <0.00%> (-15.57%)`	⬇️
moveit_core/robot_model/src/robot_model.cpp	`78.21% <0.00%> (-0.76%)`	⬇️
moveit_core/robot_state/src/robot_state.cpp	`50.28% <0.00%> (-0.49%)`	⬇️
...ipulation/pick_place/src/manipulation_pipeline.cpp	`71.85% <0.00%> (-1.94%)`	⬇️
...meterization/work_space/pose_model_state_space.cpp	`82.06% <0.00%> (-0.64%)`	⬇️
...eit/planning_scene_monitor/current_state_monitor.h	`100.00% <0.00%> (ø)`
...e/collision_detection_fcl/src/collision_common.cpp	`78.61% <0.00%> (+0.26%)`	⬆️
... and 1 more

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 47af01b...256114c. Read the comment docs.

rhaschke

Instead of repeating the same code snippet several times, I suggest providing an inline function for this sanity check.

v4hn · 2021-05-13T11:19:11Z

Funny and frustrating how many comments such a minor patch can generate... I support throwing an exception for `nan` and `inf` values, though one could argue that the valid range is documented in doxygen and thus valid by contract. (We are not writing Ada though, so there's no way to check contracts :)). Printing a warning in this case and still running invalid computations is not an option for me. I'm actually a fan of extrapolation-support here because it is supported for free and thus would allow all valid double values here, but we also provide `RobotState::integrateVariableVelocity` for related use-cases. So if you insist on restricting the use of the method to its documented specification it should throw for values outside [0;1] as well.

JStech · 2021-05-13T14:18:29Z

Instead of repeating the same code snippet several times, I suggest providing an inline function for this sanity check.

Where do you recommend I put such a function? Maybe moveit_core/macros somewhere?

rhaschke · 2021-05-13T15:05:47Z

Where do you recommend I put such a function? Maybe moveit_core/macros somewhere?

I would just put it into the same .cpp file. We don't need to expose this function, do we?

JStech · 2021-05-13T15:14:52Z

I would just put it into the same .cpp file. We don't need to expose this function, do we?

But I'm checking in both robot_state.cpp and robot_model.cpp--so define it in both? Another "local" option would be to put it in robot_model.h, which is included in both.

Also, one of the functions in robot_state.cpp then immediately calls the function in robot_model.cpp, so we don't really need the first check, although it'd probably be easier to interpret the error if it comes from the outer function.

I feel like all of this is a matter of style, and I don't see a strong reason to do it one way or another. Just tell me what will get approved :-P.

JStech · 2021-05-18T16:54:42Z

I defined the check in robot_model.h and it's used in both RobotState and RobotModel

rhaschke

One more nitpick. Otherwise, I approve.

rhaschke · 2021-05-18T18:33:11Z

moveit_core/robot_model/include/moveit/robot_model/robot_model.h

@@ -61,6 +61,15 @@ namespace core
 {
 MOVEIT_CLASS_FORWARD(RobotModel);  // Defines RobotModelPtr, ConstPtr, WeakPtr... etc

+static inline void checkInterpolationParamBounds(const char LOGNAME[], const double& t)


Don't pass simple data types as double by reference:
https://stackoverflow.com/questions/7561690/why-do-we-not-pass-pod-by-reference-in-functions

rhaschke · 2021-05-18T20:35:07Z

moveit_core/robot_model/include/moveit/robot_model/robot_model.h

@@ -61,7 +61,7 @@ namespace core
 {
 MOVEIT_CLASS_FORWARD(RobotModel);  // Defines RobotModelPtr, ConstPtr, WeakPtr... etc

-static inline void checkInterpolationParamBounds(const char LOGNAME[], const double& t)
+static inline void checkInterpolationParamBounds(const char LOGNAME[], double t)


You could have kept const. But fine now, anyway.

…ange [0, 1] (moveit#2664)

add warning if interpolation parameter is out of [0, 1]

167056e

JStech requested review from felixvd, mlautman, rhaschke and tylerjw as code owners May 12, 2021 19:50

jliukkonen-pn reviewed May 12, 2021

View reviewed changes

spelling fix

5d13717

JStech mentioned this pull request May 12, 2021

New RobotState interpolation test #2665

Merged

6 tasks

rhaschke reviewed May 13, 2021

View reviewed changes

put check in inline function

97bac99

rhaschke reviewed May 18, 2021

View reviewed changes

Don't pass double by reference

256114c

rhaschke approved these changes May 18, 2021

View reviewed changes

rhaschke merged commit 5e8bd34 into moveit:master May 18, 2021

JStech deleted the pr-interpolation-warnings branch May 18, 2021 22:05

rhaschke pushed a commit to ubi-agni/moveit that referenced this pull request May 23, 2021

RobotState interpolation: warn if interpolation parameter is out of r…

b661025

…ange [0, 1] (moveit#2664)

rhaschke pushed a commit to ubi-agni/moveit that referenced this pull request May 23, 2021

RobotState interpolation: warn if interpolation parameter is out of r…

eb6b630

…ange [0, 1] (moveit#2664)

rhaschke mentioned this pull request May 23, 2021

Melodic Release 1.0.8 #2686

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

add warning if interpolation parameter is out of range [0, 1] #2664

add warning if interpolation parameter is out of range [0, 1] #2664

JStech commented May 12, 2021

jliukkonen-pn left a comment

jliukkonen-pn May 12, 2021

jliukkonen-pn May 12, 2021

JStech May 12, 2021

JStech May 12, 2021

felixvd May 13, 2021

rhaschke May 13, 2021

jliukkonen-pn May 13, 2021

codecov bot commented May 12, 2021 •

edited

Loading

rhaschke left a comment

v4hn commented May 13, 2021 via email

JStech commented May 13, 2021

rhaschke commented May 13, 2021

JStech commented May 13, 2021

JStech commented May 18, 2021

rhaschke left a comment

rhaschke May 18, 2021

rhaschke May 18, 2021

add warning if interpolation parameter is out of range [0, 1] #2664

add warning if interpolation parameter is out of range [0, 1] #2664

Conversation

JStech commented May 12, 2021

Description

Checklist

jliukkonen-pn left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented May 12, 2021 • edited Loading

Codecov Report

rhaschke left a comment

Choose a reason for hiding this comment

v4hn commented May 13, 2021 via email

JStech commented May 13, 2021

rhaschke commented May 13, 2021

JStech commented May 13, 2021

JStech commented May 18, 2021

rhaschke left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented May 12, 2021 •

edited

Loading