Skip to content

Commit 15c0567

Browse files
jslee02jslee02
authored
Skip non-finite contacts and validate shape dimensions (#3115)
1 parent fab38a3 commit 15c0567

21 files changed

Lines changed: 814 additions & 55 deletions

‎dart/constraint/constraint_solver.cpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -528,6 +528,23 @@ void ConstraintSolver::updateConstraints()
528528
for (auto i = 0u; i < mCollisionResult.getNumContacts(); ++i) {
529529
auto& contact = mCollisionResult.getContact(i);
530530

531+
// Skip contacts with non-finite geometry. A collision shape with an invalid
532+
// (infinite or NaN) dimension, a malformed mesh, or a third-party collision
533+
// backend can report a contact whose point, normal, or penetration depth is
534+
// not finite. Such a contact would otherwise inject NaN/Inf into the
535+
// contact constraint Jacobians, corrupting the LCP solve in release builds
536+
// and tripping an assertion in ContactConstraint in debug builds. See
537+
// gz-physics issue #1010.
538+
if (!contact.point.allFinite() || !contact.normal.allFinite()
539+
|| !std::isfinite(contact.penetrationDepth)) {
540+
DART_WARN(
541+
"[ConstraintSolver] Ignoring contact with non-finite geometry "
542+
"(point, normal, or penetration depth). This usually indicates a "
543+
"malformed collision mesh or a collision backend that produced an "
544+
"invalid contact.");
545+
continue;
546+
}
547+
531548
if (collision::Contact::isZeroNormal(contact.normal)) {
532549
// Skip this contact. This is because we assume that a contact with
533550
// zero-length normal is invalid.

‎dart/dynamics/box_shape.cpp‎

Lines changed: 15 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -32,17 +32,18 @@
3232

3333
#include "dart/dynamics/box_shape.hpp"
3434

35+
#include "dart/common/logging.hpp"
3536
#include "dart/common/macros.hpp"
3637

38+
#include <cmath>
39+
3740
namespace dart {
3841
namespace dynamics {
3942

4043
//==============================================================================
41-
BoxShape::BoxShape(const Eigen::Vector3d& _size) : Shape(BOX), mSize(_size)
44+
BoxShape::BoxShape(const Eigen::Vector3d& _size) : Shape(BOX)
4245
{
43-
DART_ASSERT(_size[0] > 0.0);
44-
DART_ASSERT(_size[1] > 0.0);
45-
DART_ASSERT(_size[2] > 0.0);
46+
setSize(_size);
4647
}
4748

4849
//==============================================================================
@@ -87,9 +88,16 @@ Eigen::Matrix3d BoxShape::computeInertia(
8788
//==============================================================================
8889
void BoxShape::setSize(const Eigen::Vector3d& _size)
8990
{
90-
DART_ASSERT(_size[0] > 0.0);
91-
DART_ASSERT(_size[1] > 0.0);
92-
DART_ASSERT(_size[2] > 0.0);
91+
if (!_size.allFinite() || (_size.array() <= 0.0).any()) {
92+
DART_WARN(
93+
"BoxShape::setSize: Invalid size '[{}, {}, {}]'. Each side length must "
94+
"be a positive finite value. Ignoring request.",
95+
_size[0],
96+
_size[1],
97+
_size[2]);
98+
return;
99+
}
100+
93101
mSize = _size;
94102
mIsBoundingBoxDirty = true;
95103
mIsVolumeDirty = true;

‎dart/dynamics/box_shape.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,7 @@ class DART_API BoxShape : public Shape
8181

8282
private:
8383
/// @brief Side lengths of the box
84-
Eigen::Vector3d mSize;
84+
Eigen::Vector3d mSize{Eigen::Vector3d::Ones()};
8585
};
8686

8787
} // namespace dynamics

‎dart/dynamics/capsule_shape.cpp‎

Lines changed: 20 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232

3333
#include "dart/dynamics/capsule_shape.hpp"
3434

35+
#include "dart/common/logging.hpp"
3536
#include "dart/common/macros.hpp"
3637
#include "dart/dynamics/cylinder_shape.hpp"
3738
#include "dart/dynamics/sphere_shape.hpp"
@@ -43,11 +44,10 @@ namespace dart {
4344
namespace dynamics {
4445

4546
//==============================================================================
46-
CapsuleShape::CapsuleShape(double radius, double height)
47-
: Shape(CAPSULE), mRadius(radius), mHeight(height)
47+
CapsuleShape::CapsuleShape(double radius, double height) : Shape(CAPSULE)
4848
{
49-
DART_ASSERT(0.0 < radius);
50-
DART_ASSERT(0.0 < height);
49+
setRadius(radius);
50+
setHeight(height);
5151
}
5252

5353
//==============================================================================
@@ -72,7 +72,14 @@ double CapsuleShape::getRadius() const
7272
//==============================================================================
7373
void CapsuleShape::setRadius(double radius)
7474
{
75-
DART_ASSERT(0.0 < radius);
75+
if (!std::isfinite(radius) || radius <= 0.0) {
76+
DART_WARN(
77+
"CapsuleShape::setRadius: Invalid radius '{}'. Radius must be a "
78+
"positive finite value. Ignoring request.",
79+
radius);
80+
return;
81+
}
82+
7683
mRadius = radius;
7784
mIsBoundingBoxDirty = true;
7885
mIsVolumeDirty = true;
@@ -89,7 +96,14 @@ double CapsuleShape::getHeight() const
8996
//==============================================================================
9097
void CapsuleShape::setHeight(double height)
9198
{
92-
DART_ASSERT(0.0 < height);
99+
if (!std::isfinite(height) || height <= 0.0) {
100+
DART_WARN(
101+
"CapsuleShape::setHeight: Invalid height '{}'. Height must be a "
102+
"positive finite value. Ignoring request.",
103+
height);
104+
return;
105+
}
106+
93107
mHeight = height;
94108
mIsBoundingBoxDirty = true;
95109
mIsVolumeDirty = true;

‎dart/dynamics/capsule_shape.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -93,10 +93,10 @@ class DART_API CapsuleShape : public Shape
9393

9494
private:
9595
/// Radius of the capsule.
96-
double mRadius;
96+
double mRadius{1.0};
9797

9898
/// Height of the cylindrical part.
99-
double mHeight;
99+
double mHeight{1.0};
100100
};
101101

102102
} // namespace dynamics

‎dart/dynamics/cone_shape.cpp‎

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -32,20 +32,22 @@
3232

3333
#include "dart/dynamics/cone_shape.hpp"
3434

35+
#include "dart/common/logging.hpp"
3536
#include "dart/common/macros.hpp"
3637
#include "dart/dynamics/cylinder_shape.hpp"
3738
#include "dart/dynamics/sphere_shape.hpp"
3839
#include "dart/math/helpers.hpp"
3940

41+
#include <cmath>
42+
4043
namespace dart {
4144
namespace dynamics {
4245

4346
//==============================================================================
44-
ConeShape::ConeShape(double radius, double height)
45-
: Shape(CONE), mRadius(radius), mHeight(height)
47+
ConeShape::ConeShape(double radius, double height) : Shape(CONE)
4648
{
47-
DART_ASSERT(0.0 < radius);
48-
DART_ASSERT(0.0 < height);
49+
setRadius(radius);
50+
setHeight(height);
4951
}
5052

5153
//==============================================================================
@@ -70,7 +72,14 @@ double ConeShape::getRadius() const
7072
//==============================================================================
7173
void ConeShape::setRadius(double radius)
7274
{
73-
DART_ASSERT(0.0 < radius);
75+
if (!std::isfinite(radius) || radius <= 0.0) {
76+
DART_WARN(
77+
"ConeShape::setRadius: Invalid radius '{}'. Radius must be a "
78+
"positive finite value. Ignoring request.",
79+
radius);
80+
return;
81+
}
82+
7483
mRadius = radius;
7584
mIsBoundingBoxDirty = true;
7685
mIsVolumeDirty = true;
@@ -87,7 +96,14 @@ double ConeShape::getHeight() const
8796
//==============================================================================
8897
void ConeShape::setHeight(double height)
8998
{
90-
DART_ASSERT(0.0 < height);
99+
if (!std::isfinite(height) || height <= 0.0) {
100+
DART_WARN(
101+
"ConeShape::setHeight: Invalid height '{}'. Height must be a "
102+
"positive finite value. Ignoring request.",
103+
height);
104+
return;
105+
}
106+
91107
mHeight = height;
92108
mIsBoundingBoxDirty = true;
93109
mIsVolumeDirty = true;

‎dart/dynamics/cone_shape.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -95,10 +95,10 @@ class DART_API ConeShape : public Shape
9595

9696
private:
9797
/// Radius of the circular base.
98-
double mRadius;
98+
double mRadius{1.0};
9999

100100
/// Height of the cylindrical part.
101-
double mHeight;
101+
double mHeight{1.0};
102102
};
103103

104104
} // namespace dynamics

‎dart/dynamics/cylinder_shape.cpp‎

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -32,18 +32,20 @@
3232

3333
#include "dart/dynamics/cylinder_shape.hpp"
3434

35+
#include "dart/common/logging.hpp"
3536
#include "dart/common/macros.hpp"
3637
#include "dart/math/helpers.hpp"
3738

39+
#include <cmath>
40+
3841
namespace dart {
3942
namespace dynamics {
4043

4144
//==============================================================================
42-
CylinderShape::CylinderShape(double _radius, double _height)
43-
: Shape(CYLINDER), mRadius(_radius), mHeight(_height)
45+
CylinderShape::CylinderShape(double _radius, double _height) : Shape(CYLINDER)
4446
{
45-
DART_ASSERT(0.0 < _radius);
46-
DART_ASSERT(0.0 < _height);
47+
setRadius(_radius);
48+
setHeight(_height);
4749
}
4850

4951
//==============================================================================
@@ -68,7 +70,14 @@ double CylinderShape::getRadius() const
6870
//==============================================================================
6971
void CylinderShape::setRadius(double _radius)
7072
{
71-
DART_ASSERT(0.0 < _radius);
73+
if (!std::isfinite(_radius) || _radius <= 0.0) {
74+
DART_WARN(
75+
"CylinderShape::setRadius: Invalid radius '{}'. Radius must be a "
76+
"positive finite value. Ignoring request.",
77+
_radius);
78+
return;
79+
}
80+
7281
mRadius = _radius;
7382
mIsBoundingBoxDirty = true;
7483
mIsVolumeDirty = true;
@@ -85,7 +94,14 @@ double CylinderShape::getHeight() const
8594
//==============================================================================
8695
void CylinderShape::setHeight(double _height)
8796
{
88-
DART_ASSERT(0.0 < _height);
97+
if (!std::isfinite(_height) || _height <= 0.0) {
98+
DART_WARN(
99+
"CylinderShape::setHeight: Invalid height '{}'. Height must be a "
100+
"positive finite value. Ignoring request.",
101+
_height);
102+
return;
103+
}
104+
89105
mHeight = _height;
90106
mIsBoundingBoxDirty = true;
91107
mIsVolumeDirty = true;

‎dart/dynamics/cylinder_shape.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -84,10 +84,10 @@ class DART_API CylinderShape : public Shape
8484

8585
private:
8686
/// @brief
87-
double mRadius;
87+
double mRadius{1.0};
8888

8989
/// @brief Height along z-axis.
90-
double mHeight;
90+
double mHeight{1.0};
9191
};
9292

9393
} // namespace dynamics

‎dart/dynamics/ellipsoid_shape.cpp‎

Lines changed: 13 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -32,9 +32,12 @@
3232

3333
#include "dart/dynamics/ellipsoid_shape.hpp"
3434

35+
#include "dart/common/logging.hpp"
3536
#include "dart/common/macros.hpp"
3637
#include "dart/math/helpers.hpp"
3738

39+
#include <cmath>
40+
3841
namespace dart {
3942
namespace dynamics {
4043

@@ -67,9 +70,15 @@ std::string_view EllipsoidShape::getStaticType()
6770
//==============================================================================
6871
void EllipsoidShape::setDiameters(const Eigen::Vector3d& diameters)
6972
{
70-
DART_ASSERT(diameters[0] > 0.0);
71-
DART_ASSERT(diameters[1] > 0.0);
72-
DART_ASSERT(diameters[2] > 0.0);
73+
if (!diameters.allFinite() || (diameters.array() <= 0.0).any()) {
74+
DART_WARN(
75+
"EllipsoidShape::setDiameters: Invalid diameters '[{}, {}, {}]'. Each "
76+
"diameter must be a positive finite value. Ignoring request.",
77+
diameters[0],
78+
diameters[1],
79+
diameters[2]);
80+
return;
81+
}
7382

7483
mDiameters = diameters;
7584

@@ -88,12 +97,7 @@ const Eigen::Vector3d& EllipsoidShape::getDiameters() const
8897
//==============================================================================
8998
void EllipsoidShape::setRadii(const Eigen::Vector3d& radii)
9099
{
91-
mDiameters = radii * 2.0;
92-
93-
mIsBoundingBoxDirty = true;
94-
mIsVolumeDirty = true;
95-
96-
incrementVersion();
100+
setDiameters(radii * 2.0);
97101
}
98102

99103
//==============================================================================

0 commit comments

Comments
 (0)