diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 5389ba9c17a..1f1ba7e9d80 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -28,6 +28,9 @@ #include #include +#include +#include +#include #include #include #include @@ -53,6 +56,428 @@ using namespace ignition; using namespace gazebo; using namespace systems; +namespace { + +template +class Polynomial3 +{ + public: Polynomial3() = default; + + public: Polynomial3(math::Vector4 _coeffs) + : coeffs(std::move(_coeffs)) + { + } + + public: static Polynomial3 Constant(const T &_coeff) + { + return Polynomial3{{T{0.}, T{0.}, T{0.}, _coeff}}; + }; + + public: static Polynomial3 Load(const sdf::ElementPtr &_sdf) + { + math::Vector4 coeffs; + std::istringstream{_sdf->Get()} >> coeffs; + return Polynomial3{std::move(coeffs)}; + } + + public: T Evaluate(const T &_t) const + { + const T _t2 = _t * _t; + const T _t3 = _t2 * _t; + + return coeffs[0] * _t3 + coeffs[2] * _t2 + coeffs[1] * _t + coeffs[3]; + } + + public: T operator()(const T &_t) const + { + return this->Evaluate(_t); + } + + public: T Minimum(const T &_tmin, const T &_tmax, bool open = false) const + { + constexpr T epsilon = std::numeric_limits::epsilon(); + if (open) + { + // for open intervals, assume continuity in the limit + return Minimum(_tmin + epsilon, _tmax - epsilon); + } + using std::min; + T minimum = min(this->Evaluate(_tmin), this->Evaluate(_tmax)); + if (std::abs(coeffs[0]) < epsilon) + { + // cubic poylnomial + const T a = coeffs[0] * T{3}; + const T b = coeffs[1] * T{2.}; + const T c = coeffs[2]; + + const T discriminant = b * b - T{4.} * a * c; + if (discriminant >= T{0.}) + { + using std::sqrt; + const T t0 = (-b + sqrt(discriminant)) / T{2.} * a; + if (_tmin <= t0 && t0 <= _tmax) + { + minimum = min(this->Evaluate(t0), minimum); + } + const T t1 = (-b - sqrt(discriminant)) / T{2.} * a; + if (_tmin <= t1 && t1 <= _tmax) + { + minimum = min(this->Evaluate(t1), minimum); + } + } + } + else if (std::abs(coeffs[1]) < epsilon) + { + // quadratic polynomial + const T b = coeffs[1] * T{2.}; + const T c = coeffs[2]; + + const T t0 = -c / b; + if (_tmin <= t0 && t0 <= _tmax) + { + minimum = min(this->Evaluate(t0), minimum); + } + } + return minimum; + } + + private: math::Vector4 coeffs; +}; + +using Polynomial3d = Polynomial3; + +class Region3d +{ + public: Region3d(math::AxisAlignedBox _volume, bool _open = false) + : volume(std::move(_volume)), open(_open) + { + } + + public: const math::AxisAlignedBox &Volume() const { return volume; } + + public: bool Open() const { return open; } + + public: bool Contains(const math::Vector3d &_v) const + { + if (Open()) + { + return (_v.X() > Volume().Min().X() && _v.X() < Volume().Max().X() && + _v.Y() > Volume().Min().Y() && _v.Y() < Volume().Max().Y() && + _v.Z() > Volume().Min().Z() && _v.Z() < Volume().Max().Z()); + } + return (_v.X() >= Volume().Min().X() && _v.X() <= Volume().Max().X() && + _v.Y() >= Volume().Min().Y() && _v.Y() <= Volume().Max().Y() && + _v.Z() >= Volume().Min().Z() && _v.Z() <= Volume().Max().Z()); + } + + bool Intersects(const Region3d &_other) const + { + if (Open() || _other.Open()) + { + if (Volume().Max().X() <= _other.Volume().Min().X()) + return false; + if (Volume().Max().Y() <= _other.Volume().Min().Y()) + return false; + if (Volume().Max().Z() <= _other.Volume().Min().Z()) + return false; + if (Volume().Min().X() >= _other.Volume().Max().X()) + return false; + if (Volume().Min().Y() >= _other.Volume().Max().Y()) + return false; + if (Volume().Min().Z() >= _other.Volume().Max().Z()) + return false; + return true; + } + return Volume().Intersects(_other.Volume()); + } + + private: math::AxisAlignedBox volume; + + private: bool open; +}; + +std::ostream &operator<<(std::ostream &_out, const Region3d &_region) +{ + const char * lbound = _region.Open() ? "(" : "["; + const char * rbound = _region.Open() ? ")" : "]"; + + _out << "Min" << lbound << _region.Volume().Min() << rbound << " " + << "Max" << lbound << _region.Volume().Max() << rbound; + return _out; +} + +template +class SeparableScalarField3d +{ + public: SeparableScalarField3d() = default; + + public: SeparableScalarField3d( + double _k, FunctionT &&_p, FunctionT &&_q, FunctionT &&_r) + : k(_k), p(std::move(_p)), q(std::move(_q)), r(std::move(_r)) + { + } + + public: static SeparableScalarField3d Constant(double _k) + { + return SeparableScalarField3d{ + _k, + FunctionT::Constant(1.), + FunctionT::Constant(1.), + FunctionT::Constant(1.)}; + } + + public: static SeparableScalarField3d Load(const sdf::ElementPtr &_sdf) + { + if (!_sdf->GetFirstElement()) + { + return Constant(_sdf->Get()); + } + double k = 1.; + if (_sdf->HasElement("k")) + { + k = _sdf->GetElementImpl("k")->Get(); + } + FunctionT p = FunctionT::Constant(1.); + if (_sdf->HasElement("px")) + { + p = FunctionT::Load(_sdf->GetElementImpl("px")); + } + FunctionT q = FunctionT::Constant(1.); + if (_sdf->HasElement("qx")) + { + q = FunctionT::Load(_sdf->GetElementImpl("qx")); + } + FunctionT r = FunctionT::Constant(1.); + if (_sdf->HasElement("rx")) + { + r = FunctionT::Load(_sdf->GetElementImpl("rx")); + } + return SeparableScalarField3d{ + k, std::move(p), std::move(q), std::move(r)}; + } + + public: double Evaluate(const math::Vector3d &_v) const + { + return k * p(_v.X()) * q(_v.Y()) * r(_v.Z()); + } + + public: double operator()(const math::Vector3d &_v) const + { + return this->Evaluate(_v); + } + + public: double Minimum(const Region3d &_region) const + { + const math::Vector3d rmin = _region.Volume().Min(); + const math::Vector3d rmax = _region.Volume().Max(); + const bool ropen = _region.Open(); + + return k * (p.Minimum(rmin.X(), rmax.X(), ropen) * + q.Minimum(rmin.Y(), rmax.Y(), ropen) * + r.Minimum(rmin.Z(), rmax.Z(), ropen)); + } + + private: double k; + + private: FunctionT p; + + private: FunctionT q; + + private: FunctionT r; +}; + +template +class PiecewiseScalarField3d +{ + public: struct Piece { + Region3d region; + ScalarField3dT field; + }; + + public: PiecewiseScalarField3d() = default; + + public: PiecewiseScalarField3d(std::vector _pieces) + : pieces(std::move(_pieces)) + { + for (size_t i = 0; i < pieces.size(); ++i) + { + for (size_t j = i + 1; j < pieces.size(); ++j) + { + if (pieces[i].region.Intersects(pieces[j].region)) + { + ignwarn << "Detected overlap between regions in " + << "piecewise scalar field: #" << i + << pieces[i].region << " overlaps with #" + << j << " " << pieces[j].region << ". " + << "#" << i << " will take precedence " + << "when overlapping." << std::endl; + } + } + } + } + + public: static PiecewiseScalarField3d Constant(double _c) + { + return PiecewiseScalarField3d{{{ + Region3d{{ + math::LOW_D, math::LOW_D, math::LOW_D, + math::MAX_D, math::MAX_D, math::MAX_D}}, + ScalarField3dT::Constant(_c), + }}}; + } + + public: static PiecewiseScalarField3d Load(const sdf::ElementPtr _sdf) + { + if (!_sdf->HasElement("when")) + { + return Constant(_sdf->Get()); + } + std::vector pieces; + for (sdf::ElementPtr sdfPiece = _sdf->GetElement("when"); + sdfPiece; sdfPiece = sdfPiece->GetNextElement("when")) + { + double xabove = math::LOW_D; + if (sdfPiece->HasAttribute("xabove")) + { + sdf::ParamPtr sdfXAbove = + sdfPiece->GetAttribute("xabove"); + if(!sdfXAbove->Get(xabove)) + { + ignerr << "Invalid xabove attribute value. " + << "Falling back to default: " << xabove + << std::endl; + } + } + double xbelow = math::MAX_D; + if (sdfPiece->HasAttribute("xbelow")) + { + sdf::ParamPtr sdfXBelow = + sdfPiece->GetAttribute("xbelow"); + if (!sdfXBelow->Get(xbelow)) + { + ignerr << "Invalid xbelow attribute value. " + << "Falling back to default: " << xbelow + << std::endl; + } + } + double yabove = math::LOW_D; + if (sdfPiece->HasAttribute("yabove")) + { + sdf::ParamPtr sdfYAbove = + sdfPiece->GetAttribute("yabove"); + if (!sdfYAbove->Get(yabove)) + { + ignerr << "Invalid yabove attribute value. " + << "Falling back to default: " << yabove + << std::endl; + } + } + double ybelow = math::MAX_D; + if (sdfPiece->HasAttribute("ybelow")) + { + sdf::ParamPtr sdfYBelow = + sdfPiece->GetAttribute("ybelow"); + if (!sdfYBelow->Get(ybelow)) + { + ignerr << "Invalid ybelow attribute value. " + << "Falling back to default: " << ybelow + << std::endl; + } + } + double zabove = math::LOW_D; + if (sdfPiece->HasAttribute("zabove")) + { + sdf::ParamPtr sdfZAbove = + sdfPiece->GetAttribute("zabove"); + if (!sdfZAbove->Get(zabove)) + { + ignerr << "Invalid zabove attribute value. " + << "Falling back to default: " << zabove + << std::endl; + } + } + double zbelow = math::MAX_D; + if (sdfPiece->HasAttribute("zbelow")) + { + sdf::ParamPtr sdfZBelow = + sdfPiece->GetAttribute("zbelow"); + if (!sdfZBelow->Get(zbelow)) + { + ignerr << "Invalid zbelow attribute value. " + << "Falling back to default: " << zbelow + << std::endl; + } + } + bool open = true; + if (sdfPiece->HasAttribute("bounds")) + { + sdf::ParamPtr sdfBounds = + sdfPiece->GetAttribute("bounds"); + + std::string bounds; + if (!sdfBounds->Get(bounds)) { + ignerr << "Invalid bounds attribute value. " + << "Falling back to default: 'exclude'" + << std::endl; + } + else if (bounds == "include") + { + open = false; + } + else if (bounds != "exclude") + { + ignerr << "Invalid bounds attribute value: " << bounds << "." + << "Valid attribute values are 'exclude' and 'include'. " + << "Falling back to default: 'exclude'" << std::endl; + } + } + pieces.push_back({ + Region3d{ + math::AxisAlignedBox{ + xabove, yabove, zabove, + xbelow, ybelow, zbelow}, + open}, + ScalarField3dT::Load(sdfPiece), + }); + } + return PiecewiseScalarField3d(std::move(pieces)); + } + + public: double Evaluate(const math::Vector3d &_v) const + { + for (const Piece &piece : pieces) { + if (piece.region.Contains(_v)) { + return piece.field(_v); + } + } + return 0.; + } + + public: double operator()(const math::Vector3d &_v) const { + return this->Evaluate(_v); + } + + public: bool IsNonNegative() const + { + for (const Piece &piece : pieces) + { + if (piece.field.Minimum(piece.region) < 0.) + { + return false; + } + } + return true; + } + + private: std::vector pieces; +}; + +using ScalingFactor = SeparableScalarField3d; + +using PiecewiseScalingFactor = PiecewiseScalarField3d; + +} // namespace + /// \brief Private WindEffects data class. class ignition::gazebo::systems::WindEffectsPrivate { @@ -134,7 +559,7 @@ class ignition::gazebo::systems::WindEffectsPrivate public: double magnitudeMeanVertical{0.0}; /// \brief The scaling factor to approximate wind as force on a mass. - public: double forceApproximationScalingFactor{1.0}; + public: PiecewiseScalingFactor forceApproximationScalingFactor; /// \brief Noise added to magnitude. public: sensors::NoisePtr noiseMagnitude; @@ -281,20 +706,23 @@ void WindEffectsPrivate::Load(EntityComponentManager &_ecm, { sdf::ElementPtr sdfForceApprox = _sdf->GetElementImpl("force_approximation_scaling_factor"); - - this->forceApproximationScalingFactor = sdfForceApprox->Get(); + this->forceApproximationScalingFactor = + PiecewiseScalingFactor::Load(sdfForceApprox); + } + else + { + this->forceApproximationScalingFactor = + PiecewiseScalingFactor::Constant(1.); } - // If the forceApproximationScalingFactor is very small don't update. // It doesn't make sense to be negative, that would be negative wind drag. - if (std::fabs(this->forceApproximationScalingFactor) < 1e-6) + if (!this->forceApproximationScalingFactor.IsNonNegative()) { - ignerr << "Please set to a value " - << "greater than 0" << std::endl; + ignerr << " must " + << "always be a nonnegative quantity" << std::endl; return; } - this->validConfig = true; } @@ -411,12 +839,16 @@ void WindEffectsPrivate::ApplyWindForce(const UpdateInfo &, Link link; - _ecm.Each( [&](const Entity &_entity, components::Link *, components::Inertial *_inertial, components::WindMode *_windMode, + components::WorldPose *_linkPose, components::WorldLinearVelocity *_linkVel) -> bool { // Skip links for which the wind is disabled @@ -427,9 +859,11 @@ void WindEffectsPrivate::ApplyWindForce(const UpdateInfo &, link.ResetEntity(_entity); - math::Vector3d windForce = _inertial->Data().MassMatrix().Mass() * - this->forceApproximationScalingFactor * - (windVel->Data() - _linkVel->Data()); + const math::Vector3d windForce = + _inertial->Data().MassMatrix().Mass() * + this->forceApproximationScalingFactor( + _linkPose->Data().Pos()) * + (windVel->Data() - _linkVel->Data()); // Apply force at center of mass link.AddWorldForce(_ecm, windForce); diff --git a/test/integration/wind_effects.cc b/test/integration/wind_effects.cc index ac30ffe240c..695871f5e2b 100644 --- a/test/integration/wind_effects.cc +++ b/test/integration/wind_effects.cc @@ -218,7 +218,6 @@ TEST_F(WindEffectsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(WindEnabledInLink)) EXPECT_TRUE(linkWindMode.values.back().Data()); } - //////////////////////////////////////////////// TEST_F(WindEffectsTest , IGN_UTILS_TEST_DISABLED_ON_WIN32(WindForce)) { @@ -256,6 +255,48 @@ TEST_F(WindEffectsTest , IGN_UTILS_TEST_DISABLED_ON_WIN32(WindForce)) } } +//////////////////////////////////////////////// +TEST_F(WindEffectsTest , IGN_UTILS_TEST_DISABLED_ON_WIN32(ComplexWindForce)) +{ + this->StartServer("/test/worlds/sea_storm_effects.sdf"); + LinkComponentRecorder + belowSurfaceAccelerations("box_below_surface", true); + LinkComponentRecorder + aboveSurfaceAccelerations("box_above_surface", true); + LinkComponentRecorder + upHighAccelerations("box_up_high", true); + + using namespace std::chrono_literals; + this->server->SetUpdatePeriod(0ns); + + this->server->AddSystem(belowSurfaceAccelerations.systemPtr); + this->server->AddSystem(aboveSurfaceAccelerations.systemPtr); + this->server->AddSystem(upHighAccelerations.systemPtr); + + const std::size_t nIters{3000}; + this->server->Run(true, nIters, false); + + ASSERT_EQ(nIters, belowSurfaceAccelerations.values.size()); + ASSERT_EQ(nIters, aboveSurfaceAccelerations.values.size()); + ASSERT_EQ(nIters, upHighAccelerations.values.size()); + + double maxAboveSurfaceAccelMagnitude = 0.; + for (std::size_t i = 0; i < nIters; ++i) + { + const double belowSurfaceAccelMagnitude = + belowSurfaceAccelerations.values[i].Data().Length(); + const double aboveSurfaceAccelMagnitude = + aboveSurfaceAccelerations.values[i].Data().Length(); + const double upHighAccelMagnitude = + upHighAccelerations.values[i].Data().Length(); + maxAboveSurfaceAccelMagnitude = std::max( + maxAboveSurfaceAccelMagnitude, aboveSurfaceAccelMagnitude); + EXPECT_LE(aboveSurfaceAccelMagnitude, upHighAccelMagnitude); + EXPECT_LT(belowSurfaceAccelMagnitude, 1e-6); + } + EXPECT_GT(maxAboveSurfaceAccelMagnitude, 1e-6); +} + //////////////////////////////////////////////// TEST_F(WindEffectsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TopicsAndServices)) { diff --git a/test/worlds/sea_storm_effects.sdf b/test/worlds/sea_storm_effects.sdf new file mode 100644 index 00000000000..869c6051412 --- /dev/null +++ b/test/worlds/sea_storm_effects.sdf @@ -0,0 +1,193 @@ + + + + 0 0 0 + + + 0 + + + + + + + + + + + 0.5 + + + 0.5 + 0 0.01 0 1 + 0 0.01 0 1 + 0 0 0.01 0.45 + + + + + 1 + + 0.05 + 60 + + + + 1 + + 5 + 20 + + + + + + + 50 50 0 + + + + 0 0 -5 0 0 0 + true + + 0 0 1 0 0 0 + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + 0.0 + 0.0 + + + + + + + + 2.0 2.0 2.0 + + + + 1.0 0.0 0.0 1 + 1.0 0.0 0.0 1 + 1.0 0.0 0.0 1 + + + + + + + 0 0 1 0 0 0 + + true + 0 0 1 0 0 0 + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + 0.0 + 0.0 + + + + + + + + 2.0 2.0 2.0 + + + + 1.0 0.0 0.0 1 + 1.0 0.0 0.0 1 + 1.0 0.0 0.0 1 + + + + + + + 50 50 20 0 0 0 + + true + 0 0 1 0 0 0 + + 1 + + 0.667 + 0 + 0 + 0.667 + 0 + 0.667 + + + + + + 2.0 2.0 2.0 + + + + + + 0.0 + 0.0 + + + + + + + + 2.0 2.0 2.0 + + + + 1.0 0.0 0.0 1 + 1.0 0.0 0.0 1 + 1.0 0.0 0.0 1 + + + + + +