Wheel slip nonlinear effect part 2 (not for merging)

gazebo/physics/Model.cc

@@ -297,6 +297,14 @@ void Model::LoadJoints()
 //////////////////////////////////////////////////
 void Model::Init()
 {
+  // Cache the total mass of the model
+  this->mass = 0.0;
+  for (uint64_t i = 0; i < this->modelSDFDom->LinkCount(); ++i)
+  {
+    this->mass +=
+        this->modelSDFDom->LinkByIndex(i)->Inertial().MassMatrix().Mass();
+  }
+
   // Record the model's initial pose (for reseting)
   this->SetInitialRelativePose(this->SDFPoseRelativeToParent());
   this->SetRelativePose(this->SDFPoseRelativeToParent());
@@ -1861,6 +1869,7 @@ void Model::PluginInfo(const common::URI &_pluginUri,
       << std::endl;
 }
 
+//////////////////////////////////////////////////
 std::optional<sdf::SemanticPose> Model::SDFSemanticPose() const
 {
   if (nullptr != this->modelSDFDom)
@@ -1869,3 +1878,9 @@ std::optional<sdf::SemanticPose> Model::SDFSemanticPose() const
   }
   return std::nullopt;
 }
+
+//////////////////////////////////////////////////
+double Model::GetMass() const
+{
+  return this->mass;
+}

gazebo/physics/Model.hh

@@ -93,6 +93,10 @@ namespace gazebo
       /// \return The SDF DOM for this model.
       public: const sdf::Model *GetSDFDom() const;
 
+      /// \brief Get the total mass of this model.
+      /// \return The mass of the model, cached during initialization.
+      public: double GetMass() const;
+
       /// \internal
       /// \brief Get the SDF element for the model, without all effects of
       /// scaling. This is useful in cases when the scale will be applied
@@ -567,6 +571,9 @@ namespace gazebo
 
       /// \brief SDF Model DOM object
       private: const sdf::Model *modelSDFDom = nullptr;
+
+      /// \brief Cached mass of the entire model.
+      private: double mass = 0.0;
     };
     /// \}
   }

gazebo/physics/ode/ODECollision.cc

@@ -105,6 +105,15 @@ bool ODECollision::ParseWheelPlowingParams(
   }
   sdf::ElementPtr wheelElem = _sdf->GetElement(kPlowingWheel);
 
+  // Check if scaling slip by number of contact points should be disabled.
+  const std::string kDisableScaling =
+    "disable_scaling_slip_by_number_of_contact_points";
+  if (wheelElem->HasElement(kDisableScaling))
+  {
+    _plowing.disableScalingSlipByNumberOfContactPoints =
+      wheelElem->Get<bool>(kDisableScaling);
+  }
+
   // Check for required elements: max_degrees and saturation velocity
   const std::string kPlowingMaxDegrees = "max_degrees";
   if (!wheelElem->HasElement(kPlowingMaxDegrees))
@@ -185,6 +194,162 @@ bool ODECollision::ParseWheelPlowingParams(
     return false;
   }
 
+  // Parse nonlinear slip parameters
+  auto parseNonlinearSlipParams = [&](
+      const std::string &_nonlinearSlipElementName,
+      ODECollisionWheelPlowingParams::NonlinearSlipParams &_nonlinearParams)
+      -> bool
+  {
+    if (wheelElem->HasElement(_nonlinearSlipElementName))
+    {
+      auto parse_degrees_perDegrees =
+        [](const std::string &_elementName, sdf::ElementPtr _nonlinearSlipElem,
+           std::vector<double> &_parsedDegrees,
+           std::vector<double> &_parsedPerDegrees) -> void
+        {
+          if (_nonlinearSlipElem->HasElement(_elementName))
+          {
+            sdf::ElementPtr elem = _nonlinearSlipElem->GetElement(_elementName);
+            while (elem)
+            {
+              _parsedDegrees.push_back(elem->Get<double>("degree"));
+              _parsedPerDegrees.push_back(elem->Get<double>("per_degree"));
+              elem = elem->GetNextElement(_elementName);
+            }
+          }
+        };
+
+      sdf::ElementPtr nonlinearSlipElem =
+          wheelElem->GetElement(_nonlinearSlipElementName);
+      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<double> parsedDegrees;
+        std::vector<double> parsedPerDegrees;
+        parse_degrees_perDegrees(
+            "lower", nonlinearSlipElem, parsedDegrees, parsedPerDegrees);
+        //
+        std::optional<double> lastDegree, lastPerDegree;
+        double multiplier = 1.0;
+        for (std::size_t i = 0; i < parsedDegrees.size(); ++i)
+        {
+          // 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
+          {
+            // Verify that slope values are in order
+            if (degree >= *lastDegree)
+            {
+              if (verbose)
+              {
+                gzerr << "Element <" << kPlowingWheel << "> in collision with name ["
+                      << _scopedNameForErrorMessages << "] has a //nonlinear_slip/lower "
+                      << "parameter with unsorted <degree> values: "
+                      << degree << " should be less than " << *lastDegree
+                      << std::endl;
+              }
+              return false;
+            }
+            double degreesBelowThreshold = *lastDegree - degree;
+            multiplier += *lastPerDegree * degreesBelowThreshold;
+            _nonlinearParams.lowerDegreesMultipliers.push_back(
+                {degree, multiplier});
+            _nonlinearParams.lowerPerDegrees.push_back(perDegree);
+          }
+          lastDegree = degree;
+          lastPerDegree = perDegree;
+        }
+      }
+      if (nonlinearSlipElem->HasElement("upper"))
+      {
+        std::vector<double> parsedDegrees;
+        std::vector<double> parsedPerDegrees;
+        parse_degrees_perDegrees(
+            "upper", nonlinearSlipElem, parsedDegrees, parsedPerDegrees);
+
+        std::optional<double> lastDegree, lastPerDegree;
+        double multiplier = 1.0;
+        for (std::size_t i = 0; i < parsedDegrees.size(); ++i)
+        {
+          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
+          {
+            // Verify that slope values are in order
+            if (degree <= *lastDegree)
+            {
+              if (verbose)
+              {
+                gzerr << "Element <" << kPlowingWheel << "> in collision with name ["
+                      << _scopedNameForErrorMessages << "] has a //nonlinear_slip/upper "
+                      << "parameter with unsorted <degree> values: "
+                      << degree << " should be greater than " << *lastDegree
+                      << std::endl;
+              }
+              return false;
+            }
+            double degreesAboveThreshold = degree - *lastDegree;
+            multiplier += *lastPerDegree * degreesAboveThreshold;
+            _nonlinearParams.upperDegreesMultipliers.push_back(
+                {degree, multiplier});
+            _nonlinearParams.upperPerDegrees.push_back(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"
+          << "    " << nonlinearParams.lowerDegreesMultipliers.back().X() << " deg"
+          << ", " << nonlinearParams.lowerDegreesMultipliers.back().Y() << '\n'
+          << "    " << nonlinearParams.lowerDegreesMultipliers.front().X() << " deg"
+          << ", " << nonlinearParams.lowerDegreesMultipliers.front().Y() << '\n'
+          << "  Upper:\n"
+          << "    " << nonlinearParams.upperDegreesMultipliers.front().X() << " deg"
+          << ", " << nonlinearParams.upperDegreesMultipliers.front().Y() << '\n'
+          << "    " << nonlinearParams.upperDegreesMultipliers.back().X() << " deg"
+          << ", " << nonlinearParams.upperDegreesMultipliers.back().Y() << '\n'
+          << std::endl;
+  }
+
   _plowing.maxAngle = plowingMaxAngle;
   _plowing.saturationVelocity = plowingSaturationVelocity;
   _plowing.deadbandVelocity = plowingDeadbandVelocity;

gazebo/physics/ode/ODECollision.hh

@@ -39,6 +39,13 @@ namespace gazebo
     /// \brief Data structure for wheel plowing parameters.
     class ODECollisionWheelPlowingParams
     {
+      /// \brief Flag to disable scaling of slip parameters by the number
+      /// of contact points. This behavior was added in bitbucket PR 2965,
+      /// but it can interact poorly with wheels on heightmap collisions.
+      /// Keep current behavior (scaling on) as default.
+      /// https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2965
+      public: bool disableScalingSlipByNumberOfContactPoints = false;
+
       /// \brief Maximum angle by which wheel contact points are rotated.
       public: ignition::math::Angle maxAngle;
 
@@ -49,6 +56,111 @@ namespace gazebo
       /// \brief Plowing deadband velocity: the linear wheel velocity [m/s]
       /// below which no plowing effect occurs.
       public: double deadbandVelocity = 0.0;
+
+      /// Nonlinear slip model:
+      /// The nonlinear slip model applies a multiplier to the slip compliance
+      /// depending on the terrain slope at the contact point. The slip
+      /// compliance multipliers are defined as a piecewise linear function of
+      /// the terrain slope at the contact point in degrees. Two sets of
+      /// piecewise linear segments are defined as the lower and upper
+      /// nonlinear parameters. The slip compliance multiplier is 1.0 for
+      /// terrain slope values in between the largest lower slope value and
+      /// the smallest upper slope value.
+      ///
+      /// A graphical representation of upper piecewise linear segments is
+      /// provided below. The horizontal axis corresponds to terrain slope in
+      /// degrees and the vertical axis corresponds to the slip compliance
+      /// multiplier. This example shows three piecewise linear segments
+      /// connecting from 0 to 1, 1 to 2, and from 2 onwards. The piecewise
+      /// linear endpoint coordinates and rates of change are encoded in
+      /// vectors of the same length for which the values at each index are
+      /// related:
+      ///
+      /// * per_degree[i]: the rate of change of the slip multiplier per
+      ///   degree of change in terrain slope. In this example the final
+      ///   slope is flat (per_degree[2] == 0), but that is not required.
+      /// * degrees[i]: the slope in degrees above which the slip compliance
+      ///   multiplier can vary according to the rate of change in
+      ///   per_degree[i].
+      /// * multipliers[i]: the starting value of the slip compliance
+      ///   multiplier at degrees[i]. Note that multipliers[0] == 1.0 always.
+      ///
+      /// The degrees[i] and per_degree[i] values are specified inside
+      /// <upper/> XML elements, and the corresponding multiplier[i] values
+      /// are computed automatically. The <upper/> XML elements should be
+      /// defined in order, such that each //upper/degree value is larger than
+      /// the previous value.
+      /** \verbatim
+                   |                                                  .
+              slip |                          per_degree[2] == 0      .
+        compliance |                    2 o o o o o o o o             .
+        multiplier |                  o (degrees[2], multipliers[2])  .
+                   |                o                                 .
+                   |              o |                                 .
+                   |            o---┘ per_degree[1]                   .
+                   |          o                                       .
+                   |        1--------                                 .
+                   |       o (degrees[1], multipliers[1])             .
+                   |      o                                           .
+                   |     o|                                           .
+                   |    o-┘ per_degree[0]                             .
+                   |   o                                              .
+         ----------+--0                                               .
+               1.0 |   (degrees[0], multipliers[0] == 1.0)            .
+                   |                                                  .
+      -------------+-------------------------- slopeDegrees
+                   |
+
+      <gz:plowing_wheel>
+        <nonlinear_slip>
+          <upper>
+            <degree>{degrees[0]}</degree>
+            <per_degree>{per_degree[0]}</per_degree>
+          </upper>
+          <upper>
+            <degree>{degrees[1]}</degree>
+            <per_degree>{per_degree[1]}</per_degree>
+          </upper>
+          <upper>
+            <degree>{degrees[2]}</degree>
+            <per_degree>{per_degree[2]}</per_degree>
+          </upper>
+        </nonlinear_slip>
+      </gz:plowing_wheel>
+      \endverbatim */
+      /// The lower nonlinear parameters are defined in a similar manner inside
+      /// <lower/> XML elements. Again, the elements should be defined in
+      /// order, such that each //lower/degree value is larger than the
+      /// previous value.
+
+      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<ignition::math::Vector2d>
+            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<ignition::math::Vector2d>
+            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<double> 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<double> 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.

gazebo/physics/ode/ODELink.cc

@@ -172,6 +172,9 @@ void ODELink::MoveCallback(dBodyID _id)
     const dReal *dtorque = dBodyGetTorque(_id);
     self->torque.Set(dtorque[0], dtorque[1], dtorque[2]);
   }
+
+  // clear cache of AddForce values
+  self->addedForce.Set(0, 0, 0);
 }
 
 //////////////////////////////////////////////////
@@ -567,13 +570,20 @@ void ODELink::SetTorque(const ignition::math::Vector3d &_torque)
           << " does not exist, unable to SetTorque" << std::endl;
 }
 
+//////////////////////////////////////////////////
+const ignition::math::Vector3d &ODELink::AddedForce() const
+{
+  return this->addedForce;
+}
+
 //////////////////////////////////////////////////
 void ODELink::AddForce(const ignition::math::Vector3d &_force)
 {
   if (this->linkId)
   {
     this->SetEnabled(true);
     dBodyAddForce(this->linkId, _force.X(), _force.Y(), _force.Z());
+    this->addedForce += _force;
   }
   else if (!this->IsStatic())
     gzlog << "ODE body for link [" << this->GetScopedName() << "]"
@@ -587,6 +597,7 @@ void ODELink::AddRelativeForce(const ignition::math::Vector3d &_force)
   {
     this->SetEnabled(true);
     dBodyAddRelForce(this->linkId, _force.X(), _force.Y(), _force.Z());
+    this->addedForce += this->WorldPose().Rot().RotateVector(_force);
   }
   else if (!this->IsStatic())
     gzlog << "ODE body for link [" << this->GetScopedName() << "]"