Merge branch 'main' into issue/870_unsymmetrical

Author: jslee02

dart/collision/RaycastOption.cpp

@@ -36,11 +36,23 @@ namespace dart {
 namespace collision {
 
 //==============================================================================
-RaycastOption::RaycastOption(bool enableAllHits, bool sortByClosest)
-  : mEnableAllHits(enableAllHits), mSortByClosest(sortByClosest)
+RaycastOption::RaycastOption(
+    bool enableAllHits, bool sortByClosest, RaycastFilter filter)
+  : mEnableAllHits(enableAllHits),
+    mSortByClosest(sortByClosest),
+    mFilter(std::move(filter))
 {
   // Do nothing
 }
 
+//==============================================================================
+bool RaycastOption::passesFilter(const CollisionObject* object) const
+{
+  if (!mFilter)
+    return true;
+
+  return mFilter(object);
+}
+
 } // namespace collision
 } // namespace dart

dart/collision/RaycastOption.hpp

@@ -35,23 +35,35 @@
 
 #include <dart/Export.hpp>
 
+#include <functional>
 #include <memory>
 
 #include <cstddef>
 
 namespace dart {
 namespace collision {
 
+class CollisionObject;
+
 struct DART_API RaycastOption
 {
+  using RaycastFilter = std::function<bool(const CollisionObject*)>;
+
   /// Constructor
-  RaycastOption(bool enableAllHits = false, bool sortByClosest = false);
+  RaycastOption(
+      bool enableAllHits = false,
+      bool sortByClosest = false,
+      RaycastFilter filter = nullptr);
+
+  /// Returns true when the filter is not set or allows the object.
+  bool passesFilter(const CollisionObject* object) const;
 
   bool mEnableAllHits;
 
   bool mSortByClosest;
 
-  // TODO(JS): Add filter
+  /// Optional filter to reject hits from specific collision objects.
+  RaycastFilter mFilter;
 };
 
 } // namespace collision

dart/collision/bullet/BulletCollisionDetector.cpp

@@ -377,16 +377,50 @@ bool BulletCollisionDetector::raycast(
   const auto btFrom = convertVector3(from);
   const auto btTo = convertVector3(to);
 
-  if (option.mEnableAllHits) {
+  const bool needsAllHits
+      = option.mEnableAllHits || static_cast<bool>(option.mFilter);
+
+  if (needsAllHits) {
+    auto lessFraction = [](const RayHit& a, const RayHit& b) {
+      return a.mFraction < b.mFraction;
+    };
+
     auto callback = btCollisionWorld::AllHitsRayResultCallback(btFrom, btTo);
     castedGroup->updateEngineData();
     collisionWorld->rayTest(btFrom, btTo, callback);
 
-    if (result == nullptr)
-      return callback.hasHit();
+    if (result == nullptr) {
+      if (!callback.hasHit())
+        return false;
+
+      if (!option.mFilter)
+        return true;
+
+      for (int i = 0; i < callback.m_collisionObjects.size(); ++i) {
+        const auto* collObj = static_cast<BulletCollisionObject*>(
+            callback.m_collisionObjects[i]->getUserPointer());
+        if (option.passesFilter(collObj))
+          return true;
+      }
+
+      return false;
+    }
 
     if (callback.hasHit()) {
       reportRayHits(callback, option, *result);
+
+      if (!option.mEnableAllHits && !result->mRayHits.empty()) {
+        if (option.mSortByClosest) {
+          result->mRayHits.resize(1);
+        } else {
+          const auto closest = std::min_element(
+              result->mRayHits.begin(), result->mRayHits.end(), lessFraction);
+          const RayHit closestHit = *closest;
+          result->mRayHits.clear();
+          result->mRayHits.emplace_back(closestHit);
+        }
+      }
+
       return result->hasHit();
     } else {
       return false;
@@ -765,7 +799,7 @@ RayHit convertRayHit(
 //==============================================================================
 void reportRayHits(
     const btCollisionWorld::ClosestRayResultCallback callback,
-    const RaycastOption& /*option*/,
+    const RaycastOption& option,
     RaycastResult& result)
 {
   // This function shouldn't be called if callback has not ray hit.
@@ -779,7 +813,9 @@ void reportRayHits(
 
   result.mRayHits.clear();
   result.mRayHits.reserve(1);
-  result.mRayHits.emplace_back(rayHit);
+
+  if (option.passesFilter(rayHit.mCollisionObject))
+    result.mRayHits.emplace_back(rayHit);
 }
 
 //==============================================================================
@@ -807,7 +843,8 @@ void reportRayHits(
         callback.m_hitPointWorld[i],
         callback.m_hitNormalWorld[i],
         callback.m_hitFractions[i]);
-    result.mRayHits.emplace_back(rayHit);
+    if (option.passesFilter(rayHit.mCollisionObject))
+      result.mRayHits.emplace_back(rayHit);
   }
 
   if (option.mSortByClosest)

dart/utils/sdf/SdfParser.cpp

@@ -1081,6 +1081,7 @@ SDFBodyNode readBodyNode(
   // Name attribute
   std::string name = getAttributeString(bodyNodeElement, "name");
   properties.mName = name;
+  const std::string bodyName = name;
 
   //--------------------------------------------------------------------------
   // gravity
@@ -1108,13 +1109,33 @@ SDFBodyNode readBodyNode(
 
   //--------------------------------------------------------------------------
   // inertia
+  constexpr double kMinReasonableMass = 1e-9; // 1 microgram
+  bool massSpecified = false;
   if (hasElement(bodyNodeElement, "inertial")) {
     const ElementPtr& inertiaElement = getElement(bodyNodeElement, "inertial");
 
     // mass
     if (hasElement(inertiaElement, "mass")) {
       double mass = getValueDouble(inertiaElement, "mass");
+      if (mass <= 0.0) {
+        DART_WARN(
+            "[SdfParser] Link [{}] has non-positive mass [{}]. Clamping to {} "
+            "to continue parsing.",
+            bodyName,
+            mass,
+            kMinReasonableMass);
+        mass = kMinReasonableMass;
+      } else if (mass < kMinReasonableMass) {
+        DART_WARN(
+            "[SdfParser] Link [{}] has a very small mass [{} kg]; clamping to "
+            "{} to avoid numerical issues.",
+            bodyName,
+            mass,
+            kMinReasonableMass);
+        mass = kMinReasonableMass;
+      }
       properties.mInertia.setMass(mass);
+      massSpecified = true;
     }
 
     // offset
@@ -1137,7 +1158,32 @@ SDFBodyNode readBodyNode(
       double iyz = getValueDouble(moiElement, "iyz");
 
       properties.mInertia.setMoment(ixx, iyy, izz, ixy, ixz, iyz);
+    } else if (massSpecified) {
+      // Keep the inertia physically meaningful by matching the moment scale
+      // to the specified mass; geometry is unknown, so use an isotropic guess.
+      const double mass = properties.mInertia.getMass();
+      properties.mInertia.setMoment(Eigen::Matrix3d::Identity() * mass);
+      DART_WARN(
+          "[SdfParser] Link [{}] defines <mass> but no <inertia>; using an "
+          "isotropic inertia tensor (mass * I). Provide <inertia> for "
+          "physically correct behavior.",
+          bodyName);
     }
+
+    if (!massSpecified) {
+      DART_WARN(
+          "[SdfParser] Link [{}] is missing <mass>; using default mass of 1 "
+          "kg. "
+          "Specify <inertial><mass> to avoid unstable simulation.",
+          bodyName);
+    }
+  } else {
+    DART_WARN(
+        "[SdfParser] Link [{}] is missing <inertial>; using default "
+        "mass/inertia "
+        "(1 kg, unit inertia). Specify <inertial> to avoid unstable "
+        "simulation.",
+        bodyName);
   }
 
   SDFBodyNode sdfBodyNode;

python/stubs/dartpy/collision.pyi

@@ -458,14 +458,20 @@ class RayHit:
 class RaycastOption:
     mEnableAllHits: bool
     mSortByClosest: bool
+    mFilter: typing.Optional[typing.Callable[[CollisionObject], bool]]
     @typing.overload
     def __init__(self) -> None:
         ...
     @typing.overload
     def __init__(self, enableAllHits: bool) -> None:
         ...
     @typing.overload
-    def __init__(self, enableAllHits: bool, sortByClosest: bool) -> None:
+    def __init__(
+        self,
+        enableAllHits: bool,
+        sortByClosest: bool,
+        filter: typing.Optional[typing.Callable[[CollisionObject], bool]] = None,
+    ) -> None:
         ...
 class RaycastResult:
     mRayHits: list[RayHit]