Integration test to defend subframe tutorial (and bugfix for subframes of attached collision objects)

[tylerjw]

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

[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!

[tylerjw]

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

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

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!

[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]

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

[tylerjw]

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

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]

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]

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

[tylerjw]

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]

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

[tylerjw]

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]

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]

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]

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]

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

[rhaschke]

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

[tylerjw]

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]

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]

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.

[tylerjw]

done @rhaschke

[felixvd]

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

[v4hn]

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

[tylerjw]

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

[poweic]

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]

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