Physics: Ray intersections (#2514)

Introduces a component to request and receive ray intersections computed by the physics engine. This is currently only supported when using the DART physics engine with the collision detector set to `bullet`.
---------

Signed-off-by: Rômulo Cerqueira <romulogcerqueira@gmail.com>
Co-authored-by: Rômulo Cerqueira <rcerqueira@krakenrobotics.com>
Co-authored-by: Addisu Z. Taddese <addisu@openrobotics.org>

include/gz/sim/components/RaycastData.hh

@@ -0,0 +1,108 @@
+/*
+ * Copyright (C) 2024 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef GZ_SIM_COMPONENTS_RAYCASTDATA_HH_
+#define GZ_SIM_COMPONENTS_RAYCASTDATA_HH_
+
+#include <gz/math/Vector3.hh>
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/components/Serialization.hh>
+#include <gz/sim/config.hh>
+
+#include <istream>
+#include <ostream>
+#include <vector>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace components
+{
+/// \brief A struct that holds the information of a ray.
+struct RayInfo
+{
+  /// \brief Starting point of the ray in entity frame
+  gz::math::Vector3d start;
+
+  /// \brief Ending point of the ray in entity frame
+  gz::math::Vector3d end;
+};
+
+/// \brief A struct that holds the result of a raycasting operation.
+struct RaycastResultInfo
+{
+  /// \brief The hit point in entity frame
+  gz::math::Vector3d point;
+
+  /// \brief The fraction of the ray length at the intersection/hit point.
+  double fraction;
+
+  /// \brief The normal at the hit point in entity frame
+  gz::math::Vector3d normal;
+};
+
+/// @brief A struct that holds the raycasting data, including ray and results
+struct RaycastDataInfo
+{
+  /// @brief The rays to cast from the entity.
+  std::vector<RayInfo> rays;
+
+  /// @brief The results of the raycasting.
+  std::vector<RaycastResultInfo> results;
+};
+}
+
+namespace serializers
+{
+  /// \brief Specialization of DefaultSerializer for RaycastDataInfo
+  template<> class DefaultSerializer<components::RaycastDataInfo>
+  {
+    public: static std::ostream &Serialize(
+      std::ostream &_out, const components::RaycastDataInfo &)
+    {
+      return _out;
+    }
+
+    public: static std::istream &Deserialize(
+      std::istream &_in, components::RaycastDataInfo &)
+    {
+      return _in;
+    }
+  };
+}
+
+namespace components
+{
+/// \brief A component type that contains the rays traced from an entity
+/// into a physics world, along with the results of the raycasting operation.
+///
+/// This component is primarily used for applications that require raycasting.
+/// The target application defines the rays, and the physics system plugin
+/// updates the raycasting results during each update loop.
+using RaycastData = Component<RaycastDataInfo, class RaycastDataTag,
+                              serializers::DefaultSerializer<RaycastDataInfo>>;
+
+GZ_SIM_REGISTER_COMPONENT("gz_sim_components.RaycastData", RaycastData)
+}
+}
+}
+}
+#endif  // GZ_SIM_COMPONENTS_RAYCASTDATA_HH_

src/systems/physics/Physics.cc

@@ -63,6 +63,7 @@
 #include <gz/physics/GetContacts.hh>
 #include <gz/physics/GetBoundingBox.hh>
 #include <gz/physics/GetEntities.hh>
+#include <gz/physics/GetRayIntersection.hh>
 #include <gz/physics/Joint.hh>
 #include <gz/physics/Link.hh>
 #include <gz/physics/RemoveEntities.hh>
@@ -132,6 +133,7 @@
 #include "gz/sim/components/Name.hh"
 #include "gz/sim/components/ParentEntity.hh"
 #include "gz/sim/components/ParentLinkName.hh"
+#include "gz/sim/components/RaycastData.hh"
 #include "gz/sim/components/ExternalWorldWrenchCmd.hh"
 #include "gz/sim/components/JointTransmittedWrench.hh"
 #include "gz/sim/components/JointForceCmd.hh"
@@ -314,6 +316,10 @@ class gz::sim::systems::PhysicsPrivate
   /// \param[in] _ecm Mutable reference to ECM.
   public: void UpdateCollisions(EntityComponentManager &_ecm);
 
+  /// \brief Update ray intersection components from physics simulation
+  /// \param[in] _ecm Mutable reference to ECM.
+  public: void UpdateRayIntersections(EntityComponentManager &_ecm);
+
   /// \brief FrameData relative to world at a given offset pose
   /// \param[in] _link gz-physics link
   /// \param[in] _pose Offset pose in which to compute the frame data
@@ -514,6 +520,11 @@ class gz::sim::systems::PhysicsPrivate
             CollisionFeatureList,
             physics::GetContactsFromLastStepFeature>{};
 
+  /// \brief Feature list to handle ray intersection information.
+  public: struct RayIntersectionFeatureList : physics::FeatureList<
+            MinimumFeatureList,
+            physics::GetRayIntersectionFromLastStepFeature>{};
+
   /// \brief Feature list to change contacts before they are applied to physics.
   public: struct SetContactPropertiesCallbackFeatureList :
             physics::FeatureList<
@@ -651,6 +662,7 @@ class gz::sim::systems::PhysicsPrivate
           MinimumFeatureList,
           CollisionFeatureList,
           ContactFeatureList,
+          RayIntersectionFeatureList,
           SetContactPropertiesCallbackFeatureList,
           NestedModelFeatureList,
           CollisionDetectorFeatureList,
@@ -3984,6 +3996,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
 
   // TODO(louise) Skip this if there are no collision features
   this->UpdateCollisions(_ecm);
+
+  this->UpdateRayIntersections(_ecm);
 }  // NOLINT readability/fn_size
 // TODO (azeey) Reduce size of function and remove the NOLINT above
 
@@ -4165,6 +4179,97 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
       });
 }
 
+//////////////////////////////////////////////////
+void PhysicsPrivate::UpdateRayIntersections(EntityComponentManager &_ecm)
+{
+  GZ_PROFILE("PhysicsPrivate::UpdateRayIntersections");
+  // Quit early if the RaycastData component hasn't been created.
+  // This means there are no systems that need raycasting information
+  if (!_ecm.HasComponentType(components::RaycastData::typeId))
+    return;
+
+  // Assume that there is only one world entity
+  Entity worldEntity = _ecm.EntityByComponents(components::World());
+
+  if (!this->entityWorldMap.HasEntity(worldEntity))
+  {
+    gzwarn << "Failed to find world [" << worldEntity << "]." << std::endl;
+    return;
+  }
+
+  auto worldRayIntersectionFeature =
+      this->entityWorldMap.EntityCast<RayIntersectionFeatureList>(worldEntity);
+
+  if (!worldRayIntersectionFeature)
+  {
+    static bool informed{false};
+    if (!informed)
+    {
+      gzdbg << "Attempting process ray intersections, but the physics "
+             << "engine doesn't support ray intersection features. "
+             << "Ray intersections won't be computed."
+             << std::endl;
+      informed = true;
+    }
+    return;
+  }
+
+  // Go through each entity that has a RaycastData components, trace the
+  // rays and store the results
+  _ecm.Each<components::RaycastData,
+            components::Pose>(
+      [&](const Entity &_entity,
+          components::RaycastData *_raycastData,
+          components::Pose */*_pose*/) -> bool
+      {
+        // Retrieve the rays from the RaycastData component
+        const auto &rays = _raycastData->Data().rays;
+
+        // Clear the previous results
+        auto &results = _raycastData->Data().results;
+        results.clear();
+        results.reserve(rays.size());
+
+        // Get the entity's world pose
+        const auto &entityWorldPose = worldPose(_entity, _ecm);
+
+        for (const auto &ray : rays)
+        {
+          // Convert ray to world frame
+          const math::Vector3d rayStart = entityWorldPose.Pos() +
+            entityWorldPose.Rot().RotateVector(ray.start);
+          const math::Vector3d rayEnd = entityWorldPose.Pos() +
+            entityWorldPose.Rot().RotateVector(ray.end);
+
+          // Perform ray intersection
+          auto rayIntersection =
+            worldRayIntersectionFeature->GetRayIntersectionFromLastStep(
+              math::eigen3::convert(rayStart),
+              math::eigen3::convert(rayEnd));
+
+          const auto rayIntersectionResult =
+            rayIntersection.Get<
+              physics::World3d<RayIntersectionFeatureList>::RayIntersection>();
+
+          results.emplace_back();
+          auto &result = results.back();
+
+          // Convert result to entity frame and store
+          const math::Vector3d intersectionPoint =
+            math::eigen3::convert(rayIntersectionResult.point);
+          result.point = entityWorldPose.Rot().RotateVectorReverse(
+            intersectionPoint - entityWorldPose.Pos());
+
+          result.fraction = rayIntersectionResult.fraction;
+
+          const math::Vector3d normal =
+            math::eigen3::convert(rayIntersectionResult.normal);
+          result.normal = entityWorldPose.Rot().RotateVectorReverse(normal);
+        }
+        return true;
+      });
+}
+
 //////////////////////////////////////////////////
 physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset(
       const LinkPtrType &_link, const math::Pose3d &_pose) const

test/integration/physics_system.cc

@@ -37,6 +37,8 @@
 #include <sdf/Sphere.hh>
 #include <sdf/World.hh>
 
+#include <gz/math/eigen3/Conversions.hh>
+
 #include "gz/sim/Entity.hh"
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/Link.hh"
@@ -76,6 +78,7 @@
 #include "gz/sim/components/Physics.hh"
 #include "gz/sim/components/Pose.hh"
 #include "gz/sim/components/PoseCmd.hh"
+#include "gz/sim/components/RaycastData.hh"
 #include "gz/sim/components/Static.hh"
 #include "gz/sim/components/Visual.hh"
 #include "gz/sim/components/World.hh"
@@ -2904,3 +2907,105 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(JointsInWorld))
   server.AddSystem(testSystem.systemPtr);
   server.Run(true, 1000, false);
 }
+
+//////////////////////////////////////////////////
+/// Test ray intersections computed by physics system during Update loop.
+TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections))
+{
+  ServerConfig serverConfig;
+
+  const auto sdfFile =
+    common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "empty.sdf");
+
+  serverConfig.SetSdfFile(sdfFile);
+  Server server(serverConfig);
+  server.SetUpdatePeriod(1ns);
+
+  Entity testEntity1;
+  Entity testEntity2;
+
+  test::Relay testSystem;
+
+  // During PreUpdate, add rays to testEntity1 and testEntity2
+  testSystem.OnPreUpdate(
+      [&](const UpdateInfo &/*_info*/, EntityComponentManager &_ecm)
+      {
+        // Set collision detector to bullet (supports ray intersections).
+        auto worldEntity = _ecm.EntityByComponents(components::World());
+        _ecm.CreateComponent(
+          worldEntity, components::PhysicsCollisionDetector("bullet"));
+
+        // Create RaycastData component for testEntity1
+        testEntity1 = _ecm.CreateEntity();
+        _ecm.CreateComponent(testEntity1, components::RaycastData());
+        _ecm.CreateComponent(
+          testEntity1, components::Pose(math::Pose3d(0, 0, 10, 0, 0, 0)));
+
+        // Create RaycastData component for testEntity2
+        testEntity2 = _ecm.CreateEntity();
+        _ecm.CreateComponent(testEntity2, components::RaycastData());
+        _ecm.CreateComponent(
+          testEntity2, components::Pose(math::Pose3d(0, 0, 10, 0, 0, 0)));
+
+        // Add 5 rays to testEntity1 that intersect with the ground plane
+        auto &rays1 =
+          _ecm.Component<components::RaycastData>(testEntity1)->Data().rays;
+        for (int i = 0; i < 5; ++i)
+        {
+          components::RayInfo ray;
+          ray.start = math::Vector3d(0, 0, -i);
+          ray.end = math::Vector3d(0, 0, -20);
+          rays1.push_back(ray);
+        }
+
+        // Add 2 rays to testEntity2 that don't intersect with the ground plane
+        auto &rays2 =
+           _ecm.Component<components::RaycastData>(testEntity2)->Data().rays;
+        for (int i = 0; i < 2; ++i)
+        {
+          components::RayInfo ray;
+          ray.start = math::Vector3d(0, 0, -i);
+          ray.end = math::Vector3d(0, 0, 5);
+          rays2.push_back(ray);
+        }
+      });
+  // Check ray intersections for testEntity1 and testEntity2
+  testSystem.OnPostUpdate(
+      [&](const UpdateInfo &/*_info*/, const EntityComponentManager &_ecm)
+      {
+        // check the raycasting results for testEntity1
+        auto &rays1 =
+          _ecm.Component<components::RaycastData>(testEntity1)->Data().rays;
+        auto &results1 =
+          _ecm.Component<components::RaycastData>(testEntity1)->Data().results;
+        ASSERT_EQ(rays1.size(), results1.size());
+
+        for (size_t i = 0; i < results1.size(); ++i) {
+          ASSERT_EQ(results1[i].point, math::Vector3d(0, 0, -10));
+          ASSERT_EQ(results1[i].normal, math::Vector3d(0, 0, 1));
+          const double expFraction =
+            (rays1[i].start - results1[i].point).Length() /
+              (rays1[i].start - rays1[i].end).Length();
+          ASSERT_NEAR(results1[i].fraction, expFraction, 1e-6);
+        }
+
+        // check the raycasting results for testEntity2
+        auto &rays2 =
+          _ecm.Component<components::RaycastData>(testEntity2)->Data().rays;
+        auto &results2 =
+          _ecm.Component<components::RaycastData>(testEntity2)->Data().results;
+        ASSERT_EQ(rays2.size(), results2.size());
+
+        for (size_t i = 0; i < results2.size(); ++i) {
+          ASSERT_TRUE(
+            math::eigen3::convert(results2[i].point).array().isNaN().all());
+          ASSERT_TRUE(
+            math::eigen3::convert(results2[i].normal).array().isNaN().all());
+          ASSERT_TRUE(
+            std::isnan(results2[i].fraction));
+        }
+      });
+
+  server.AddSystem(testSystem.systemPtr);
+  server.Run(true, 1, false);
+}