Ray intersection simulation feature

CMakeLists.txt

@@ -78,7 +78,7 @@ gz_find_package(DART
     utils
     utils-urdf
   CONFIG
-  VERSION 6.9
+  VERSION 6.10
   REQUIRED_BY dartsim
   PKGCONFIG dart
   PKGCONFIG_VER_COMPARISON >=)

dartsim/src/SimulationFeatures.cc

@@ -15,6 +15,7 @@
  *
 */
 
+#include <limits>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -165,6 +166,45 @@ void SimulationFeatures::Write(ChangedWorldPoses &_changedPoses) const
   this->prevLinkPoses = std::move(newPoses);
 }
 
+SimulationFeatures::RayIntersection
+SimulationFeatures::GetRayIntersectionFromLastStep(
+  const Identity &_worldID,
+  const LinearVector3d &_from,
+  const LinearVector3d &_to) const
+{
+  auto *const world = this->ReferenceInterface<DartWorld>(_worldID);
+  auto collisionDetector = world->getConstraintSolver()->getCollisionDetector();
+  auto collisionGroup = world->getConstraintSolver()->getCollisionGroup().get();
+
+  // Perform raycast
+  dart::collision::RaycastOption option;
+  dart::collision::RaycastResult result;
+  collisionDetector->raycast(collisionGroup, _from, _to, option, &result);
+
+  // Currently, raycast supports only the Bullet collision detector.
+  // For other collision detectors, the result will always be NaN.
+  SimulationFeatures::RayIntersection intersection;
+  if (result.hasHit())
+  {
+    // Store intersection data if there is a ray hit
+    const auto &firstHit = result.mRayHits[0];
+    intersection.point = firstHit.mPoint;
+    intersection.normal = firstHit.mNormal;
+    intersection.fraction = firstHit.mFraction;
+  }
+  else
+  {
+    // Set invalid measurements to NaN according to REP-117
+    intersection.point =
+      Eigen::Vector3d::Constant(std::numeric_limits<double>::quiet_NaN());
+    intersection.normal =
+      Eigen::Vector3d::Constant(std::numeric_limits<double>::quiet_NaN());
+    intersection.fraction = std::numeric_limits<double>::quiet_NaN();
+  }
+
+  return intersection;
+}
+
 std::vector<SimulationFeatures::ContactInternal>
 SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const
 {

dartsim/src/SimulationFeatures.hh

@@ -34,6 +34,7 @@
 
 #include <gz/physics/ForwardStep.hh>
 #include <gz/physics/GetContacts.hh>
+#include <gz/physics/GetRayIntersection.hh>
 #include <gz/physics/ContactProperties.hh>
 #include <gz/physics/SpecifyData.hh>
 
@@ -56,7 +57,8 @@ struct SimulationFeatureList : FeatureList<
 #ifdef DART_HAS_CONTACT_SURFACE
   SetContactPropertiesCallbackFeature,
 #endif
-  GetContactsFromLastStepFeature
+  GetContactsFromLastStepFeature,
+  GetRayIntersectionFromLastStepFeature
 > { };
 
 #ifdef DART_HAS_CONTACT_SURFACE
@@ -97,6 +99,9 @@ class SimulationFeatures :
   public: using GetContactsFromLastStepFeature::Implementation<FeaturePolicy3d>
     ::ContactInternal;
 
+  public: using GetRayIntersectionFromLastStepFeature::Implementation<
+    FeaturePolicy3d>::RayIntersection;
+
   public: SimulationFeatures() = default;
   public: ~SimulationFeatures() override = default;
 
@@ -113,6 +118,11 @@ class SimulationFeatures :
   public: std::vector<ContactInternal> GetContactsFromLastStep(
       const Identity &_worldID) const override;
 
+  public: RayIntersection GetRayIntersectionFromLastStep(
+      const Identity &_worldID,
+      const LinearVector3d &_from,
+      const LinearVector3d &_end) const override;
+
   /// \brief link poses from the most recent pose change/update.
   /// The key is the link's ID, and the value is the link's pose
   private: mutable std::unordered_map<std::size_t, math::Pose3d> prevLinkPoses;

include/gz/physics/GetRayIntersection.hh

@@ -0,0 +1,86 @@
+/*
+ * 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_PHYSICS_GETRAYINTERSECTION_HH_
+#define GZ_PHYSICS_GETRAYINTERSECTION_HH_
+
+#include <gz/physics/FeatureList.hh>
+#include <gz/physics/ForwardStep.hh>
+#include <gz/physics/Geometry.hh>
+#include <gz/physics/SpecifyData.hh>
+
+namespace gz
+{
+namespace physics
+{
+/// \brief GetRayIntersectionFromLastStepFeature is a feature for retrieving
+/// the ray intersection generated in the previous simulation step.
+class GZ_PHYSICS_VISIBLE GetRayIntersectionFromLastStepFeature
+    : public virtual FeatureWithRequirements<ForwardStep>
+{
+  public: template <typename PolicyT>
+  struct RayIntersectionT
+  {
+    public: using Scalar = typename PolicyT::Scalar;
+    public: using VectorType =
+      typename FromPolicy<PolicyT>::template Use<LinearVector>;
+
+    /// \brief The hit point in the world coordinates
+    VectorType point;
+
+    /// \brief The fraction of the ray length at the intersection/hit point.
+    Scalar fraction;
+
+    /// \brief The normal at the hit point in the world coordinates
+    VectorType normal;
+  };
+
+  public: template <typename PolicyT, typename FeaturesT>
+  class World : public virtual Feature::World<PolicyT, FeaturesT>
+  {
+    public: using VectorType =
+      typename FromPolicy<PolicyT>::template Use<LinearVector>;
+    public: using RayIntersection = RayIntersectionT<PolicyT>;
+    public: using RayIntersectionData =
+      SpecifyData<RequireData<RayIntersection>>;
+
+    /// \brief Get ray intersection generated in the previous simulation step
+    /// \param[in] _from The start point of the ray in world coordinates
+    /// \param[in] _to The end point of the ray in world coordinates
+    public: RayIntersectionData GetRayIntersectionFromLastStep(
+      const VectorType &_from, const VectorType &_to) const;
+  };
+
+  public: template <typename PolicyT>
+  class Implementation : public virtual Feature::Implementation<PolicyT>
+  {
+    public: using RayIntersection = RayIntersectionT<PolicyT>;
+    public: using VectorType =
+      typename FromPolicy<PolicyT>::template Use<LinearVector>;
+
+    public: virtual RayIntersection GetRayIntersectionFromLastStep(
+      const Identity &_worldID,
+      const VectorType &_from,
+      const VectorType &_to) const = 0;
+  };
+};
+}
+}
+
+#include "gz/physics/detail/GetRayIntersection.hh"
+
+#endif /* end of include guard: GZ_PHYSICS_GETRAYINTERSECTION_HH_ */

include/gz/physics/detail/GetRayIntersection.hh

@@ -0,0 +1,49 @@
+/*
+ * 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_PHYSICS_DETAIL_GETRAYINTERSECTION_HH_
+#define GZ_PHYSICS_DETAIL_GETRAYINTERSECTION_HH_
+
+#include <utility>
+#include <gz/physics/GetRayIntersection.hh>
+
+namespace gz
+{
+namespace physics
+{
+/////////////////////////////////////////////////
+template <typename PolicyT, typename FeaturesT>
+auto GetRayIntersectionFromLastStepFeature::World<
+    PolicyT, FeaturesT>::GetRayIntersectionFromLastStep(
+      const VectorType &_from,
+      const VectorType &_to) const -> RayIntersectionData
+{
+  auto result =
+    this->template Interface<GetRayIntersectionFromLastStepFeature>()
+        ->GetRayIntersectionFromLastStep(this->identity, _from, _to);
+
+  RayIntersection intersection{result.point, result.fraction, result.normal};
+
+  RayIntersectionData output;
+  output.template Get<RayIntersection>() = std::move(intersection);
+  return output;
+}
+
+}  // namespace physics
+}  // namespace gz
+
+#endif

test/common_test/simulation_features.cc

@@ -37,6 +37,7 @@
 
 #include "gz/physics/BoxShape.hh"
 #include <gz/physics/GetContacts.hh>
+#include <gz/physics/GetRayIntersection.hh>
 #include "gz/physics/ContactProperties.hh"
 #include "gz/physics/CylinderShape.hh"
 #include "gz/physics/CapsuleShape.hh"
@@ -1233,6 +1234,103 @@ TYPED_TEST(SimulationFeaturesTestBasic, MultipleCollisions)
   }
 }
 
+/////////////////////////////////////////////////
+// The features that an engine must have to be loaded by this loader.
+struct FeaturesRayIntersections : gz::physics::FeatureList<
+  gz::physics::sdf::ConstructSdfWorld,
+  gz::physics::GetRayIntersectionFromLastStepFeature,
+  gz::physics::CollisionDetector,
+  gz::physics::ForwardStep
+> {};
+
+template <class T>
+class SimulationFeaturesRayIntersectionTest :
+  public SimulationFeaturesTest<T>{};
+using SimulationFeaturesRayIntersectionTestTypes =
+    ::testing::Types<FeaturesRayIntersections>;
+TYPED_TEST_SUITE(SimulationFeaturesRayIntersectionTest,
+                 SimulationFeaturesRayIntersectionTestTypes);
+
+TYPED_TEST(SimulationFeaturesRayIntersectionTest, SupportedRayIntersections)
+{
+  std::vector<std::string> supportedCollisionDetectors = {"bullet"};
+
+  for (const std::string &name : this->pluginNames)
+  {
+    CHECK_UNSUPPORTED_ENGINE(name, "bullet", "bullet-featherstone", "tpe")
+
+    for (const std::string &collisionDetector : supportedCollisionDetectors) {
+      auto world = LoadPluginAndWorld<FeaturesRayIntersections>(
+          this->loader,
+          name,
+          common_test::worlds::kSphereSdf);
+      world->SetCollisionDetector(collisionDetector);
+      auto checkedOutput = StepWorld<FeaturesRayIntersections>(world, true, 1).first;
+      EXPECT_TRUE(checkedOutput);
+
+      // ray hits the sphere
+      auto result =
+        world->GetRayIntersectionFromLastStep(
+            Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2));
+
+      auto rayIntersection =
+          result.template
+            Get<gz::physics::World3d<FeaturesRayIntersections>::RayIntersection>();
+
+      double epsilon = 1e-3;
+      EXPECT_TRUE(
+          rayIntersection.point.isApprox(Eigen::Vector3d(-1, 0, 2), epsilon));
+      EXPECT_TRUE(
+          rayIntersection.normal.isApprox(Eigen::Vector3d(-1, 0, 0), epsilon));
+      EXPECT_DOUBLE_EQ(rayIntersection.fraction, 0.25);
+
+      // ray does not hit the sphere
+      result = world->GetRayIntersectionFromLastStep(
+          Eigen::Vector3d(2, 0, 10), Eigen::Vector3d(-2, 0, 10));
+      rayIntersection =
+          result.template
+            Get<gz::physics::World3d<FeaturesRayIntersections>::RayIntersection>();
+
+      ASSERT_TRUE(rayIntersection.point.array().isNaN().any());
+      ASSERT_TRUE(rayIntersection.normal.array().isNaN().any());
+      ASSERT_TRUE(std::isnan(rayIntersection.fraction));
+    }
+  }
+}
+
+TYPED_TEST(SimulationFeaturesRayIntersectionTest, UnsupportedRayIntersections)
+{
+  std::vector<std::string> unsupportedCollisionDetectors = {"ode", "dart", "fcl", "banana"};
+
+  for (const std::string &name : this->pluginNames)
+  {
+    CHECK_UNSUPPORTED_ENGINE(name, "bullet", "bullet-featherstone", "tpe")
+
+    for (const std::string &collisionDetector : unsupportedCollisionDetectors) {
+      auto world = LoadPluginAndWorld<FeaturesRayIntersections>(
+          this->loader,
+          name,
+          common_test::worlds::kSphereSdf);
+      world->SetCollisionDetector(collisionDetector);
+      auto checkedOutput = StepWorld<FeaturesRayIntersections>(world, true, 1).first;
+      EXPECT_TRUE(checkedOutput);
+
+      // ray would hit the sphere, but the collision detector does
+      // not support ray intersection
+      auto result = world->GetRayIntersectionFromLastStep(
+          Eigen::Vector3d(-2, 0, 2), Eigen::Vector3d(2, 0, 2));
+
+      auto rayIntersection =
+          result.template
+            Get<gz::physics::World3d<FeaturesRayIntersections>::RayIntersection>();
+
+      ASSERT_TRUE(rayIntersection.point.array().isNaN().any());
+      ASSERT_TRUE(rayIntersection.normal.array().isNaN().any());
+      ASSERT_TRUE(std::isnan(rayIntersection.fraction));
+    }
+  }
+}
+
 int main(int argc, char *argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);