From 3a8ff66f3df99de4f14777cf044790098b0eb3ca Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 7 Apr 2022 22:04:42 +0200 Subject: [PATCH 1/3] Added Friction and ODE classes Signed-off-by: ahcorde --- include/sdf/Surface.hh | 106 +++++++++++ src/Surface.cc | 252 ++++++++++++++++++++++++++ src/Surface_TEST.cc | 303 ++++++++++++++++++++++++++++++++ test/integration/surface_dom.cc | 6 + test/sdf/shapes.sdf | 9 + 5 files changed, 676 insertions(+) diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 4eb335692..871924eec 100644 --- a/include/sdf/Surface.hh +++ b/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 contact 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] _mu2 ODE fdir + public: void SetFdir1(ignition::math::Vector3d _fdir); + + /// \brief Get the Mu2 + /// \returns ODE mu2 + public: double Slip1() const; + + /// \brief Set Slip1 + /// \param[in] _mu2 ODE Slip1 + public: void SetSlip1(double _slip1); + + /// \brief Get the Slip2 + /// \returns ODE Slip2 + public: double Slip2() const; + + /// \brief Set Slip2 + /// \param[in] _mu2 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 contact 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] _cont 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] _cont 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 diff --git a/src/Surface.cc b/src/Surface.cc index 3977cf3fa..5d45eb956 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -33,8 +33,50 @@ 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; @@ -42,6 +84,197 @@ class sdf::Surface::Implementation public: sdf::ElementPtr sdf{nullptr}; }; + +///////////////////////////////////////////////// +ODE::ODE() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +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 + // 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 " + "."}); + return errors; + } + + if (_sdf->HasElement("mu")) + { + this->dataPtr->mu = + static_cast(_sdf->Get("mu")); + } + + if (_sdf->HasElement("mu2")) + { + this->dataPtr->mu2 = + static_cast(_sdf->Get("mu2")); + } + + if (_sdf->HasElement("slip1")) + { + this->dataPtr->slip1 = + static_cast(_sdf->Get("slip1")); + } + + if (_sdf->HasElement("slip2")) + { + this->dataPtr->slip2 = + static_cast(_sdf->Get("slip2")); + } + + // Read fdir1. + this->dataPtr->fdir1 = _sdf->Get("fdir1", + this->dataPtr->fdir1).first; + + // \todo(nkoenig) Parse the remaining collide properties. + 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(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()) +{ +} + +///////////////////////////////////////////////// +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 + // 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 " + "."}); + return errors; + } + + if (_sdf->HasElement("ode")) + { + Errors err = this->dataPtr->ode.Load(_sdf->GetElement("ode")); + errors.insert(errors.end(), err.begin(), err.end()); + } + + // \todo(nkoenig) Parse the remaining collide properties. + 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()) @@ -83,6 +316,7 @@ Errors Contact::Load(ElementPtr _sdf) // \todo(nkoenig) Parse the remaining collide properties. return errors; } + ///////////////////////////////////////////////// sdf::ElementPtr Contact::Element() const { @@ -139,6 +373,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 +394,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) { diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index 1e31247d9..a842b78d3 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -25,6 +25,7 @@ TEST(DOMsurface, DefaultConstruction) EXPECT_EQ(nullptr, surface.Element()); EXPECT_EQ(surface.Contact()->CollideBitmask(), 0xFF); EXPECT_EQ(surface.Contact()->Element(), nullptr); + EXPECT_EQ(surface.Friction()->Element(), nullptr); } ///////////////////////////////////////////////// @@ -32,11 +33,26 @@ TEST(DOMsurface, CopyOperator) { sdf::Surface surface1; sdf::Contact contact; + sdf::ODE ode; + ode.SetMu(0.1); + ode.SetMu2(0.2); + ode.SetSlip1(3); + ode.SetSlip2(4); + ode.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction; + friction.SetODE(ode); contact.SetCollideBitmask(0x12); surface1.SetContact(contact); + surface1.SetFriction(friction); sdf::Surface surface2(surface1); EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip2(), 4); + EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); } ///////////////////////////////////////////////// @@ -44,11 +60,26 @@ TEST(DOMsurface, CopyAssignmentOperator) { sdf::Surface surface1; sdf::Contact contact; + sdf::ODE ode; + ode.SetMu(0.1); + ode.SetMu2(0.2); + ode.SetSlip1(3); + ode.SetSlip2(4); + ode.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction; + friction.SetODE(ode); contact.SetCollideBitmask(0x12); surface1.SetContact(contact); + surface1.SetFriction(friction); sdf::Surface surface2 = surface1; EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip2(), 4); + EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); } ///////////////////////////////////////////////// @@ -64,12 +95,45 @@ TEST(DOMsurface, CopyAssignmentAfterMove) contact2.SetCollideBitmask(0x34); surface2.SetContact(contact2); + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + + sdf::ODE ode2; + ode2.SetMu(0.2); + ode2.SetMu2(0.1); + ode2.SetSlip1(7); + ode2.SetSlip2(8); + ode2.SetFdir1(ignition::math::Vector3d(3, 2, 1)); + sdf::Friction friction2; + friction2.SetODE(ode2); + + surface1.SetFriction(friction1); + surface2.SetFriction(friction2); + sdf::Surface tmp = std::move(surface1); surface1 = surface2; surface2 = tmp; EXPECT_EQ(surface2.Contact()->CollideBitmask(), 0x12); EXPECT_EQ(surface1.Contact()->CollideBitmask(), 0x34); + EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Mu(), 0.2); + EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Mu2(), 0.1); + EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Slip1(), 7); + EXPECT_DOUBLE_EQ(surface1.Friction()->ODE()->Slip2(), 8); + EXPECT_EQ(surface1.Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(3, 2, 1)); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(surface2.Friction()->ODE()->Slip2(), 4); + EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); } ///////////////////////////////////////////////// @@ -126,3 +190,242 @@ TEST(DOMcontact, CollideBitmask) EXPECT_EQ(contact.CollideBitmask(), 0x67); } +///////////////////////////////////////////////// +TEST(DOMfriction, DefaultConstruction) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + EXPECT_EQ(nullptr, friction1.Element()); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 4); + EXPECT_EQ(friction1.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMfriction, CopyOperator) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + + sdf::Friction friction2(friction1); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); + EXPECT_EQ(friction2.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMfriction, CopyAssignmentOperator) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + + sdf::Friction friction2 = friction1; + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); + EXPECT_EQ(friction2.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMfriction, CopyAssignmentAfterMove) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + sdf::Friction friction1; + friction1.SetODE(ode1); + + sdf::ODE ode2; + ode2.SetMu(0.2); + ode2.SetMu2(0.1); + ode2.SetSlip1(7); + ode2.SetSlip2(8); + ode2.SetFdir1(ignition::math::Vector3d(3, 2, 1)); + sdf::Friction friction2; + friction2.SetODE(ode2); + + sdf::Friction tmp = std::move(friction1); + friction1 = friction2; + friction2 = tmp; + + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 0.2); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 0.1); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 7); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 8); + EXPECT_EQ(friction1.ODE()->Fdir1(), + ignition::math::Vector3d(3, 2, 1)); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); + EXPECT_EQ(friction2.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMfriction, Set) +{ + sdf::ODE ode1; + sdf::Friction friction1; + + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 1.0); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 1.0); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 0); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 0); + EXPECT_EQ(friction1.ODE()->Fdir1(), + ignition::math::Vector3d(0, 0, 0)); + + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + friction1.SetODE(ode1); + + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu(), 0.1); + EXPECT_DOUBLE_EQ(friction1.ODE()->Mu2(), 0.2); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip1(), 3); + EXPECT_DOUBLE_EQ(friction1.ODE()->Slip2(), 4); + EXPECT_EQ(friction1.ODE()->Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMode, DefaultConstruction) +{ + sdf::ODE ode; + EXPECT_EQ(nullptr, ode.Element()); + EXPECT_DOUBLE_EQ(ode.Mu(), 1.0); + EXPECT_DOUBLE_EQ(ode.Mu2(), 1.0); + EXPECT_DOUBLE_EQ(ode.Slip1(), 0); + EXPECT_DOUBLE_EQ(ode.Slip2(), 0); + EXPECT_EQ(ode.Fdir1(), + ignition::math::Vector3d(0, 0, 0)); +} + +///////////////////////////////////////////////// +TEST(DOMode, CopyOperator) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + + sdf::ODE ode2(ode1); + EXPECT_DOUBLE_EQ(ode2.Mu(), 0.1); + EXPECT_DOUBLE_EQ(ode2.Mu2(), 0.2); + EXPECT_DOUBLE_EQ(ode2.Slip1(), 3); + EXPECT_DOUBLE_EQ(ode2.Slip2(), 4); + EXPECT_EQ(ode2.Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMode, CopyAssignmentOperator) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + + sdf::ODE ode2 = ode1; + EXPECT_DOUBLE_EQ(ode2.Mu(), 0.1); + EXPECT_DOUBLE_EQ(ode2.Mu2(), 0.2); + EXPECT_DOUBLE_EQ(ode2.Slip1(), 3); + EXPECT_DOUBLE_EQ(ode2.Slip2(), 4); + EXPECT_EQ(ode2.Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMode, CopyAssignmentAfterMove) +{ + sdf::ODE ode1; + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + + sdf::ODE ode2; + ode2.SetMu(0.2); + ode2.SetMu2(0.1); + ode2.SetSlip1(7); + ode2.SetSlip2(8); + ode2.SetFdir1(ignition::math::Vector3d(3, 2, 1)); + + sdf::ODE tmp = std::move(ode1); + ode1 = ode2; + ode2 = tmp; + + EXPECT_DOUBLE_EQ(ode1.Mu(), 0.2); + EXPECT_DOUBLE_EQ(ode1.Mu2(), 0.1); + EXPECT_DOUBLE_EQ(ode1.Slip1(), 7); + EXPECT_DOUBLE_EQ(ode1.Slip2(), 8); + EXPECT_EQ(ode1.Fdir1(), + ignition::math::Vector3d(3, 2, 1)); + EXPECT_DOUBLE_EQ(ode2.Mu(), 0.1); + EXPECT_DOUBLE_EQ(ode2.Mu2(), 0.2); + EXPECT_DOUBLE_EQ(ode2.Slip1(), 3); + EXPECT_DOUBLE_EQ(ode2.Slip2(), 4); + EXPECT_EQ(ode2.Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} + +///////////////////////////////////////////////// +TEST(DOMode, Set) +{ + sdf::ODE ode1; + + EXPECT_DOUBLE_EQ(ode1.Mu(), 1.0); + EXPECT_DOUBLE_EQ(ode1.Mu2(), 1.0); + EXPECT_DOUBLE_EQ(ode1.Slip1(), 0); + EXPECT_DOUBLE_EQ(ode1.Slip2(), 0); + EXPECT_EQ(ode1.Fdir1(), + ignition::math::Vector3d(0, 0, 0)); + + ode1.SetMu(0.1); + ode1.SetMu2(0.2); + ode1.SetSlip1(3); + ode1.SetSlip2(4); + ode1.SetFdir1(ignition::math::Vector3d(1, 2, 3)); + EXPECT_DOUBLE_EQ(ode1.Mu(), 0.1); + EXPECT_DOUBLE_EQ(ode1.Mu2(), 0.2); + EXPECT_DOUBLE_EQ(ode1.Slip1(), 3); + EXPECT_DOUBLE_EQ(ode1.Slip2(), 4); + EXPECT_EQ(ode1.Fdir1(), + ignition::math::Vector3d(1, 2, 3)); +} diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index 27d9d09a3..22de924ff 100644 --- a/test/integration/surface_dom.cc +++ b/test/integration/surface_dom.cc @@ -50,4 +50,10 @@ TEST(DOMSurface, Shapes) ASSERT_NE(nullptr, boxCol->Surface()); ASSERT_NE(nullptr, boxCol->Surface()->Contact()); EXPECT_EQ(boxCol->Surface()->Contact()->CollideBitmask(), 0xAB); + EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Mu(), 0.6); + EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Mu2(), 0.7); + EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Slip1(), 4); + EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Slip2(), 5); + EXPECT_EQ(boxCol->Surface()->Friction()->ODE()->Fdir1(), + ignition::math::Vector3d(1.2, 3.4, 5.6)); } diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index fa21449e6..922b6d747 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -12,6 +12,15 @@ 0xAB + + + 0.6 + 0.7 + 4 + 5 + 1.2 3.4 5.6 + + From f60198f4ccd944372cd6701984df5f4d77b334e0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Wed, 13 Apr 2022 19:19:23 +0200 Subject: [PATCH 2/3] review feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- include/sdf/Surface.hh | 30 +++++++++++++++--------------- src/Surface.cc | 38 ++++++++------------------------------ src/Surface_TEST.cc | 4 ++-- 3 files changed, 25 insertions(+), 47 deletions(-) diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 871924eec..724b45e63 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -64,7 +64,7 @@ namespace sdf /// \brief Default constructor public: ODE(); - /// \brief Load the contact based on a element pointer. This is *not* the + /// \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 @@ -90,18 +90,18 @@ namespace sdf /// \brief Get the fdir /// \returns ODE fdir - public: const ignition::math::Vector3d Fdir1() const; + public: const ignition::math::Vector3d &Fdir1() const; /// \brief Set fdir - /// \param[in] _mu2 ODE fdir - public: void SetFdir1(ignition::math::Vector3d _fdir); + /// \param[in] _fdir ODE fdir + public: void SetFdir1(const ignition::math::Vector3d &_fdir); - /// \brief Get the Mu2 - /// \returns ODE mu2 + /// \brief Get the slip1 + /// \returns ODE slip1 public: double Slip1() const; /// \brief Set Slip1 - /// \param[in] _mu2 ODE Slip1 + /// \param[in] _slip1 ODE Slip1 public: void SetSlip1(double _slip1); /// \brief Get the Slip2 @@ -109,7 +109,7 @@ namespace sdf public: double Slip2() const; /// \brief Set Slip2 - /// \param[in] _mu2 ODE Slip2 + /// \param[in] _slip2 ODE Slip2 public: void SetSlip2(double _slip2); /// \brief Get a pointer to the SDF element that was used during @@ -128,7 +128,7 @@ namespace sdf /// \brief Default constructor public: Friction(); - /// \brief Load the contact based on a element pointer. This is *not* the + /// \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 @@ -136,13 +136,13 @@ namespace sdf /// 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. + /// \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] _cont The ode object. + /// \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 @@ -190,7 +190,7 @@ namespace sdf public: const sdf::Friction *Friction() const; /// \brief Set the associated friction object. - /// \param[in] _cont The 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 diff --git a/src/Surface.cc b/src/Surface.cc index 5d45eb956..20ff97f47 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -107,7 +107,7 @@ Errors ODE::Load(ElementPtr _sdf) return errors; } - // Check that the provided SDF element is a + // Check that the provided SDF element is a // This is an error that cannot be recovered, so return an error. if (_sdf->GetName() != "ode") { @@ -117,31 +117,10 @@ Errors ODE::Load(ElementPtr _sdf) return errors; } - if (_sdf->HasElement("mu")) - { - this->dataPtr->mu = - static_cast(_sdf->Get("mu")); - } - - if (_sdf->HasElement("mu2")) - { - this->dataPtr->mu2 = - static_cast(_sdf->Get("mu2")); - } - - if (_sdf->HasElement("slip1")) - { - this->dataPtr->slip1 = - static_cast(_sdf->Get("slip1")); - } - - if (_sdf->HasElement("slip2")) - { - this->dataPtr->slip2 = - static_cast(_sdf->Get("slip2")); - } - - // Read fdir1. + this->dataPtr->mu = _sdf->Get("mu", this->dataPtr->mu).first; + this->dataPtr->mu2 = _sdf->Get("mu2", this->dataPtr->mu2).first; + this->dataPtr->slip1 = _sdf->Get("slip1", this->dataPtr->slip1).first; + this->dataPtr->slip2 = _sdf->Get("slip2", this->dataPtr->slip2).first; this->dataPtr->fdir1 = _sdf->Get("fdir1", this->dataPtr->fdir1).first; @@ -174,13 +153,13 @@ void ODE::SetMu2(double _mu2) } ///////////////////////////////////////////////// -const ignition::math::Vector3d ODE::Fdir1() const +const ignition::math::Vector3d &ODE::Fdir1() const { return this->dataPtr->fdir1; } ///////////////////////////////////////////////// -void ODE::SetFdir1(ignition::math::Vector3d _fdir) +void ODE::SetFdir1(const ignition::math::Vector3d &_fdir) { this->dataPtr->fdir1 = _fdir; } @@ -237,7 +216,7 @@ Errors Friction::Load(ElementPtr _sdf) return errors; } - // Check that the provided SDF element is a + // Check that the provided SDF element is a // This is an error that cannot be recovered, so return an error. if (_sdf->GetName() != "friction") { @@ -253,7 +232,6 @@ Errors Friction::Load(ElementPtr _sdf) errors.insert(errors.end(), err.begin(), err.end()); } - // \todo(nkoenig) Parse the remaining collide properties. return errors; } diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index a842b78d3..c1f475755 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -191,7 +191,7 @@ TEST(DOMcontact, CollideBitmask) } ///////////////////////////////////////////////// -TEST(DOMfriction, DefaultConstruction) +TEST(DOMfriction, SetFriction) { sdf::ODE ode1; ode1.SetMu(0.1); @@ -320,7 +320,7 @@ TEST(DOMfriction, Set) } ///////////////////////////////////////////////// -TEST(DOMode, DefaultConstruction) +TEST(DOMode, DefaultValues) { sdf::ODE ode; EXPECT_EQ(nullptr, ode.Element()); From 80117d1df70b52c91ed230fff5d4423682dd6686 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Wed, 13 Apr 2022 23:06:00 +0200 Subject: [PATCH 3/3] Remove TODO MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/Surface.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Surface.cc b/src/Surface.cc index 20ff97f47..1811280bc 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -124,7 +124,6 @@ Errors ODE::Load(ElementPtr _sdf) this->dataPtr->fdir1 = _sdf->Get("fdir1", this->dataPtr->fdir1).first; - // \todo(nkoenig) Parse the remaining collide properties. return errors; }