Added Friction and ODE classes (#955)

Signed-off-by: Alejandro Hernández <ahcorde@gmail.com>

include/sdf/Surface.hh

@@ -58,6 +58,103 @@ namespace sdf
     IGN_UTILS_IMPL_PTR(dataPtr)
   };
 
+  /// \brief ODE information for a friction.
+  class SDFORMAT_VISIBLE ODE
+  {
+    /// \brief Default constructor
+    public: ODE();
+
+    /// \brief Load the ODE based on a element pointer. This is *not* the
+    /// usual entry point. Typical usage of the SDF DOM is through the Root
+    /// object.
+    /// \param[in] _sdf The SDF Element pointer
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error.
+    public: Errors Load(ElementPtr _sdf);
+
+    /// \brief Set the Mu
+    /// \returns ODE mu
+    public: double Mu() const;
+
+    /// \brief Set Mu
+    /// \param[in] _mu ODE mu
+    public: void SetMu(double _mu);
+
+    /// \brief Get the Mu2
+    /// \returns ODE mu2
+    public: double Mu2() const;
+
+    /// \brief Set Mu2
+    /// \param[in] _mu2 ODE mu2
+    public: void SetMu2(double _mu2);
+
+    /// \brief Get the fdir
+    /// \returns ODE fdir
+    public: const ignition::math::Vector3d &Fdir1() const;
+
+    /// \brief Set fdir
+    /// \param[in] _fdir ODE fdir
+    public: void SetFdir1(const ignition::math::Vector3d &_fdir);
+
+    /// \brief Get the slip1
+    /// \returns ODE slip1
+    public: double Slip1() const;
+
+    /// \brief Set Slip1
+    /// \param[in] _slip1 ODE Slip1
+    public: void SetSlip1(double _slip1);
+
+    /// \brief Get the Slip2
+    /// \returns ODE Slip2
+    public: double Slip2() const;
+
+    /// \brief Set Slip2
+    /// \param[in] _slip2 ODE Slip2
+    public: void SetSlip2(double _slip2);
+
+    /// \brief Get a pointer to the SDF element that was used during
+    /// load.
+    /// \return SDF element pointer. The value will be nullptr if Load has
+    /// not been called.
+    public: sdf::ElementPtr Element() const;
+
+    /// \brief Private data pointer.
+    IGN_UTILS_IMPL_PTR(dataPtr)
+  };
+
+  /// \brief Friction information for a surface.
+  class SDFORMAT_VISIBLE Friction
+  {
+    /// \brief Default constructor
+    public: Friction();
+
+    /// \brief Load the friction based on a element pointer. This is *not* the
+    /// usual entry point. Typical usage of the SDF DOM is through the Root
+    /// object.
+    /// \param[in] _sdf The SDF Element pointer
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error.
+    public: Errors Load(ElementPtr _sdf);
+
+    /// \brief Get the associated ODE object
+    /// \returns Pointer to the associated ODE object,
+    /// nullptr if the Surface doesn't contain a ODE element.
+    public: const sdf::ODE *ODE() const;
+
+    /// \brief Set the associated ODE object.
+    /// \param[in] _ode The ODE object.
+    public: void SetODE(const sdf::ODE &_ode);
+
+    /// \brief Get a pointer to the SDF element that was used during
+    /// load.
+    /// \return SDF element pointer. The value will be nullptr if Load has
+    /// not been called.
+    public: sdf::ElementPtr Element() const;
+
+    /// \brief Private data pointer.
+    IGN_UTILS_IMPL_PTR(dataPtr)
+  };
+
   /// \brief Surface information for a collision.
   class SDFORMAT_VISIBLE Surface
   {
@@ -87,6 +184,15 @@ namespace sdf
     /// \param[in] _cont The contact object.
     public: void SetContact(const sdf::Contact &_contact);
 
+    /// \brief Get the associated friction object
+    /// \returns Pointer to the associated friction object,
+    /// nullptr if the Surface doesn't contain a friction element.
+    public: const sdf::Friction *Friction() const;
+
+    /// \brief Set the associated friction object.
+    /// \param[in] _friction The friction object.
+    public: void SetFriction(const sdf::Friction &_friction);
+
     /// \brief Create and return an SDF element filled with data from this
     /// surface.
     /// Note that parameter passing functionality is not captured with this

src/Surface.cc

@@ -33,15 +33,225 @@ class sdf::Contact::Implementation
   public: sdf::ElementPtr sdf{nullptr};
 };
 
+class sdf::ODE::Implementation
+{
+  /// \brief The SDF element pointer used during load.
+  public: sdf::ElementPtr sdf{nullptr};
+
+  /// \brief Coefficient of friction in first friction pyramid direction,
+  /// the unitless maximum ratio of force in first friction pyramid
+  /// direction to normal force.
+  public: double mu = 1.0;
+
+  /// \brief Coefficient of friction in second friction pyramid direction,
+  /// the unitless maximum ratio of force in second friction pyramid
+  /// direction to normal force.
+  public: double mu2 = 1.0;
+
+  /// \brief Unit vector specifying first friction pyramid direction in
+  /// collision-fixed reference frame.
+  public: ignition::math::Vector3d fdir1 = {0, 0, 0};
+
+  /// \brief Force dependent slip in first friction pyramid direction,
+  /// equivalent to inverse of viscous damping coefficient
+  /// with units of m/s/N.
+  public: double slip1 = 0.0;
+
+  /// \brief Force dependent slip in second friction pyramid direction,
+  /// equivalent to inverse of viscous damping coefficient
+  /// with units of m/s/N.
+  public: double slip2 = 0.0;
+};
+
+class sdf::Friction::Implementation
+{
+  /// \brief The object storing contact parameters
+  public: sdf::ODE ode;
+
+  /// \brief The SDF element pointer used during load.
+  public: sdf::ElementPtr sdf{nullptr};
+};
+
 class sdf::Surface::Implementation
 {
+  /// \brief The object storing contact parameters
+  public: sdf::Friction friction;
+
   /// \brief The object storing contact parameters
   public: sdf::Contact contact;
 
   /// \brief The SDF element pointer used during load.
   public: sdf::ElementPtr sdf{nullptr};
 };
 
+
+/////////////////////////////////////////////////
+ODE::ODE()
+  : dataPtr(ignition::utils::MakeImpl<Implementation>())
+{
+}
+
+/////////////////////////////////////////////////
+Errors ODE::Load(ElementPtr _sdf)
+{
+  Errors errors;
+
+  this->dataPtr->sdf = _sdf;
+
+  // Check that sdf is a valid pointer
+  if (!_sdf)
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Attempting to load a ODE, but the provided SDF "
+        "element is null."});
+    return errors;
+  }
+
+  // Check that the provided SDF element is a <ode>
+  // This is an error that cannot be recovered, so return an error.
+  if (_sdf->GetName() != "ode")
+  {
+    errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
+        "Attempting to load a ODE, but the provided SDF element is not a "
+        "<ode>."});
+    return errors;
+  }
+
+  this->dataPtr->mu = _sdf->Get<double>("mu", this->dataPtr->mu).first;
+  this->dataPtr->mu2 = _sdf->Get<double>("mu2", this->dataPtr->mu2).first;
+  this->dataPtr->slip1 = _sdf->Get<double>("slip1", this->dataPtr->slip1).first;
+  this->dataPtr->slip2 = _sdf->Get<double>("slip2", this->dataPtr->slip2).first;
+  this->dataPtr->fdir1 = _sdf->Get<ignition::math::Vector3d>("fdir1",
+        this->dataPtr->fdir1).first;
+
+  return errors;
+}
+
+/////////////////////////////////////////////////
+double ODE::Mu() const
+{
+  return this->dataPtr->mu;
+}
+
+/////////////////////////////////////////////////
+void ODE::SetMu(double _mu)
+{
+  this->dataPtr->mu = _mu;
+}
+
+/////////////////////////////////////////////////
+double ODE::Mu2() const
+{
+  return this->dataPtr->mu2;
+}
+
+/////////////////////////////////////////////////
+void ODE::SetMu2(double _mu2)
+{
+  this->dataPtr->mu2 = _mu2;
+}
+
+/////////////////////////////////////////////////
+const ignition::math::Vector3d &ODE::Fdir1() const
+{
+  return this->dataPtr->fdir1;
+}
+
+/////////////////////////////////////////////////
+void ODE::SetFdir1(const ignition::math::Vector3d &_fdir)
+{
+  this->dataPtr->fdir1 = _fdir;
+}
+
+/////////////////////////////////////////////////
+double ODE::Slip1() const
+{
+  return this->dataPtr->slip1;
+}
+
+/////////////////////////////////////////////////
+void ODE::SetSlip1(double _slip1)
+{
+  this->dataPtr->slip1 = _slip1;
+}
+
+/////////////////////////////////////////////////
+double ODE::Slip2() const
+{
+  return this->dataPtr->slip2;
+}
+
+/////////////////////////////////////////////////
+void ODE::SetSlip2(double _slip2)
+{
+  this->dataPtr->slip2 = _slip2;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr ODE::Element() const
+{
+  return this->dataPtr->sdf;
+}
+
+/////////////////////////////////////////////////
+Friction::Friction()
+  : dataPtr(ignition::utils::MakeImpl<Implementation>())
+{
+}
+
+/////////////////////////////////////////////////
+Errors Friction::Load(ElementPtr _sdf)
+{
+  Errors errors;
+
+  this->dataPtr->sdf = _sdf;
+
+  // Check that sdf is a valid pointer
+  if (!_sdf)
+  {
+    errors.push_back({ErrorCode::ELEMENT_MISSING,
+        "Attempting to load a Friction, but the provided SDF "
+        "element is null."});
+    return errors;
+  }
+
+  // Check that the provided SDF element is a <friction>
+  // This is an error that cannot be recovered, so return an error.
+  if (_sdf->GetName() != "friction")
+  {
+    errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE,
+        "Attempting to load a Friction, but the provided SDF element is not a "
+        "<friction>."});
+    return errors;
+  }
+
+  if (_sdf->HasElement("ode"))
+  {
+    Errors err = this->dataPtr->ode.Load(_sdf->GetElement("ode"));
+    errors.insert(errors.end(), err.begin(), err.end());
+  }
+
+  return errors;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Friction::Element() const
+{
+  return this->dataPtr->sdf;
+}
+
+/////////////////////////////////////////////////
+void Friction::SetODE(const sdf::ODE &_ode)
+{
+  this->dataPtr->ode = _ode;
+}
+
+/////////////////////////////////////////////////
+const sdf::ODE *Friction::ODE() const
+{
+  return &this->dataPtr->ode;
+}
+
 /////////////////////////////////////////////////
 Contact::Contact()
   : dataPtr(ignition::utils::MakeImpl<Implementation>())
@@ -83,6 +293,7 @@ Errors Contact::Load(ElementPtr _sdf)
   // \todo(nkoenig) Parse the remaining collide properties.
   return errors;
 }
+
 /////////////////////////////////////////////////
 sdf::ElementPtr Contact::Element() const
 {
@@ -139,6 +350,12 @@ Errors Surface::Load(ElementPtr _sdf)
     errors.insert(errors.end(), err.begin(), err.end());
   }
 
+  if (_sdf->HasElement("friction"))
+  {
+    Errors err = this->dataPtr->friction.Load(_sdf->GetElement("friction"));
+    errors.insert(errors.end(), err.begin(), err.end());
+  }
+
   // \todo(nkoenig) Parse the remaining surface properties.
   return errors;
 }
@@ -154,6 +371,18 @@ const sdf::Contact *Surface::Contact() const
   return &this->dataPtr->contact;
 }
 
+/////////////////////////////////////////////////
+void Surface::SetFriction(const sdf::Friction &_friction)
+{
+  this->dataPtr->friction = _friction;
+}
+
+/////////////////////////////////////////////////
+const sdf::Friction *Surface::Friction() const
+{
+  return &this->dataPtr->friction;
+}
+
 /////////////////////////////////////////////////
 void Surface::SetContact(const sdf::Contact &_contact)
 {