Skip to content

Commit

Permalink
Make WindEffects configurable on a location basis (#1357)
Browse files Browse the repository at this point in the history 
Its <force_approximation_scaling_factor> is now a
piecewise scalar field configurable from the SDF file.
Backwards compatibility has been observed.

Signed-off-by: Michel Hidalgo <[email protected]>
  • Loading branch information
@hidmic
hidmic authored Apr 6, 2022
1 parent 360d573 commit a5464eb
Show file tree
Hide file tree
Showing 4 changed files with 451 additions and 16 deletions.
187 changes: 175 additions & 12 deletions src/systems/wind_effects/WindEffects.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,19 @@
#include <ignition/msgs/entity_factory.pb.h>

#include <string>
#include <utility>
#include <vector>

#include <sdf/Root.hh>
#include <sdf/Error.hh>

#include <ignition/common/Profiler.hh>
#include <ignition/math/AdditivelySeparableScalarField3.hh>
#include <ignition/math/PiecewiseScalarField3.hh>
#include <ignition/math/Polynomial3.hh>
#include <ignition/math/Region3.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Vector4.hh>
#include <ignition/msgs/Utility.hh>
#include <ignition/plugin/Register.hh>
#include <ignition/transport/Node.hh>
Expand All @@ -53,6 +60,153 @@ using namespace ignition;
using namespace gazebo;
using namespace systems;

namespace {
using ScalingFactor =
math::AdditivelySeparableScalarField3d<math::Polynomial3d>;
using PiecewiseScalingFactor = math::PiecewiseScalarField3d<ScalingFactor>;

//////////////////////////////////////////////////
ScalingFactor MakeConstantScalingFactor(double _value)
{
return ScalingFactor(
_value / 3.,
math::Polynomial3d::Constant(_value),
math::Polynomial3d::Constant(_value),
math::Polynomial3d::Constant(_value));
}

//////////////////////////////////////////////////
math::Polynomial3d LoadPolynomial3d(const sdf::ElementPtr &_sdf)
{
math::Vector4<double> coeffs;
std::istringstream(_sdf->Get<std::string>()) >> coeffs;
return math::Polynomial3d(std::move(coeffs));
}

//////////////////////////////////////////////////
ScalingFactor LoadScalingFactor(const sdf::ElementPtr &_sdf)
{
if (!_sdf->GetFirstElement())
{
return MakeConstantScalingFactor(_sdf->Get<double>());
}
double k = 1.;
if (_sdf->HasElement("k"))
{
k = _sdf->GetElementImpl("k")->Get<double>();
}
math::Polynomial3d p = math::Polynomial3d::Constant(0.);
if (_sdf->HasElement("px"))
{
p = LoadPolynomial3d(_sdf->GetElementImpl("px"));
}
math::Polynomial3d q = math::Polynomial3d::Constant(0.);
if (_sdf->HasElement("qy"))
{
q = LoadPolynomial3d(_sdf->GetElementImpl("qy"));
}
math::Polynomial3d r = math::Polynomial3d::Constant(0.);
if (_sdf->HasElement("rz"))
{
r = LoadPolynomial3d(_sdf->GetElementImpl("rz"));
}
return ScalingFactor(k, std::move(p), std::move(q), std::move(r));
}

//////////////////////////////////////////////////
math::Intervald
LoadIntervald(const sdf::ElementPtr _sdf, const std::string &_prefix)
{
bool leftClosed = false;
double leftValue = -math::INF_D;
const std::string geAttrName = _prefix + "ge";
const std::string gtAttrName = _prefix + "gt";
if (_sdf->HasAttribute(geAttrName) && _sdf->HasAttribute(gtAttrName))
{
ignerr << "Attributes '" << geAttrName << "' and '" << gtAttrName << "'"
<< " are mutually exclusive. Ignoring both." << std::endl;
}
else if (_sdf->HasAttribute(geAttrName))
{
sdf::ParamPtr sdfGeAttrValue = _sdf->GetAttribute(geAttrName);
if (!sdfGeAttrValue->Get<double>(leftValue))
{
ignerr << "Invalid '" << geAttrName << "' attribute value. "
<< "Ignoring." << std::endl;
}
else
{
leftClosed = true;
}
}
else if (_sdf->HasAttribute(gtAttrName))
{
sdf::ParamPtr sdfGtAttrValue = _sdf->GetAttribute(gtAttrName);
if(!sdfGtAttrValue->Get<double>(leftValue))
{
ignerr << "Invalid '" << gtAttrName << "' attribute value. "
<< "Ignoring." << std::endl;
}
}

bool rightClosed = false;
double rightValue = math::INF_D;
const std::string leAttrName = _prefix + "le";
const std::string ltAttrName = _prefix + "lt";
if (_sdf->HasAttribute(leAttrName) && _sdf->HasAttribute(ltAttrName))
{
ignerr << "Attributes '" << leAttrName << "' and '" << ltAttrName << "'"
<< " are mutually exclusive. Ignoring both." << std::endl;
}
else if (_sdf->HasAttribute(leAttrName))
{
sdf::ParamPtr sdfLeAttrValue = _sdf->GetAttribute(leAttrName);
if (!sdfLeAttrValue->Get<double>(rightValue))
{
ignerr << "Invalid '" << leAttrName << "' attribute value. "
<< "Ignoring." << std::endl;
}
else
{
rightClosed = true;
}
}
else if (_sdf->HasAttribute(ltAttrName))
{
sdf::ParamPtr sdfLtAttrValue = _sdf->GetAttribute(ltAttrName);
if (!sdfLtAttrValue->Get<double>(rightValue))
{
ignerr << "Invalid '" << gtAttrName << "'"
<< "attribute value. Ignoring."
<< std::endl;
}
}

return math::Intervald(leftValue, leftClosed, rightValue, rightClosed);
}

//////////////////////////////////////////////////
PiecewiseScalingFactor LoadPiecewiseScalingFactor(const sdf::ElementPtr _sdf)
{
if (!_sdf->HasElement("when"))
{
return PiecewiseScalingFactor::Throughout(LoadScalingFactor(_sdf));
}
std::vector<PiecewiseScalingFactor::Piece> pieces;
for (sdf::ElementPtr sdfPiece = _sdf->GetElement("when");
sdfPiece; sdfPiece = sdfPiece->GetNextElement("when"))
{
pieces.push_back({
math::Region3d(LoadIntervald(sdfPiece, "x"),
LoadIntervald(sdfPiece, "y"),
LoadIntervald(sdfPiece, "z")),
LoadScalingFactor(sdfPiece),
});
}
return PiecewiseScalingFactor(std::move(pieces));
}
} // namespace

/// \brief Private WindEffects data class.
class ignition::gazebo::systems::WindEffectsPrivate
{
Expand Down Expand Up @@ -134,7 +288,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;
Expand Down Expand Up @@ -281,20 +435,23 @@ void WindEffectsPrivate::Load(EntityComponentManager &_ecm,
{
sdf::ElementPtr sdfForceApprox =
_sdf->GetElementImpl("force_approximation_scaling_factor");

this->forceApproximationScalingFactor = sdfForceApprox->Get<double>();
this->forceApproximationScalingFactor =
LoadPiecewiseScalingFactor(sdfForceApprox);
}
else
{
this->forceApproximationScalingFactor =
PiecewiseScalingFactor::Throughout(MakeConstantScalingFactor(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.Minimum() < 0.)
{
ignerr << "Please set <force_approximation_scaling_factor> to a value "
<< "greater than 0" << std::endl;
ignerr << "<force_approximation_scaling_factor> must "
<< "always be a nonnegative quantity" << std::endl;
return;
}


this->validConfig = true;
}

Expand Down Expand Up @@ -411,12 +568,16 @@ void WindEffectsPrivate::ApplyWindForce(const UpdateInfo &,

Link link;

_ecm.Each<components::Link, components::Inertial, components::WindMode,
_ecm.Each<components::Link,
components::Inertial,
components::WindMode,
components::WorldPose,
components::WorldLinearVelocity>(
[&](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
Expand All @@ -427,9 +588,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);
Expand Down
45 changes: 42 additions & 3 deletions src/systems/wind_effects/WindEffects.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,8 @@ namespace systems
class WindEffectsPrivate;

/// \brief A system that simulates a simple wind model.
/// The wind is described as a uniform worldwide model. So it is independent
/// from model position for simple computations. Its components are computed
/// separately:
/// The wind is described as a uniform worldwide model.
/// Its components are computed separately:
/// - Horizontal amplitude:
/// Low pass filtering on user input (complementary gain)
/// + small local fluctuations
Expand All @@ -51,6 +50,12 @@ namespace systems
/// + small local fluctuations
/// + noise on value
///
/// Forces exerted by the wind on model links are approximated from
/// link mass and velocity with respect to wind velocity, and applied
/// to the link frame origin. These approximations can be amplified or
/// attenuated on a per location basis by specifying a piecewise scalar
/// field for a scaling factor.
///
/// The following parameters are used by the system:
///
/// - `<horizontal><magnitude><time_for_rise>`
Expand Down Expand Up @@ -86,6 +91,40 @@ namespace systems
/// - `<vertical><noise>`
/// Parameters for the noise that is added to the vertical wind velocity
/// magnitude.
///
/// - `<force_approximation_scaling_factor>`
/// Proportionality constant used for wind force approximations as a
/// piecewise, separable scalar field:
/// \verbatim
/// <force_approximation_scaling_factor>
/// <when xlt="0"> <!-- Half space where x < 0 -->
/// <k>1</k>
/// <px>0 0 0 1</px> <!-- p(x) = x -->
/// <qy>0 0 0 1</qy> <!-- q(y) = 1 -->
/// <rz>0 1 0 1</rz> <!-- r(z) = z^2 -->
/// </when>
/// </force_approximation_scaling_factor>
/// \endverbatim
/// When the scaling factor is to be constant in a region, a numerical
/// constant may be used in place for the scalar field definition:
/// \verbatim
/// <force_approximation_scaling_factor>
/// <!-- First octant -->
/// <when xge="0" yge="0" zge="0">1</when>
/// </force_approximation_scaling_factor>
/// \endverbatim
/// To use the same constant or scalar field in all space, region
/// definition may be dropped:
/// \verbatim
/// <force_approximation_scaling_factor>
/// <k>2</k>
/// <px>1 0 1 0</px> <!-- p(x) = x^3 + x -->
/// <qy>0 1 0 1</qy> <!-- q(y) = x^2 + 1 -->
/// <rz>1 0 1 0</rz> <!-- r(z) = z^3 + z -->
/// </force_approximation_scaling_factor>
/// \endverbatim
/// Regions may not overlap.
///
class WindEffects final:
public System,
public ISystemConfigure,
Expand Down
43 changes: 42 additions & 1 deletion test/integration/wind_effects.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
{
Expand Down Expand Up @@ -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<components::WorldLinearAcceleration>
belowSurfaceAccelerations("box_below_surface", true);
LinkComponentRecorder<components::WorldLinearAcceleration>
aboveSurfaceAccelerations("box_above_surface", true);
LinkComponentRecorder<components::WorldLinearAcceleration>
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))
{
Expand Down
Loading

0 comments on commit a5464eb

Please sign in to comment.