diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 4eb335692..724b45e63 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 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 diff --git a/src/Surface.cc b/src/Surface.cc index 3977cf3fa..1811280bc 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,174 @@ 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; + } + + 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; + + 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()) +{ +} + +///////////////////////////////////////////////// +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()); + } + + 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 +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) { diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index 1e31247d9..c1f475755 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, SetFriction) +{ + 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, DefaultValues) +{ + 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 + +