Merge pull request RobotLocomotion#10866 from SeanCurtis-TRI/PR_ancho…

…red_geometry_part_2 Corrects cloning of anchored tree
YinjieLin · Mar 8, 2019 · f281fc3 · f281fc3
2 parents 79ea455 + d6ac7af
commit f281fc3
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git a/geometry/proximity_engine.cc b/geometry/proximity_engine.cc
@@ -906,6 +906,7 @@ unique_ptr<fcl::CollisionObjectd> CopyFclObjectOrThrow(
   auto copy = make_unique<fcl::CollisionObjectd>(geometry_copy);
   copy->setUserData(object.getUserData());
   copy->setTransform(object.getTransform());
+  copy->computeAABB();
   return copy;
 }
 
@@ -941,6 +942,7 @@ void BuildTreeFromReference(
   for (auto* other_object : other_objects) {
     target->registerObject(copy_map.at(other_object));
   }
+  target->update();
 }
 
 }  // namespace

diff --git a/geometry/test/proximity_engine_test.cc b/geometry/test/proximity_engine_test.cc
@@ -2594,6 +2594,12 @@ GTEST_TEST(ProximityEngineTests, AnchoredBroadPhaseInitialization) {
   std::vector<GeometryId> geometry_map{id_D, id_A};
   auto pairs = engine.ComputePointPairPenetration(geometry_map);
   EXPECT_EQ(pairs.size(), 1);
+
+  // Confirm that it survives copying.
+  ProximityEngine<double> engine_copy(engine);
+  engine_copy.UpdateWorldPoses({X_WD}, {index_D});
+  auto pairs_copy = engine_copy.ComputePointPairPenetration(geometry_map);
+  EXPECT_EQ(pairs_copy.size(), 1);
 }