From 89eaf90dcc00f476edb5c8eada30c12a24eff827 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 7 Dec 2023 01:14:55 -0800 Subject: [PATCH] Add lateral nonlinear slip parameters as well * Refactor the data definitions and parsing method to reduce duplication. * Refine some unit vector calculations in ODEPhysics as well. Signed-off-by: Steve Peters --- gazebo/physics/ode/ODECollision.cc | 228 ++++++++++-------- gazebo/physics/ode/ODECollision.hh | 47 ++-- gazebo/physics/ode/ODEPhysics.cc | 87 ++++++- .../model.sdf | 54 +++++ 4 files changed, 281 insertions(+), 135 deletions(-) diff --git a/gazebo/physics/ode/ODECollision.cc b/gazebo/physics/ode/ODECollision.cc index 4a27e99008..5f22bfabcf 100644 --- a/gazebo/physics/ode/ODECollision.cc +++ b/gazebo/physics/ode/ODECollision.cc @@ -195,140 +195,158 @@ bool ODECollision::ParseWheelPlowingParams( } // Parse nonlinear slip parameters - const std::string kNonlinearSlip = "nonlinear_slip"; - if (wheelElem->HasElement(kNonlinearSlip)) + auto parseNonlinearSlipParams = [&]( + const std::string &_nonlinearSlipElementName, + ODECollisionWheelPlowingParams::NonlinearSlipParams &_nonlinearParams) + -> bool { - auto parse_degrees_perDegrees = - [](const std::string &_elementName, sdf::ElementPtr _nonlinearSlipElem, - std::vector &_parsedDegrees, - std::vector &_parsedPerDegrees) -> void - { - if (_nonlinearSlipElem->HasElement(_elementName)) + if (wheelElem->HasElement(_nonlinearSlipElementName)) + { + auto parse_degrees_perDegrees = + [](const std::string &_elementName, sdf::ElementPtr _nonlinearSlipElem, + std::vector &_parsedDegrees, + std::vector &_parsedPerDegrees) -> void { - sdf::ElementPtr elem = _nonlinearSlipElem->GetElement(_elementName); - while (elem) + if (_nonlinearSlipElem->HasElement(_elementName)) { - _parsedDegrees.push_back(elem->Get("degree")); - _parsedPerDegrees.push_back(elem->Get("per_degree")); - elem = elem->GetNextElement(_elementName); + sdf::ElementPtr elem = _nonlinearSlipElem->GetElement(_elementName); + while (elem) + { + _parsedDegrees.push_back(elem->Get("degree")); + _parsedPerDegrees.push_back(elem->Get("per_degree")); + elem = elem->GetNextElement(_elementName); + } } - } - }; + }; - sdf::ElementPtr nonlinearSlipElem = wheelElem->GetElement(kNonlinearSlip); - if (nonlinearSlipElem->HasElement("lower")) - { - // In XML, all //lower/degree and //upper/degree values should be in - // ascending order, but the //lower/degree values should be in - // descending order in the c++ data structures. So parse them first - // and then iterate over the values in reverse order. - std::vector parsedDegrees; - std::vector parsedPerDegrees; - parse_degrees_perDegrees( - "lower", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); - // - std::optional lastDegree, lastPerDegree; - double multiplier = 1.0; - for (std::size_t i = 0; i < parsedDegrees.size(); ++i) + sdf::ElementPtr nonlinearSlipElem = + wheelElem->GetElement(_nonlinearSlipElementName); + if (nonlinearSlipElem->HasElement("lower")) { - // reversed index - std::size_t r = parsedDegrees.size() - 1 - i; - double degree = parsedDegrees[r]; - double perDegree = parsedPerDegrees[r]; - - if (!lastDegree) - { - // This is the first pair of parameters. - // Replace the first vector values. - _plowing.nonlinearSlipLowerDegreesMultipliers[0] = {degree, 1.0}; - _plowing.nonlinearSlipLowerPerDegrees[0] = perDegree; - } - else + // In XML, all //lower/degree and //upper/degree values should be in + // ascending order, but the //lower/degree values should be in + // descending order in the c++ data structures. So parse them first + // and then iterate over the values in reverse order. + std::vector parsedDegrees; + std::vector parsedPerDegrees; + parse_degrees_perDegrees( + "lower", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); + // + std::optional lastDegree, lastPerDegree; + double multiplier = 1.0; + for (std::size_t i = 0; i < parsedDegrees.size(); ++i) { - // Verify that slope values are in order - if (degree >= *lastDegree) + // reversed index + std::size_t r = parsedDegrees.size() - 1 - i; + double degree = parsedDegrees[r]; + double perDegree = parsedPerDegrees[r]; + + if (!lastDegree) + { + // This is the first pair of parameters. + // Replace the first vector values. + _nonlinearParams.lowerDegreesMultipliers[0] = {degree, 1.0}; + _nonlinearParams.lowerPerDegrees[0] = perDegree; + } + else { - if (verbose) + // Verify that slope values are in order + if (degree >= *lastDegree) { - gzerr << "Element <" << kPlowingWheel << "> in collision with name [" - << _scopedNameForErrorMessages << "] has a //nonlinear_slip/lower " - << "parameter with unsorted values: " - << degree << " should be less than " << *lastDegree - << std::endl; + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a //nonlinear_slip/lower " + << "parameter with unsorted values: " + << degree << " should be less than " << *lastDegree + << std::endl; + } + return false; } - return false; + double degreesBelowThreshold = *lastDegree - degree; + multiplier += *lastPerDegree * degreesBelowThreshold; + _nonlinearParams.lowerDegreesMultipliers.push_back( + {degree, multiplier}); + _nonlinearParams.lowerPerDegrees.push_back(perDegree); } - double degreesBelowThreshold = *lastDegree - degree; - multiplier += *lastPerDegree * degreesBelowThreshold; - _plowing.nonlinearSlipLowerDegreesMultipliers.push_back( - {degree, multiplier}); - _plowing.nonlinearSlipLowerPerDegrees.push_back(perDegree); + lastDegree = degree; + lastPerDegree = perDegree; } - lastDegree = degree; - lastPerDegree = perDegree; } - } - if (nonlinearSlipElem->HasElement("upper")) - { - std::vector parsedDegrees; - std::vector parsedPerDegrees; - parse_degrees_perDegrees( - "upper", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); - - std::optional lastDegree, lastPerDegree; - double multiplier = 1.0; - for (std::size_t i = 0; i < parsedDegrees.size(); ++i) + if (nonlinearSlipElem->HasElement("upper")) { - double degree = parsedDegrees[i]; - double perDegree = parsedPerDegrees[i]; - - if (!lastDegree) - { - // This is the first pair of parameters. - // Replace the first vector values. - _plowing.nonlinearSlipUpperDegreesMultipliers[0] = {degree, 1.0}; - _plowing.nonlinearSlipUpperPerDegrees[0] = perDegree; - } - else + std::vector parsedDegrees; + std::vector parsedPerDegrees; + parse_degrees_perDegrees( + "upper", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); + + std::optional lastDegree, lastPerDegree; + double multiplier = 1.0; + for (std::size_t i = 0; i < parsedDegrees.size(); ++i) { - // Verify that slope values are in order - if (degree <= *lastDegree) + double degree = parsedDegrees[i]; + double perDegree = parsedPerDegrees[i]; + + if (!lastDegree) + { + // This is the first pair of parameters. + // Replace the first vector values. + _nonlinearParams.upperDegreesMultipliers[0] = {degree, 1.0}; + _nonlinearParams.upperPerDegrees[0] = perDegree; + } + else { - if (verbose) + // Verify that slope values are in order + if (degree <= *lastDegree) { - gzerr << "Element <" << kPlowingWheel << "> in collision with name [" - << _scopedNameForErrorMessages << "] has a //nonlinear_slip/upper " - << "parameter with unsorted values: " - << degree << " should be greater than " << *lastDegree - << std::endl; + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a //nonlinear_slip/upper " + << "parameter with unsorted values: " + << degree << " should be greater than " << *lastDegree + << std::endl; + } + return false; } - return false; + double degreesAboveThreshold = degree - *lastDegree; + multiplier += *lastPerDegree * degreesAboveThreshold; + _nonlinearParams.upperDegreesMultipliers.push_back( + {degree, multiplier}); + _nonlinearParams.upperPerDegrees.push_back(perDegree); } - double degreesAboveThreshold = degree - *lastDegree; - multiplier += *lastPerDegree * degreesAboveThreshold; - _plowing.nonlinearSlipUpperDegreesMultipliers.push_back( - {degree, multiplier}); - _plowing.nonlinearSlipUpperPerDegrees.push_back(perDegree); + lastDegree = degree; + lastPerDegree = perDegree; } - lastDegree = degree; - lastPerDegree = perDegree; } } + return true; + }; + bool longitudinalParseResult, lateralParseResult; + longitudinalParseResult = parseNonlinearSlipParams( + "nonlinear_slip", _plowing.longitudinalNonlinearSlipParams); + lateralParseResult = parseNonlinearSlipParams( + "nonlinear_lateral_slip", _plowing.lateralNonlinearSlipParams); + + if (!longitudinalParseResult || !lateralParseResult) + { + return false; } if (verbose) { + const auto &nonlinearParams = _plowing.longitudinalNonlinearSlipParams; gzdbg << "Plowing params for " << _scopedNameForErrorMessages << ":\n" << " Lower:\n" - << " " << _plowing.nonlinearSlipLowerDegreesMultipliers.back().X() << " deg" - << ", " << _plowing.nonlinearSlipLowerDegreesMultipliers.back().Y() << '\n' - << " " << _plowing.nonlinearSlipLowerDegreesMultipliers.front().X() << " deg" - << ", " << _plowing.nonlinearSlipLowerDegreesMultipliers.front().Y() << '\n' + << " " << nonlinearParams.lowerDegreesMultipliers.back().X() << " deg" + << ", " << nonlinearParams.lowerDegreesMultipliers.back().Y() << '\n' + << " " << nonlinearParams.lowerDegreesMultipliers.front().X() << " deg" + << ", " << nonlinearParams.lowerDegreesMultipliers.front().Y() << '\n' << " Upper:\n" - << " " << _plowing.nonlinearSlipUpperDegreesMultipliers.front().X() << " deg" - << ", " << _plowing.nonlinearSlipUpperDegreesMultipliers.front().Y() << '\n' - << " " << _plowing.nonlinearSlipUpperDegreesMultipliers.back().X() << " deg" - << ", " << _plowing.nonlinearSlipUpperDegreesMultipliers.back().Y() << '\n' + << " " << nonlinearParams.upperDegreesMultipliers.front().X() << " deg" + << ", " << nonlinearParams.upperDegreesMultipliers.front().Y() << '\n' + << " " << nonlinearParams.upperDegreesMultipliers.back().X() << " deg" + << ", " << nonlinearParams.upperDegreesMultipliers.back().Y() << '\n' << std::endl; } diff --git a/gazebo/physics/ode/ODECollision.hh b/gazebo/physics/ode/ODECollision.hh index c50a096158..506fba5f6f 100644 --- a/gazebo/physics/ode/ODECollision.hh +++ b/gazebo/physics/ode/ODECollision.hh @@ -133,25 +133,34 @@ namespace gazebo /// order, such that each //lower/degree value is larger than the /// previous value. - /// \brief The pairs of slope in degrees and slip multipliers that - /// define the piecewise linear behavior for the lower slope values. - public: std::vector - nonlinearSlipLowerDegreesMultipliers{{-361.0, 1.0}}; - - /// \brief The pairs of slope in degrees and slip multipliers that - /// define the piecewise linear behavior for the upper slope values. - public: std::vector - nonlinearSlipUpperDegreesMultipliers{{361.0, 1.0}}; - - /// \brief The rates of change in slip compliance multiplier per degree - /// of slope in slope below the lower degrees[i] value for each - /// piecewise linear segment. - public: std::vector nonlinearSlipLowerPerDegrees{0.0}; - - /// \brief The rates of change in slip compliance multiplier per degree - /// of slope in slope above the upper degrees[i] value for each - /// piecewise linear segment. - public: std::vector nonlinearSlipUpperPerDegrees{0.0}; + class NonlinearSlipParams + { + /// \brief The pairs of slope in degrees and slip multipliers that + /// define the piecewise linear behavior for the lower slope values. + public: std::vector + lowerDegreesMultipliers{{-361.0, 1.0}}; + + /// \brief The pairs of slope in degrees and slip multipliers that + /// define the piecewise linear behavior for the upper slope values. + public: std::vector + upperDegreesMultipliers{{361.0, 1.0}}; + + /// \brief The rates of change in slip compliance multiplier per degree + /// of slope in slope below the lower degrees[i] value for each + /// piecewise linear segment. + public: std::vector lowerPerDegrees{0.0}; + + /// \brief The rates of change in slip compliance multiplier per degree + /// of slope in slope above the upper degrees[i] value for each + /// piecewise linear segment. + public: std::vector upperPerDegrees{0.0}; + }; + + /// \brief Nonlinear parameters for longitudinal wheel slip. + public: NonlinearSlipParams longitudinalNonlinearSlipParams; + + /// \brief Nonlinear parameters for lateral wheel slip. + public: NonlinearSlipParams lateralNonlinearSlipParams; }; /// \brief Base class for all ODE collisions. diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc index 2f3bcc0dc4..e023a3e8af 100644 --- a/gazebo/physics/ode/ODEPhysics.cc +++ b/gazebo/physics/ode/ODEPhysics.cc @@ -1160,8 +1160,10 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, /// << " from surface with smaller mu1\n"; } - // Longitudinal slope angle in degrees averaged over each contact point. - ignition::math::SignalMean meanSlopeDegrees; + // Slope angle in degrees averaged over each contact point computed in both + // the longitudinal/contact normal and lateral/contact normal planes. + ignition::math::SignalMean meanLongitudinalSlopeDegrees; + ignition::math::SignalMean meanLateralSlopeDegrees; ODECollisionWheelPlowingParams wheelPlowing; if (fd != ignition::math::Vector3d::Zero) @@ -1230,19 +1232,33 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, contactNormalCopy.Set( contactNormal[0], contactNormal[1], contactNormal[2]); + // Compute lateral unit vector as normalized component of fdir1 + // orthogonal to contact normal + ignition::math::Vector3d unitLateral = + fdir1 - fdir1.Dot(contactNormalCopy) * contactNormalCopy; + unitLateral.Normalize(); + // Compute longitudinal unit vector as normal cross fdir1 ignition::math::Vector3d unitLongitudinal = - contactNormalCopy.Cross(fdir1); + contactNormalCopy.Cross(unitLateral); // Compute normal and longitudinal forces (before plowing) double normalForce = -worldForce.Dot(contactNormalCopy); double longitudinalForce = worldForce.Dot(unitLongitudinal); + double lateralForce = worldForce.Dot(unitLateral); // Estimate slope angle from world force in longitudinal/normal plane - ignition::math::Angle slopeAngle(atan2(longitudinalForce, normalForce)); + // and in lateral/normal plane + ignition::math::Angle longitudinalSlopeAngle( + atan2(longitudinalForce, normalForce)); + ignition::math::Angle lateralSlopeAngle( + atan2(lateralForce, normalForce)); // Store average slope degrees - meanSlopeDegrees.InsertData(slopeAngle.Degree()); + meanLongitudinalSlopeDegrees.InsertData( + longitudinalSlopeAngle.Degree()); + meanLateralSlopeDegrees.InsertData( + lateralSlopeAngle.Degree()); // Compute longitudinal speed (dot product) double wheelSpeedLongitudinal = @@ -1308,15 +1324,16 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, contact.surface.slip3 *= numc; } - if (meanSlopeDegrees.Count() > 0) + // nonlinear longitudinal wheel slip effects + if (meanLongitudinalSlopeDegrees.Count() > 0) { - const double slopeDegrees = meanSlopeDegrees.Value(); + const double slopeDegrees = meanLongitudinalSlopeDegrees.Value(); // Increase slip compliance at a specified rate above and below thresholds // modify slip2 value to affect longitudinal slip const auto &upperDegreesMultipliers = - wheelPlowing.nonlinearSlipUpperDegreesMultipliers; + wheelPlowing.longitudinalNonlinearSlipParams.upperDegreesMultipliers; const auto &lowerDegreesMultipliers = - wheelPlowing.nonlinearSlipLowerDegreesMultipliers; + wheelPlowing.longitudinalNonlinearSlipParams.lowerDegreesMultipliers; if (slopeDegrees > upperDegreesMultipliers[0].X()) { std::size_t i = 0; @@ -1325,10 +1342,12 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, { ++i; } + const auto &upperPerDegrees = + wheelPlowing.longitudinalNonlinearSlipParams.upperPerDegrees; const double degreesAboveThreshold = slopeDegrees - upperDegreesMultipliers[i].X(); const double multiplier = upperDegreesMultipliers[i].Y() + - wheelPlowing.nonlinearSlipUpperPerDegrees[i] * degreesAboveThreshold; + upperPerDegrees[i] * degreesAboveThreshold; contact.surface.slip2 *= multiplier; } else if (slopeDegrees < lowerDegreesMultipliers[0].X()) @@ -1339,14 +1358,60 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, { ++i; } + const auto &lowerPerDegrees = + wheelPlowing.longitudinalNonlinearSlipParams.lowerPerDegrees; const double degreesBelowThreshold = lowerDegreesMultipliers[i].X() - slopeDegrees; const double multiplier = lowerDegreesMultipliers[i].Y() + - wheelPlowing.nonlinearSlipLowerPerDegrees[i] * degreesBelowThreshold; + lowerPerDegrees[i] * degreesBelowThreshold; contact.surface.slip2 *= multiplier; } } + // nonlinear lateral wheel slip effects + if (meanLateralSlopeDegrees.Count() > 0) + { + const double slopeDegrees = meanLateralSlopeDegrees.Value(); + // Increase slip compliance at a specified rate above and below thresholds + // modify slip1 value to affect lateral slip + const auto &upperDegreesMultipliers = + wheelPlowing.lateralNonlinearSlipParams.upperDegreesMultipliers; + const auto &lowerDegreesMultipliers = + wheelPlowing.lateralNonlinearSlipParams.lowerDegreesMultipliers; + if (slopeDegrees > upperDegreesMultipliers[0].X()) + { + std::size_t i = 0; + while (i + 1 < upperDegreesMultipliers.size() && + slopeDegrees > upperDegreesMultipliers[i + 1].X()) + { + ++i; + } + const auto &upperPerDegrees = + wheelPlowing.lateralNonlinearSlipParams.upperPerDegrees; + const double degreesAboveThreshold = + slopeDegrees - upperDegreesMultipliers[i].X(); + const double multiplier = upperDegreesMultipliers[i].Y() + + upperPerDegrees[i] * degreesAboveThreshold; + contact.surface.slip1 *= multiplier; + } + else if (slopeDegrees < lowerDegreesMultipliers[0].X()) + { + std::size_t i = 0; + while (i + 1 < lowerDegreesMultipliers.size() && + slopeDegrees < lowerDegreesMultipliers[i + 1].X()) + { + ++i; + } + const auto &lowerPerDegrees = + wheelPlowing.lateralNonlinearSlipParams.lowerPerDegrees; + const double degreesBelowThreshold = + lowerDegreesMultipliers[i].X() - slopeDegrees; + const double multiplier = lowerDegreesMultipliers[i].Y() + + lowerPerDegrees[i] * degreesBelowThreshold; + contact.surface.slip1 *= multiplier; + } + } + // Combine torsional friction patch radius values contact.surface.patch_radius = std::max(surf1->FrictionPyramid()->PatchRadius(), diff --git a/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf index ffcc0967ff..fcab725e82 100644 --- a/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf +++ b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf @@ -258,6 +258,24 @@ 0.0 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + @@ -353,6 +371,24 @@ 0.0 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + @@ -434,6 +470,24 @@ 0.0 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + +