Wheel slip nonlinear effect (breaks ABI, 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

@@ -185,6 +185,24 @@ bool ODECollision::ParseWheelPlowingParams(
     return false;
   }
 
+  const std::string kNonlinearSlip = "nonlinear_slip";
+  if (wheelElem->HasElement(kNonlinearSlip))
+  {
+    sdf::ElementPtr nonlinearSlipElem = wheelElem->GetElement(kNonlinearSlip);
+    if (nonlinearSlipElem->HasElement("lower"))
+    {
+      sdf::ElementPtr lowerElem = nonlinearSlipElem->GetElement("lower");
+      _plowing.nonlinearSlipLowerDegree = lowerElem->Get<double>("degree");
+      _plowing.nonlinearSlipLowerPerDegree = lowerElem->Get<double>("per_degree");
+    }
+    if (nonlinearSlipElem->HasElement("upper"))
+    {
+      sdf::ElementPtr upperElem = nonlinearSlipElem->GetElement("upper");
+      _plowing.nonlinearSlipUpperDegree = upperElem->Get<double>("degree");
+      _plowing.nonlinearSlipUpperPerDegree = upperElem->Get<double>("per_degree");
+    }
+  }
+
   _plowing.maxAngle = plowingMaxAngle;
   _plowing.saturationVelocity = plowingSaturationVelocity;
   _plowing.deadbandVelocity = plowingDeadbandVelocity;

gazebo/physics/ode/ODECollision.hh

@@ -49,6 +49,22 @@ namespace gazebo
       /// \brief Plowing deadband velocity: the linear wheel velocity [m/s]
       /// below which no plowing effect occurs.
       public: double deadbandVelocity = 0.0;
+
+      /// \brief The slope value in degrees below which the nonlinear slip
+      /// effect is applied.
+      public: double nonlinearSlipLowerDegree = -361.0;
+
+      /// \brief The slope value in degrees above which the nonlinear slip
+      /// effect is applied.
+      public: double nonlinearSlipUpperDegree = 361.0;
+
+      /// \brief The rate of change in slip compliance per degree of slope
+      /// below the "lower degree" value.
+      public: double nonlinearSlipLowerPerDegree = 0.0;
+
+      /// \brief The rate of change in slip compliance per degree of slope
+      /// above the "upper degree" value.
+      public: double nonlinearSlipUpperPerDegree = 0.0;
     };
 
     /// \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() << "]"

gazebo/physics/ode/ODELink.hh

@@ -83,6 +83,10 @@ namespace gazebo
       // Documentation inherited
       public: virtual void SetTorque(const ignition::math::Vector3d &_torque);
 
+      /// Get sum of forces expressed in world frame that have been added by
+      /// Link::Add*Force during this timestep.
+      public: const ignition::math::Vector3d &AddedForce() const;
+
       // Documentation inherited
       public: virtual void AddForce(const ignition::math::Vector3d &_force);
 
@@ -196,6 +200,9 @@ namespace gazebo
 
       /// \brief Cache torque applied on body
       private: ignition::math::Vector3d torque;
+
+      /// \brief Cache force applied by AddForce
+      private: ignition::math::Vector3d addedForce;
     };
     /// \}
   }

gazebo/physics/ode/ODEPhysics.cc

@@ -26,7 +26,9 @@
 #include <utility>
 #include <vector>
 
+#include <ignition/math/Angle.hh>
 #include <ignition/math/Rand.hh>
+#include <ignition/math/SignalStats.hh>
 #include <ignition/math/Vector3.hh>
 #include <ignition/common/Profiler.hh>
 
@@ -1158,6 +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;
+  ODECollisionWheelPlowingParams wheelPlowing;
+
   if (fd != ignition::math::Vector3d::Zero)
   {
     contact.surface.mode |= dContactFDir1;
@@ -1170,7 +1176,6 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
     auto collision2Sdf = _collision2->GetSDF();
 
     const std::string kPlowingTerrain = "gz:plowing_terrain";
-    ODECollisionWheelPlowingParams wheelPlowing;
 
     ODECollision *wheelCollision = nullptr;
     sdf::ElementPtr terrainSdf = nullptr;
@@ -1190,8 +1195,20 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
 
     if (wheelCollision && terrainSdf)
     {
-      // compute slope
-      double slope = wheelPlowing.maxAngle.Radian() /
+      // compute chassis world force
+      ModelPtr wheelModel = wheelCollision->GetModel();
+      // initialize from model's gravity force
+      ignition::math::Vector3d worldForce =
+          wheelModel->GetMass() * this->world->Gravity();
+      // Get the added force applied to the canonical link
+      LinkPtr link = wheelModel->GetLink();
+      ODELinkPtr chassisLink = boost::dynamic_pointer_cast<ODELink>(link);
+      if (chassisLink)
+      {
+        worldForce += chassisLink->AddedForce();
+      }
+      // compute sensitivity of plowing angle to velocity
+      double plowingSensitivity = wheelPlowing.maxAngle.Radian() /
           (wheelPlowing.saturationVelocity - wheelPlowing.deadbandVelocity);
 
       // Assume origin of collision frame is wheel center
@@ -1217,6 +1234,16 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
         ignition::math::Vector3d unitLongitudinal =
           contactNormalCopy.Cross(fdir1);
 
+        // Compute normal and longitudinal forces (before plowing)
+        double normalForce = -worldForce.Dot(contactNormalCopy);
+        double longitudinalForce = worldForce.Dot(unitLongitudinal);
+
+        // Estimate slope angle from world force in longitudinal/normal plane
+        ignition::math::Angle slopeAngle(atan2(longitudinalForce, normalForce));
+
+        // Store average slope degrees
+        meanSlopeDegrees.InsertData(slopeAngle.Degree());
+
         // Compute longitudinal speed (dot product)
         double wheelSpeedLongitudinal =
           wheelLinearVelocity.Dot(unitLongitudinal);
@@ -1225,7 +1252,7 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
         double plowingAngle =
           saturation_deadband(wheelPlowing.maxAngle.Radian(),
           wheelPlowing.deadbandVelocity,
-          slope,
+          plowingSensitivity,
           wheelSpeedLongitudinal);
 
         // Construct axis-angle quaternion using fdir1 and plowing angle
@@ -1278,6 +1305,29 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
   contact.surface.slip2 *= numc;
   contact.surface.slip3 *= numc;
 
+  if (meanSlopeDegrees.Count() > 0)
+  {
+    // multiplying by numc has issues with plowing so undo that operation
+    contact.surface.slip1 /= numc;
+    contact.surface.slip2 /= numc;
+    contact.surface.slip3 /= numc;
+
+    // Increase slip compliance at a specified rate above and below thresholds
+    // modify slip2 value to affect longitudinal slip
+    if (meanSlopeDegrees.Value() > wheelPlowing.nonlinearSlipUpperDegree)
+    {
+      const double degreesAboveThreshold =
+          meanSlopeDegrees.Value() - wheelPlowing.nonlinearSlipUpperDegree;
+      contact.surface.slip2 *= (1 + wheelPlowing.nonlinearSlipUpperPerDegree * degreesAboveThreshold);
+    }
+    else if (meanSlopeDegrees.Value() < wheelPlowing.nonlinearSlipLowerDegree)
+    {
+      const double degreesBelowThreshold =
+          wheelPlowing.nonlinearSlipLowerDegree - meanSlopeDegrees.Value();
+      contact.surface.slip2 *= (1 + wheelPlowing.nonlinearSlipLowerPerDegree * degreesBelowThreshold);
+    }
+  }
+
   // Combine torsional friction patch radius values
   contact.surface.patch_radius =
       std::max(surf1->FrictionPyramid()->PatchRadius(),

test/models/plowing_nonlinear_slip_trisphere_cycle/model.config

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Tricycle with spherical wheels and plowing effect with nonlinear slip</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>Steve Peters</name>
+    <email>[email protected]</email>
+  </author>
+
+  <author>
+    <name>Aditya Pande</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    A tricycle with spherical wheels and front-wheel steering.
+    The first friction direction for each wheel is parallel to the joint axis
+    for each wheel spin joint, which corresponds to the wheel's lateral
+    direction.
+    The plowing and nonlinear slip parameters are specified in the //collision/gz:plowing_wheel
+    element for each wheel's sperical collision element.
+  </description>
+</model>