Integration test to defend subframe tutorial (and bugfix for subframes of attached collision objects) by tylerjw · Pull Request #1757 · moveit/moveit

tylerjw · 2019-11-20T23:40:59Z

Description

Issue: #1733
This PR is an integration test for the subframe tutorial that tests that the tutorial continues to work.

Future work:

test orientation

Thank you @jaas14 for helping me write this.

davetcoleman

Really glad we're improving the test coverage!!

moveit_ros/planning_interface/test/plan_using_subframes.cpp

felixvd

Thanks for doing the test I should have had in my PR to start with!

The testAtSubframe function seems very specialized and I found it pretty hard to read. It seemed like a lot of lines are spent getting the subframe pose for each object in different ways and with different variable names. I think a function getSubframePoseInWorld(subframe, psi, group) would have made it a lot clearer. If you have any cycles left I think this would be a good exercise and good for the code. Otherwise just correcting the comments/bugs will be fine.

Thanks again!

moveit_ros/planning_interface/test/plan_using_subframes.cpp

tylerjw · 2019-11-24T15:34:17Z

@felixvd I'm not sure how this passed locally for me. It looks like even with the huge margin (0.1) I didn't get close enough with the math. I'll revisit this and address your feedback this next week when I have time. Thank you for the review.

tylerjw · 2019-12-16T23:28:07Z

@felixvd I'm sorry this took me a while to get around to. I responded to your feedback and I believe this is now in a much better state. Please review (assuming it passes CI).

tylerjw · 2019-12-16T23:59:53Z

This doesn't pass on kinetic. Is there a chance subframes isn't backported or something similar?

felixvd

Thanks for iterating on this. But somehow I feel like this is now a lot longer and more complex.

The logic of searching subframes and applying transforms should not be reimplemented in this test – you can just use planning_scene::getFrameTransform. Reimplementing it here would only hide issues in the codebase.

Also, the functions getCylinderTipInWorld, getBoxSubframeInWorld and getTargetInWorld seem redundant. Why not simply confirm that the code below doesn't produce an error or null pointer?

Eigen::Isometry3d tip_in_world = planning_scene.getFrameTransform("cylinder/tip");
Eigen::Isometry3d box_subframe_in_world = planning_scene.getFrameTransform("box/" + subframe_name);
Eigen::Isometry3d target_in_world = planning_scene.getFrameTransform("box/" + target_subframe_name);

Lastly, please confirm the rotation as well. I suggested a way to implement it and some code.

Thanks again!

moveit_ros/planning_interface/test/subframes_test.cpp

tylerjw

@felixvd thank you for your patience with me on this one. I am new to the moveit codebase and still don't know the quick way to do things. I will ask around but I'm unsure how to get a PlanningScene from the MoveGroupInterface or PlanningSceneInterace. I have found some things saying I should create a PlanningSceneMonitor. I will ask around and update here if I figure it out.

davetcoleman · 2020-01-21T17:37:23Z

@tylerjw let's get this merged in :-)

tylerjw · 2020-03-23T08:17:27Z

@mlautman I got the psm to work as expected. Now I'm actually having some weird issue with subframes. Here is the debug output (getFrameTransform translations), the values in cylinder_tip look wrong, however box_subframe seem reasonable:

panda_hand: 0.306957, 0.0899721, 0.460283
box_subframe: 0.307, 0.05, 0.34027
cylinder: 0.307055, 0.0899991, 0.340283
cylinder_tip: 0.03, 1.68051e-18, 0.12

@felixvd do you see what I'm doing wrong here?

tylerjw · 2020-03-23T08:27:44Z

For more debugging here is the planning scene message representation of cylinder:

    attached_collision_objects: 
      - 
        link_name: "panda_hand"
        object: 
          header: 
            seq: 0
            stamp: 
              secs: 0
              nsecs:         0
            frame_id: "panda_hand"
          id: "cylinder"
          type: 
            key: ''
            db: ''
          primitives: 
            - 
              type: 3
              dimensions: [0.06, 0.005]
          primitive_poses: 
            - 
              position: 
                x: 0.0
                y: 0.0
                z: 0.12
              orientation: 
                x: -1.96644880614e-17
                y: 0.707106781187
                z: -1.96644880626e-17
                w: 0.707106781187
          meshes: []
          mesh_poses: []
          planes: []
          plane_poses: []
          subframe_names: [tip]
          subframe_poses: 
            - 
              position: 
                x: 0.03
                y: 1.68051336735e-18
                z: 0.12
              orientation: 
                x: -1.96644880614e-17
                y: 0.707106781187
                z: -1.96644880626e-17
                w: 0.707106781187
          operation: 0
        touch_links: []
        detach_posture: 
          header: 
            seq: 0
            stamp: 
              secs: 0
              nsecs:         0
            frame_id: ''
          joint_names: []
          points: []
        weight: 0.0
    is_diff: False

tylerjw · 2020-03-23T08:29:23Z

Based on what I posted above, I think there might be a bug in how getFrameTransform gets the transform for subframes of attached collision objects. You'll notice the "tip" position reported is relative to the hand and not the world.

tylerjw · 2020-03-23T09:13:20Z

Before you ask, I rebased on master and pushed. Did not change the results.

tylerjw · 2020-03-23T21:41:48Z

To run the test use this command after sourcing your workspace and building with the --no-install option:

rostest moveit_ros_planning_interface subframes_test.test

tylerjw · 2020-03-26T22:24:30Z

@rhaschke thank you for fixing the bug for this. Assuming this passes travis I believe this is ready for review again. @felixvd

tylerjw · 2020-03-27T00:09:59Z

Sadness, this failed on line 259 of the test for clang only (but passed for gcc). Any ideas why that would be?

Here is a link to the test that failed: https://travis-ci.org/github/ros-planning/moveit/jobs/667472631?utm_medium=notification&utm_source=github_status

Somehow travis is failing to report it's status on this one, weird.

tylerjw · 2020-03-27T19:11:39Z

rebased on master to see if that'd kick travis integration into working here. Didn't work, it still just shows "expected" below for me.

Here is the travis run: https://travis-ci.org/github/ros-planning/moveit/builds/667849848

codecov-io · 2020-03-27T19:39:20Z

Codecov Report

Merging #1757 into master will increase coverage by 1.78%.
The diff coverage is 100.00%.

@@            Coverage Diff             @@
##           master    #1757      +/-   ##
==========================================
+ Coverage   49.98%   51.77%   +1.78%     
==========================================
  Files         315      319       +4     
  Lines       24776    25000     +224     
==========================================
+ Hits        12384    12943     +559     
+ Misses      12392    12057     -335

Impacted Files	Coverage Δ
...t_state/include/moveit/robot_state/attached_body.h	`100.00% <ø> (ø)`
moveit_core/kinematic_constraints/src/utils.cpp	`25.72% <100.00%> (+6.08%)`	⬆️
moveit_core/robot_state/src/attached_body.cpp	`59.61% <100.00%> (+40.17%)`	⬆️
moveit_core/robot_state/src/robot_state.cpp	`45.21% <100.00%> (+1.61%)`	⬆️
...ove_group_interface/src/wrap_python_move_group.cpp	`39.87% <0.00%> (-3.17%)`	⬇️
...octomap_updater/src/pointcloud_octomap_updater.cpp	`38.51% <0.00%> (ø)`
...ion/pointcloud_octomap_updater/src/plugin_init.cpp	`100.00% <0.00%> (ø)`
...eption/point_containment_filter/src/shape_mask.cpp	`52.70% <0.00%> (ø)`
...clude/moveit/point_containment_filter/shape_mask.h	`100.00% <0.00%> (ø)`
...e/collision_detection_fcl/src/collision_common.cpp	`78.15% <0.00%> (+0.61%)`	⬆️
... and 22 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 13bf03e...529e929. Read the comment docs.

tylerjw · 2020-03-27T20:45:42Z

It looks like this is a flaky test (as it failed on one of the other tests this time). @rhaschke do you have any ideas on how to make this less flaky and yet still test the behavior of the tutorial and subframes?

Here is the output of the one that failed:

Value of: eef.isApprox(box_subframe * target_pose, EPSILON)
  Actual: false
Expected: true
box frame: 
           1  7.41052e-17 -0.000398163        0.307
 0.000398163 -6.92801e-12            1         0.05
-2.68432e-15           -1 -6.92801e-12      0.34027
           0            0            0            1
cylinder frame: 
-0.000838256    -0.999999   0.00129967     0.306966
-0.000934845  -0.00129888    -0.999999    0.0599642
    0.999999  -0.00083947 -0.000933755     0.340291
           0            0            0            1]]

Those look to be really cose to me in translation (I can't intuit the rotation portion). Is there a chance I could reduce EPSILON to something not so strict and this would be more robust and still be a good test?

tylerjw · 2020-03-27T23:20:41Z

Once again, travis doesn't seem to be syncing with github here, but this latest one passed on all tests.

rhaschke · 2020-03-28T10:57:37Z

We need to investigate this flaky test in more detail. The orientation should match as well!

tylerjw · 2020-03-28T17:00:53Z

Putting epsilon back to 1e-3 (1mm) I can get it to fail locally after running the test a dozen or so times. Here is what the robot looks like when it failed (I'd post one of success but it looks the same to me):

Here is the output from the console:

[ROSTEST]-----------------------------------------------------------------------

[moveit_ros_planning_interface.rosunit-subframes_test/SubframesTests][FAILURE]--
/home/tyler/workspace/ws_moveit/src/moveit/moveit_ros/planning_interface/test/subframes_test.cpp:184
Value of: eef.isApprox(box_subframe * target_pose, EPSILON)
  Actual: false
Expected: true
box frame: 
           1  7.41052e-17 -0.000398163        0.307
 0.000398163 -6.92801e-12            1         0.05
-2.68432e-15           -1 -6.92801e-12      0.34027
           0            0            0            1
cylinder frame: 
-0.000920403    -0.999999     0.001371     0.306928
-0.000729613  -0.00137033    -0.999999    0.0600662
    0.999999 -0.000921402 -0.000728351     0.340262
           0            0            0            1
--------------------------------------------------------------------------------

For comparison this is a success:

ros.moveit_ros_planning_interface.test_plan_using_subframes: box frame: 
           1  7.41052e-17 -0.000398163        0.307
 0.000398163 -6.92801e-12            1         0.05
-2.68432e-15           -1 -6.92801e-12      0.34027
           0            0            0            1
cylinder frame: 
 -0.00084591    -0.999999   0.00136466     0.306935
  0.00015513  -0.00136479    -0.999999    0.0599787
           1 -0.000845698  0.000156284     0.340268
           0            0            0            1

Can someone who knows more about the math of a rotation matrix see if they can understand what is happening wrong here? @AndyZe @rhaschke

tylerjw · 2020-03-28T17:17:43Z

I wrote a bit of python to compare the failing and passing difference between the box and cylinder subframes and this is the result:

Failing diff:
[[  1.00092040e+00   9.99999000e-01  -1.76916300e-03   7.20000000e-05]
 [  1.12777600e-03   1.37032999e-03   1.99999900e+00  -1.00662000e-02]
 [ -9.99999000e-01  -9.99078598e-01   7.28350993e-04   8.00000000e-06]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00]]
Passing diff:
[[  1.00084591e+00   9.99999000e-01  -1.76282300e-03   6.50000000e-05]
 [  2.43033000e-04   1.36478999e-03   1.99999900e+00  -9.97870000e-03]
 [ -1.00000000e+00  -9.99154302e-01  -1.56284007e-04   2.00000000e-06]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00]]

Note that some of this difference is expected as the comparison is actually between (box_subframe * target_pose) and the cylinder tip pose to account for the target pose offset. If you look at just the orders of magnitude of the diffs there is only one that differs from e-3 to e-4.

I honestly can't tell a difference between passing and failing transforms, they both look really similar to me.

tylerjw · 2020-03-28T17:33:40Z

My suggestion is we relax the requirement of epsilon to account for small differences in the ending pose from 1mm to 1cm. I can't get it to fail locally at 1cm.

I don't think is actually a flaky test, I think we were just testing the result of a motion plan too finely.

@rhaschke

rhaschke

I didn't notice that the printed target frame was different from the evaluated one, resulting in strong deviations of the rotation parts. I think, when the fix below is applied and we stick with 1cm resolution, this is ready for merging.

moveit_ros/planning_interface/test/subframes_test.cpp

Co-Authored-By: Robert Haschke <rhaschke@users.noreply.github.com>

tylerjw · 2020-03-29T18:03:10Z

done @rhaschke

felixvd · 2020-03-30T04:49:37Z

Thanks to both of you for cleaning up after this :)

v4hn · 2020-03-30T09:40:13Z

Congratulations @tylerjw on seeing this through to the merge! :)

tylerjw · 2020-03-30T15:30:12Z

@felixvd @v4hn @rhaschke thank you for the help getting this done. I learned so much about the interfaces of moveit doing this.

poweic · 2020-05-04T16:37:04Z

Just stumbled on this. Should we add eigen_conversions to CATKIN_DEPENDS section in catkin_package in file moveit_core/CMakeLists.txt?

catkin_package(
  INCLUDE_DIRS
    ${THIS_PACKAGE_INCLUDE_DIRS}
  LIBRARIES
    moveit_exceptions
    # more here ...
    ${OCTOMAP_LIBRARIES}
  CATKIN_DEPENDS
    eigen_stl_containers
    eigen_conversions  # <=============== should we add this ???
    geometric_shapes
    geometry_msgs
    kdl_parser
    moveit_msgs
    octomap_msgs
    random_numbers
    sensor_msgs
    shape_msgs
    srdfdom
    std_msgs
    tf2_eigen
    tf2_geometry_msgs
    trajectory_msgs
    visualization_msgs
  DEPENDS
    Boost
    # more here ...
    ${BULLET_ENABLE}
    )

rhaschke · 2020-05-04T16:50:18Z

@poweic, what kind of issue did you observe?
Please file a new PR, if you think you have a fix for something.
CATKIN_DEPENS should list dependencies that are transitive, i.e. are required by downstream packages of moveit_core, e.g. because the eigen_conversion headers are included in our headers.

tylerjw requested a review from rhaschke as a code owner November 20, 2019 23:41

davetcoleman reviewed Nov 21, 2019

View reviewed changes

tylerjw force-pushed the p/tylerjw/subframes_test branch from 37f8c2d to 5551a33 Compare November 21, 2019 05:32

felixvd suggested changes Nov 22, 2019

View reviewed changes

rhaschke force-pushed the master branch from 68982c0 to 11c82ec Compare November 24, 2019 14:20

tylerjw force-pushed the p/tylerjw/subframes_test branch from 892228f to 42999b5 Compare December 16, 2019 21:23

felixvd suggested changes Dec 17, 2019

View reviewed changes

tylerjw commented Dec 17, 2019

View reviewed changes

felixvd mentioned this pull request Feb 18, 2020

Fix subframes disappearing when object is detached/scaled/renamed #1866

Merged

3 tasks

tylerjw force-pushed the p/tylerjw/subframes_test branch from e372f9e to 8f871a4 Compare February 27, 2020 23:27

tylerjw changed the title ~~integration test to defend subframe tutorial~~ WIP: integration test to defend subframe tutorial Mar 23, 2020

tylerjw force-pushed the p/tylerjw/subframes_test branch from d1041e6 to cfa5b10 Compare March 23, 2020 09:12

tylerjw mentioned this pull request Mar 24, 2020

PlanningScene::getFrameTransform on subframe on attached collision object is not wrt world #1974

Closed

tylerjw requested a review from mlautman as a code owner March 25, 2020 23:45

tylerjw force-pushed the p/tylerjw/subframes_test branch from b8ee81c to 384563b Compare March 25, 2020 23:48

This was referenced Mar 26, 2020

small fixes to https://github.com/ros-planning/moveit/pull/1757 #1979

Closed

fixes to the subframes integration test PickNikRobotics/moveit#7

Merged

tylerjw requested a review from v4hn as a code owner March 26, 2020 22:14

tylerjw changed the title ~~WIP: integration test to defend subframe tutorial~~ Integration test to defend subframe tutorial (and bugfix for subframes of attached collision objects) Mar 26, 2020

tylerjw and others added 5 commits March 27, 2020 13:10

put z_offset_cylinder distance back to 0.12

1cfd26d

move and test each box subframe

92c177b

factor out tests into a function

4b0d930

remove extra test steps

e8c25d9

set the distance back to 0.1

529e929

tylerjw force-pushed the p/tylerjw/subframes_test branch from 4d2f2b2 to 529e929 Compare March 27, 2020 19:10

reduce strictness of EPSILON

1c283da

reduce test time to 30 seconds

ca3f6d8

rhaschke reviewed Mar 29, 2020

View reviewed changes

moveit_ros/planning_interface/test/subframes_test.cpp Outdated Show resolved Hide resolved

Update moveit_ros/planning_interface/test/subframes_test.cpp

fafd383

Co-Authored-By: Robert Haschke <rhaschke@users.noreply.github.com>

rhaschke approved these changes Mar 29, 2020

View reviewed changes

rhaschke merged commit 379acf9 into moveit:master Mar 29, 2020

tylerjw deleted the p/tylerjw/subframes_test branch March 29, 2020 20:21

v4hn mentioned this pull request Mar 31, 2020

Fix TfPublisher subframe publishing #2002

Merged

3 tasks

tylerjw mentioned this pull request Apr 9, 2020

Code coverage (testing our core functionality) #2016

Open

4 tasks

felixvd mentioned this pull request May 28, 2020

Add object pose #2037

Merged

5 tasks

Conversation

tylerjw commented Nov 20, 2019

Description

Uh oh!

davetcoleman left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

felixvd left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

tylerjw commented Nov 24, 2019

Uh oh!

tylerjw commented Dec 16, 2019

Uh oh!

tylerjw commented Dec 16, 2019

Uh oh!

felixvd left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

tylerjw left a comment

Choose a reason for hiding this comment

Uh oh!

davetcoleman commented Jan 21, 2020

Uh oh!

tylerjw commented Mar 23, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

tylerjw commented Mar 23, 2020

Uh oh!

tylerjw commented Mar 23, 2020

Uh oh!

tylerjw commented Mar 23, 2020

Uh oh!

tylerjw commented Mar 23, 2020

Uh oh!

tylerjw commented Mar 26, 2020

Uh oh!

tylerjw commented Mar 27, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

tylerjw commented Mar 27, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov-io commented Mar 27, 2020

Codecov Report

Uh oh!

tylerjw commented Mar 27, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

tylerjw commented Mar 27, 2020

Uh oh!

rhaschke commented Mar 28, 2020

Uh oh!

tylerjw commented Mar 28, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

tylerjw commented Mar 28, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

felixvd left a comment •

edited

Loading

tylerjw commented Mar 23, 2020 •

edited

Loading

tylerjw commented Mar 27, 2020 •

edited

Loading

tylerjw commented Mar 27, 2020 •

edited

Loading

tylerjw commented Mar 27, 2020 •

edited

Loading

tylerjw commented Mar 28, 2020 •

edited

Loading

tylerjw commented Mar 28, 2020 •

edited

Loading