Wheel plowing approximation in ODEPhysics (#3164)

This adds an approximation of a wheel plowing effect
by rotating contact points and normals forward to
approximate the normal pressure distribution when
plowing longitudinally through deformable soil.

This builds on the implementation of the plowing effect
from the aditya/contact_pts branch to implement a model
discussed in #3085.

* Collision::WorldLinearVel: account for pose offset

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

Add example world and models, plowing SDFormat tags

Signed-off-by: Aditya <aditya050995@gmail.com>

Plowing effect unit test (#3229)

Signed-off-by: deepanshu <deepanshubansal01@gmail.com>

Co-authored-by: Aditya <aditya050995@gmail.com>
Co-authored-by: Deepanshu Bansal <36300643+deepanshubansal01@users.noreply.github.com>

gazebo/physics/Collision.cc

@@ -222,7 +222,7 @@ ignition::math::Vector3d Collision::RelativeLinearVel() const
 ignition::math::Vector3d Collision::WorldLinearVel() const
 {
   if (this->link)
-    return this->link->WorldLinearVel();
+    return this->link->WorldLinearVel(this->RelativePose().Pos());
   else
     return ignition::math::Vector3d::Zero;
 }

gazebo/physics/ode/ODECollision.cc

@@ -73,6 +73,122 @@ void ODECollision::Load(sdf::ElementPtr _sdf)
   {
     this->GetODESurface()->maxVel = 0.0;
   }
+
+  // Check validity of plowing parameters
+  const std::string kPlowingWheel = "gz:plowing_wheel";
+  sdf::ElementPtr wheelElem = nullptr;
+  if (_sdf->HasElement(kPlowingWheel))
+  {
+    wheelElem = _sdf->GetElement(kPlowingWheel);
+  }
+
+  // Try to parse wheel plowing params with verbose == true so errors will
+  // print once during loading
+  ODECollisionWheelPlowingParams plowing;
+  ParseWheelPlowingParams(_sdf, plowing, this->GetScopedName());
+}
+
+//////////////////////////////////////////////////
+bool ODECollision::ParseWheelPlowingParams(
+    sdf::ElementPtr _sdf,
+    ODECollisionWheelPlowingParams &_plowing,
+    const std::string &_scopedNameForErrorMessages)
+{
+  // Only print error messages of the scoped name is supplied.
+  bool verbose = !_scopedNameForErrorMessages.empty();
+
+  // Check for a plowing wheel element
+  const std::string kPlowingWheel = "gz:plowing_wheel";
+  if (!_sdf->HasElement(kPlowingWheel))
+  {
+    return false;
+  }
+  sdf::ElementPtr wheelElem = _sdf->GetElement(kPlowingWheel);
+
+  // Check for required elements: max_degrees and saturation velocity
+  const std::string kPlowingMaxDegrees = "max_degrees";
+  if (!wheelElem->HasElement(kPlowingMaxDegrees))
+  {
+    if (verbose)
+    {
+      gzerr << "Element <" << kPlowingWheel << "> in collision with name ["
+            << _scopedNameForErrorMessages << "] is missing required parameter"
+            << " <" << kPlowingMaxDegrees << ">" << std::endl;
+    }
+    return false;
+  }
+
+  const std::string kPlowingSaturationVelocity = "saturation_velocity";
+  if (!wheelElem->HasElement(kPlowingSaturationVelocity))
+  {
+    if (verbose)
+    {
+      gzerr << "Element <" << kPlowingWheel << "> in collision with name ["
+            << _scopedNameForErrorMessages << "] is missing required parameter"
+            << " <" << kPlowingSaturationVelocity << ">" << std::endl;
+    }
+    return false;
+  }
+
+  // Error unless max_degrees >= 0
+  double plowingMaxDegrees = wheelElem->Get<double>(kPlowingMaxDegrees);
+  if (!(plowingMaxDegrees >= 0))
+  {
+    if (verbose)
+    {
+      gzerr << "Element <" << kPlowingWheel << "> in collision with name ["
+            << _scopedNameForErrorMessages << "] has a parameter "
+            << "<" << kPlowingMaxDegrees << "> with a value of ["
+            << plowingMaxDegrees << "] that should be non-negative."
+            << std::endl;
+    }
+    return false;
+  }
+  ignition::math::Angle plowingMaxAngle;
+  plowingMaxAngle.SetDegree(plowingMaxDegrees);
+
+  // Error unless saturation_velocity > 0
+  double plowingSaturationVelocity =
+      wheelElem->Get<double>(kPlowingSaturationVelocity);
+  if (!(plowingSaturationVelocity > 0))
+  {
+    if (verbose)
+    {
+      gzerr << "Element <" << kPlowingWheel << "> in collision with name ["
+            << _scopedNameForErrorMessages << "] has a parameter "
+            << "<" << kPlowingSaturationVelocity << "> with a value of ["
+            << plowingSaturationVelocity << "] that should be positive."
+            << std::endl;
+    }
+    return false;
+  }
+
+  // Check also for optional element: deadband velocity
+  const std::string kPlowingDeadbandVelocity = "deadband_velocity";
+  // Use deadband velocity = 0 if parameter is not set
+  double plowingDeadbandVelocity =
+      wheelElem->Get<double>(kPlowingDeadbandVelocity, 0.0).first;
+
+  // Error unless deadband velocity < saturation velocity
+  if (!(plowingDeadbandVelocity < plowingSaturationVelocity))
+  {
+    if (verbose)
+    {
+      gzerr << "Element <" << kPlowingWheel << "> in collision with name ["
+            << _scopedNameForErrorMessages << "] has a parameter "
+            << "<" << kPlowingDeadbandVelocity << "> with a value of ["
+            << plowingDeadbandVelocity << "] that should be less than the "
+            << "<" << kPlowingSaturationVelocity << "> whose value is ["
+            << plowingSaturationVelocity << "]."
+            << std::endl;
+    }
+    return false;
+  }
+
+  _plowing.maxAngle = plowingMaxAngle;
+  _plowing.saturationVelocity = plowingSaturationVelocity;
+  _plowing.deadbandVelocity = plowingDeadbandVelocity;
+  return true;
 }
 
 //////////////////////////////////////////////////

gazebo/physics/ode/ODECollision.hh

@@ -36,6 +36,21 @@ namespace gazebo
     /// \addtogroup gazebo_physics_ode
     /// \{
 
+    /// \brief Data structure for wheel plowing parameters.
+    class ODECollisionWheelPlowingParams
+    {
+      /// \brief Maximum angle by which wheel contact points are rotated.
+      public: ignition::math::Angle maxAngle;
+
+      /// \brief Plowing saturation velocity: the linear wheel velocity [m/s]
+      /// at which maximum plowing effect is reached.
+      public: double saturationVelocity = 0.0;
+
+      /// \brief Plowing deadband velocity: the linear wheel velocity [m/s]
+      /// below which no plowing effect occurs.
+      public: double deadbandVelocity = 0.0;
+    };
+
     /// \brief Base class for all ODE collisions.
     class GZ_PHYSICS_VISIBLE ODECollision : public Collision
     {
@@ -90,6 +105,22 @@ namespace gazebo
       /// \return Dynamically casted pointer to ODESurfaceParams.
       public: ODESurfaceParamsPtr GetODESurface() const;
 
+      /// \brief Parse wheel plowing parameters from a Collision SDF Element.
+      /// \param[in] _sdf Collision SDF Element to parse from.
+      /// \param[out] _plowing Wheel plowing parameters object to write to.
+      /// \param[in] _scopedNameForErrorMessages Scoped name of collision to
+      /// use in error messages. If empty, no error messages will be printed.
+      /// parsed.
+      /// \return True if valid wheel plowing parameters were parsed.
+      private: static bool ParseWheelPlowingParams(
+          sdf::ElementPtr _sdf,
+          ODECollisionWheelPlowingParams &_plowing,
+          const std::string &_scopedNameForErrorMessages = "");
+
+      /// \brief Friend with ODEPhysics to allow Collide callback to call
+      /// ParseWheelPlowingParams.
+      private: friend class ODEPhysics;
+
       /// \brief Used when this is static to set the posse.
       private: void OnPoseChangeGlobal();
 

gazebo/physics/ode/ODEPhysics.cc

@@ -77,6 +77,9 @@
 
 #include "gazebo/physics/ode/ODEPhysicsPrivate.hh"
 
+double saturation_deadband(double, double, double, double);
+
+
 using namespace gazebo;
 using namespace physics;
 
@@ -1161,6 +1164,94 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2,
     contact.fdir1[0] = fd.X();
     contact.fdir1[1] = fd.Y();
     contact.fdir1[2] = fd.Z();
+
+    // Check for plowing parameters if friction direction is specified
+    auto collision1Sdf = _collision1->GetSDF();
+    auto collision2Sdf = _collision2->GetSDF();
+
+    const std::string kPlowingTerrain = "gz:plowing_terrain";
+    ODECollisionWheelPlowingParams wheelPlowing;
+
+    ODECollision *wheelCollision = nullptr;
+    sdf::ElementPtr terrainSdf = nullptr;
+
+    if (ODECollision::ParseWheelPlowingParams(collision1Sdf, wheelPlowing) &&
+        collision2Sdf->HasElement(kPlowingTerrain))
+    {
+      wheelCollision = _collision1;
+      terrainSdf = collision2Sdf;
+    }
+    else if (collision1Sdf->HasElement(kPlowingTerrain) &&
+        ODECollision::ParseWheelPlowingParams(collision2Sdf, wheelPlowing))
+    {
+      wheelCollision = _collision2;
+      terrainSdf = collision1Sdf;
+    }
+
+    if (wheelCollision && terrainSdf)
+    {
+      // compute slope
+      double slope = wheelPlowing.maxAngle.Radian() /
+          (wheelPlowing.saturationVelocity - wheelPlowing.deadbandVelocity);
+
+      // Assume origin of collision frame is wheel center
+      // Compute position and linear velocity of wheel center
+      // (all vectors in world coordinates unless otherwise specified)
+      ignition::math::Vector3d wheelPosition =
+          wheelCollision->WorldPose().Pos();
+      ignition::math::Vector3d wheelLinearVelocity =
+          wheelCollision->WorldLinearVel();
+
+      ignition::math::Vector3d fdir1 = fd.Normalized();
+      ignition::math::Vector3d contactNormalCopy, contactPositionCopy;
+      // for each pair of contact point and normal
+      for (unsigned int c = 0; c < numc; ++c)
+      {
+        // Copy the contact normal
+        dReal *contactNormal =
+          _contactCollisions[this->dataPtr->indices[c]].normal;
+        contactNormalCopy.Set(
+          contactNormal[0], contactNormal[1], contactNormal[2]);
+
+        // Compute longitudinal unit vector as normal cross fdir1
+        ignition::math::Vector3d unitLongitudinal =
+          contactNormalCopy.Cross(fdir1);
+
+        // Compute longitudinal speed (dot product)
+        double wheelSpeedLongitudinal =
+          wheelLinearVelocity.Dot(unitLongitudinal);
+
+        // Compute plowing angle as function of longitudinal speed
+        double plowingAngle =
+          saturation_deadband(wheelPlowing.maxAngle.Radian(),
+          wheelPlowing.deadbandVelocity,
+          slope,
+          wheelSpeedLongitudinal);
+
+        // Construct axis-angle quaternion using fdir1 and plowing angle
+        ignition::math::Quaterniond plowingRotation(fdir1, plowingAngle);
+
+        // Rotate normal unit vector by plowingRotation and set normal vector
+        contactNormalCopy = plowingRotation * contactNormalCopy;
+        contactNormal[0] = contactNormalCopy[0];
+        contactNormal[1] = contactNormalCopy[1];
+        contactNormal[2] = contactNormalCopy[2];
+
+        // Construct displacement vector from wheel center to contact point
+        dReal *contactPosition =
+          _contactCollisions[this->dataPtr->indices[c]].pos;
+        contactPositionCopy.Set(contactPosition[0] - wheelPosition[0],
+                                contactPosition[1] - wheelPosition[1],
+                                contactPosition[2] - wheelPosition[2]);
+
+        // Rotate displacement vector by plowingRotation and set contact point
+        contactPositionCopy = wheelPosition +
+            plowingRotation * contactPositionCopy;
+        contactPosition[0] = contactPositionCopy[0];
+        contactPosition[1] = contactPositionCopy[1];
+        contactPosition[2] = contactPositionCopy[2];
+      }
+    }
   }
 
   // Set the friction coefficients.
@@ -1692,3 +1783,41 @@ bool ODEPhysics::GetParam(const std::string &_key, boost::any &_value) const
   }
   return true;
 }
+
+/// \brief A mathematical function that represents a saturation function
+/// that increases linearly until reaching a constant value with the addition
+/// of a deadband near zero. The shape of this function for positive input
+/// values is illustrated below.
+/// \param[in] _maxOutput the maximum output of the function, which is at
+/// saturation.
+/// \param[in] _deadband the size of the deadband. Any inputs with a smaller
+/// absolute value than the deadband produce zero output.
+/// \param[in] _slope the slope of the linear portion of the output.
+/// \param[in] _input the input to the function.
+/// \return the output computed using saturation and deadband.
+/** \verbatim
+      |                                                  .
+      |            _________ saturation value            .
+      |           /                                      .
+      |          /|                                      .
+      |         /-┘ slope                                .
+      |        /                                         .
+      |       /                                          .
+      |      /                                           .
+    --+-------------------------- input
+      |<----> dead-band
+
+ \endverbatim */
+double saturation_deadband(
+    double _maxOutput, double _deadband, double _slope, double _input)
+{
+  if (std::abs(_input) <= std::abs(_deadband))
+  {
+    return 0.0;
+  }
+
+  double result = _input - std::abs(_deadband) * ignition::math::sgn(_input);
+  result *= _slope;
+
+  return ignition::math::clamp(result, -_maxOutput, _maxOutput);
+}

test/integration/CMakeLists.txt

@@ -88,6 +88,7 @@ set(tests
   physics_thread_safe.cc
   physics_torsional_friction.cc
   pioneer2dx.cc
+  plowing_effect.cc
   plugin.cc
   plugin_interface.cc
   rayshape.cc