From b5d1508bb7011240d64755506b599c5cd3f18ffa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=B4mulo=20Cerqueira?= Date: Wed, 12 Jun 2024 14:10:13 -0300 Subject: [PATCH] Ray intersection simulation feature (#641) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit dartsim does support ray-based collisions via Bullet backend, which is also supported by gz-physics. This PR creates a simulation feature to compute and retrieve ray castings. Also, it updates dartsim version >= 6.10, which fixes the issues when no ray hit. This addition is useful for range-based applications (e.g. laser, altimeter etc.). --------- Signed-off-by: Rômulo Cerqueira Co-authored-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- dartsim/src/SimulationFeatures.cc | 40 ++++++++ dartsim/src/SimulationFeatures.hh | 12 ++- include/gz/physics/GetRayIntersection.hh | 86 ++++++++++++++++ .../gz/physics/detail/GetRayIntersection.hh | 49 ++++++++++ test/common_test/simulation_features.cc | 98 +++++++++++++++++++ 6 files changed, 285 insertions(+), 2 deletions(-) create mode 100644 include/gz/physics/GetRayIntersection.hh create mode 100644 include/gz/physics/detail/GetRayIntersection.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index aa41e2dcb..fd48ffb51 100644 --- a/CMakeLists.txt +++ b/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 >=) diff --git a/dartsim/src/SimulationFeatures.cc b/dartsim/src/SimulationFeatures.cc index 47caa9b0b..9475ef984 100644 --- a/dartsim/src/SimulationFeatures.cc +++ b/dartsim/src/SimulationFeatures.cc @@ -15,6 +15,7 @@ * */ +#include #include #include #include @@ -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(_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::quiet_NaN()); + intersection.normal = + Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()); + intersection.fraction = std::numeric_limits::quiet_NaN(); + } + + return intersection; +} + std::vector SimulationFeatures::GetContactsFromLastStep(const Identity &_worldID) const { diff --git a/dartsim/src/SimulationFeatures.hh b/dartsim/src/SimulationFeatures.hh index c13873837..a3365feb8 100644 --- a/dartsim/src/SimulationFeatures.hh +++ b/dartsim/src/SimulationFeatures.hh @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -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 ::ContactInternal; + public: using GetRayIntersectionFromLastStepFeature::Implementation< + FeaturePolicy3d>::RayIntersection; + public: SimulationFeatures() = default; public: ~SimulationFeatures() override = default; @@ -113,6 +118,11 @@ class SimulationFeatures : public: std::vector 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 prevLinkPoses; diff --git a/include/gz/physics/GetRayIntersection.hh b/include/gz/physics/GetRayIntersection.hh new file mode 100644 index 000000000..1c9e31223 --- /dev/null +++ b/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 +#include +#include +#include + +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 +{ + public: template + struct RayIntersectionT + { + public: using Scalar = typename PolicyT::Scalar; + public: using VectorType = + typename FromPolicy::template Use; + + /// \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 + class World : public virtual Feature::World + { + public: using VectorType = + typename FromPolicy::template Use; + public: using RayIntersection = RayIntersectionT; + public: using RayIntersectionData = + SpecifyData>; + + /// \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 + class Implementation : public virtual Feature::Implementation + { + public: using RayIntersection = RayIntersectionT; + public: using VectorType = + typename FromPolicy::template Use; + + 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_ */ diff --git a/include/gz/physics/detail/GetRayIntersection.hh b/include/gz/physics/detail/GetRayIntersection.hh new file mode 100644 index 000000000..8448ea608 --- /dev/null +++ b/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 +#include + +namespace gz +{ +namespace physics +{ +///////////////////////////////////////////////// +template +auto GetRayIntersectionFromLastStepFeature::World< + PolicyT, FeaturesT>::GetRayIntersectionFromLastStep( + const VectorType &_from, + const VectorType &_to) const -> RayIntersectionData +{ + auto result = + this->template Interface() + ->GetRayIntersectionFromLastStep(this->identity, _from, _to); + + RayIntersection intersection{result.point, result.fraction, result.normal}; + + RayIntersectionData output; + output.template Get() = std::move(intersection); + return output; +} + +} // namespace physics +} // namespace gz + +#endif diff --git a/test/common_test/simulation_features.cc b/test/common_test/simulation_features.cc index d92e474f9..83afc7bdd 100644 --- a/test/common_test/simulation_features.cc +++ b/test/common_test/simulation_features.cc @@ -37,6 +37,7 @@ #include "gz/physics/BoxShape.hh" #include +#include #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 SimulationFeaturesRayIntersectionTest : + public SimulationFeaturesTest{}; +using SimulationFeaturesRayIntersectionTestTypes = + ::testing::Types; +TYPED_TEST_SUITE(SimulationFeaturesRayIntersectionTest, + SimulationFeaturesRayIntersectionTestTypes); + +TYPED_TEST(SimulationFeaturesRayIntersectionTest, SupportedRayIntersections) +{ + std::vector 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( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = StepWorld(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::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::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 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( + this->loader, + name, + common_test::worlds::kSphereSdf); + world->SetCollisionDetector(collisionDetector); + auto checkedOutput = StepWorld(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::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);