From 9101e86d3f03cdd32d3e73b3a9a7a1cb96696f59 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Mar 2024 01:58:37 -0700 Subject: [PATCH 01/25] Add bullet and torsional friction DOM (#1351) Signed-off-by: Ian Chen --- include/sdf/Surface.hh | 145 ++++++++++++ src/Surface.cc | 344 +++++++++++++++++++++++++++- src/Surface_TEST.cc | 383 +++++++++++++++++++++++++++++--- test/integration/surface_dom.cc | 18 ++ test/sdf/shapes.sdf | 25 ++- 5 files changed, 880 insertions(+), 35 deletions(-) diff --git a/include/sdf/Surface.hh b/include/sdf/Surface.hh index 790b8b2f5..bc3f34132 100644 --- a/include/sdf/Surface.hh +++ b/include/sdf/Surface.hh @@ -17,6 +17,7 @@ #ifndef SDF_SURFACE_HH_ #define SDF_SURFACE_HH_ +#include #include #include "sdf/Element.hh" #include "sdf/Types.hh" @@ -122,6 +123,132 @@ namespace sdf IGN_UTILS_IMPL_PTR(dataPtr) }; + /// \brief BulletFriction information for a friction. + class SDFORMAT_VISIBLE BulletFriction + { + /// \brief Default constructor + public: BulletFriction(); + + /// \brief Load BulletFriction 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 friction coefficient in first friction pyramid direction. + /// \returns Friction coefficient + public: double Friction() const; + + /// \brief Set friction coefficient in first friction pyramid direction. + /// \param[in] _fricton Friction coefficient + public: void SetFriction(double _friction); + + /// \brief Get the friction coefficient in second friction pyramid + /// direction. + /// \return Second friction coefficient + public: double Friction2() const; + + /// \brief Set friction coefficient in second friction pyramid direction. + /// \param[in] _fricton Friction coefficient + public: void SetFriction2(double _friction); + + /// \brief Get the first friction pyramid direction in collision-fixed + /// reference + /// \return First friction pyramid direction. + public: const gz::math::Vector3d &Fdir1() const; + + /// \brief Set the first friction pyramid direction in collision-fixed + /// reference + /// \param[in] _fdir First friction pyramid direction. + public: void SetFdir1(const gz::math::Vector3d &_fdir); + + /// \brief Get the rolling friction coefficient + /// \return Rolling friction coefficient + public: double RollingFriction() const; + + /// \brief Set the rolling friction coefficient + /// \param[in] _slip1 Rolling friction coefficient + public: void SetRollingFriction(double _friction); + + /// \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 Torsional friction + class SDFORMAT_VISIBLE Torsional + { + /// \brief Default constructor + public: Torsional(); + + /// \brief Load torsional 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 torsional friction coefficient. + /// \return Torsional friction coefficient + public: double Coefficient() const; + + /// \brief Set the torsional friction coefficient. + /// \param[in] _fricton Torsional friction coefficient + public: void SetCoefficient(double _coefficient); + + /// \brief Get whether the patch radius is used to calculate torsional + /// friction. + /// \return True if patch radius is used. + public: bool UsePatchRadius() const; + + /// \brief Set whether to use patch radius for torsional friction + /// calculation. + /// \param[in] _usePatchRadius True to use patch radius. + /// False to use surface radius. + public: void SetUsePatchRadius(bool _usePatchRadius); + + /// \brief Get the radius of contact patch surface. + /// \return Patch radius + public: double PatchRadius() const; + + /// \brief Set the radius of contact patch surface. + /// \param[in] _radius Patch radius + public: void SetPatchRadius(double _radius); + + /// \brief Get the surface radius on the contact point + /// \return Surface radius + public: double SurfaceRadius() const; + + /// \brief Set the surface radius on the contact point. + /// \param[in] _radius Surface radius + public: void SetSurfaceRadius(double _radius); + + /// \brief Get the ODE force dependent slip for torsional friction + /// \return Force dependent slip for torsional friction. + public: double ODESlip() const; + + /// \brief Set the ODE force dependent slip for torsional friction + /// \param[in] _slip Force dependent slip for torsional friction. + public: void SetODESlip(double _slip); + + /// \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 { @@ -145,6 +272,24 @@ namespace sdf /// \param[in] _ode The ODE object. public: void SetODE(const sdf::ODE &_ode); + /// \brief Get the associated BulletFriction object + /// \return Pointer to the associated BulletFriction object, + /// nullptr if the Surface doesn't contain a BulletFriction element. + public: const sdf::BulletFriction *BulletFriction() const; + + /// \brief Set the associated BulletFriction object. + /// \param[in] _bullet The BulletFriction object. + public: void SetBulletFriction(const sdf::BulletFriction &_bullet); + + /// \brief Get the torsional friction + /// \return Pointer to the torsional friction + /// nullptr if the Surface doesn't contain a torsional friction element. + public: const sdf::Torsional *Torsional() const; + + /// \brief Set the torsional friction + /// \param[in] _torsional The torsional friction. + public: void SetTorsional(const sdf::Torsional &_torsional); + /// \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 diff --git a/src/Surface.cc b/src/Surface.cc index 8ade5b406..57f05f95b 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -15,12 +15,16 @@ * */ +#include +#include + #include "sdf/Element.hh" #include "sdf/parser.hh" #include "sdf/Surface.hh" #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" +#include "Utils.hh" using namespace sdf; @@ -63,18 +67,74 @@ class sdf::ODE::Implementation public: double slip2 = 0.0; }; +class sdf::BulletFriction::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 friction{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 friction2{1.0}; + + /// \brief Unit vector specifying first friction pyramid direction in + /// collision-fixed reference frame. + public: gz::math::Vector3d fdir1{0, 0, 0}; + + /// \brief Rolling friction coefficient. + public: double rollingFriction{1.0}; +}; + +class sdf::Torsional::Implementation +{ + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; + + /// \brief Torsional friction coefficient. Uunitless maximum ratio of + /// tangential stress to normal stress. + public: double coefficient{1.0}; + + /// \brief If this flag is true, torsional friction is calculated using the + /// "patch_radius" parameter. If this flag is set to false, + /// "surface_radius" (R) and contact depth (d) are used to compute the patch + /// radius as sqrt(R*d). + public: bool usePatchRadius{true}; + + /// \brief Radius of contact patch surface. + public: double patchRadius{0.0}; + + /// \brief Surface radius on the point of contact. + public: double surfaceRadius{0.0}; + + /// \brief Force dependent slip for torsional friction. + /// equivalent to inverse of viscous damping coefficient with units of + /// rad/s/(Nm). A slip value of 0 is infinitely viscous. + public: double odeSlip{0.0}; +}; + class sdf::Friction::Implementation { - /// \brief The object storing contact parameters + /// \brief The object storing ode parameters public: sdf::ODE ode; + /// \brief The object storing bullet friction parameters + public: std::optional bullet; + + /// \brief The object storing torsional parameters + public: std::optional torsional; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf{nullptr}; }; class sdf::Surface::Implementation { - /// \brief The object storing contact parameters + /// \brief The object storing friction parameters public: sdf::Friction friction; /// \brief The object storing contact parameters @@ -84,6 +144,219 @@ class sdf::Surface::Implementation public: sdf::ElementPtr sdf{nullptr}; }; +///////////////////////////////////////////////// +Torsional::Torsional() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Torsional::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 BulletFriction, 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() != "torsional") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a BulletFriction, but the provided SDF element " + "is not a ."}); + return errors; + } + + this->dataPtr->coefficient = _sdf->Get( + "coefficient", this->dataPtr->coefficient).first; + this->dataPtr->usePatchRadius = _sdf->Get( + "use_patch_radius", this->dataPtr->usePatchRadius).first; + this->dataPtr->patchRadius = _sdf->Get( + "patch_radius", this->dataPtr->patchRadius).first; + this->dataPtr->surfaceRadius = _sdf->Get( + "surface_radius", this->dataPtr->surfaceRadius).first; + + if (_sdf->HasElement("ode")) + { + this->dataPtr->odeSlip = _sdf->GetElement("ode")->Get( + "slip", this->dataPtr->odeSlip).first; + } + + return errors; +} + +///////////////////////////////////////////////// +double Torsional::Coefficient() const +{ + return this->dataPtr->coefficient; +} + +///////////////////////////////////////////////// +void Torsional::SetCoefficient(double _coefficient) +{ + this->dataPtr->coefficient = _coefficient; +} + +///////////////////////////////////////////////// +bool Torsional::UsePatchRadius() const +{ + return this->dataPtr->usePatchRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetUsePatchRadius(bool _usePatchRadius) +{ + this->dataPtr->usePatchRadius = _usePatchRadius; +} + +///////////////////////////////////////////////// +double Torsional::PatchRadius() const +{ + return this->dataPtr->patchRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetPatchRadius(double _radius) +{ + this->dataPtr->patchRadius = _radius; +} + +///////////////////////////////////////////////// +double Torsional::SurfaceRadius() const +{ + return this->dataPtr->surfaceRadius; +} + +///////////////////////////////////////////////// +void Torsional::SetSurfaceRadius(double _radius) +{ + this->dataPtr->surfaceRadius = _radius; +} + +///////////////////////////////////////////////// +double Torsional::ODESlip() const +{ + return this->dataPtr->odeSlip; +} + +///////////////////////////////////////////////// +void Torsional::SetODESlip(double _slip) +{ + this->dataPtr->odeSlip = _slip; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Torsional::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +BulletFriction::BulletFriction() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors BulletFriction::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 BulletFriction, 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() != "bullet") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a BulletFriction, but the provided SDF element " + "is not a ."}); + return errors; + } + + this->dataPtr->friction = _sdf->Get( + "friction", this->dataPtr->friction).first; + this->dataPtr->friction2 = _sdf->Get( + "friction2", this->dataPtr->friction2).first; + this->dataPtr->fdir1 = _sdf->Get("fdir1", + this->dataPtr->fdir1).first; + this->dataPtr->rollingFriction = _sdf->Get( + "rolling_friction", this->dataPtr->rollingFriction).first; + + return errors; +} + +///////////////////////////////////////////////// +double BulletFriction::Friction() const +{ + return this->dataPtr->friction; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFriction(double _friction) +{ + this->dataPtr->friction = _friction; +} + +///////////////////////////////////////////////// +double BulletFriction::Friction2() const +{ + return this->dataPtr->friction2; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFriction2(double _friction2) +{ + this->dataPtr->friction2 = _friction2; +} + +///////////////////////////////////////////////// +const gz::math::Vector3d &BulletFriction::Fdir1() const +{ + return this->dataPtr->fdir1; +} + +///////////////////////////////////////////////// +void BulletFriction::SetFdir1(const gz::math::Vector3d &_fdir) +{ + this->dataPtr->fdir1 = _fdir; +} + +///////////////////////////////////////////////// +double BulletFriction::RollingFriction() const +{ + return this->dataPtr->rollingFriction; +} + +///////////////////////////////////////////////// +void BulletFriction::SetRollingFriction(double _rollingFriction) +{ + this->dataPtr->rollingFriction = _rollingFriction; +} + +///////////////////////////////////////////////// +sdf::ElementPtr BulletFriction::Element() const +{ + return this->dataPtr->sdf; +} ///////////////////////////////////////////////// ODE::ODE() @@ -231,6 +504,20 @@ Errors Friction::Load(ElementPtr _sdf) errors.insert(errors.end(), err.begin(), err.end()); } + if (_sdf->HasElement("bullet")) + { + this->dataPtr->bullet.emplace(); + Errors err = this->dataPtr->bullet->Load(_sdf->GetElement("bullet")); + errors.insert(errors.end(), err.begin(), err.end()); + } + + if (_sdf->HasElement("torsional")) + { + this->dataPtr->torsional.emplace(); + Errors err = this->dataPtr->torsional->Load(_sdf->GetElement("torsional")); + errors.insert(errors.end(), err.begin(), err.end()); + } + return errors; } @@ -252,6 +539,30 @@ const sdf::ODE *Friction::ODE() const return &this->dataPtr->ode; } +///////////////////////////////////////////////// +void Friction::SetBulletFriction(const sdf::BulletFriction &_bullet) +{ + this->dataPtr->bullet = _bullet; +} + +///////////////////////////////////////////////// +const sdf::BulletFriction *Friction::BulletFriction() const +{ + return optionalToPointer(this->dataPtr->bullet); +} + +///////////////////////////////////////////////// +void Friction::SetTorsional(const sdf::Torsional &_torsional) +{ + this->dataPtr->torsional = _torsional; +} + +///////////////////////////////////////////////// +const sdf::Torsional *Friction::Torsional() const +{ + return optionalToPointer(this->dataPtr->torsional); +} + ///////////////////////////////////////////////// Contact::Contact() : dataPtr(gz::utils::MakeImpl()) @@ -407,5 +718,34 @@ sdf::ElementPtr Surface::ToElement() const ode->GetElement("slip2")->Set(this->dataPtr->friction.ODE()->Slip2()); ode->GetElement("fdir1")->Set(this->dataPtr->friction.ODE()->Fdir1()); + if (this->dataPtr->friction.BulletFriction()) + { + sdf::ElementPtr bullet = frictionElem->GetElement("bullet"); + bullet->GetElement("friction")->Set( + this->dataPtr->friction.BulletFriction()->Friction()); + bullet->GetElement("friction2")->Set( + this->dataPtr->friction.BulletFriction()->Friction2()); + bullet->GetElement("fdir1")->Set( + this->dataPtr->friction.BulletFriction()->Fdir1()); + bullet->GetElement("rolling_friction")->Set( + this->dataPtr->friction.BulletFriction()->RollingFriction()); + } + + if (this->dataPtr->friction.Torsional()) + { + sdf::ElementPtr torsional = frictionElem->GetElement("torsional"); + torsional->GetElement("coefficient")->Set( + this->dataPtr->friction.Torsional()->Coefficient()); + torsional->GetElement("use_patch_radius")->Set( + this->dataPtr->friction.Torsional()->UsePatchRadius()); + torsional->GetElement("patch_radius")->Set( + this->dataPtr->friction.Torsional()->PatchRadius()); + torsional->GetElement("surface_radius")->Set( + this->dataPtr->friction.Torsional()->SurfaceRadius()); + + torsional->GetElement("ode")->GetElement("slip")->Set( + this->dataPtr->friction.Torsional()->ODESlip()); + } + return elem; } diff --git a/src/Surface_TEST.cc b/src/Surface_TEST.cc index db942df54..f5bdc24be 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include "sdf/Surface.hh" ///////////////////////////////////////////////// @@ -149,6 +150,19 @@ TEST(DOMsurface, ToElement) ode.SetSlip2(4); ode.SetFdir1(gz::math::Vector3d(1, 2, 3)); friction.SetODE(ode); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction.SetBulletFriction(bullet); + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction.SetTorsional(torsional); contact.SetCollideBitmask(0x12); surface1.SetContact(contact); surface1.SetFriction(friction); @@ -167,6 +181,18 @@ TEST(DOMsurface, ToElement) EXPECT_EQ(surface2.Friction()->ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + EXPECT_DOUBLE_EQ(0.3, surface2.Friction()->BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, surface2.Friction()->BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), + surface2.Friction()->BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, + surface2.Friction()->BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, surface2.Friction()->Torsional()->Coefficient()); + EXPECT_FALSE(surface2.Friction()->Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, surface2.Friction()->Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, surface2.Friction()->Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, surface2.Friction()->Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -234,13 +260,39 @@ TEST(DOMfriction, SetFriction) ode1.SetFdir1(gz::math::Vector3d(1, 2, 3)); sdf::Friction friction1; friction1.SetODE(ode1); - EXPECT_EQ(nullptr, friction1.Element()); + + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + 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(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction1.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction1.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction1.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction1.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction1.Torsional()->Coefficient()); + EXPECT_FALSE(friction1.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction1.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction1.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction1.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -255,6 +307,21 @@ TEST(DOMfriction, CopyOperator) sdf::Friction friction1; friction1.SetODE(ode1); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + sdf::Friction friction2(friction1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); @@ -262,6 +329,17 @@ TEST(DOMfriction, CopyOperator) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -276,6 +354,21 @@ TEST(DOMfriction, CopyAssignmentOperator) sdf::Friction friction1; friction1.SetODE(ode1); + sdf::BulletFriction bullet; + bullet.SetFriction(0.3); + bullet.SetFriction2(0.5); + bullet.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet); + + sdf::Torsional torsional; + torsional.SetCoefficient(0.5); + torsional.SetUsePatchRadius(false); + torsional.SetPatchRadius(0.1); + torsional.SetSurfaceRadius(0.3); + torsional.SetODESlip(0.2); + friction1.SetTorsional(torsional); + sdf::Friction friction2 = friction1; EXPECT_DOUBLE_EQ(friction2.ODE()->Mu(), 0.1); EXPECT_DOUBLE_EQ(friction2.ODE()->Mu2(), 0.2); @@ -283,6 +376,17 @@ TEST(DOMfriction, CopyAssignmentOperator) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::math::Vector3d(1, 2, 3)); + + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -306,6 +410,36 @@ TEST(DOMfriction, CopyAssignmentAfterMove) sdf::Friction friction2; friction2.SetODE(ode2); + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.3); + bullet1.SetFriction2(0.5); + bullet1.SetFdir1(gz::math::Vector3d(2, 1, 4)); + bullet1.SetRollingFriction(1.3); + friction1.SetBulletFriction(bullet1); + + sdf::BulletFriction bullet2; + bullet2.SetFriction(0.1); + bullet2.SetFriction2(0.2); + bullet2.SetFdir1(gz::math::Vector3d(3, 4, 5)); + bullet2.SetRollingFriction(3.1); + friction2.SetBulletFriction(bullet2); + + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.5); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.1); + torsional1.SetSurfaceRadius(0.3); + torsional1.SetODESlip(0.2); + friction1.SetTorsional(torsional1); + + sdf::Torsional torsional2; + torsional2.SetCoefficient(1.5); + torsional2.SetUsePatchRadius(true); + torsional2.SetPatchRadius(1.1); + torsional2.SetSurfaceRadius(3.3); + torsional2.SetODESlip(2.2); + friction2.SetTorsional(torsional2); + sdf::Friction tmp = std::move(friction1); friction1 = friction2; friction2 = tmp; @@ -322,34 +456,26 @@ TEST(DOMfriction, CopyAssignmentAfterMove) EXPECT_DOUBLE_EQ(friction2.ODE()->Slip2(), 4); EXPECT_EQ(friction2.ODE()->Fdir1(), gz::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(), - gz::math::Vector3d(0, 0, 0)); - ode1.SetMu(0.1); - ode1.SetMu2(0.2); - ode1.SetSlip1(3); - ode1.SetSlip2(4); - ode1.SetFdir1(gz::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(), - gz::math::Vector3d(1, 2, 3)); + EXPECT_DOUBLE_EQ(0.3, friction2.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.5, friction2.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(2, 1, 4), friction2.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(1.3, friction2.BulletFriction()->RollingFriction()); + EXPECT_DOUBLE_EQ(0.1, friction1.BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(0.2, friction1.BulletFriction()->Friction2()); + EXPECT_EQ(gz::math::Vector3d(3, 4, 5), friction1.BulletFriction()->Fdir1()); + EXPECT_DOUBLE_EQ(3.1, friction1.BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(0.5, friction2.Torsional()->Coefficient()); + EXPECT_FALSE(friction2.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.1, friction2.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(0.3, friction2.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.2, friction2.Torsional()->ODESlip()); + EXPECT_DOUBLE_EQ(1.5, friction1.Torsional()->Coefficient()); + EXPECT_TRUE(friction1.Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.1, friction1.Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(3.3, friction1.Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(2.2, friction1.Torsional()->ODESlip()); } ///////////////////////////////////////////////// @@ -462,3 +588,204 @@ TEST(DOMode, Set) EXPECT_EQ(ode1.Fdir1(), gz::math::Vector3d(1, 2, 3)); } + +///////////////////////////////////////////////// +TEST(DOMbullet, DefaultValues) +{ + sdf::BulletFriction bullet; + EXPECT_EQ(nullptr, bullet.Element()); + EXPECT_DOUBLE_EQ(1.0, bullet.Friction()); + EXPECT_DOUBLE_EQ(1.0, bullet.Friction2()); + EXPECT_EQ(gz::math::Vector3d(0, 0, 0), bullet.Fdir1()); + EXPECT_DOUBLE_EQ(1.0, bullet.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyOperator) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2(bullet1); + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyAssignmentOperator) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2 = bullet1; + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, CopyAssignmentAfterMove) +{ + sdf::BulletFriction bullet1; + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4.0); + + sdf::BulletFriction bullet2; + bullet2.SetFriction(0.2); + bullet2.SetFriction2(0.1); + bullet2.SetFdir1(gz::math::Vector3d(3, 2, 1)); + bullet2.SetRollingFriction(3.0); + + sdf::BulletFriction tmp = std::move(bullet1); + bullet1 = bullet2; + bullet2 = tmp; + + EXPECT_DOUBLE_EQ(0.2, bullet1.Friction()); + EXPECT_DOUBLE_EQ(0.1, bullet1.Friction2()); + EXPECT_EQ(gz::math::Vector3d(3, 2, 1), bullet1.Fdir1()); + EXPECT_DOUBLE_EQ(3.0, bullet1.RollingFriction()); + + EXPECT_DOUBLE_EQ(0.1, bullet2.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet2.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet2.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet2.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMbullet, Set) +{ + sdf::BulletFriction bullet1; + + EXPECT_DOUBLE_EQ(bullet1.Friction(), 1.0); + EXPECT_DOUBLE_EQ(bullet1.Friction2(), 1.0); + EXPECT_EQ(bullet1.Fdir1(), + gz::math::Vector3d(0, 0, 0)); + EXPECT_DOUBLE_EQ(bullet1.RollingFriction(), 1.0); + + bullet1.SetFriction(0.1); + bullet1.SetFriction2(0.2); + bullet1.SetFdir1(gz::math::Vector3d(1, 2, 3)); + bullet1.SetRollingFriction(4); + + EXPECT_DOUBLE_EQ(0.1, bullet1.Friction()); + EXPECT_DOUBLE_EQ(0.2, bullet1.Friction2()); + EXPECT_EQ(gz::math::Vector3d(1, 2, 3), bullet1.Fdir1()); + EXPECT_DOUBLE_EQ(4.0, bullet1.RollingFriction()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, DefaultValues) +{ + sdf::Torsional torsional; + EXPECT_EQ(nullptr, torsional.Element()); + EXPECT_DOUBLE_EQ(1.0, torsional.Coefficient()); + EXPECT_TRUE(torsional.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.PatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyOperator) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2(torsional1); + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyAssignmentOperator) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2 = torsional1; + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, CopyAssignmentAfterMove) +{ + sdf::Torsional torsional1; + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4.0); + torsional1.SetODESlip(1.0); + + sdf::Torsional torsional2; + torsional2.SetCoefficient(1.1); + torsional2.SetUsePatchRadius(true); + torsional2.SetPatchRadius(1.2); + torsional2.SetSurfaceRadius(4.1); + torsional2.SetODESlip(1.1); + + sdf::Torsional tmp = std::move(torsional1); + torsional1 = torsional2; + torsional2 = tmp; + + EXPECT_DOUBLE_EQ(0.1, torsional2.Coefficient()); + EXPECT_FALSE(torsional2.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional2.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional2.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional2.ODESlip()); + + EXPECT_DOUBLE_EQ(1.1, torsional1.Coefficient()); + EXPECT_TRUE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.2, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(4.1, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.1, torsional1.ODESlip()); +} + +///////////////////////////////////////////////// +TEST(DOMtorsional, Set) +{ + sdf::Torsional torsional1; + + EXPECT_DOUBLE_EQ(1.0, torsional1.Coefficient()); + EXPECT_TRUE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(0.0, torsional1.ODESlip()); + + torsional1.SetCoefficient(0.1); + torsional1.SetUsePatchRadius(false); + torsional1.SetPatchRadius(0.2); + torsional1.SetSurfaceRadius(4); + torsional1.SetODESlip(1.0); + + EXPECT_DOUBLE_EQ(0.1, torsional1.Coefficient()); + EXPECT_FALSE(torsional1.UsePatchRadius()); + EXPECT_DOUBLE_EQ(0.2, torsional1.PatchRadius()); + EXPECT_DOUBLE_EQ(4.0, torsional1.SurfaceRadius()); + EXPECT_DOUBLE_EQ(1.0, torsional1.ODESlip()); +} diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index cad9580d2..cccd3689f 100644 --- a/test/integration/surface_dom.cc +++ b/test/integration/surface_dom.cc @@ -56,4 +56,22 @@ TEST(DOMSurface, Shapes) EXPECT_DOUBLE_EQ(boxCol->Surface()->Friction()->ODE()->Slip2(), 5); EXPECT_EQ(boxCol->Surface()->Friction()->ODE()->Fdir1(), gz::math::Vector3d(1.2, 3.4, 5.6)); + + EXPECT_DOUBLE_EQ(1.6, + boxCol->Surface()->Friction()->BulletFriction()->Friction()); + EXPECT_DOUBLE_EQ(1.7, + boxCol->Surface()->Friction()->BulletFriction()->Friction2()); + EXPECT_EQ(boxCol->Surface()->Friction()->BulletFriction()->Fdir1(), + gz::math::Vector3d(6.5, 4.3, 2.1)); + EXPECT_DOUBLE_EQ(1.5, + boxCol->Surface()->Friction()->BulletFriction()->RollingFriction()); + + EXPECT_DOUBLE_EQ(5.1, + boxCol->Surface()->Friction()->Torsional()->Coefficient()); + EXPECT_FALSE(boxCol->Surface()->Friction()->Torsional()->UsePatchRadius()); + EXPECT_DOUBLE_EQ(1.3, + boxCol->Surface()->Friction()->Torsional()->PatchRadius()); + EXPECT_DOUBLE_EQ(3.7, + boxCol->Surface()->Friction()->Torsional()->SurfaceRadius()); + EXPECT_DOUBLE_EQ(3.1, boxCol->Surface()->Friction()->Torsional()->ODESlip()); } diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 49d393314..89762dae4 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -14,12 +14,27 @@ - 0.6 - 0.7 - 4 - 5 - 1.2 3.4 5.6 + 0.6 + 0.7 + 4 + 5 + 1.2 3.4 5.6 + + 1.6 + 1.7 + 6.5 4.3 2.1 + 1.5 + + + 5.1 + false + 1.3 + 3.7 + + 3.1 + + From 939786fcaa7b9fc8fa533420d2d6afd47c1f8c51 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 8 Apr 2024 00:46:11 -0700 Subject: [PATCH 02/25] Param_TEST: Check return values of Param::Get/Set (#1394) Signed-off-by: Steve Peters --- src/Param_TEST.cc | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/Param_TEST.cc b/src/Param_TEST.cc index 56f30b4e0..378a088bc 100644 --- a/src/Param_TEST.cc +++ b/src/Param_TEST.cc @@ -46,45 +46,45 @@ TEST(Param, Bool) { sdf::Param boolParam("key", "bool", "true", false, "description"); bool value = true; - boolParam.Get(value); + EXPECT_TRUE(boolParam.Get(value)); EXPECT_TRUE(value); - boolParam.Set(false); - boolParam.Get(value); + EXPECT_TRUE(boolParam.Set(false)); + EXPECT_TRUE(boolParam.Get(value)); EXPECT_FALSE(value); // String parameter that represents a boolean. sdf::Param strParam("key", "string", "true", false, "description"); - strParam.Get(value); + EXPECT_TRUE(strParam.Get(value)); EXPECT_TRUE(value); - strParam.Set("false"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("false")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_FALSE(value); - strParam.Set("1"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("1")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_TRUE(value); - strParam.Set("0"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("0")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_FALSE(value); - strParam.Set("True"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("True")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_TRUE(value); - strParam.Set("TRUE"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("TRUE")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_TRUE(value); // Anything other than 1 or true is treated as a false value - strParam.Set("%"); - strParam.Get(value); + EXPECT_TRUE(strParam.Set("%")); + EXPECT_TRUE(strParam.Get(value)); EXPECT_FALSE(value); - boolParam.Set(true); + EXPECT_TRUE(boolParam.Set(true)); std::any anyValue; EXPECT_TRUE(boolParam.GetAny(anyValue)); try From aa14ba8d11ba5b67b4ec0bf53ae48dce4e7811ad Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Mar 2024 17:41:37 -0700 Subject: [PATCH 03/25] backport - Add mesh optimization attribute to #1382 Signed-off-by: Ian Chen --- include/sdf/Mesh.hh | 72 ++++++++++++++ sdf/1.11/mesh_shape.sdf | 14 +++ src/Mesh.cc | 163 ++++++++++++++++++++++++++++++- src/Mesh_TEST.cc | 74 ++++++++++++++ test/integration/geometry_dom.cc | 6 ++ test/sdf/shapes.sdf | 5 +- 6 files changed, 332 insertions(+), 2 deletions(-) diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index f19d98818..7cc48c955 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -37,6 +37,47 @@ namespace sdf // Forward declarations. class ParserConfig; + /// \brief Mesh optimization method + enum class MeshOptimization + { + /// \brief No mesh optimization + NONE, + /// \brief Convex hull + CONVEX_HULL, + /// \brief Convex decomposition + CONVEX_DECOMPOSITION + }; + + /// \brief Convex decomposition + class SDFORMAT_VISIBLE ConvexDecomposition + { + /// \brief Default constructor + public: ConvexDecomposition(); + + /// \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 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 Get the maximum number of convex hulls that can be generated. + public: unsigned int MaxConvexHulls() const; + + /// \brief Set the maximum number of convex hulls that can be generated. + public: void SetMaxConvexHulls(unsigned int _maxConvexHulls); + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + /// \brief Mesh represents a mesh shape, and is usually accessed through a /// Geometry. class SDFORMAT_VISIBLE Mesh @@ -61,6 +102,37 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors Load(sdf::ElementPtr _sdf, const ParserConfig &_config); + /// \brief Get the mesh's optimization method + /// \return The mesh optimization method. + /// MeshOptimization::NONE if no mesh simplificaton is done. + public: MeshOptimization Optimization() const; + + /// \brief Get the mesh's optimization method + /// \return The mesh optimization method. + /// Empty string if no mesh simplificaton is done. + public: std::string OptimizationStr() const; + + /// \brief Set the mesh optimization method. + /// \param[in] _optimization The mesh optimization method. + public: void SetOptimization(MeshOptimization _optimization); + + /// \brief Set the mesh optimization method. + /// \param[in] _optimization The mesh optimization method. + /// \return True if the _optimizationStr parameter matched a known + /// mesh optimization method. False if the mesh optimization method + /// could not be set. + public: bool SetOptimization(const std::string &_optimizationStr); + + /// \brief Get the associated ConvexDecomposition object + /// \returns Pointer to the associated ConvexDecomposition object, + /// nullptr if the Mesh doesn't contain a ConvexDecomposition element. + public: const sdf::ConvexDecomposition *ConvexDecomposition() const; + + /// \brief Set the associated ConvexDecomposition object. + /// \param[in] _convexDecomposition The ConvexDecomposition object. + public: void SetConvexDecomposition( + const sdf::ConvexDecomposition &_convexDecomposition); + /// \brief Get the mesh's URI. /// \return The URI of the mesh data. public: std::string Uri() const; diff --git a/sdf/1.11/mesh_shape.sdf b/sdf/1.11/mesh_shape.sdf index 61bce76dd..a59300d46 100644 --- a/sdf/1.11/mesh_shape.sdf +++ b/sdf/1.11/mesh_shape.sdf @@ -1,5 +1,19 @@ Mesh shape + + + + Set whether to optimize the mesh using one of the specified methods. Values include: "convex_hull" - a single convex hull that encapsulates the mesh, "convex_decomposition" - decompose the mesh into multiple convex hull meshes. Default value is an empty string which means no mesh optimization. + + + + + Convex decomposition parameters. Applicable if the mesh optimization attribute is set to convex_decomposition + + Maximum number of convex hulls to decompose into. If the input mesh has multiple submeshes, this limit is applied when decomposing each submesh + + + Mesh uri diff --git a/src/Mesh.cc b/src/Mesh.cc index 9dfddcf2b..f026e2d63 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -14,8 +14,10 @@ * limitations under the License. * */ +#include #include #include +#include #include #include "sdf/CustomInertiaCalcProperties.hh" @@ -27,9 +29,35 @@ using namespace sdf; -// Private data class +/// Mesh Optimization method strings. These should match the data in +/// `enum class MeshOptimization` located in Mesh.hh, and the size +/// template parameter should match the number of elements as well. +constexpr std::array kMeshOptimizationStrs = +{ + "", + "convex_hull", + "convex_decomposition" +}; + +// Private data class for ConvexDecomposition +class sdf::ConvexDecomposition::Implementation +{ + /// \brief Maximum number of convex hulls to generate. + public: unsigned int maxConvexHulls{16u}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf = nullptr; +}; + +// Private data class for Mesh class sdf::Mesh::Implementation { + /// \brief Mesh optimization method + public: MeshOptimization optimization = MeshOptimization::NONE; + + /// \brief Optional convex decomposition. + public: std::optional convexDecomposition; + /// \brief The mesh's URI. public: std::string uri = ""; @@ -49,6 +77,62 @@ class sdf::Mesh::Implementation public: sdf::ElementPtr sdf = nullptr; }; +///////////////////////////////////////////////// +ConvexDecomposition::ConvexDecomposition() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors ConvexDecomposition::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 convex decomposition, " + "but the provided SDF element is null."}); + return errors; + } + + // We need a convex_decomposition element + if (_sdf->GetName() != "convex_decomposition") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load convex decomposition, but the provided SDF " + "element is not ."}); + return errors; + } + + this->dataPtr->maxConvexHulls = _sdf->Get( + errors, "max_convex_hulls", + this->dataPtr->maxConvexHulls).first; + + return errors; +} + +///////////////////////////////////////////////// +sdf::ElementPtr ConvexDecomposition::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +unsigned int ConvexDecomposition::MaxConvexHulls() const +{ + return this->dataPtr->maxConvexHulls; +} + +///////////////////////////////////////////////// +void ConvexDecomposition::SetMaxConvexHulls(unsigned int _maxConvexHulls) +{ + this->dataPtr->maxConvexHulls = _maxConvexHulls; +} + ///////////////////////////////////////////////// Mesh::Mesh() : dataPtr(gz::utils::MakeImpl()) @@ -61,6 +145,7 @@ Errors Mesh::Load(ElementPtr _sdf) return this->Load(_sdf, ParserConfig::GlobalConfig()); } + ///////////////////////////////////////////////// Errors Mesh::Load(ElementPtr _sdf, const ParserConfig &_config) { @@ -87,6 +172,20 @@ Errors Mesh::Load(ElementPtr _sdf, const ParserConfig &_config) return errors; } + // Optimization + if (_sdf->HasAttribute("optimization")) + { + this->SetOptimization(_sdf->Get("optimization", "").first); + } + + if (_sdf->HasElement("convex_decomposition")) + { + this->dataPtr->convexDecomposition.emplace(); + Errors err = this->dataPtr->convexDecomposition->Load( + _sdf->GetElement("convex_decomposition", errors)); + errors.insert(errors.end(), err.begin(), err.end()); + } + if (_sdf->HasElement("uri")) { std::unordered_set paths; @@ -140,6 +239,56 @@ sdf::ElementPtr Mesh::Element() const return this->dataPtr->sdf; } +////////////////////////////////////////////////// +MeshOptimization Mesh::Optimization() const +{ + return this->dataPtr->optimization; +} + +////////////////////////////////////////////////// +std::string Mesh::OptimizationStr() const +{ + size_t index = static_cast(this->dataPtr->optimization); + if (index < kMeshOptimizationStrs.size()) + return std::string(kMeshOptimizationStrs[index]); + return ""; +} + +////////////////////////////////////////////////// +bool Mesh::SetOptimization(const std::string &_optimizationStr) +{ + for (size_t i = 0; i < kMeshOptimizationStrs.size(); ++i) + { + if (_optimizationStr == kMeshOptimizationStrs[i]) + { + this->dataPtr->optimization = static_cast(i); + return true; + } + } + return false; +} + +////////////////////////////////////////////////// +void Mesh::SetOptimization(MeshOptimization _optimization) +{ + this->dataPtr->optimization = _optimization; +} + +////////////////////////////////////////////////// +const sdf::ConvexDecomposition *Mesh::ConvexDecomposition() const +{ + if (this->dataPtr->convexDecomposition.has_value()) + return &this->dataPtr->convexDecomposition.value(); + return nullptr; +} + +////////////////////////////////////////////////// + void Mesh::SetConvexDecomposition( + const sdf::ConvexDecomposition &_convexDecomposition) +{ + this->dataPtr->convexDecomposition = _convexDecomposition; +} + ////////////////////////////////////////////////// std::string Mesh::Uri() const { @@ -244,6 +393,18 @@ sdf::ElementPtr Mesh::ToElement(sdf::Errors &_errors) const sdf::ElementPtr elem(new sdf::Element); sdf::initFile("mesh_shape.sdf", elem); + // Optimization + elem->GetAttribute("optimization")->Set( + this->OptimizationStr()); + + if (this->dataPtr->convexDecomposition.has_value()) + { + sdf::ElementPtr convexDecomp = elem->GetElement("convex_decomposition", + _errors); + convexDecomp->GetElement("max_convex_hulls")->Set( + this->dataPtr->convexDecomposition->MaxConvexHulls()); + } + // Uri sdf::ElementPtr uriElem = elem->GetElement("uri", _errors); uriElem->Set(_errors, this->Uri()); diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 51c1751f4..29ef70d85 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -34,6 +34,9 @@ TEST(DOMMesh, Construction) sdf::Mesh mesh; EXPECT_EQ(nullptr, mesh.Element()); + EXPECT_EQ(std::string(), mesh.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::NONE, mesh.Optimization()); + EXPECT_EQ(nullptr, mesh.ConvexDecomposition()); EXPECT_EQ(std::string(), mesh.FilePath()); EXPECT_EQ(std::string(), mesh.Uri()); EXPECT_EQ(std::string(), mesh.Submesh()); @@ -45,24 +48,37 @@ TEST(DOMMesh, Construction) TEST(DOMMesh, MoveConstructor) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_decomposition")); mesh.SetUri("banana"); mesh.SetSubmesh("watermelon"); mesh.SetCenterSubmesh(true); mesh.SetScale({0.5, 0.6, 0.7}); mesh.SetFilePath("/pear"); + sdf::ConvexDecomposition convexDecomp; + EXPECT_EQ(nullptr, convexDecomp.Element()); + convexDecomp.SetMaxConvexHulls(10u); + mesh.SetConvexDecomposition(convexDecomp); + sdf::Mesh mesh2(std::move(mesh)); + EXPECT_EQ("convex_decomposition", mesh2.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_DECOMPOSITION, mesh2.Optimization()); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); EXPECT_TRUE(mesh2.CenterSubmesh()); EXPECT_EQ("/pear", mesh2.FilePath()); + + auto convexDecomp2 = mesh2.ConvexDecomposition(); + ASSERT_NE(nullptr, convexDecomp2); + EXPECT_EQ(10u, convexDecomp2->MaxConvexHulls()); } ///////////////////////////////////////////////// TEST(DOMMesh, CopyConstructor) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); mesh.SetUri("banana"); mesh.SetSubmesh("watermelon"); mesh.SetCenterSubmesh(true); @@ -70,6 +86,9 @@ TEST(DOMMesh, CopyConstructor) mesh.SetFilePath("/pear"); sdf::Mesh mesh2(mesh); + EXPECT_EQ("convex_hull", mesh2.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_HULL, mesh2.Optimization()); + EXPECT_EQ(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); @@ -81,6 +100,7 @@ TEST(DOMMesh, CopyConstructor) TEST(DOMMesh, CopyAssignmentOperator) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); mesh.SetUri("banana"); mesh.SetSubmesh("watermelon"); mesh.SetCenterSubmesh(true); @@ -89,6 +109,9 @@ TEST(DOMMesh, CopyAssignmentOperator) sdf::Mesh mesh2; mesh2 = mesh; + EXPECT_EQ("convex_hull", mesh2.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_HULL, mesh2.Optimization()); + EXPECT_EQ(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); @@ -100,6 +123,7 @@ TEST(DOMMesh, CopyAssignmentOperator) TEST(DOMMesh, MoveAssignmentOperator) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); mesh.SetUri("banana"); mesh.SetSubmesh("watermelon"); mesh.SetCenterSubmesh(true); @@ -108,6 +132,9 @@ TEST(DOMMesh, MoveAssignmentOperator) sdf::Mesh mesh2; mesh2 = std::move(mesh); + EXPECT_EQ("convex_hull", mesh2.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_HULL, mesh2.Optimization()); + EXPECT_EQ(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ("banana", mesh2.Uri()); EXPECT_EQ("watermelon", mesh2.Submesh()); EXPECT_EQ(gz::math::Vector3d(0.5, 0.6, 0.7), mesh2.Scale()); @@ -140,6 +167,29 @@ TEST(DOMMesh, Set) sdf::Mesh mesh; EXPECT_EQ(nullptr, mesh.Element()); + EXPECT_EQ(std::string(), mesh.OptimizationStr()); + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); + EXPECT_EQ("convex_hull", mesh.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_HULL, mesh.Optimization()); + mesh.SetOptimization(sdf::MeshOptimization::CONVEX_DECOMPOSITION); + EXPECT_EQ("convex_decomposition", mesh.OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_DECOMPOSITION, + mesh.Optimization()); + // check invalid inputs + EXPECT_FALSE(mesh.SetOptimization("invalid")); + { + auto invalidMeshOpt = static_cast(99); + mesh.SetOptimization(invalidMeshOpt); + EXPECT_EQ(invalidMeshOpt, mesh.Optimization()); + EXPECT_EQ("", mesh.OptimizationStr()); + } + + sdf::ConvexDecomposition convexDecomp; + convexDecomp.SetMaxConvexHulls(10u); + mesh.SetConvexDecomposition(convexDecomp); + ASSERT_NE(nullptr, mesh.ConvexDecomposition()); + EXPECT_EQ(10u, mesh.ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ(std::string(), mesh.Uri()); mesh.SetUri("http://myuri.com"); EXPECT_EQ("http://myuri.com", mesh.Uri()); @@ -165,6 +215,7 @@ TEST(DOMMesh, Set) TEST(DOMMesh, Load) { sdf::Mesh mesh; + sdf::ConvexDecomposition convexDecomp; sdf::Errors errors; // Null element name @@ -173,6 +224,11 @@ TEST(DOMMesh, Load) EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); EXPECT_EQ(nullptr, mesh.Element()); + errors = convexDecomp.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, convexDecomp.Element()); + // Bad element name sdf::ElementPtr sdf(new sdf::Element()); sdf->SetName("bad"); @@ -181,6 +237,11 @@ TEST(DOMMesh, Load) EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); EXPECT_NE(nullptr, mesh.Element()); + errors = convexDecomp.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, convexDecomp.Element()); + // Missing element sdf->SetName("mesh"); errors = mesh.Load(sdf); @@ -296,21 +357,30 @@ TEST(DOMMesh, ToElement) { sdf::Mesh mesh; + EXPECT_TRUE(mesh.SetOptimization("convex_decomposition")); mesh.SetUri("mesh-uri"); mesh.SetScale(gz::math::Vector3d(1, 2, 3)); mesh.SetSubmesh("submesh"); mesh.SetCenterSubmesh(false); + sdf::ConvexDecomposition convexDecomp; + convexDecomp.SetMaxConvexHulls(10u); + mesh.SetConvexDecomposition(convexDecomp); + sdf::ElementPtr elem = mesh.ToElement(); ASSERT_NE(nullptr, elem); sdf::Mesh mesh2; mesh2.Load(elem); + EXPECT_EQ(mesh.OptimizationStr(), mesh2.OptimizationStr()); + EXPECT_EQ(mesh.Optimization(), mesh2.Optimization()); EXPECT_EQ(mesh.Uri(), mesh2.Uri()); EXPECT_EQ(mesh.Scale(), mesh2.Scale()); EXPECT_EQ(mesh.Submesh(), mesh2.Submesh()); EXPECT_EQ(mesh.CenterSubmesh(), mesh2.CenterSubmesh()); + ASSERT_NE(nullptr, mesh2.ConvexDecomposition()); + EXPECT_EQ(10u, mesh2.ConvexDecomposition()->MaxConvexHulls()); } ///////////////////////////////////////////////// @@ -332,6 +402,7 @@ TEST(DOMMesh, ToElementErrorOutput) sdf::Mesh mesh; sdf::Errors errors; + EXPECT_TRUE(mesh.SetOptimization("convex_hull")); mesh.SetUri("mesh-uri"); mesh.SetScale(gz::math::Vector3d(1, 2, 3)); mesh.SetSubmesh("submesh"); @@ -345,6 +416,9 @@ TEST(DOMMesh, ToElementErrorOutput) errors = mesh2.Load(elem); EXPECT_TRUE(errors.empty()); + EXPECT_EQ(mesh.OptimizationStr(), mesh2.OptimizationStr()); + EXPECT_EQ(mesh.Optimization(), mesh2.Optimization()); + EXPECT_EQ(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ(mesh.Uri(), mesh2.Uri()); EXPECT_EQ(mesh.Scale(), mesh2.Scale()); EXPECT_EQ(mesh.Submesh(), mesh2.Submesh()); diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 07d50d570..297d1f0af 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -179,6 +179,12 @@ TEST(DOMGeometry, Shapes) EXPECT_EQ(sdf::GeometryType::MESH, meshCol->Geom()->Type()); const sdf::Mesh *meshColGeom = meshCol->Geom()->MeshShape(); ASSERT_NE(nullptr, meshColGeom); + EXPECT_EQ("convex_decomposition", meshColGeom->OptimizationStr()); + EXPECT_EQ(sdf::MeshOptimization::CONVEX_DECOMPOSITION, + meshColGeom->Optimization()); + ASSERT_NE(nullptr, meshColGeom->ConvexDecomposition()); + EXPECT_EQ(4u, meshColGeom->ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ("https://fuel.gazebosim.org/1.0/an_org/models/a_model/mesh/" "mesh.dae", meshColGeom->Uri()); EXPECT_TRUE(gz::math::Vector3d(0.1, 0.2, 0.3) == diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 1f5f4fa27..f411afa37 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -120,7 +120,10 @@ - + + + 4 + https://fuel.gazebosim.org/1.0/an_org/models/a_model/mesh/mesh.dae my_submesh From d50b3ff90e3b709ef64bfca9b400d0e071f97009 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 26 Mar 2024 14:00:35 -0700 Subject: [PATCH 04/25] backport - Update max_convex_hulls description #1386 Signed-off-by: Ian Chen --- sdf/1.11/mesh_shape.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sdf/1.11/mesh_shape.sdf b/sdf/1.11/mesh_shape.sdf index a59300d46..1ed64ca44 100644 --- a/sdf/1.11/mesh_shape.sdf +++ b/sdf/1.11/mesh_shape.sdf @@ -10,7 +10,7 @@ Convex decomposition parameters. Applicable if the mesh optimization attribute is set to convex_decomposition - Maximum number of convex hulls to decompose into. If the input mesh has multiple submeshes, this limit is applied when decomposing each submesh + Maximum number of convex hulls to decompose into. This sets the maximum number of submeshes that the final decomposed mesh will contain. From 1c0bc304d43285223e69e52abc8c3d9269d67713 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 9 Apr 2024 22:45:58 +0200 Subject: [PATCH 05/25] Added Python wrapper to ConvexDecomposition (#1398) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Ian Chen --- python/CMakeLists.txt | 1 + python/src/sdf/_gz_sdformat_pybind11.cc | 2 + python/src/sdf/pyConvexDecomposition.cc | 50 +++++++++++++++++++++++++ python/src/sdf/pyConvexDecomposition.hh | 41 ++++++++++++++++++++ python/src/sdf/pyMesh.cc | 22 +++++++++++ python/test/pyMesh_TEST.py | 44 +++++++++++++++++++++- 6 files changed, 159 insertions(+), 1 deletion(-) create mode 100644 python/src/sdf/pyConvexDecomposition.cc create mode 100644 python/src/sdf/pyConvexDecomposition.hh diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 62078d8db..f9cc0917d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -52,6 +52,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/sdf/pyCamera.cc src/sdf/pyCapsule.cc src/sdf/pyCollision.cc + src/sdf/pyConvexDecomposition.cc src/sdf/pyCylinder.cc src/sdf/pyElement.cc src/sdf/pyEllipsoid.cc diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index d982e13f4..298cd1362 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -26,6 +26,7 @@ #include "pyCamera.hh" #include "pyCapsule.hh" #include "pyCollision.hh" +#include "pyConvexDecomposition.hh" #include "pyCylinder.hh" #include "pyElement.hh" #include "pyEllipsoid.hh" @@ -85,6 +86,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineCamera(m); sdf::python::defineCapsule(m); sdf::python::defineCollision(m); + sdf::python::defineConvexDecomposition(m); sdf::python::defineContact(m); sdf::python::defineCylinder(m); // PrintConfig has to be defined before Param and Element because it's used as diff --git a/python/src/sdf/pyConvexDecomposition.cc b/python/src/sdf/pyConvexDecomposition.cc new file mode 100644 index 000000000..aacd7ee42 --- /dev/null +++ b/python/src/sdf/pyConvexDecomposition.cc @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "pyMesh.hh" + +#include + +#include "sdf/ParserConfig.hh" +#include "sdf/Mesh.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineConvexDecomposition(pybind11::object module) +{ + pybind11::class_(module, "ConvexDecomposition") + .def(pybind11::init<>()) + .def("max_convex_hulls", &sdf::ConvexDecomposition::MaxConvexHulls, + "Get the maximum number of convex hulls that can be generated.") + .def("set_max_convex_hulls", &sdf::ConvexDecomposition::SetMaxConvexHulls, + "Set the maximum number of convex hulls that can be generated.") + .def("__copy__", [](const sdf::ConvexDecomposition &self) { + return sdf::ConvexDecomposition(self); + }) + .def("__deepcopy__", [](const sdf::ConvexDecomposition &self, pybind11::dict) { + return sdf::ConvexDecomposition(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyConvexDecomposition.hh b/python/src/sdf/pyConvexDecomposition.hh new file mode 100644 index 000000000..78a58840f --- /dev/null +++ b/python/src/sdf/pyConvexDecomposition.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SDFORMAT_PYTHON_CONVEX_DECOMPOSITION_HH_ +#define SDFORMAT_PYTHON_CONVEX_DECOMPOSITION_HH_ + +#include + +#include "sdf/Mesh.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::ConvexDecomposition +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineConvexDecomposition(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_CONVEX_DECOMPOSITION_HH_ diff --git a/python/src/sdf/pyMesh.cc b/python/src/sdf/pyMesh.cc index 85705f874..b9fdafd17 100644 --- a/python/src/sdf/pyMesh.cc +++ b/python/src/sdf/pyMesh.cc @@ -35,6 +35,23 @@ void defineMesh(pybind11::object module) pybind11::class_(module, "Mesh") .def(pybind11::init<>()) .def(pybind11::init()) + .def("optimization", &sdf::Mesh::Optimization, + "Get the mesh's optimization method.") + .def("optimization_str", &sdf::Mesh::OptimizationStr, + "Get the mesh's optimization method") + .def("set_optimization", + pybind11::overload_cast( + &sdf::Mesh::SetOptimization), + "Set the mesh optimization method.") + .def("set_optimization", + pybind11::overload_cast( + &sdf::Mesh::SetOptimization), + "Set the mesh optimization method.") + .def("convex_decomposition", &sdf::Mesh::ConvexDecomposition, + pybind11::return_value_policy::reference_internal, + "Get the associated ConvexDecomposition object") + .def("set_convex_decomposition", &sdf::Mesh::SetConvexDecomposition, + "Set the associated ConvexDecomposition object.") .def("uri", &sdf::Mesh::Uri, "Get the mesh's URI.") .def("set_uri", &sdf::Mesh::SetUri, @@ -67,6 +84,11 @@ void defineMesh(pybind11::object module) .def("__deepcopy__", [](const sdf::Mesh &self, pybind11::dict) { return sdf::Mesh(self); }, "memo"_a); + + pybind11::enum_(module, "MeshOptimization") + .value("NONE", sdf::MeshOptimization::NONE) + .value("CONVEX_HULL", sdf::MeshOptimization::CONVEX_HULL) + .value("CONVEX_DECOMPOSITION", sdf::MeshOptimization::CONVEX_DECOMPOSITION); } } // namespace python } // namespace SDF_VERSION_NAMESPACE diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index 64d89a57a..ac9840ea6 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -13,8 +13,9 @@ # limitations under the License. import copy -from gz_test_deps.sdformat import Mesh +from gz_test_deps.sdformat import Mesh, ConvexDecomposition from gz_test_deps.math import Vector3d +import gz_test_deps.sdformat as sdf import unittest @@ -23,6 +24,9 @@ class MeshTEST(unittest.TestCase): def test_default_construction(self): mesh = Mesh() + self.assertEqual("", mesh.optimization_str()) + self.assertEqual(sdf.MeshOptimization.NONE, mesh.optimization()) + self.assertEqual(None, mesh.convex_decomposition()) self.assertEqual("", mesh.file_path()) self.assertEqual("", mesh.uri()) self.assertEqual("", mesh.submesh()) @@ -32,19 +36,29 @@ def test_default_construction(self): def test_assigment(self): mesh = Mesh() + self.assertTrue(mesh.set_optimization("convex_decomposition")) mesh.set_uri("banana") mesh.set_submesh("watermelon") mesh.set_center_submesh(True) mesh.set_scale(Vector3d(0.5, 0.6, 0.7)) mesh.set_file_path("/pear") + convexDecomp = ConvexDecomposition() + convexDecomp.set_max_convex_hulls(10) + mesh.set_convex_decomposition(convexDecomp) + mesh2 = mesh + self.assertEqual("convex_decomposition", mesh2.optimization_str()) + self.assertEqual(sdf.MeshOptimization.CONVEX_DECOMPOSITION, mesh2.optimization()) self.assertEqual("banana", mesh2.uri()) self.assertEqual("watermelon", mesh2.submesh()) self.assertEqual(Vector3d(0.5, 0.6, 0.7), mesh2.scale()) self.assertTrue(mesh2.center_submesh()) self.assertEqual("/pear", mesh2.file_path()) + convexDecomp2 = mesh2.convex_decomposition() + self.assertEqual(10, convexDecomp2.max_convex_hulls()) + mesh.set_file_path("/apple") self.assertEqual("/apple", mesh2.file_path()) @@ -63,19 +77,29 @@ def test_assigment(self): def test_deepcopy_construction(self): mesh = Mesh() + self.assertTrue(mesh.set_optimization("convex_decomposition")) mesh.set_uri("banana") mesh.set_submesh("watermelon") mesh.set_center_submesh(True) mesh.set_scale(Vector3d(0.5, 0.6, 0.7)) mesh.set_file_path("/pear") + convexDecomp = ConvexDecomposition() + convexDecomp.set_max_convex_hulls(10) + mesh.set_convex_decomposition(convexDecomp) + mesh2 = copy.deepcopy(mesh) + self.assertEqual("convex_decomposition", mesh2.optimization_str()) + self.assertEqual(sdf.MeshOptimization.CONVEX_DECOMPOSITION, mesh2.optimization()) self.assertEqual("banana", mesh2.uri()) self.assertEqual("watermelon", mesh2.submesh()) self.assertEqual(Vector3d(0.5, 0.6, 0.7), mesh2.scale()) self.assertTrue(mesh2.center_submesh()) self.assertEqual("/pear", mesh2.file_path()) + convexDecomp2 = mesh2.convex_decomposition() + self.assertEqual(10, convexDecomp2.max_convex_hulls()) + mesh.set_file_path("/apple") mesh.set_scale(Vector3d(0.3, 0.2, 0.4)) mesh.set_center_submesh(False) @@ -92,6 +116,24 @@ def test_deepcopy_construction(self): def test_set(self): mesh = Mesh() + self.assertEqual("", mesh.optimization_str()) + self.assertTrue(mesh.set_optimization("convex_hull")) + self.assertEqual("convex_hull", mesh.optimization_str()) + self.assertEqual(sdf.MeshOptimization.CONVEX_HULL, mesh.optimization()) + mesh.set_optimization(sdf.MeshOptimization.CONVEX_DECOMPOSITION) + self.assertEqual("convex_decomposition", mesh.optimization_str()) + self.assertEqual(sdf.MeshOptimization.CONVEX_DECOMPOSITION, mesh.optimization()) + + self.assertFalse(mesh.set_optimization("invalid")) + mesh.set_optimization(sdf.MeshOptimization(99)) + self.assertEqual(sdf.MeshOptimization(99), mesh.optimization()) + self.assertEqual("", mesh.optimization_str()) + + convexDecomp = ConvexDecomposition() + convexDecomp.set_max_convex_hulls(10) + mesh.set_convex_decomposition(convexDecomp) + self.assertEqual(10, mesh.convex_decomposition().max_convex_hulls()) + self.assertEqual("", mesh.uri()) mesh.set_uri("http://myuri.com") self.assertEqual("http://myuri.com", mesh.uri()) From 3e93e685486ea4a1eed710284179aebce2505f88 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 19 Apr 2024 15:13:50 -0500 Subject: [PATCH 06/25] Add package.xml, fix `gz sdf` tests on Windows (#1374) The `gz sdf` tests are fixed by * fixing dll path issue with Ruby on windows * Setting home path --------- Signed-off-by: Addisu Z. Taddese --- .github/workflows/package_xml.yml | 11 ++++++ package.xml | 32 +++++++++++++++++ src/Console.cc | 6 ---- src/cmd/CMakeLists.txt | 8 ++++- src/cmd/cmdsdformat.rb.in | 16 ++++++--- src/gz_TEST.cc | 58 +++++++++++++++++-------------- 6 files changed, 94 insertions(+), 37 deletions(-) create mode 100644 .github/workflows/package_xml.yml create mode 100644 package.xml diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 000000000..4bd4a9aa0 --- /dev/null +++ b/.github/workflows/package_xml.yml @@ -0,0 +1,11 @@ +name: Validate package.xml + +on: + pull_request: + +jobs: + package-xml: + runs-on: ubuntu-latest + name: Validate package.xml + steps: + - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy diff --git a/package.xml b/package.xml new file mode 100644 index 000000000..33d829ad8 --- /dev/null +++ b/package.xml @@ -0,0 +1,32 @@ + + + sdformat14 + 14.1.1 + SDFormat is an XML file format that describes environments, objects, and robots +in a manner suitable for robotic applications + + Addisu Z. Taddese + Steve Peters + + Apache License 2.0 + + https://github.com/gazebosim/sdformat + + cmake + gz-cmake3 + gz-math7 + gz-utils2 + tinyxml2 + liburdfdom-dev + pybind11-dev + + gz-tools2 + + libxml2-utils + python3-psutil + python3-pytest + + + cmake + + diff --git a/src/Console.cc b/src/Console.cc index d9ec8d044..558f10d87 100644 --- a/src/Console.cc +++ b/src/Console.cc @@ -34,13 +34,7 @@ using namespace sdf; static std::shared_ptr myself; static std::mutex g_instance_mutex; -/// \todo Output disabled for windows, to allow tests to pass. We should -/// disable output just for tests on windows. -#ifndef _WIN32 static bool g_quiet = false; -#else -static bool g_quiet = true; -#endif static Console::ConsoleStream g_NullStream(nullptr); diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index a1cf707c1..c1c91078b 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -31,7 +31,13 @@ set(cmd_script_configured "${CMAKE_CURRENT_BINARY_DIR}/cmd${PROJECT_NAME}.rb.con # Set the library_location variable to the relative path to the library file # within the install directory structure. -set(library_location "../../../${CMAKE_INSTALL_LIBDIR}/$") +if (MSVC) + set(library_location_prefix "${CMAKE_INSTALL_BINDIR}") +else() + set(library_location_prefix "${CMAKE_INSTALL_LIBDIR}") +endif() + +set(library_location "../../../${library_location_prefix}/$") configure_file( "cmd${PROJECT_NAME_NO_VERSION_LOWER}.rb.in" diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 7c93d7680..c4bd35f4c 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -26,6 +26,8 @@ else end require 'optparse' +require 'pathname' + # Constants. LIBRARY_NAME = '@library_location@' @@ -174,9 +176,7 @@ class Cmd # puts options # Read the plugin that handles the command. - if LIBRARY_NAME[0] == '/' - # If the first character is a slash, we'll assume that we've been given an - # absolute path to the library. This is only used during test mode. + if Pathname.new(LIBRARY_NAME).absolute? plugin = LIBRARY_NAME else # We're assuming that the library path is relative to the current @@ -185,10 +185,18 @@ class Cmd end conf_version = LIBRARY_VERSION + if defined? RubyInstaller + # RubyInstaller does not search for dlls in PATH or the directory that tests are running from, + # so we'll add the parent directory of the plugin to the search path. + # https://github.com/oneclick/rubyinstaller2/wiki/For-gem-developers#-dll-loading + RubyInstaller::Runtime.add_dll_directory(File.dirname(plugin)) + end + begin Importer.dlload plugin - rescue DLError + rescue DLError => error puts "Library error: [#{plugin}] not found." + puts "DLError: #{error.message}" exit(-1) end diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 816882f56..2e16d19db 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -73,7 +73,7 @@ std::string custom_exec_str(std::string _cmd) } ///////////////////////////////////////////////// -TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(checkUnrecognizedElements, SDF) { // Check an SDFormat file with unrecognized elements { @@ -120,7 +120,7 @@ TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check, SDF) { // Check a good SDF file { @@ -1011,7 +1011,7 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check_shapes_sdf, SDF) { { const auto path = @@ -1035,7 +1035,7 @@ TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check_model_sdf, SDF) { // Check a good SDF file by passing the absolute path { @@ -1062,7 +1062,7 @@ TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(describe, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(describe, SDF) { // Get the description std::string output = @@ -1074,7 +1074,7 @@ TEST(describe, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print, SDF) { // Check a good SDF file { @@ -1103,7 +1103,7 @@ TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_degrees, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_degrees.sdf"); @@ -1171,7 +1171,7 @@ TEST(print_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_radians, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_radians.sdf"); @@ -1239,7 +1239,7 @@ TEST(print_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_quaternions, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_quaternions, SDF) { const auto path = sdf::testing::TestFile( "sdf", "rotations_in_quaternions.sdf"); @@ -1308,7 +1308,7 @@ TEST(print_rotations_in_quaternions, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_degrees, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1379,7 +1379,7 @@ TEST(print_includes_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_radians, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1450,8 +1450,7 @@ TEST(print_includes_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_quaternions, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_quaternions, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1523,8 +1522,7 @@ TEST(print_includes_rotations_in_quaternions, } ///////////////////////////////////////////////// -TEST(print_rotations_in_unnormalized_degrees, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_unnormalized_degrees, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_degrees.sdf"); @@ -1595,8 +1593,7 @@ TEST(print_rotations_in_unnormalized_degrees, } ///////////////////////////////////////////////// -TEST(print_rotations_in_unnormalized_radians, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_unnormalized_radians, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); @@ -1664,7 +1661,7 @@ TEST(print_rotations_in_unnormalized_radians, } ///////////////////////////////////////////////// -TEST(shuffled_cmd_flags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(shuffled_cmd_flags, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); @@ -1713,8 +1710,7 @@ TEST(shuffled_cmd_flags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_snap_to_degrees_tolerance_too_high, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_snap_to_degrees_tolerance_too_high, SDF) { const std::string path = sdf::testing::TestFile( "sdf", @@ -1731,7 +1727,7 @@ TEST(print_snap_to_degrees_tolerance_too_high, } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) +TEST(GraphCmd, WorldPoseRelativeTo) { // world pose relative_to graph const std::string path = @@ -1780,7 +1776,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) +TEST(GraphCmd, ModelPoseRelativeTo) { const auto path = sdf::testing::TestFile("sdf", "model_relative_to_nested_reference.sdf"); @@ -1857,7 +1853,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) +TEST(GraphCmd, WorldFrameAttachedTo) { const auto path = sdf::testing::TestFile("sdf", "world_nested_frame_attached_to.sdf"); @@ -1903,7 +1899,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) +TEST(GraphCmd, ModelFrameAttachedTo) { const auto path = sdf::testing::TestFile("sdf", "model_nested_frame_attached_to.sdf"); @@ -1955,7 +1951,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) // Disable on arm #if !defined __ARM_ARCH ///////////////////////////////////////////////// -TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(inertial_stats, SDF) { std::string expectedOutput = "Inertial statistics for model: test_model\n" @@ -2043,7 +2039,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ////////////////////////////////////////////////// /// \brief Check help message and bash completion script for consistent flags -TEST(HelpVsCompletionFlags, SDF) +TEST(HelpVsCompletionFlags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { // Flags in help message std::string helpOutput = custom_exec_str(GzCommand() + " sdf --help"); @@ -2098,6 +2094,16 @@ int main(int argc, char **argv) gz::utils::setenv("LD_LIBRARY_PATH", testLibraryPath); #endif + // temporarily set HOME + std::string homeDir; + sdf::testing::TestTmpPath(homeDir); + +#ifdef _WIN32 + gz::utils::setenv("HOMEPATH", homeDir); +#else + gz::utils::setenv("HOME", homeDir); +#endif + ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From 2799934dc3429b2ce9234043756756ee52847c3c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 28 Mar 2024 17:20:15 -0700 Subject: [PATCH 07/25] Fix warning with pybind11 2.12 (#1389) Signed-off-by: Steve Peters --- python/src/sdf/_gz_sdformat_pybind11.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index a6f587193..80caf4396 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -141,7 +141,11 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdfErrorsException.attr("errors") = pybind11::cast(e.Errors()); // This has to be called last since it's the call that sets // PyErr_SetString. +#if PYBIND11_VERSION_HEX >= 0x020C0000 + pybind11::set_error(sdfErrorsException, e.what()); +#else sdfErrorsException(e.what()); +#endif } }); From 2f2d8e13a23a7473b5903f1a0f8e74ad2c1e9344 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 22 Apr 2024 19:32:34 +0200 Subject: [PATCH 08/25] Fix trivial warning on 24.04 for JointAxis_TEST.cc (#1402) * Fix trivial warning on 24.04 for JointAxis_TEST.cc --------- Signed-off-by: Jose Luis Rivero Co-authored-by: Steve Peters --- src/JointAxis_TEST.cc | 50 +++++++++++++++++++++---------------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/src/JointAxis_TEST.cc b/src/JointAxis_TEST.cc index 0e04be6fd..bc612c689 100644 --- a/src/JointAxis_TEST.cc +++ b/src/JointAxis_TEST.cc @@ -226,58 +226,58 @@ TEST(DOMJointAxis, ToElement) sdf::ElementPtr dynElem = elem->GetElement("dynamics", errors); ASSERT_TRUE(errors.empty()); - double damping = 0; - damping = dynElem->Get(errors, "damping", damping).first; + double damping; + damping = dynElem->Get(errors, "damping", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(0.2, damping); - double friction = 0; - friction = dynElem->Get(errors, "friction", friction).first; + double friction; + friction = dynElem->Get(errors, "friction", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(1.3, friction); - double springReference = 0; + double springReference; springReference = dynElem->Get( - errors, "spring_reference", springReference).first; + errors, "spring_reference", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(2.4, springReference); - double springStiffness = 0; + double springStiffness; springStiffness = dynElem->Get( - errors, "spring_stiffness", springStiffness).first; + errors, "spring_stiffness", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(-1.2, springStiffness); // Check //axis/limit sdf::ElementPtr limitElem = elem->GetElement("limit", errors); - double lower = 0; - lower = limitElem->Get(errors, "lower", lower).first; + double lower; + lower = limitElem->Get(errors, "lower", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(-10.8, lower); - double upper = 0; - upper = limitElem->Get(errors, "upper", upper).first; + double upper; + upper = limitElem->Get(errors, "upper", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(123.4, upper); - double effort = 0; - effort = limitElem->Get(errors, "effort", effort).first; + double effort; + effort = limitElem->Get(errors, "effort", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(3.2, effort); - double maxVel = 0; - maxVel = limitElem->Get(errors, "velocity", maxVel).first; + double maxVel; + maxVel = limitElem->Get(errors, "velocity", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(54.2, maxVel); - double stiffness = 0; - stiffness = limitElem->Get(errors, "stiffness", stiffness).first; + double stiffness; + stiffness = limitElem->Get(errors, "stiffness", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(1e2, stiffness); - double dissipation = 0; + double dissipation; dissipation = limitElem->Get( - errors, "dissipation", dissipation).first; + errors, "dissipation", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(1.5, dissipation); @@ -286,31 +286,31 @@ TEST(DOMJointAxis, ToElement) ASSERT_NE(nullptr, mimicElem); std::string mimicJointName; mimicJointName = mimicElem->Get( - errors, "joint", mimicJointName).first; + errors, "joint", "").first; ASSERT_TRUE(errors.empty()); EXPECT_EQ("test_joint", mimicJointName); std::string mimicAxisName; mimicAxisName = mimicElem->Get( - errors, "axis", mimicAxisName).first; + errors, "axis", "").first; ASSERT_TRUE(errors.empty()); EXPECT_EQ("axis2", mimicAxisName); double multiplier; multiplier = mimicElem->Get( - errors, "multiplier", multiplier).first; + errors, "multiplier", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(5.0, multiplier); double offset; offset = mimicElem->Get( - errors, "offset", offset).first; + errors, "offset", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(1.0, offset); double reference; reference = mimicElem->Get( - errors, "reference", reference).first; + errors, "reference", 0.0).first; ASSERT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(2.0, reference); From 6f1c36502f1085836ab9876e26afac3238f26820 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 23 Apr 2024 10:54:04 -0700 Subject: [PATCH 09/25] Prepare for 14.2.0 release (#1405) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 16 ++++++++++++++++ package.xml | 2 +- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f18d73f91..f771a9c65 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0004 NEW) endif(COMMAND CMAKE_POLICY) -project (sdformat14 VERSION 14.1.1) +project (sdformat14 VERSION 14.2.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software diff --git a/Changelog.md b/Changelog.md index c41c7af4e..ecb491ea1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,21 @@ ## libsdformat 14.X +### libsdformat 14.2.0 (2024-04-23) + +1. Fix trivial warning on 24.04 for JointAxis_TEST.cc + * [Pull request #1402](https://github.com/gazebosim/sdformat/pull/1402) + +1. Add package.xml, fix `gz sdf` tests on Windows + * [Pull request #1374](https://github.com/gazebosim/sdformat/pull/1374) + +1. Backport mesh optimization feature + * [Pull request #1398](https://github.com/gazebosim/sdformat/pull/1398) + * [Pull request #1386](https://github.com/gazebosim/sdformat/pull/1386) + * [Pull request #1382](https://github.com/gazebosim/sdformat/pull/1382) + +1. Param_TEST: Check return values of Param::Get/Set + * [Pull request #1394](https://github.com/gazebosim/sdformat/pull/1394) + ### libsdformat 14.1.1 (2024-03-28) 1. Fix warning with pybind11 2.12 diff --git a/package.xml b/package.xml index 33d829ad8..3132f5f8f 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ sdformat14 - 14.1.1 + 14.2.0 SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications From e9a2c01d7fef44d32b26e3d2c87bfd1a7bbf745c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 10 May 2024 03:03:24 -0500 Subject: [PATCH 10/25] Fix macOS workflow and backport windows fix (#1409) Partially backports #1374 to fix #1321 * Add package.xml, fix `gz sdf` tests on Windows (#1374) The `gz sdf` tests are fixed by * fixing dll path issue with Ruby on windows * Setting home path * Fix macOS workflow Signed-off-by: Addisu Z. Taddese --- .github/workflows/macos.yml | 14 ++++----- src/Console.cc | 6 ---- src/cmd/CMakeLists.txt | 8 ++++- src/cmd/cmdsdformat.rb.in | 16 +++++++--- src/gz_TEST.cc | 58 ++++++++++++++++++++----------------- 5 files changed, 58 insertions(+), 44 deletions(-) diff --git a/.github/workflows/macos.yml b/.github/workflows/macos.yml index 0842e3e5d..4430c4fef 100644 --- a/.github/workflows/macos.yml +++ b/.github/workflows/macos.yml @@ -19,11 +19,11 @@ jobs: # Workaround for https://github.com/actions/setup-python/issues/577 - name: Clean up python binaries run: | - rm -f /usr/local/bin/2to3*; - rm -f /usr/local/bin/idle3*; - rm -f /usr/local/bin/pydoc3*; - rm -f /usr/local/bin/python3*; - rm -f /usr/local/bin/python3*-config; + rm -f $(brew --prefix)/bin/2to3*; + rm -f $(brew --prefix)/bin/idle3*; + rm -f $(brew --prefix)/bin/pydoc3*; + rm -f $(brew --prefix)/bin/python3*; + rm -f $(brew --prefix)/bin/python3*-config; - name: Install base dependencies env: HOMEBREW_NO_INSTALLED_DEPENDENTS_CHECK: 1 @@ -50,8 +50,8 @@ jobs: - name: cmake working-directory: build run: | - export PYTHONPATH=$PYTHONPATH:/usr/local/lib/python - cmake .. -DCMAKE_INSTALL_PREFIX=/usr/local/Cellar/${PACKAGE}/HEAD + export PYTHONPATH=$PYTHONPATH:$(brew --prefix)/lib/python + cmake .. -DCMAKE_INSTALL_PREFIX=$(brew --prefix)/Cellar/${PACKAGE}/HEAD -DPython3_EXECUTABLE=$(brew --prefix python3)/bin/python3 - run: make working-directory: build - run: make test diff --git a/src/Console.cc b/src/Console.cc index d9ec8d044..558f10d87 100644 --- a/src/Console.cc +++ b/src/Console.cc @@ -34,13 +34,7 @@ using namespace sdf; static std::shared_ptr myself; static std::mutex g_instance_mutex; -/// \todo Output disabled for windows, to allow tests to pass. We should -/// disable output just for tests on windows. -#ifndef _WIN32 static bool g_quiet = false; -#else -static bool g_quiet = true; -#endif static Console::ConsoleStream g_NullStream(nullptr); diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index a869d5fca..222ce5c5e 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -31,7 +31,13 @@ set(cmd_script_configured "${CMAKE_CURRENT_BINARY_DIR}/cmd${PROJECT_NAME}.rb.con # Set the library_location variable to the relative path to the library file # within the install directory structure. -set(library_location "../../../${CMAKE_INSTALL_LIBDIR}/$") +if (MSVC) + set(library_location_prefix "${CMAKE_INSTALL_BINDIR}") +else() + set(library_location_prefix "${CMAKE_INSTALL_LIBDIR}") +endif() + +set(library_location "../../../${library_location_prefix}/$") configure_file( "cmd${PROJECT_NAME_NO_VERSION_LOWER}.rb.in" diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 7c93d7680..c4bd35f4c 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -26,6 +26,8 @@ else end require 'optparse' +require 'pathname' + # Constants. LIBRARY_NAME = '@library_location@' @@ -174,9 +176,7 @@ class Cmd # puts options # Read the plugin that handles the command. - if LIBRARY_NAME[0] == '/' - # If the first character is a slash, we'll assume that we've been given an - # absolute path to the library. This is only used during test mode. + if Pathname.new(LIBRARY_NAME).absolute? plugin = LIBRARY_NAME else # We're assuming that the library path is relative to the current @@ -185,10 +185,18 @@ class Cmd end conf_version = LIBRARY_VERSION + if defined? RubyInstaller + # RubyInstaller does not search for dlls in PATH or the directory that tests are running from, + # so we'll add the parent directory of the plugin to the search path. + # https://github.com/oneclick/rubyinstaller2/wiki/For-gem-developers#-dll-loading + RubyInstaller::Runtime.add_dll_directory(File.dirname(plugin)) + end + begin Importer.dlload plugin - rescue DLError + rescue DLError => error puts "Library error: [#{plugin}] not found." + puts "DLError: #{error.message}" exit(-1) end diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 3d145b911..0683f0982 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -73,7 +73,7 @@ std::string custom_exec_str(std::string _cmd) } ///////////////////////////////////////////////// -TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(checkUnrecognizedElements, SDF) { // Check an SDFormat file with unrecognized elements { @@ -120,7 +120,7 @@ TEST(checkUnrecognizedElements, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check, SDF) { // Check a good SDF file { @@ -1011,7 +1011,7 @@ TEST(check, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check_shapes_sdf, SDF) { { const auto path = @@ -1035,7 +1035,7 @@ TEST(check_shapes_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(check_model_sdf, SDF) { // Check a good SDF file by passing the absolute path { @@ -1062,7 +1062,7 @@ TEST(check_model_sdf, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(describe, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(describe, SDF) { // Get the description std::string output = @@ -1074,7 +1074,7 @@ TEST(describe, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print, SDF) { // Check a good SDF file { @@ -1103,7 +1103,7 @@ TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_degrees, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_degrees.sdf"); @@ -1171,7 +1171,7 @@ TEST(print_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_radians, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_radians.sdf"); @@ -1239,7 +1239,7 @@ TEST(print_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_rotations_in_quaternions, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_quaternions, SDF) { const auto path = sdf::testing::TestFile( "sdf", "rotations_in_quaternions.sdf"); @@ -1308,7 +1308,7 @@ TEST(print_rotations_in_quaternions, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_degrees, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1379,7 +1379,7 @@ TEST(print_includes_rotations_in_degrees, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_radians, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1450,8 +1450,7 @@ TEST(print_includes_rotations_in_radians, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_includes_rotations_in_quaternions, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_includes_rotations_in_quaternions, SDF) { // Set SDF_PATH so that included models can be found gz::utils::setenv( @@ -1523,8 +1522,7 @@ TEST(print_includes_rotations_in_quaternions, } ///////////////////////////////////////////////// -TEST(print_rotations_in_unnormalized_degrees, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_unnormalized_degrees, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_degrees.sdf"); @@ -1595,8 +1593,7 @@ TEST(print_rotations_in_unnormalized_degrees, } ///////////////////////////////////////////////// -TEST(print_rotations_in_unnormalized_radians, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_rotations_in_unnormalized_radians, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); @@ -1664,7 +1661,7 @@ TEST(print_rotations_in_unnormalized_radians, } ///////////////////////////////////////////////// -TEST(shuffled_cmd_flags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(shuffled_cmd_flags, SDF) { const std::string path = sdf::testing::TestFile("sdf", "rotations_in_unnormalized_radians.sdf"); @@ -1713,8 +1710,7 @@ TEST(shuffled_cmd_flags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) } ///////////////////////////////////////////////// -TEST(print_snap_to_degrees_tolerance_too_high, - GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(print_snap_to_degrees_tolerance_too_high, SDF) { const std::string path = sdf::testing::TestFile( "sdf", @@ -1731,7 +1727,7 @@ TEST(print_snap_to_degrees_tolerance_too_high, } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) +TEST(GraphCmd, WorldPoseRelativeTo) { // world pose relative_to graph const std::string path = @@ -1780,7 +1776,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) +TEST(GraphCmd, ModelPoseRelativeTo) { const auto path = sdf::testing::TestFile("sdf", "model_relative_to_nested_reference.sdf"); @@ -1857,7 +1853,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseRelativeTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) +TEST(GraphCmd, WorldFrameAttachedTo) { const auto path = sdf::testing::TestFile("sdf", "world_nested_frame_attached_to.sdf"); @@ -1903,7 +1899,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldFrameAttachedTo)) } ///////////////////////////////////////////////// -TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) +TEST(GraphCmd, ModelFrameAttachedTo) { const auto path = sdf::testing::TestFile("sdf", "model_nested_frame_attached_to.sdf"); @@ -1955,7 +1951,7 @@ TEST(GraphCmd, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) // Disable on arm #if !defined __ARM_ARCH ///////////////////////////////////////////////// -TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +TEST(inertial_stats, SDF) { std::string expectedOutput = "Inertial statistics for model: test_model\n" @@ -2043,7 +2039,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) ////////////////////////////////////////////////// /// \brief Check help message and bash completion script for consistent flags -TEST(HelpVsCompletionFlags, SDF) +TEST(HelpVsCompletionFlags, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { // Flags in help message std::string helpOutput = custom_exec_str(GzCommand() + " sdf --help"); @@ -2098,6 +2094,16 @@ int main(int argc, char **argv) gz::utils::setenv("LD_LIBRARY_PATH", testLibraryPath); #endif + // temporarily set HOME + std::string homeDir; + sdf::testing::TestTmpPath(homeDir); + +#ifdef _WIN32 + gz::utils::setenv("HOMEPATH", homeDir); +#else + gz::utils::setenv("HOME", homeDir); +#endif + ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } From 321d85ec87ad838e42e43748acb1cdf48f32ea41 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 14 May 2024 05:10:51 -0300 Subject: [PATCH 11/25] (Backport) Enable 24.04 CI, remove distutils dependency (#1413) * Enable 24.04 CI, remove distutils dependency (#1408) distutils is no longer required since this branch requires a new enough version of cmake. Signed-off-by: Steve Peters Signed-off-by: Jorge Perez Co-authored-by: Steve Peters --- .github/ci/packages.apt | 1 - .github/workflows/ci.yml | 9 +++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index acee87feb..1c65edcc2 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -7,7 +7,6 @@ libtinyxml2-dev liburdfdom-dev libxml2-utils python3-dev -python3-distutils python3-gz-math7 python3-psutil python3-pybind11 diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index fe275a3cb..8a0a9947a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -21,3 +21,12 @@ jobs: codecov-enabled: true cppcheck-enabled: true cpplint-enabled: true + noble-ci: + runs-on: ubuntu-latest + name: Ubuntu Noble CI + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Compile and test + id: ci + uses: gazebo-tooling/action-gz-ci@noble From c9779cb7f19bac92b512e213b7fd50dbcf2c8c8d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 May 2024 23:00:20 +0200 Subject: [PATCH 12/25] Allow empty strings in plugin and custom attributes (#1407) Currently errors are generated when adding an attribute containing an empty string to a block or as a custom attribute. This adds failing tests to confirm the bug and fixes the errors in by setting `_required == 0` when calling Element::AddAttribute. This also changes Element::ToString to print empty custom attributes. Fixes #725. Signed-off-by: Steve Peters --- src/Element.cc | 3 ++- src/Utils.cc | 3 ++- src/parser.cc | 3 ++- test/integration/custom_elems_attrs.sdf | 2 +- test/integration/plugin_attribute.cc | 4 +++- test/integration/sdf_custom.cc | 14 ++++++++++++++ 6 files changed, 24 insertions(+), 5 deletions(-) diff --git a/src/Element.cc b/src/Element.cc index 9ded14004..fd8950d7f 100644 --- a/src/Element.cc +++ b/src/Element.cc @@ -671,7 +671,8 @@ void ElementPrivate::PrintAttributes(sdf::Errors &_errors, // better separation of concerns if the conversion process set the // required attributes with their default values. if ((*aiter)->GetSet() || (*aiter)->GetRequired() || - _includeDefaultAttributes) + _includeDefaultAttributes || + ((*aiter)->GetKey().find(':') != std::string::npos)) { const std::string key = (*aiter)->GetKey(); const auto it = attributeExceptions.find(key); diff --git a/src/Utils.cc b/src/Utils.cc index c6a89e867..a76315e0f 100644 --- a/src/Utils.cc +++ b/src/Utils.cc @@ -366,7 +366,8 @@ void copyChildren(ElementPtr _sdf, tinyxml2::XMLElement *_xml, for (const tinyxml2::XMLAttribute *attribute = elemXml->FirstAttribute(); attribute; attribute = attribute->Next()) { - element->AddAttribute(attribute->Name(), "string", "", 1, ""); + // Add with required == 0 to allow unrecognized attribute to be empty + element->AddAttribute(attribute->Name(), "string", "", 0, ""); element->GetAttribute(attribute->Name())->SetFromString( attribute->Value()); } diff --git a/src/parser.cc b/src/parser.cc index 84a7593d6..5a914a948 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -1440,7 +1440,8 @@ static bool readAttributes(tinyxml2::XMLElement *_xml, ElementPtr _sdf, // attribute is found if (std::strchr(attribute->Name(), ':') != nullptr) { - _sdf->AddAttribute(attribute->Name(), "string", "", 1, ""); + // Add with required == 0 to allow custom attribute to be empty + _sdf->AddAttribute(attribute->Name(), "string", "", 0, ""); _sdf->GetAttribute(attribute->Name())->SetFromString(attribute->Value()); attribute = attribute->Next(); continue; diff --git a/test/integration/custom_elems_attrs.sdf b/test/integration/custom_elems_attrs.sdf index 8f051fc79..54c9f9c9e 100644 --- a/test/integration/custom_elems_attrs.sdf +++ b/test/integration/custom_elems_attrs.sdf @@ -4,7 +4,7 @@ Description of this world - + L1 L2 diff --git a/test/integration/plugin_attribute.cc b/test/integration/plugin_attribute.cc index 02af21046..7f4f9f4cb 100644 --- a/test/integration/plugin_attribute.cc +++ b/test/integration/plugin_attribute.cc @@ -30,7 +30,7 @@ std::string get_sdf_string() << "" << " " << " " - << " " + << " " << " value" << " " << "" @@ -54,6 +54,8 @@ TEST(PluginAttribute, ParseAttributes) EXPECT_TRUE(user->HasAttribute("attribute")); EXPECT_EQ(user->GetAttribute("attribute")->GetAsString(), std::string("attribute")); + EXPECT_TRUE(user->HasAttribute("empty_attribute")); + EXPECT_EQ(user->GetAttribute("empty_attribute")->GetAsString(), ""); } { sdf::ElementPtr value = plugin->GetElement("value"); diff --git a/test/integration/sdf_custom.cc b/test/integration/sdf_custom.cc index 53ecedcec..cdf1be1ee 100644 --- a/test/integration/sdf_custom.cc +++ b/test/integration/sdf_custom.cc @@ -60,6 +60,20 @@ TEST(SDFParser, CustomElements) auto customAttrInt = link1Element->Get("mysim:custom_attr_int"); EXPECT_EQ(5, customAttrInt); + // Check empty attribute in link L2 + const sdf::Link *link2 = model->LinkByName("L2"); + ASSERT_TRUE(link2 != nullptr); + sdf::ElementPtr link2Element = link2->Element(); + ASSERT_TRUE(link2Element != nullptr); + EXPECT_TRUE(link2Element->HasAttribute("mysim:empty_attr")); + auto emptyAttrStr = link2Element->Get("mysim:empty_attr"); + EXPECT_EQ("", emptyAttrStr); + + // Ensure that mysim:empty_attr appears when calling ToString("") + auto rootToString = link2Element->ToString(""); + EXPECT_NE(std::string::npos, rootToString.find("mysim:empty_attr=''")) + << rootToString; + // Use of sdf::Model::Element() to obtain an sdf::ElementPtr object sdf::ElementPtr modelElement = model->Element(); From 835d0d8f37312f75e4161ca1016e5fbaf73262ef Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Tue, 28 May 2024 01:43:22 -0700 Subject: [PATCH 13/25] Update default camera instrinsics skew to 0, which matches spec (#1423) Signed-off-by: Shameek Ganguly --- python/test/pyCamera_TEST.py | 2 +- src/Camera.cc | 2 +- src/Camera_TEST.cc | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/python/test/pyCamera_TEST.py b/python/test/pyCamera_TEST.py index 6daca4cbe..c2a33312e 100644 --- a/python/test/pyCamera_TEST.py +++ b/python/test/pyCamera_TEST.py @@ -191,7 +191,7 @@ def test_default_construction(self): cam.set_lens_intrinsics_cy(123) self.assertAlmostEqual(123, cam.lens_intrinsics_cy()) - self.assertAlmostEqual(1.0, cam.lens_intrinsics_skew()) + self.assertAlmostEqual(0.0, cam.lens_intrinsics_skew()) cam.set_lens_intrinsics_skew(2.3) self.assertAlmostEqual(2.3, cam.lens_intrinsics_skew()) diff --git a/src/Camera.cc b/src/Camera.cc index 2f2ea7a2b..20f26f57e 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -208,7 +208,7 @@ class sdf::Camera::Implementation public: double lensProjectionTy{0.0}; /// \brief lens instrinsics s. - public: double lensIntrinsicsS{1.0}; + public: double lensIntrinsicsS{0.0}; /// \brief True if this camera has custom intrinsics values public: bool hasIntrinsics = false; diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 68b61beac..c68609b48 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -214,7 +214,7 @@ TEST(DOMCamera, Construction) cam.SetLensProjectionTy(2); EXPECT_DOUBLE_EQ(2, cam.LensProjectionTy()); - EXPECT_DOUBLE_EQ(1.0, cam.LensIntrinsicsSkew()); + EXPECT_DOUBLE_EQ(0, cam.LensIntrinsicsSkew()); cam.SetLensIntrinsicsSkew(2.3); EXPECT_DOUBLE_EQ(2.3, cam.LensIntrinsicsSkew()); From c71fc829d4bedb766f1ea8aeeb11095fcf238951 Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Tue, 28 May 2024 10:19:17 -0500 Subject: [PATCH 14/25] Add support for no gravity link (#1410) Signed-off-by: youhy --- include/sdf/Link.hh | 10 ++++++++++ src/Link.cc | 20 +++++++++++++++++++- src/Link_TEST.cc | 4 ++++ 3 files changed, 33 insertions(+), 1 deletion(-) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index dd0d7c73f..752cef6eb 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -321,12 +321,22 @@ namespace sdf /// \sa bool Model::EnableWind public: bool EnableWind() const; + /// \brief Check if this link should be subject to gravity. + /// If true, this link should be affected by gravity. + /// \return true if the model should be subject to gravity, false otherwise. + public: bool EnableGravity() const; + /// \brief Set whether this link should be subject to wind. /// \param[in] _enableWind True or false depending on whether the link /// should be subject to wind. /// \sa Model::SetEnableWind(bool) public: void SetEnableWind(bool _enableWind); + /// \brief Set whether this link should be subject to gravity. + /// \param[in] _enableGravity True or false depending on whether the link + /// should be subject to gravity. + public: void SetEnableGravity(bool _enableGravity); + /// \brief Add a collision to the link. /// \param[in] _collision Collision to add. /// \return True if successful, false if a collision with the name already diff --git a/src/Link.cc b/src/Link.cc index 19a0439f7..863edaabe 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -74,6 +74,9 @@ class sdf::Link::Implementation /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; + /// \brief True if this link should be subject to gravity, false otherwise. + public: bool enableGravity = true; + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; }; @@ -189,6 +192,9 @@ Errors Link::Load(ElementPtr _sdf) this->dataPtr->enableWind = _sdf->Get("enable_wind", this->dataPtr->enableWind).first; + this->dataPtr->enableGravity = _sdf->Get("gravity", + this->dataPtr->enableGravity).first; + return errors; } @@ -579,11 +585,23 @@ bool Link::EnableWind() const } ///////////////////////////////////////////////// -void Link::SetEnableWind(const bool _enableWind) +bool Link::EnableGravity() const +{ + return this->dataPtr->enableGravity; +} + +///////////////////////////////////////////////// +void Link::SetEnableWind(bool _enableWind) { this->dataPtr->enableWind = _enableWind; } +///////////////////////////////////////////////// +void Link::SetEnableGravity(bool _enableGravity) +{ + this->dataPtr->enableGravity = _enableGravity; +} + ////////////////////////////////////////////////// bool Link::AddCollision(const Collision &_collision) { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 232c49d24..89ba09dbe 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -60,6 +60,10 @@ TEST(DOMLink, Construction) link.SetEnableWind(true); EXPECT_TRUE(link.EnableWind()); + EXPECT_TRUE(link.EnableGravity()); + link.SetEnableGravity(false); + EXPECT_FALSE(link.EnableGravity()); + EXPECT_EQ(0u, link.SensorCount()); EXPECT_EQ(nullptr, link.SensorByIndex(0)); EXPECT_EQ(nullptr, link.SensorByIndex(1)); From a21d28d45a2d431036236d8a9510f0b0dd53f314 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 30 May 2024 15:36:47 +0900 Subject: [PATCH 15/25] Add note in migration guide on camera skew value change (#1425) Signed-off-by: Ian Chen --- Migration.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Migration.md b/Migration.md index be560d702..1f859ee5f 100644 --- a/Migration.md +++ b/Migration.md @@ -22,6 +22,9 @@ but with improved human-readability.. ### Modifications +1. The default camera lens intrinsics skew value in the Camera DOM class changed + from `1` to `0` to match the SDF specification. + 1. World class only renames frames with name collisions if original file version is 1.6 or earlier. Name collisions in newer files will cause `DUPLICATE_NAME` errors, which now matches the behavior of the Model class. From f9414e76fd3bfb26f5e41c58bc164fe2cc4b624d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 30 May 2024 13:15:05 +0200 Subject: [PATCH 16/25] =?UTF-8?q?Added=20Automatic=20Moment=20of=20Inertia?= =?UTF-8?q?=20Calculations=20for=20Basic=20Shapes=20Pytho=E2=80=A6=20(#142?= =?UTF-8?q?4)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- python/src/sdf/pyBox.cc | 3 + python/src/sdf/pyCapsule.cc | 3 + python/src/sdf/pyCollision.cc | 7 ++ python/src/sdf/pyCylinder.cc | 3 + python/src/sdf/pyEllipsoid.cc | 3 + python/src/sdf/pyGeometry.cc | 3 + python/src/sdf/pyLink.cc | 10 ++ python/src/sdf/pyModel.cc | 2 + python/src/sdf/pyParserConfig.cc | 10 ++ python/src/sdf/pyRoot.cc | 4 +- python/src/sdf/pySphere.cc | 3 + python/src/sdf/pyWorld.cc | 2 + python/test/pyBox_TEST.py | 38 ++++++- python/test/pyCapsule_TEST.py | 54 +++++++-- python/test/pyCollision_TEST.py | 140 ++++++++++++++++++++++- python/test/pyCylinder_TEST.py | 47 +++++++- python/test/pyEllipsoid_TEST.py | 46 +++++++- python/test/pyGeometry_TEST.py | 174 ++++++++++++++++++++++++++++- python/test/pyLink_TEST.py | 172 ++++++++++++++++++++++++++-- python/test/pyParserConfig_TEST.py | 6 +- python/test/pyRoot_TEST.py | 37 +++++- python/test/pySphere_TEST.py | 97 +++++++++++----- python/test/pyWorld_TEST.py | 56 +++++++++- 23 files changed, 846 insertions(+), 74 deletions(-) diff --git a/python/src/sdf/pyBox.cc b/python/src/sdf/pyBox.cc index 31d9d8f08..453438119 100644 --- a/python/src/sdf/pyBox.cc +++ b/python/src/sdf/pyBox.cc @@ -16,6 +16,7 @@ #include "pyBox.hh" #include +#include #include "sdf/Box.hh" @@ -42,6 +43,8 @@ void defineBox(pybind11::object module) pybind11::overload_cast<>(&sdf::Box::Shape, pybind11::const_), pybind11::return_value_policy::reference, "Get a mutable Gazebo Math representation of this Box.") + .def("calculate_inertial", &sdf::Box::CalculateInertial, + "Calculate and return the Inertial values for the Box.") .def("__copy__", [](const sdf::Box &self) { return sdf::Box(self); }) diff --git a/python/src/sdf/pyCapsule.cc b/python/src/sdf/pyCapsule.cc index ac25cd917..50ca4337d 100644 --- a/python/src/sdf/pyCapsule.cc +++ b/python/src/sdf/pyCapsule.cc @@ -16,6 +16,7 @@ #include "pyCapsule.hh" #include +#include #include "sdf/Capsule.hh" @@ -41,6 +42,8 @@ void defineCapsule(pybind11::object module) "Get the capsule's length in meters.") .def("set_length", &sdf::Capsule::SetLength, "Set the capsule's length in meters.") + .def("calculate_inertial", &sdf::Capsule::CalculateInertial, + "Calculate and return the Inertial values for the Capsule.") .def( "shape", pybind11::overload_cast<>(&sdf::Capsule::Shape, pybind11::const_), diff --git a/python/src/sdf/pyCollision.cc b/python/src/sdf/pyCollision.cc index c7305063e..d01f6fba3 100644 --- a/python/src/sdf/pyCollision.cc +++ b/python/src/sdf/pyCollision.cc @@ -17,6 +17,7 @@ #include "pyCollision.hh" #include +#include #include "sdf/Collision.hh" #include "sdf/Geometry.hh" @@ -37,6 +38,12 @@ void defineCollision(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("set_density", &sdf::Collision::SetDensity, "Set the density of the collision.") + .def("density", &sdf::Collision::Density, "Get the density of the collision.") + .def("calculate_inertial", + pybind11::overload_cast( + &sdf::Collision::CalculateInertial), + "Calculate and return the MassMatrix for the collision.") .def("name", &sdf::Collision::Name, "Get the name of the collision. " "The name of the collision must be unique within the scope of a Link.") diff --git a/python/src/sdf/pyCylinder.cc b/python/src/sdf/pyCylinder.cc index 83af7dda4..d8515ddc6 100644 --- a/python/src/sdf/pyCylinder.cc +++ b/python/src/sdf/pyCylinder.cc @@ -16,6 +16,7 @@ #include "pyCylinder.hh" #include +#include #include "sdf/Cylinder.hh" @@ -33,6 +34,8 @@ void defineCylinder(pybind11::object module) pybind11::class_(module, "Cylinder") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Cylinder::CalculateInertial, + "Calculate and return the Inertial values for the Cylinder.") .def("radius", &sdf::Cylinder::Radius, "Get the cylinder's radius in meters.") .def("set_radius", &sdf::Cylinder::SetRadius, diff --git a/python/src/sdf/pyEllipsoid.cc b/python/src/sdf/pyEllipsoid.cc index 89fdb1454..47ea60c96 100644 --- a/python/src/sdf/pyEllipsoid.cc +++ b/python/src/sdf/pyEllipsoid.cc @@ -16,6 +16,7 @@ #include "pyEllipsoid.hh" #include +#include #include "sdf/Ellipsoid.hh" @@ -33,6 +34,8 @@ void defineEllipsoid(pybind11::object module) pybind11::class_(module, "Ellipsoid") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Ellipsoid::CalculateInertial, + "Calculate and return the Inertial values for the Ellipsoid.") .def("radii", &sdf::Ellipsoid::Radii, "Get the ellipsoid's radii in meters.") .def("set_radii", &sdf::Ellipsoid::SetRadii, diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index 84a146c07..ffcbabbe5 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -17,6 +17,7 @@ #include "pyGeometry.hh" #include +#include #include "sdf/ParserConfig.hh" @@ -45,6 +46,8 @@ void defineGeometry(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Geometry::CalculateInertial, + "Calculate and return the Mass Matrix values for the Geometry") .def("type", &sdf::Geometry::Type, "Get the type of geometry.") .def("set_type", &sdf::Geometry::SetType, diff --git a/python/src/sdf/pyLink.cc b/python/src/sdf/pyLink.cc index 3f77912fa..a101d5512 100644 --- a/python/src/sdf/pyLink.cc +++ b/python/src/sdf/pyLink.cc @@ -41,6 +41,16 @@ void defineLink(pybind11::object module) pybind11::class_(module, "Link") .def(pybind11::init<>()) .def(pybind11::init()) + .def("resolve_auto_inertials", &sdf::Link::ResolveAutoInertials, + "Calculate & set inertial values for the link") + .def("auto_inertia", &sdf::Link::AutoInertia, + "Check if the automatic calculation for the link inertial is enabled or not.") + .def("set_auto_inertia", &sdf::Link::SetAutoInertia, + "Enable automatic inertial calculations by setting autoInertia to true") + .def("auto_inertia_saved", &sdf::Link::AutoInertiaSaved, + "Check if the inertial values for this link were saved.") + .def("set_auto_inertia_saved", &sdf::Link::SetAutoInertiaSaved, + "Set the autoInertiaSaved() values") .def("name", &sdf::Link::Name, "Get the name of the link.") .def("set_name", &sdf::Link::SetName, diff --git a/python/src/sdf/pyModel.cc b/python/src/sdf/pyModel.cc index fce94d0b9..1127945de 100644 --- a/python/src/sdf/pyModel.cc +++ b/python/src/sdf/pyModel.cc @@ -46,6 +46,8 @@ void defineModel(pybind11::object module) }, "Check that the FrameAttachedToGraph and PoseRelativeToGraph " "are valid.") + .def("resolve_auto_inertials", &sdf::Model::ResolveAutoInertials, + "Calculate and set the inertials for all the links belonging to the model object") .def("name", &sdf::Model::Name, "Get the name of model.") .def("set_name", &sdf::Model::SetName, diff --git a/python/src/sdf/pyParserConfig.cc b/python/src/sdf/pyParserConfig.cc index 73d8b1990..2e58422fe 100644 --- a/python/src/sdf/pyParserConfig.cc +++ b/python/src/sdf/pyParserConfig.cc @@ -46,6 +46,12 @@ void defineParserConfig(pybind11::object module) .def("find_file_callback", &sdf::ParserConfig::FindFileCallback, "Get the find file callback function") + .def("calculate_inertial_configuration", + &sdf::ParserConfig::CalculateInertialConfiguration, + "Get the current configuration for the CalculateInertial() function") + .def("set_calculate_inertial_configuration", + &sdf::ParserConfig::SetCalculateInertialConfiguration, + "Set the configuration for the CalculateInertial() function") .def("set_find_callback", &sdf::ParserConfig::SetFindCallback, "Set the callback to use when libsdformat can't find a file.") @@ -96,6 +102,10 @@ void defineParserConfig(pybind11::object module) .value("WARN", sdf::EnforcementPolicy::WARN) .value("LOG", sdf::EnforcementPolicy::LOG); + pybind11::enum_( + module, "ConfigureResolveAutoInertials") + .value("SKIP_CALCULATION_IN_LOAD", sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD) + .value("SAVE_CALCULATION", sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); } } // namespace python } // namespace SDF_VERSION_NAMESPACE diff --git a/python/src/sdf/pyRoot.cc b/python/src/sdf/pyRoot.cc index 42c21907a..2da83830a 100644 --- a/python/src/sdf/pyRoot.cc +++ b/python/src/sdf/pyRoot.cc @@ -38,6 +38,8 @@ void defineRoot(pybind11::object module) { pybind11::class_(module, "Root") .def(pybind11::init<>()) + .def("resolve_auto_inertials", &sdf::Root::ResolveAutoInertials, + "Calculate and set the inertial properties") .def("load", [](Root &self, const std::string &_filename) { @@ -46,7 +48,7 @@ void defineRoot(pybind11::object module) "Parse the given SDF file, and generate objects based on types " "specified in the SDF file.") .def("load", - [](Root &self, const std::string &_filename, + [](Root &self, const std::string &_filename, const ParserConfig &_config) { ThrowIfErrors(self.Load(_filename, _config)); diff --git a/python/src/sdf/pySphere.cc b/python/src/sdf/pySphere.cc index 06e991df7..ee6197b38 100644 --- a/python/src/sdf/pySphere.cc +++ b/python/src/sdf/pySphere.cc @@ -16,6 +16,7 @@ #include "pySphere.hh" #include +#include #include "sdf/Sphere.hh" @@ -33,6 +34,8 @@ void defineSphere(pybind11::object module) pybind11::class_(module, "Sphere") .def(pybind11::init<>()) .def(pybind11::init()) + .def("calculate_inertial", &sdf::Sphere::CalculateInertial, + "Calculate and return the Inertial values for the Sphere.") .def("radius", &sdf::Sphere::Radius, "Get the sphere's radius in meters.") .def("set_radius", &sdf::Sphere::SetRadius, diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index 41b930fc4..108708ae4 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -48,6 +48,8 @@ void defineWorld(pybind11::object module) geometryModule .def(pybind11::init<>()) .def(pybind11::init()) + .def("resolve_auto_inertials", &sdf::World::ResolveAutoInertials, + "Calculate and set the inertials for all the models in the world object") .def("validate_graphs", &sdf::World::ValidateGraphs, "Check that the FrameAttachedToGraph and PoseRelativeToGraph " "are valid.") diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index d98440de5..84ce40bee 100644 --- a/python/test/pyBox_TEST.py +++ b/python/test/pyBox_TEST.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from gz_test_deps.math import Vector3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Box import unittest @@ -33,7 +33,7 @@ def test_assignment(self): box = Box() box.set_size(size) - box2 = box; + box2 = box self.assertEqual(size, box2.size()) self.assertEqual(1 * 2 * 3, box2.shape().volume()) @@ -55,5 +55,39 @@ def test_shape(self): box.shape().set_size(Vector3d(1, 2, 3)) self.assertEqual(Vector3d(1, 2, 3), box.size()) + def test_calculate_inertial(self): + box = Box() + density = 2710 + + box.set_size(Vector3d(-1, 1, 0)) + invalidBoxInertial = box.calculate_inertial(density) + self.assertEqual(None, invalidBoxInertial) + + l = 2.0 + w = 2.0 + h = 2.0 + box.set_size(Vector3d(l, w, h)) + + expectedMass = box.shape().volume() * density + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + boxInertial = box.calculate_inertial(density) + self.assertEqual(box.shape().material().density(), density) + self.assertNotEqual(None, boxInertial) + self.assertEqual(expectedInertial, boxInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + boxInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + boxInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 52472ed91..976e53c7b 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -16,6 +16,7 @@ import math +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Capsule import unittest @@ -56,7 +57,7 @@ def test_move_construction(self): def test_copy_construction(self): - capsule = Capsule(); + capsule = Capsule() capsule.set_radius(0.2) capsule.set_length(3.0) @@ -65,29 +66,29 @@ def test_copy_construction(self): self.assertEqual(3.0, capsule2.length()) def test_copy_construction(self): - capsule = Capsule(); + capsule = Capsule() capsule.set_radius(0.2) capsule.set_length(3.0) - capsule2 = copy.deepcopy(capsule); + capsule2 = copy.deepcopy(capsule) self.assertEqual(0.2, capsule2.radius()) self.assertEqual(3.0, capsule2.length()) def test_assignment_after_move(self): - capsule1 = Capsule(); + capsule1 = Capsule() capsule1.set_radius(0.2) capsule1.set_length(3.0) - capsule2 = Capsule(); + capsule2 = Capsule() capsule2.set_radius(2) capsule2.set_length(30.0) # This is similar to what std::swap does except it uses std::move for each # assignment tmp = capsule1 - capsule1 = copy.deepcopy(capsule2); - capsule2 = tmp; + capsule1 = copy.deepcopy(capsule2) + capsule2 = tmp self.assertEqual(2, capsule1.radius()) self.assertEqual(30, capsule1.length()) @@ -97,7 +98,7 @@ def test_assignment_after_move(self): def test_shape(self): - capsule = Capsule(); + capsule = Capsule() self.assertEqual(0.5, capsule.radius()) self.assertEqual(1.0, capsule.length()) @@ -106,6 +107,43 @@ def test_shape(self): self.assertEqual(0.123, capsule.radius()) self.assertEqual(0.456, capsule.length()) + def test_calculate_inertial(self): + capsule = Capsule() + + # density of aluminium + density = 2710 + l = 2.0 + r = 0.1 + + capsule.set_length(l) + capsule.set_radius(r) + + expectedMass = capsule.shape().volume() * density + cylinderVolume = math.pi * r * r * l + sphereVolume = math.pi * 4. / 3. * r * r * r + volume = cylinderVolume + sphereVolume + cylinderMass = expectedMass * cylinderVolume / volume + sphereMass = expectedMass * sphereVolume / volume + + ixxIyy = (1 / 12.0) * cylinderMass * (3 * r * r + l * l) + sphereMass * ( + 0.4 * r * r + 0.375 * r * l + 0.25 * l * l) + izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixxIyy, ixxIyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + capsuleInertial = capsule.calculate_inertial(density) + self.assertEqual(capsule.shape().material().density(), density) + self.assertNotEqual(None, capsuleInertial) + self.assertEqual(expectedInertial, capsuleInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + capsuleInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + capsuleInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index 9e92f521b..8d8d96735 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -13,9 +13,9 @@ # limitations under the License. import copy -from gz_test_deps.math import Pose3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error, - Geometry, Plane, Surface, Sphere, + Geometry, ParserConfig, Plane, Root, Surface, Sphere, SDFErrorsException) import gz_test_deps.sdformat as sdf import unittest @@ -26,6 +26,7 @@ class CollisionTEST(unittest.TestCase): def test_default_construction(self): collision = Collision() self.assertTrue(not collision.name()) + self.assertEqual(collision.density(), 1000.0) collision.set_name("test_collison") self.assertEqual(collision.name(), "test_collison") @@ -40,6 +41,9 @@ def test_default_construction(self): with self.assertRaises(SDFErrorsException): semanticPose.resolve() + collision.set_density(1240.0) + self.assertAlmostEqual(collision.density(), 1240.0) + collision.set_raw_pose(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi)) self.assertEqual(Pose3d(-10, -20, -30, math.pi, math.pi, math.pi), collision.raw_pose()) @@ -130,5 +134,137 @@ def test_set_surface(self): self.assertEqual(collision.surface().contact().collide_bitmask(), 0x2) + def test_incorrect_box_collision_calculate_inertial(self): + collision = Collision() + self.assertAlmostEqual(1000.0, collision.density()) + + # sdf::ElementPtr sdf(new sdf::Element()) + # collision.Load(sdf) + + collisionInertial = Inertiald() + sdfParserConfig = ParserConfig() + geom = Geometry() + box = Box() + + # Invalid Inertial test + box.set_size(Vector3d(-1, 1, 0)) + geom.set_type(sdf.GeometryType.BOX) + geom.set_box_shape(box) + collision.set_geometry(geom) + + errors = [] + + collision.calculate_inertial(errors, collisionInertial, sdfParserConfig) + self.assertFalse(len(errors)) + + + def test_correct_box_collision_calculate_inertial(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + model = root.model() + link = model.link_by_index(0) + collision = link.collision_by_index(0) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * collision.density() + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(len(inertialErr), 0) + self.assertAlmostEqual(1240.0, collision.density()) + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(expectedInertial.mass_matrix(), link.inertial().mass_matrix()) + self.assertEqual(expectedInertial.pose(), link.inertial().pose()) + + def test_calculate_inertial_pose_not_relative_to_link(self): + + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " 0 0 1 0 0 0" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0 0 -1 0 0 0" + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + # self.assertNotEqual(None, root.Element()) + + model = root.model() + link = model.link_by_index(0) + collision = link.collision_by_index(0) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * collision.density() + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(len(inertialErr), 0) + self.assertAlmostEqual(1240.0, collision.density()) + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(expectedInertial.mass_matrix(), link.inertial().mass_matrix()) + self.assertEqual(expectedInertial.pose(), link.inertial().pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index 99bcf7823..b5387dd0b 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -16,6 +16,7 @@ import math +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Cylinder import unittest @@ -64,7 +65,7 @@ def test_assignment(self): def test_copy_construction(self): - cylinder = Cylinder(); + cylinder = Cylinder() cylinder.set_radius(0.2) cylinder.set_length(3.0) @@ -81,11 +82,11 @@ def test_copy_construction(self): self.assertEqual(3.0, cylinder2.length()) def test_deepcopy(self): - cylinder = Cylinder(); + cylinder = Cylinder() cylinder.set_radius(0.2) cylinder.set_length(3.0) - cylinder2 = copy.deepcopy(cylinder); + cylinder2 = copy.deepcopy(cylinder) self.assertEqual(0.2, cylinder2.radius()) self.assertEqual(3.0, cylinder2.length()) @@ -98,7 +99,7 @@ def test_deepcopy(self): self.assertEqual(3.0, cylinder2.length()) def test_shape(self): - cylinder = Cylinder(); + cylinder = Cylinder() self.assertEqual(0.5, cylinder.radius()) self.assertEqual(1.0, cylinder.length()) @@ -107,6 +108,44 @@ def test_shape(self): self.assertEqual(0.123, cylinder.radius()) self.assertEqual(0.456, cylinder.length()) + def test_calculate_interial(self): + cylinder = Cylinder() + + # density of aluminium + density = 2170 + + # Invalid dimensions leading to None return in + # CalculateInertial() + cylinder.set_length(-1) + cylinder.set_radius(0) + invalidCylinderInertial = cylinder.calculate_inertial(density) + self.assertEqual(None, invalidCylinderInertial) + + l = 2.0 + r = 0.1 + + cylinder.set_length(l) + cylinder.set_radius(r) + + expectedMass = cylinder.shape().volume() * density + ixxIyy = (1 / 12.0) * expectedMass * (3 * r * r + l * l) + izz = 0.5 * expectedMass * r * r + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixxIyy, ixxIyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + cylinderInertial = cylinder.calculate_inertial(density) + self.assertEqual(cylinder.shape().mat().density(), density) + self.assertNotEqual(None, cylinderInertial) + self.assertEqual(expectedInertial, cylinderInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + cylinderInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + cylinderInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index a102a42ad..c12690ffe 100644 --- a/python/test/pyEllipsoid_TEST.py +++ b/python/test/pyEllipsoid_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz_test_deps.math import Vector3d +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d import math from gz_test_deps.sdformat import Ellipsoid import unittest @@ -51,7 +51,7 @@ def test_deepcopy(self): def test_deepcopy_after_assignment(self): - ellipsoid1 = Ellipsoid(); + ellipsoid1 = Ellipsoid() expectedradii1 = Vector3d(1.0, 2.0, 3.0) ellipsoid1.set_radii(expectedradii1) @@ -62,8 +62,8 @@ def test_deepcopy_after_assignment(self): # This is similar to what std::swap does except it uses std::move for each # assignment tmp = copy.deepcopy(ellipsoid1) - ellipsoid1 = ellipsoid2; - ellipsoid2 = tmp; + ellipsoid1 = ellipsoid2 + ellipsoid2 = tmp self.assertEqual(expectedradii1, ellipsoid2.shape().radii()) self.assertEqual(expectedradii2, ellipsoid1.shape().radii()) @@ -77,6 +77,44 @@ def test_shape(self): ellipsoid.shape().set_radii(expectedradii) self.assertEqual(expectedradii, ellipsoid.radii()) + def test_calculate_inertial(self): + ellipsoid = Ellipsoid() + + # density of aluminium + density = 2170 + + # Invalid dimensions leading to std::nullopt return in + # CalculateInertial() + ellipsoid.set_radii(Vector3d(-1, 2, 0)) + invalidEllipsoidInertial = ellipsoid.calculate_inertial(density) + self.assertEqual(None, invalidEllipsoidInertial) + + a = 1.0 + b = 10.0 + c = 100.0 + + ellipsoid.set_radii(Vector3d(a, b, c)) + + expectedMass = ellipsoid.shape().volume() * density + ixx = (expectedMass / 5.0) * (b * b + c * c) + iyy = (expectedMass / 5.0) * (a * a + c * c) + izz = (expectedMass / 5.0) * (a * a + b * b) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + ellipsoidInertial = ellipsoid.calculate_inertial(density) + self.assertEqual(ellipsoid.shape().material().density(), density) + self.assertNotEqual(None, ellipsoidInertial) + self.assertEqual(expectedInertial, ellipsoidInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + ellipsoidInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + ellipsoidInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index a1f4ec65b..c4cd9c511 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,10 +13,11 @@ # limitations under the License. import copy -from gz_test_deps.sdformat import (Geometry, Box, Capsule, Cylinder, Ellipsoid, - Mesh, Plane, Sphere) -from gz_test_deps.math import Vector3d, Vector2d +from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cylinder, Ellipsoid, + Mesh, ParserConfig, Plane, Sphere) +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf +import math import unittest @@ -185,5 +186,172 @@ def test_plane(self): self.assertEqual(Vector2d(9, 8), geom.plane_shape().size()) + def test_calculate_inertial(self): + geom = Geometry() + + # Density of Aluminimum + density = 2170.0 + expectedMass = 0 + expectedMassMat = MassMatrix3d() + expectedInertial = Inertiald() + sdfParserConfig = ParserConfig() + errors = [] + + # Not supported geom type + geom.set_type(sdf.GeometryType.EMPTY) + element = Element() + notSupportedInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + self.assertEqual(notSupportedInertial, None) + + box = Box() + l = 2.0 + w = 2.0 + h = 2.0 + box.set_size(Vector3d(l, w, h)) + + expectedMass = box.shape().volume() * density + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixx, iyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.BOX) + geom.set_box_shape(box) + boxInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, boxInertial) + self.assertEqual(expectedInertial, boxInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + + # Capsule + capsule = Capsule() + l = 2.0 + r = 0.1 + capsule.set_length(l) + capsule.set_radius(r) + + expectedMass = capsule.shape().volume() * density + cylinderVolume = math.pi * r * r * l + sphereVolume = math.pi * 4. / 3. * r * r * r + volume = cylinderVolume + sphereVolume + cylinderMass = expectedMass * cylinderVolume / volume + sphereMass = expectedMass * sphereVolume / volume + ixxIyy = (1 / 12.0) * cylinderMass * (3 * r *r + l * l) + sphereMass * ( + 0.4 * r * r + 0.375 * r * l + 0.25 * l * l) + izz = r * r * (0.5 * cylinderMass + 0.4 * sphereMass) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixxIyy, ixxIyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.CAPSULE) + geom.set_capsule_shape(capsule) + capsuleInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, capsuleInertial) + self.assertEqual(expectedInertial, capsuleInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) + + # Cylinder + cylinder = Cylinder() + l = 2.0 + r = 0.1 + + cylinder.set_length(l) + cylinder.set_radius(r) + + expectedMass = cylinder.shape().volume() * density + ixxIyy = (1/12.0) * expectedMass * (3*r*r + l*l) + izz = 0.5 * expectedMass * r * r + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixxIyy, ixxIyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.CYLINDER) + geom.set_cylinder_shape(cylinder) + cylinderInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, cylinderInertial) + self.assertEqual(expectedInertial, cylinderInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) + + # Ellipsoid + ellipsoid = Ellipsoid() + + a = 1.0 + b = 10.0 + c = 100.0 + + ellipsoid.set_radii(Vector3d(a, b, c)) + + expectedMass = ellipsoid.shape().volume() * density + ixx = (expectedMass / 5.0) * (b*b + c*c) + iyy = (expectedMass / 5.0) * (a*a + c*c) + izz = (expectedMass / 5.0) * (a*a + b*b) + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments(Vector3d(ixx, iyy, izz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.ELLIPSOID) + geom.set_ellipsoid_shape(ellipsoid) + ellipsoidInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, ellipsoidInertial) + self.assertEqual(expectedInertial, ellipsoidInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) + + # Sphere + sphere = Sphere() + r = 0.1 + + sphere.set_radius(r) + + expectedMass = sphere.shape().volume() * density + ixxIyyIzz = 0.4 * expectedMass * r * r + + expectedMassMat.set_mass(expectedMass) + expectedMassMat.set_diagonal_moments( + Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz)) + expectedMassMat.set_off_diagonal_moments(Vector3d.ZERO) + + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + geom.set_type(sdf.GeometryType.SPHERE) + geom.set_sphere_shape(sphere) + sphereInertial = geom.calculate_inertial(errors, + sdfParserConfig, density, element) + + self.assertNotEqual(None, sphereInertial) + self.assertEqual(expectedInertial, sphereInertial) + self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) + self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index e4ad505a4..0f2c8615f 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -14,8 +14,8 @@ import copy from gz_test_deps.math import Pose3d, Inertiald, MassMatrix3d, Vector3d -from gz_test_deps.sdformat import (Collision, Light, Link, Projector, Sensor, - Visual, SDFErrorsException) +from gz_test_deps.sdformat import (Collision, Light, Link, ParserConfig, Projector, Sensor, + Visual, Root, SDFErrorsException) import unittest import math @@ -61,6 +61,14 @@ def test_default_construction(self): link.set_enable_wind(True) self.assertTrue(link.enable_wind()) + self.assertFalse(link.auto_inertia_saved()) + link.set_auto_inertia_saved(True) + self.assertTrue(link.auto_inertia_saved()) + + self.assertFalse(link.auto_inertia()) + link.set_auto_inertia(True) + self.assertTrue(link.auto_inertia()) + self.assertEqual(0, link.sensor_count()) self.assertEqual(None, link.sensor_by_index(0)) self.assertEqual(None, link.sensor_by_index(1)) @@ -313,8 +321,8 @@ def test_mutable_by_index(self): pr = link.projector_by_index(0) self.assertNotEqual(None, pr) self.assertEqual("projector1", pr.name()) - pr.set_name("projector2"); - self.assertEqual("projector2", link.projector_by_index(0).name()); + pr.set_name("projector2") + self.assertEqual("projector2", link.projector_by_index(0).name()) def test_mutable_by_name(self): link = Link() @@ -376,7 +384,7 @@ def test_mutable_by_name(self): self.assertFalse(link.sensor_name_exists("sensor1")) self.assertTrue(link.sensor_name_exists("sensor2")) - # // Modify the particle emitter + # # Modify the particle emitter # sdf::ParticleEmitter *p = link.ParticleEmitterByName("pe1") # self.assertNotEqual(None, p) # self.assertEqual("pe1", p.name()) @@ -385,12 +393,154 @@ def test_mutable_by_name(self): # self.assertTrue(link.particle_emitter_name_exists("pe2")) # Modify the projector - pr = link.projector_by_name("projector1"); - self.assertNotEqual(None, pr); - self.assertEqual("projector1", pr.name()); - pr.set_name("projector2"); - self.assertFalse(link.projector_name_exists("projector1")); - self.assertTrue(link.projector_name_exists("projector2")); + pr = link.projector_by_name("projector1") + self.assertNotEqual(None, pr) + self.assertEqual("projector1", pr.name()) + pr.set_name("projector2") + self.assertFalse(link.projector_name_exists("projector1")) + self.assertTrue(link.projector_name_exists("projector2")) + + def test_resolveauto_inertialsWithNoCollisionsInLink(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + with self.assertRaises(SDFErrorsException): + errors = root.load_sdf_string(sdf, sdfParserConfig) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Default Inertial values set during load + self.assertEqual(1.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d.ONE, + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsWithMultipleCollisions(self): + sdf = "" + \ + "" + \ + " " + \ + " 0 0 1.0 0 0 0" + \ + " " + \ + " " + \ + " " + \ + " 0 0 -0.5 0 0 0" + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0 0 0.5 0 0 0" + \ + " 4" + \ + " " + \ + " " + \ + " 0.5" + \ + " 1.0" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Mass of cube(volume * density) + mass of cylinder(volume * density) + expectedMass = 1.0 * 2.0 + math.pi * 0.5 * 0.5 * 1 * 4.0 + + self.assertAlmostEqual(expectedMass, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d(2.013513, 2.013513, 0.72603), + link.inertial().mass_matrix().diagonal_moments()) + + def test_inertial_values_given_with_auto_set_to_true(self): + sdf = "" + \ + "" + \ + " " + \ + " " + \ + " " + \ + " 4.0" + \ + " 1 1 1 2 2 2" + \ + " " + \ + " 1" + \ + " 1" + \ + " 1" + \ + " " + \ + " " + \ + " " + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + self.assertNotEqual(4.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Pose3d.ZERO, link.inertial().pose()) + self.assertEqual(Vector3d(0.33333, 0.33333, 0.33333), + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsCalledWithAutoFalse(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(errors, None) + + model = root.model() + link = model.link_by_index(0) + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + self.assertEqual(len(errors), 0) + + # Default Inertial values set during load + self.assertEqual(1.0, link.inertial().mass_matrix().mass()) + self.assertEqual(Vector3d.ONE, + link.inertial().mass_matrix().diagonal_moments()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pyParserConfig_TEST.py b/python/test/pyParserConfig_TEST.py index a56bfc405..52917dd30 100644 --- a/python/test/pyParserConfig_TEST.py +++ b/python/test/pyParserConfig_TEST.py @@ -26,7 +26,7 @@ def test_construction(self): self.assertFalse(config.uri_path_map()) self.assertFalse(config.find_file_callback()) - testDir = source_file(); + testDir = source_file() config.add_uri_path("file://", testDir) self.assertTrue(config.uri_path_map()) @@ -35,7 +35,7 @@ def test_construction(self): self.assertEqual(it[0], testDir) def testFunc(argument): - return "test/dir2"; + return "test/dir2" config.set_find_callback(testFunc) self.assertTrue(config.find_file_callback()) @@ -102,7 +102,7 @@ def test_copy(self): # so we'll use the source path testDir1 = source_file() - config1 = ParserConfig(); + config1 = ParserConfig() config1.add_uri_path("file://", testDir1) it = config1.uri_path_map().get("file://") diff --git a/python/test/pyRoot_TEST.py b/python/test/pyRoot_TEST.py index 0df1d3023..0a41bf667 100644 --- a/python/test/pyRoot_TEST.py +++ b/python/test/pyRoot_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.math import Pose3d -from gz_test_deps.sdformat import (Error, Model, Light, Root, SDF_VERSION, +from gz_test_deps.sdformat import (ConfigureResolveAutoInertials, Error, Model, ParserConfig, Light, Root, SDF_VERSION, SDFErrorsException, SDF_PROTOCOL_VERSION, World) import gz_test_deps.sdformat as sdf @@ -121,7 +121,7 @@ def test_string_light_sdf_parse(self): # ///////////////////////////////////////////////// # TEST(DOMRoot, StringActorSdfParse) # { - # std::string sdf = "" + # sdf = "" # " " # " " # " 0 0 1.0 0 0 0" @@ -324,6 +324,39 @@ def test_element_to_light(self): # ASSERT_EQ(None, root2.Actor()) self.assertEqual(0, root2.world_count()) + def test_resolve_auto_inertials_with_save_calculation_configuration(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + model = root.model() + link = model.link_by_index(0) + + sdfParserConfig.set_calculate_inertial_configuration( + ConfigureResolveAutoInertials.SAVE_CALCULATION) + + inertialErr = [] + root.resolve_auto_inertials(inertialErr, sdfParserConfig) + self.assertEqual(len(inertialErr), 0) + self.assertTrue(link.auto_inertia_saved()) if __name__ == '__main__': unittest.main() diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index 0636d602b..f687f6815 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -15,66 +15,101 @@ import copy import math from gz_test_deps.sdformat import Sphere +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d import unittest class SphereTEST(unittest.TestCase): def test_default_construction(self): - sphere = Sphere(); - self.assertEqual(1.0, sphere.radius()); - sphere.set_radius(0.5); - self.assertEqual(0.5, sphere.radius()); + sphere = Sphere() + self.assertEqual(1.0, sphere.radius()) + sphere.set_radius(0.5) + self.assertEqual(0.5, sphere.radius()) def test_assignment(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = sphere; - self.assertEqual(0.2, sphere2.radius()); + sphere2 = sphere + self.assertEqual(0.2, sphere2.radius()) def test_copy_construction(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = Sphere(sphere); - self.assertEqual(0.2, sphere2.radius()); + sphere2 = Sphere(sphere) + self.assertEqual(0.2, sphere2.radius()) - self.assertEqual(4.0/3.0*math.pi*math.pow(0.2, 3), sphere2.shape().volume()); - self.assertEqual(0.2, sphere2.shape().radius()); + self.assertEqual(4.0/3.0*math.pi*math.pow(0.2, 3), sphere2.shape().volume()) + self.assertEqual(0.2, sphere2.shape().radius()) def test_deepcopy_construction(self): - sphere = Sphere(); - sphere.set_radius(0.2); + sphere = Sphere() + sphere.set_radius(0.2) - sphere2 = copy.deepcopy(sphere); - self.assertEqual(0.2, sphere2.radius()); + sphere2 = copy.deepcopy(sphere) + self.assertEqual(0.2, sphere2.radius()) def test_deepcopy_after_assignment(self): - sphere1 = Sphere(); - sphere1.set_radius(0.1); + sphere1 = Sphere() + sphere1.set_radius(0.1) - sphere2 = Sphere(); - sphere2.set_radius(0.2); + sphere2 = Sphere() + sphere2.set_radius(0.2) # This is similar to what swap does - tmp = copy.deepcopy(sphere1); - sphere1 = sphere2; - sphere2 = tmp; + tmp = copy.deepcopy(sphere1) + sphere1 = sphere2 + sphere2 = tmp - self.assertEqual(0.2, sphere1.radius()); - self.assertEqual(0.1, sphere2.radius()); + self.assertEqual(0.2, sphere1.radius()) + self.assertEqual(0.1, sphere2.radius()) def test_shape(self): - sphere = Sphere(); - self.assertEqual(1.0, sphere.radius()); + sphere = Sphere() + self.assertEqual(1.0, sphere.radius()) - sphere.shape().set_radius(0.123); - self.assertEqual(0.123, sphere.radius()); + sphere.shape().set_radius(0.123) + self.assertEqual(0.123, sphere.radius()) + + def test_calculate_inertial(self): + sphere = Sphere() + + # density of aluminium + density = 2170 + + sphere.set_radius(-2) + invalidSphereInertial = sphere.calculate_inertial(density) + self.assertEqual(None, invalidSphereInertial) + + r = 0.1 + + sphere.set_radius(r) + + expectedMass = sphere.shape().volume() * density + ixxIyyIzz = 0.4 * expectedMass * r * r + + expectedMassMat = MassMatrix3d( + expectedMass, Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + sphereInertial = sphere.calculate_inertial(density) + self.assertEqual(sphere.shape().material().density(), density) + self.assertNotEqual(None, sphereInertial) + self.assertEqual(expectedInertial, sphereInertial) + self.assertEqual(expectedInertial.mass_matrix().diagonal_moments(), + sphereInertial.mass_matrix().diagonal_moments()) + self.assertEqual(expectedInertial.mass_matrix().mass(), + sphereInertial.mass_matrix().mass()) + self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) if __name__ == '__main__': diff --git a/python/test/pyWorld_TEST.py b/python/test/pyWorld_TEST.py index f77fcdc8e..f8f5f9d0c 100644 --- a/python/test/pyWorld_TEST.py +++ b/python/test/pyWorld_TEST.py @@ -14,8 +14,9 @@ import copy from gz_test_deps.math import Color, Vector3d, SphericalCoordinates +from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import (Atmosphere, Gui, Physics, Plugin, Error, - Frame, Joint, Light, Model, Scene, World) + Frame, Joint, Light, Model, ParserConfig, Root, Scene, World) import gz_test_deps.sdformat as sdf import unittest import math @@ -87,8 +88,8 @@ def test_default_construction(self): "PoseRelativeToGraph error: scope does not point to a valid graph.") # world doesn't have graphs, so no names should exist in graphs - self.assertFalse(world.name_exists_in_frame_attached_to_graph("")); - self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")); + self.assertFalse(world.name_exists_in_frame_attached_to_graph("")) + self.assertFalse(world.name_exists_in_frame_attached_to_graph("link")) def test_copy_construction(self): @@ -443,6 +444,55 @@ def test_plugins(self): world.clear_plugins() self.assertEqual(0, len(world.plugins())) + def test_resolve_auto_inertials(self): + sdf = "" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 1240.0" + \ + " " + \ + " " + \ + " 2 2 2" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + + root = Root() + sdfParserConfig = ParserConfig() + errors = root.load_sdf_string(sdf, sdfParserConfig) + self.assertEqual(None, errors) + + world = root.world_by_index(0) + model = world.model_by_index(0) + link = model.link_by_index(0) + + errors = [] + root.resolve_auto_inertials(errors, sdfParserConfig) + + l = 2.0 + w = 2.0 + h = 2.0 + + expectedMass = l * w * h * 1240.0 + ixx = (1.0 / 12.0) * expectedMass * (w * w + h * h) + iyy = (1.0 / 12.0) * expectedMass * (l * l + h * h) + izz = (1.0 / 12.0) * expectedMass * (l * l + w * w) + + expectedMassMat = MassMatrix3d(expectedMass, Vector3d(ixx, iyy, izz), Vector3d.ZERO) + + expectedInertial = Inertiald() + expectedInertial.set_mass_matrix(expectedMassMat) + expectedInertial.set_pose(Pose3d.ZERO) + + self.assertEqual(0, len(errors)) + self.assertEqual(expectedInertial, link.inertial()) if __name__ == '__main__': unittest.main() From 7ac3696caf64b6e5aefa86d92a2b823a1d9fb58d Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Thu, 30 May 2024 10:31:09 -0500 Subject: [PATCH 17/25] add python binding for link gravity (#1419) Signed-off-by: youhy Co-authored-by: Ian Chen --- python/src/sdf/pyLink.cc | 7 +++++++ python/test/pyLink_TEST.py | 4 ++++ 2 files changed, 11 insertions(+) diff --git a/python/src/sdf/pyLink.cc b/python/src/sdf/pyLink.cc index 3f77912fa..74f4d45bf 100644 --- a/python/src/sdf/pyLink.cc +++ b/python/src/sdf/pyLink.cc @@ -162,6 +162,13 @@ void defineLink(pybind11::object module) .def("set_enable_wind", &sdf::Link::SetEnableWind, "Set whether this link should be subject to wind.") + .def("enable_gravity", + &sdf::Link::EnableGravity, + "Check if this link should be subject to gravity. " + "If true, this link should be affected by gravity.") + .def("set_enable_gravity", + &sdf::Link::SetEnableGravity, + "Set whether this link should be subject to gravity.") .def("add_collision", &sdf::Link::AddCollision, "Add a collision to the link.") diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index e4ad505a4..dfedcf95b 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -61,6 +61,10 @@ def test_default_construction(self): link.set_enable_wind(True) self.assertTrue(link.enable_wind()) + self.assertTrue(link.enable_gravity()) + link.set_enable_gravity(False) + self.assertFalse(link.enable_gravity()) + self.assertEqual(0, link.sensor_count()) self.assertEqual(None, link.sensor_by_index(0)) self.assertEqual(None, link.sensor_by_index(1)) From ae0ca8ee663fe34937a2d73097f20ee81654450e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 31 May 2024 02:12:00 +0900 Subject: [PATCH 18/25] Add python bindings for bullet and torsional friction (#1427) Signed-off-by: Ian Chen --- python/src/sdf/_gz_sdformat_pybind11.cc | 2 + python/src/sdf/pySurface.cc | 72 +++++++++ python/src/sdf/pySurface.hh | 12 ++ python/test/pySurface_TEST.py | 202 +++++++++++++++++++++++- 4 files changed, 287 insertions(+), 1 deletion(-) diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 80caf4396..5621fd17a 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -79,6 +79,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineAltimeter(m); sdf::python::defineAtmosphere(m); sdf::python::defineBox(m); + sdf::python::defineBulletFriction(m); sdf::python::defineCamera(m); sdf::python::defineCapsule(m); sdf::python::defineCollision(m); @@ -123,6 +124,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineSky(m); sdf::python::defineSphere(m); sdf::python::defineSurface(m); + sdf::python::defineTorsional(m); sdf::python::defineVisual(m); sdf::python::defineWorld(m); diff --git a/python/src/sdf/pySurface.cc b/python/src/sdf/pySurface.cc index dc7be622b..ed8545e95 100644 --- a/python/src/sdf/pySurface.cc +++ b/python/src/sdf/pySurface.cc @@ -55,6 +55,16 @@ void defineFriction(pybind11::object module) "Get the ODE object.") .def("set_ode", &sdf::Friction::SetODE, "Set the ODE object.") + .def("bullet_friction", &sdf::Friction::BulletFriction, + pybind11::return_value_policy::reference_internal, + "Get the bullet friction object.") + .def("set_bullet_friction", &sdf::Friction::SetBulletFriction, + "Set the bullet friction object.") + .def("torsional", &sdf::Friction::Torsional, + pybind11::return_value_policy::reference_internal, + "Get the torsional friction object.") + .def("set_torsional", &sdf::Friction::SetTorsional, + "Set the torsional friction object.") .def("__copy__", [](const sdf::Friction &self) { return sdf::Friction(self); }) @@ -118,6 +128,68 @@ void defineSurface(pybind11::object module) return sdf::Surface(self); }, "memo"_a); } +///////////////////////////////////////////////// +void defineBulletFriction(pybind11::object module) +{ + pybind11::class_(module, "BulletFriction") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("friction", &sdf::BulletFriction::Friction, + "Get the friction parameter.") + .def("set_friction", &sdf::BulletFriction::SetFriction, + "Set the friction parameter.") + .def("friction2", &sdf::BulletFriction::Friction2, + "Get the friction parameter.") + .def("set_friction2", &sdf::BulletFriction::SetFriction2, + "Set the friction2 parameter.") + .def("fdir1", &sdf::BulletFriction::Fdir1, + "Get the fdir1 parameter.") + .def("set_fdir1", &sdf::BulletFriction::SetFdir1, + "Set the fdir1 parameter.") + .def("rolling_friction", &sdf::BulletFriction::RollingFriction, + "Get the rolling fricion parameter.") + .def("set_rolling_friction", &sdf::BulletFriction::SetRollingFriction, + "Set the rolling friction parameter.") + .def("__copy__", [](const sdf::BulletFriction &self) { + return sdf::BulletFriction(self); + }) + .def("__deepcopy__", [](const sdf::BulletFriction &self, pybind11::dict) { + return sdf::BulletFriction(self); + }, "memo"_a); +} +///////////////////////////////////////////////// +void defineTorsional(pybind11::object module) +{ + pybind11::class_(module, "Torsional") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("coefficient", &sdf::Torsional::Coefficient, + "Get the coefficient parameter.") + .def("set_coefficient", &sdf::Torsional::SetCoefficient, + "Set the coefficient parameter.") + .def("use_patch_radius", &sdf::Torsional::UsePatchRadius, + "Get whether to use patch radius for torsional friction calculation.") + .def("set_use_patch_radius", &sdf::Torsional::SetUsePatchRadius, + "Set whether to use patch radius for torsional friction calculation.") + .def("patch_radius", &sdf::Torsional::PatchRadius, + "Get the radius of contact patch surface.") + .def("set_patch_radius", &sdf::Torsional::SetPatchRadius, + "Set the radius of contact patch surface.") + .def("surface_radius", &sdf::Torsional::SurfaceRadius, + "Get the surface radius on the contact point.") + .def("set_surface_radius", &sdf::Torsional::SetSurfaceRadius, + "Set the surface radius on the contact point.") + .def("ode_slip", &sdf::Torsional::ODESlip, + "Get the ODE force dependent slip for torsional friction.") + .def("set_ode_slip", &sdf::Torsional::SetODESlip, + "Set the ODE force dependent slip for torsional friction.") + .def("__copy__", [](const sdf::Torsional &self) { + return sdf::Torsional(self); + }) + .def("__deepcopy__", [](const sdf::Torsional &self, pybind11::dict) { + return sdf::Torsional(self); + }, "memo"_a); +} } // namespace python } // namespace SDF_VERSION_NAMESPACE } // namespace sdf diff --git a/python/src/sdf/pySurface.hh b/python/src/sdf/pySurface.hh index 75e29f691..b45b547a2 100644 --- a/python/src/sdf/pySurface.hh +++ b/python/src/sdf/pySurface.hh @@ -52,6 +52,18 @@ void defineODE(pybind11::object module); * \param[in] module a pybind11 module to add the definition to */ void defineSurface(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::BulletFriction +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineBulletFriction(pybind11::object module); + +/// Define a pybind11 wrapper for an sdf::Torsional +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineTorsional(pybind11::object module); } // namespace python } // namespace SDF_VERSION_NAMESPACE } // namespace sdf diff --git a/python/test/pySurface_TEST.py b/python/test/pySurface_TEST.py index 2651b1113..ed6875c0d 100644 --- a/python/test/pySurface_TEST.py +++ b/python/test/pySurface_TEST.py @@ -14,7 +14,8 @@ import copy from gz_test_deps.math import Vector3d -from gz_test_deps.sdformat import Surface, Contact, Friction, ODE +from gz_test_deps.sdformat import Surface, Contact, Friction, ODE, \ + BulletFriction, Torsional import unittest @@ -34,8 +35,24 @@ def test_assigment_construction(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) + friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -48,6 +65,17 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -60,6 +88,20 @@ def test_assigment_construction(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) + surface1.set_friction(friction) self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) @@ -74,6 +116,29 @@ def test_assigment_construction(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1.1, 2.1, 3.1)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.03) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.7) def test_copy_construction(self): surface1 = Surface() @@ -84,8 +149,21 @@ def test_copy_construction(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -98,6 +176,17 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -110,6 +199,20 @@ def test_copy_construction(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) + surface1.set_friction(friction) self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) @@ -124,6 +227,29 @@ def test_copy_construction(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) def test_deepcopy(self): surface1 = Surface() @@ -134,8 +260,21 @@ def test_deepcopy(self): ode.set_slip1(3) ode.set_slip2(4) ode.set_fdir1(Vector3d(1, 2, 3)) + bullet = BulletFriction() + bullet.set_friction(0.11) + bullet.set_friction2(0.22) + bullet.set_fdir1(Vector3d(3, 2, 1)) + bullet.set_rolling_friction(0.33) + torsional = Torsional() + torsional.set_coefficient(1.2) + torsional.set_use_patch_radius(True) + torsional.set_patch_radius(0.5) + torsional.set_surface_radius(0.15) + torsional.set_ode_slip(0.01) friction = Friction() friction.set_ode(ode) + friction.set_bullet_friction(bullet) + friction.set_torsional(torsional) contact.set_collide_bitmask(0x12) surface1.set_contact(contact) surface1.set_friction(friction) @@ -148,6 +287,17 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().ode().slip2(), 4) self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) contact.set_collide_bitmask(0x21) surface1.set_contact(contact) @@ -160,7 +310,19 @@ def test_deepcopy(self): ode.set_slip2(4.1) ode.set_fdir1(Vector3d(1.1, 2.1, 3.1)) friction.set_ode(ode) + bullet.set_friction(0.33) + bullet.set_friction2(0.45) + bullet.set_fdir1(Vector3d(0, 1, 2)) + bullet.set_rolling_friction(0.03) + friction.set_bullet_friction(bullet) + torsional.set_coefficient(2.1) + torsional.set_use_patch_radius(False) + torsional.set_patch_radius(0.1) + torsional.set_surface_radius(0.9) + torsional.set_ode_slip(0.7) + friction.set_torsional(torsional) surface1.set_friction(friction) + self.assertEqual(surface1.friction().ode().mu(), 1.1) self.assertEqual(surface1.friction().ode().mu2(), 1.2) self.assertEqual(surface1.friction().ode().slip1(), 3.1) @@ -174,6 +336,29 @@ def test_deepcopy(self): self.assertEqual(surface2.friction().ode().fdir1(), Vector3d(1, 2, 3)) + self.assertEqual(surface1.friction().bullet_friction().friction(), 0.33) + self.assertEqual(surface1.friction().bullet_friction().friction2(), 0.45) + self.assertEqual(surface1.friction().bullet_friction().fdir1(), + Vector3d(0, 1, 2)) + self.assertEqual(surface1.friction().bullet_friction().rolling_friction(), + 0.03) + self.assertEqual(surface2.friction().bullet_friction().friction(), 0.11) + self.assertEqual(surface2.friction().bullet_friction().friction2(), 0.22) + self.assertEqual(surface2.friction().bullet_friction().fdir1(), + Vector3d(3, 2, 1)) + self.assertEqual(surface2.friction().bullet_friction().rolling_friction(), + 0.33) + + self.assertEqual(surface1.friction().torsional().coefficient(), 2.1) + self.assertFalse(surface1.friction().torsional().use_patch_radius()) + self.assertEqual(surface1.friction().torsional().patch_radius(), 0.1) + self.assertEqual(surface1.friction().torsional().surface_radius(), 0.9) + self.assertEqual(surface1.friction().torsional().ode_slip(), 0.7) + self.assertEqual(surface2.friction().torsional().coefficient(), 1.2) + self.assertTrue(surface2.friction().torsional().use_patch_radius()) + self.assertEqual(surface2.friction().torsional().patch_radius(), 0.5) + self.assertEqual(surface2.friction().torsional().surface_radius(), 0.15) + self.assertEqual(surface2.friction().torsional().ode_slip(), 0.01) def test_default_contact_construction(self): contact = Contact() @@ -196,6 +381,21 @@ def test_default_ode_construction(self): self.assertEqual(ode.fdir1(), Vector3d(0, 0, 0)) + def test_default_bullet_friction_construction(self): + bullet = BulletFriction() + self.assertEqual(bullet.friction(), 1.0) + self.assertEqual(bullet.friction2(), 1.0) + self.assertEqual(bullet.fdir1(), + Vector3d(0, 0, 0)) + self.assertEqual(bullet.rolling_friction(), 1.0) + + def test_default_torsional_construction(self): + torsional = Torsional() + self.assertEqual(torsional.coefficient(), 1.0) + self.assertTrue(torsional.use_patch_radius()) + self.assertEqual(torsional.patch_radius(), 0.0) + self.assertEqual(torsional.surface_radius(), 0.0) + self.assertEqual(torsional.ode_slip(), 0.0) if __name__ == '__main__': unittest.main() From fc84f94d147bf31fd594e17bade68946246236b3 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 28 May 2024 17:57:37 +0900 Subject: [PATCH 19/25] Backport voxel_resolution sdf element Signed-off-by: Ian Chen --- include/sdf/Mesh.hh | 12 ++++++++++++ python/src/sdf/pyConvexDecomposition.cc | 7 +++++++ python/test/pyMesh_TEST.py | 6 ++++++ sdf/1.11/mesh_shape.sdf | 3 +++ src/Mesh.cc | 22 ++++++++++++++++++++++ src/Mesh_TEST.cc | 6 ++++++ test/integration/geometry_dom.cc | 1 + test/sdf/shapes.sdf | 1 + 8 files changed, 58 insertions(+) diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 7cc48c955..c77cb7bff 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -69,11 +69,23 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Get the maximum number of convex hulls that can be generated. + /// \return Maximum number of convex hulls. public: unsigned int MaxConvexHulls() const; /// \brief Set the maximum number of convex hulls that can be generated. + /// \param[in] _maxConvexHulls Maximum number of convex hulls. public: void SetMaxConvexHulls(unsigned int _maxConvexHulls); + /// Get the voxel resolution to use for representing the mesh. Applicable + /// only to voxel based convex decomposition methods. + /// \param[in] _voxelResolution Voxel resolution to use. + public: unsigned int VoxelResolution() const; + + /// Set the voxel resolution to use for representing the mesh. Applicable + /// only to voxel based convex decomposition methods. + /// \param[in] _voxelResolution Voxel resolution to use. + public: void SetVoxelResolution(unsigned int _voxelResolution); + /// \brief Private data pointer. GZ_UTILS_IMPL_PTR(dataPtr) }; diff --git a/python/src/sdf/pyConvexDecomposition.cc b/python/src/sdf/pyConvexDecomposition.cc index aacd7ee42..c3cefd5d8 100644 --- a/python/src/sdf/pyConvexDecomposition.cc +++ b/python/src/sdf/pyConvexDecomposition.cc @@ -38,6 +38,13 @@ void defineConvexDecomposition(pybind11::object module) "Get the maximum number of convex hulls that can be generated.") .def("set_max_convex_hulls", &sdf::ConvexDecomposition::SetMaxConvexHulls, "Set the maximum number of convex hulls that can be generated.") + .def("voxel_resolution", &sdf::ConvexDecomposition::VoxelResolution, + "Get the voxel resolution to use. Applicable only to voxel based " + "convex decomposition methods.") + .def("set_voxel_resolution", &sdf::ConvexDecomposition::SetVoxelResolution, + "Set the voxel resolution to use. Applicable only to voxel based " + "convex decomposition methods.") + .def("__copy__", [](const sdf::ConvexDecomposition &self) { return sdf::ConvexDecomposition(self); }) diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index ac9840ea6..0990095d6 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -45,6 +45,7 @@ def test_assigment(self): convexDecomp = ConvexDecomposition() convexDecomp.set_max_convex_hulls(10) + convexDecomp.set_voxel_resolution(100000) mesh.set_convex_decomposition(convexDecomp) mesh2 = mesh @@ -58,6 +59,7 @@ def test_assigment(self): convexDecomp2 = mesh2.convex_decomposition() self.assertEqual(10, convexDecomp2.max_convex_hulls()) + self.assertEqual(100000, convexDecomp2.voxel_resolution()) mesh.set_file_path("/apple") self.assertEqual("/apple", mesh2.file_path()) @@ -86,6 +88,7 @@ def test_deepcopy_construction(self): convexDecomp = ConvexDecomposition() convexDecomp.set_max_convex_hulls(10) + convexDecomp.set_voxel_resolution(100000) mesh.set_convex_decomposition(convexDecomp) mesh2 = copy.deepcopy(mesh) @@ -99,6 +102,7 @@ def test_deepcopy_construction(self): convexDecomp2 = mesh2.convex_decomposition() self.assertEqual(10, convexDecomp2.max_convex_hulls()) + self.assertEqual(100000, convexDecomp2.voxel_resolution()) mesh.set_file_path("/apple") mesh.set_scale(Vector3d(0.3, 0.2, 0.4)) @@ -131,8 +135,10 @@ def test_set(self): convexDecomp = ConvexDecomposition() convexDecomp.set_max_convex_hulls(10) + convexDecomp.set_voxel_resolution(100000) mesh.set_convex_decomposition(convexDecomp) self.assertEqual(10, mesh.convex_decomposition().max_convex_hulls()) + self.assertEqual(100000, mesh.convex_decomposition().voxel_resolution()) self.assertEqual("", mesh.uri()) mesh.set_uri("http://myuri.com") diff --git a/sdf/1.11/mesh_shape.sdf b/sdf/1.11/mesh_shape.sdf index 1ed64ca44..38754e5f5 100644 --- a/sdf/1.11/mesh_shape.sdf +++ b/sdf/1.11/mesh_shape.sdf @@ -12,6 +12,9 @@ Maximum number of convex hulls to decompose into. This sets the maximum number of submeshes that the final decomposed mesh will contain. + + The number of voxels to use for representing the source mesh before decomposition. Applicable only to voxel based convex decomposition methods. + diff --git a/src/Mesh.cc b/src/Mesh.cc index f026e2d63..d2ee4b7a8 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -45,6 +45,10 @@ class sdf::ConvexDecomposition::Implementation /// \brief Maximum number of convex hulls to generate. public: unsigned int maxConvexHulls{16u}; + /// \brief Voxel resolution. Applicable only to voxel based methods for + /// representing the mesh before decomposition + public: unsigned int voxelResolution{200000u}; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf = nullptr; }; @@ -112,6 +116,10 @@ Errors ConvexDecomposition::Load(ElementPtr _sdf) errors, "max_convex_hulls", this->dataPtr->maxConvexHulls).first; + this->dataPtr->voxelResolution = _sdf->Get( + errors, "voxel_resolution", + this->dataPtr->voxelResolution).first; + return errors; } @@ -133,6 +141,18 @@ void ConvexDecomposition::SetMaxConvexHulls(unsigned int _maxConvexHulls) this->dataPtr->maxConvexHulls = _maxConvexHulls; } +///////////////////////////////////////////////// +unsigned int ConvexDecomposition::VoxelResolution() const +{ + return this->dataPtr->voxelResolution; +} + +///////////////////////////////////////////////// +void ConvexDecomposition::SetVoxelResolution(unsigned int _voxelResolution) +{ + this->dataPtr->voxelResolution = _voxelResolution; +} + ///////////////////////////////////////////////// Mesh::Mesh() : dataPtr(gz::utils::MakeImpl()) @@ -403,6 +423,8 @@ sdf::ElementPtr Mesh::ToElement(sdf::Errors &_errors) const _errors); convexDecomp->GetElement("max_convex_hulls")->Set( this->dataPtr->convexDecomposition->MaxConvexHulls()); + convexDecomp->GetElement("voxel_resolution")->Set( + this->dataPtr->convexDecomposition->VoxelResolution()); } // Uri diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 29ef70d85..391283976 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -58,6 +58,7 @@ TEST(DOMMesh, MoveConstructor) sdf::ConvexDecomposition convexDecomp; EXPECT_EQ(nullptr, convexDecomp.Element()); convexDecomp.SetMaxConvexHulls(10u); + convexDecomp.SetVoxelResolution(100000u); mesh.SetConvexDecomposition(convexDecomp); sdf::Mesh mesh2(std::move(mesh)); @@ -72,6 +73,7 @@ TEST(DOMMesh, MoveConstructor) auto convexDecomp2 = mesh2.ConvexDecomposition(); ASSERT_NE(nullptr, convexDecomp2); EXPECT_EQ(10u, convexDecomp2->MaxConvexHulls()); + EXPECT_EQ(100000u, convexDecomp2->VoxelResolution()); } ///////////////////////////////////////////////// @@ -186,9 +188,11 @@ TEST(DOMMesh, Set) sdf::ConvexDecomposition convexDecomp; convexDecomp.SetMaxConvexHulls(10u); + convexDecomp.SetVoxelResolution(200000u); mesh.SetConvexDecomposition(convexDecomp); ASSERT_NE(nullptr, mesh.ConvexDecomposition()); EXPECT_EQ(10u, mesh.ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ(200000u, mesh.ConvexDecomposition()->VoxelResolution()); EXPECT_EQ(std::string(), mesh.Uri()); mesh.SetUri("http://myuri.com"); @@ -365,6 +369,7 @@ TEST(DOMMesh, ToElement) sdf::ConvexDecomposition convexDecomp; convexDecomp.SetMaxConvexHulls(10u); + convexDecomp.SetVoxelResolution(300000u); mesh.SetConvexDecomposition(convexDecomp); sdf::ElementPtr elem = mesh.ToElement(); @@ -381,6 +386,7 @@ TEST(DOMMesh, ToElement) EXPECT_EQ(mesh.CenterSubmesh(), mesh2.CenterSubmesh()); ASSERT_NE(nullptr, mesh2.ConvexDecomposition()); EXPECT_EQ(10u, mesh2.ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ(300000u, mesh2.ConvexDecomposition()->VoxelResolution()); } ///////////////////////////////////////////////// diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 297d1f0af..419a04dc9 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -184,6 +184,7 @@ TEST(DOMGeometry, Shapes) meshColGeom->Optimization()); ASSERT_NE(nullptr, meshColGeom->ConvexDecomposition()); EXPECT_EQ(4u, meshColGeom->ConvexDecomposition()->MaxConvexHulls()); + EXPECT_EQ(400000u, meshColGeom->ConvexDecomposition()->VoxelResolution()); EXPECT_EQ("https://fuel.gazebosim.org/1.0/an_org/models/a_model/mesh/" "mesh.dae", meshColGeom->Uri()); diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 036348ef2..37c56277c 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -138,6 +138,7 @@ 4 + 400000 https://fuel.gazebosim.org/1.0/an_org/models/a_model/mesh/mesh.dae From 23d318c3eda13b5b7d4275e0cc989b9bfe85cad2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 13 Jun 2024 00:32:53 -0700 Subject: [PATCH 20/25] Prepare for 12.8.0 release (#1430) (#1432) Signed-off-by: Ian Chen --- Changelog.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/Changelog.md b/Changelog.md index 5c59f7e7f..b5eaa34e2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -462,6 +462,21 @@ ## libsdformat 12.X +### libsdformat 12.8.0 (2024-06-06) + +1. Add support for no gravity link + * [Pull request #1410](https://github.com/gazebosim/sdformat/pull/1410) + +1. Add bullet and torsional friction DOM + * [Pull request #1351](https://github.com/gazebosim/sdformat/pull/1351) + +1. Fix static builds and optimize test compilation + * [Pull request #1343](https://github.com/gazebosim/sdformat/pull/1343) + * [Pull request #1347](https://github.com/gazebosim/sdformat/pull/1347) + +1. Update github action workflows + * [Pull request #1345](https://github.com/gazebosim/sdformat/pull/1345) + ### libsdformat 12.7.2 (2023-09-01) 1. Fixed 1.9/light.sdf From f86734881bff7b87fc2f0ad89be1d36151e07f98 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 13 Jun 2024 13:52:51 -0700 Subject: [PATCH 21/25] Prepare for 13.7.0 release (#1433) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c6ce4b9c..308478f8a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0004 NEW) endif(COMMAND CMAKE_POLICY) -project (sdformat13 VERSION 13.6.0) +project (sdformat13 VERSION 13.7.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software diff --git a/Changelog.md b/Changelog.md index b5eaa34e2..605aefd81 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,41 @@ ## libsdformat 13.X +### libsdformat 13.7.0 (2024-06-13) + +1. Add support for no gravity link + * [Pull request #1410](https://github.com/gazebosim/sdformat/pull/1410) + * [Pull request #1419](https://github.com/gazebosim/sdformat/pull/1419) + +1. Fix macOS workflow and backport windows fix + * [Pull request #1409](https://github.com/gazebosim/sdformat/pull/1409) + +1. Fix warning with pybind11 2.12 + * [Pull request #1389](https://github.com/gazebosim/sdformat/pull/1389) + +1. Add bullet and torsional friction DOM + * [Pull request #1351](https://github.com/gazebosim/sdformat/pull/1351) + * [Pull request #1427](https://github.com/gazebosim/sdformat/pull/1427) + +1. Resolve URIs relative to file path + * [Pull request #1373](https://github.com/gazebosim/sdformat/pull/1373) + +1. Bazel updates for Garden build + * [Pull request #1239](https://github.com/gazebosim/sdformat/pull/1239) + +1. Fix static builds and optimize test compilation + * [Pull request #1343](https://github.com/gazebosim/sdformat/pull/1343) + * [Pull request #1347](https://github.com/gazebosim/sdformat/pull/1347) + +1. Install ruby commands on Windows + * [Pull request #1339](https://github.com/gazebosim/sdformat/pull/1339) + * [Pull request #1341](https://github.com/gazebosim/sdformat/pull/1341) + +1. Update github action workflows + * [Pull request #1345](https://github.com/gazebosim/sdformat/pull/1345) + +1. URDF->SDF handle links with no inertia or small mass + * [Pull request #1238](https://github.com/gazebosim/sdformat/pull/1238) + ### libsdformat 13.6.0 (2023-08-30) 1. Use relative path in an urdf include to avoid confusion between internal and system headers From 37ca7fcaf96e93c2cf26bd6bf4fdc62e51aa08cc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 14 Jun 2024 12:55:53 -0700 Subject: [PATCH 22/25] Prepare for 14.3.0 release (#1437) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 32 ++++++++++++++++++++++++++++++++ package.xml | 2 +- 3 files changed, 34 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f771a9c65..877198921 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0004 NEW) endif(COMMAND CMAKE_POLICY) -project (sdformat14 VERSION 14.2.0) +project (sdformat14 VERSION 14.3.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software diff --git a/Changelog.md b/Changelog.md index 7a4c28e30..e094e604b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,37 @@ ## libsdformat 14.X +### libsdformat 14.3.0 (2024-06-14) + +1. Backport voxel_resolution sdf element + * [Pull request #1429](https://github.com/gazebosim/sdformat/pull/1429) + +1. Added Automatic Moment of Inertia Calculations for Basic Shapes Python wrappers + * [Pull request #1424](https://github.com/gazebosim/sdformat/pull/1424) + +1. Add support for no gravity link + * [Pull request #1410](https://github.com/gazebosim/sdformat/pull/1410) + * [Pull request #1419](https://github.com/gazebosim/sdformat/pull/1419) + +1. Update default camera instrinsics skew to 0, which matches spec + * [Pull request #1423](https://github.com/gazebosim/sdformat/pull/1423) + * [Pull request #1425](https://github.com/gazebosim/sdformat/pull/1425) + +1. Allow empty strings in plugin and custom attributes + * [Pull request #1407](https://github.com/gazebosim/sdformat/pull/1407) + +1. (Backport) Enable 24.04 CI, remove distutils dependency + * [Pull request #1413](https://github.com/gazebosim/sdformat/pull/1413) + +1. Fix macOS workflow and backport windows fix + * [Pull request #1409](https://github.com/gazebosim/sdformat/pull/1409) + +1. Fix warning with pybind11 2.12 + * [Pull request #1389](https://github.com/gazebosim/sdformat/pull/1389) + +1. Add bullet and torsional friction DOM + * [Pull request #1351](https://github.com/gazebosim/sdformat/pull/1351) + * [Pull request #1427](https://github.com/gazebosim/sdformat/pull/1427) + ### libsdformat 14.2.0 (2024-04-23) 1. Fix trivial warning on 24.04 for JointAxis_TEST.cc diff --git a/package.xml b/package.xml index 3132f5f8f..b065874cd 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ sdformat14 - 14.2.0 + 14.3.0 SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications From 44cc342d7eea1be5747128cf0be5e8cc05e5a189 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 18 Jun 2024 21:32:59 -0700 Subject: [PATCH 23/25] Add custom attribute to custom element in test (#1406) Update test to confirm that #54 is fixed. Signed-off-by: Steve Peters --- test/integration/custom_elems_attrs.sdf | 2 +- test/integration/sdf_custom.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/custom_elems_attrs.sdf b/test/integration/custom_elems_attrs.sdf index 54c9f9c9e..e936450e1 100644 --- a/test/integration/custom_elems_attrs.sdf +++ b/test/integration/custom_elems_attrs.sdf @@ -10,7 +10,7 @@ L2 - + transmission_interface/SimpleTransmission EffortJointInterface diff --git a/test/integration/sdf_custom.cc b/test/integration/sdf_custom.cc index cdf1be1ee..ef04256b9 100644 --- a/test/integration/sdf_custom.cc +++ b/test/integration/sdf_custom.cc @@ -139,7 +139,7 @@ TEST(SDFParser, ReloadCustomElements) ASSERT_NE(nullptr, customElem2); const std::string customElemStr = -R"( +R"( transmission_interface/SimpleTransmission EffortJointInterface From 7f119b443900f25b0d2a3d0f5aca6ab76b6707fa Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Thu, 20 Jun 2024 14:24:32 -0400 Subject: [PATCH 24/25] Add Cone as a primitive parametric shape. (#1415) * Backport: Add cone shape to SDFormat spec (#1418) Signed-off-by: Benjamin Perseghetti Co-authored-by: Steve Peters --- include/sdf/Cone.hh | 108 +++++++++ include/sdf/Geometry.hh | 15 ++ include/sdf/Visual.hh | 1 + python/CMakeLists.txt | 2 + python/src/sdf/_gz_sdformat_pybind11.cc | 2 + python/src/sdf/pyCone.cc | 60 +++++ python/src/sdf/pyCone.hh | 43 ++++ python/src/sdf/pyGeometry.cc | 8 + python/test/pyCollision_TEST.py | 3 +- python/test/pyCone_TEST.py | 114 ++++++++++ python/test/pyGeometry_TEST.py | 20 +- python/test/pyVisual_TEST.py | 1 + sdf/1.11/CMakeLists.txt | 1 + sdf/1.11/cone_shape.sdf | 9 + sdf/1.11/geometry.sdf | 1 + src/Collision_TEST.cc | 1 + src/Cone.cc | 189 ++++++++++++++++ src/Cone_TEST.cc | 288 ++++++++++++++++++++++++ src/Geometry.cc | 30 +++ src/Geometry_TEST.cc | 126 +++++++++++ src/Visual_TEST.cc | 1 + test/integration/geometry_dom.cc | 21 ++ test/sdf/basic_shapes.sdf | 25 +- test/sdf/shapes.sdf | 20 +- test/sdf/shapes_world.sdf | 25 +- 25 files changed, 1109 insertions(+), 5 deletions(-) create mode 100644 include/sdf/Cone.hh create mode 100644 python/src/sdf/pyCone.cc create mode 100644 python/src/sdf/pyCone.hh create mode 100644 python/test/pyCone_TEST.py create mode 100644 sdf/1.11/cone_shape.sdf create mode 100644 src/Cone.cc create mode 100644 src/Cone_TEST.cc diff --git a/include/sdf/Cone.hh b/include/sdf/Cone.hh new file mode 100644 index 000000000..641c5a304 --- /dev/null +++ b/include/sdf/Cone.hh @@ -0,0 +1,108 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_CONE_HH_ +#define SDF_CONE_HH_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + /// \brief Cone represents a cone shape, and is usually accessed + /// through a Geometry. + class SDFORMAT_VISIBLE Cone + { + /// \brief Constructor + public: Cone(); + + /// \brief Load the cone geometry 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 cone's radius in meters. + /// \return The radius of the cone in meters. + public: double Radius() const; + + /// \brief Set the cone's radius in meters. + /// \param[in] _radius The radius of the cone in meters. + public: void SetRadius(double _radius); + + /// \brief Get the cone's length in meters. + /// \return The length of the cone in meters. + public: double Length() const; + + /// \brief Set the cone's length in meters. + /// \param[in] _length The length of the cone in meters. + public: void SetLength(double _length); + + /// \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 Get the Gazebo Math representation of this cone. + /// \return A const reference to a gz::math::Sphered object. + public: const gz::math::Coned &Shape() const; + + /// \brief Get a mutable Gazebo Math representation of this cone. + /// \return A reference to a gz::math::Coned object. + public: gz::math::Coned &Shape(); + + /// \brief Calculate and return the Inertial values for the cone. In + /// order to calculate the inertial properties, the function mutates the + /// object by updating its material properties. + /// \param[in] _density Density of the cone in kg/m^3 + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional + CalculateInertial(double _density); + + /// \brief Create and return an SDF element filled with data from this + /// cone. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \return SDF element pointer with updated cone values. + public: sdf::ElementPtr ToElement() const; + + /// \brief Create and return an SDF element filled with data from this + /// cone. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \param[out] _errors Vector of errors. + /// \return SDF element pointer with updated cone values. + public: sdf::ElementPtr ToElement(sdf::Errors &_errors) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } +} +#endif diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 35fd1e1cd..5d75b860d 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -37,6 +37,7 @@ namespace sdf // Forward declare private data class. class Box; class Capsule; + class Cone; class Cylinder; class Ellipsoid; class Heightmap; @@ -79,6 +80,9 @@ namespace sdf /// \brief A polyline geometry. POLYLINE = 9, + + /// \brief A polyline geometry. + CONE = 10, }; /// \brief Geometry provides access to a shape, such as a Box. Use the @@ -137,6 +141,17 @@ namespace sdf /// \param[in] _capsule The capsule shape. public: void SetCapsuleShape(const Capsule &_capsule); + /// \brief Get the cone geometry, or nullptr if the contained + /// geometry is not a cone. + /// \return Pointer to the visual's cone geometry, or nullptr if the + /// geometry is not a cone. + /// \sa GeometryType Type() const + public: const Cone *ConeShape() const; + + /// \brief Set the cone shape. + /// \param[in] _cone The cone shape. + public: void SetConeShape(const Cone &_cone); + /// \brief Get the cylinder geometry, or nullptr if the contained /// geometry is not a cylinder. /// \return Pointer to the visual's cylinder geometry, or nullptr if the diff --git a/include/sdf/Visual.hh b/include/sdf/Visual.hh index ff9da957f..f8e46a2bf 100644 --- a/include/sdf/Visual.hh +++ b/include/sdf/Visual.hh @@ -22,6 +22,7 @@ #include #include #include "sdf/Box.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Element.hh" #include "sdf/Material.hh" diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f9cc0917d..864d38c18 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -52,6 +52,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/sdf/pyCamera.cc src/sdf/pyCapsule.cc src/sdf/pyCollision.cc + src/sdf/pyCone.cc src/sdf/pyConvexDecomposition.cc src/sdf/pyCylinder.cc src/sdf/pyElement.cc @@ -135,6 +136,7 @@ if (BUILD_TESTING AND NOT WIN32) pyCamera_TEST pyCapsule_TEST pyCollision_TEST + pyCone_TEST pyCylinder_TEST pyElement_TEST pyEllipsoid_TEST diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 0a2c9fff4..80892100e 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -26,6 +26,7 @@ #include "pyCamera.hh" #include "pyCapsule.hh" #include "pyCollision.hh" +#include "pyCone.hh" #include "pyConvexDecomposition.hh" #include "pyCylinder.hh" #include "pyElement.hh" @@ -87,6 +88,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { sdf::python::defineCamera(m); sdf::python::defineCapsule(m); sdf::python::defineCollision(m); + sdf::python::defineCone(m); sdf::python::defineConvexDecomposition(m); sdf::python::defineContact(m); sdf::python::defineCylinder(m); diff --git a/python/src/sdf/pyCone.cc b/python/src/sdf/pyCone.cc new file mode 100644 index 000000000..05cb9cb15 --- /dev/null +++ b/python/src/sdf/pyCone.cc @@ -0,0 +1,60 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "pyCone.hh" + +#include + +#include "sdf/Cone.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineCone(pybind11::object module) +{ + pybind11::class_(module, "Cone") + .def(pybind11::init<>()) + .def(pybind11::init()) + .def("radius", &sdf::Cone::Radius, + "Get the cone's radius in meters.") + .def("set_radius", &sdf::Cone::SetRadius, + "Set the cone's radius in meters.") + .def("length", &sdf::Cone::Length, + "Get the cone's length in meters.") + .def("set_length", &sdf::Cone::SetLength, + "Set the cone's length in meters.") + .def( + "shape", + pybind11::overload_cast<>(&sdf::Cone::Shape, pybind11::const_), + pybind11::return_value_policy::reference, + "Get a mutable Gazebo Math representation of this Cone.") + .def("__copy__", [](const sdf::Cone &self) { + return sdf::Cone(self); + }) + .def("__deepcopy__", [](const sdf::Cone &self, pybind11::dict) { + return sdf::Cone(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyCone.hh b/python/src/sdf/pyCone.hh new file mode 100644 index 000000000..1f7f3dc85 --- /dev/null +++ b/python/src/sdf/pyCone.hh @@ -0,0 +1,43 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SDFORMAT_PYTHON_CONE_HH_ +#define SDFORMAT_PYTHON_CONE_HH_ + +#include + +#include "sdf/Cone.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::Cone +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineCone(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_CONE_HH_ diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index ffcbabbe5..a30d3106f 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -23,6 +23,7 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" @@ -64,6 +65,12 @@ void defineGeometry(pybind11::object module) "geometry is not a capsule.") .def("set_capsule_shape", &sdf::Geometry::SetCapsuleShape, "Set the capsule shape.") + .def("cone_shape", &sdf::Geometry::ConeShape, + pybind11::return_value_policy::reference, + "Get the cone geometry, or None if the contained " + "geometry is not a cone.") + .def("set_cone_shape", &sdf::Geometry::SetConeShape, + "Set the cone shape.") .def("cylinder_shape", &sdf::Geometry::CylinderShape, pybind11::return_value_policy::reference, "Get the cylinder geometry, or None if the contained " @@ -104,6 +111,7 @@ void defineGeometry(pybind11::object module) pybind11::enum_(module, "GeometryType") .value("EMPTY", sdf::GeometryType::EMPTY) .value("BOX", sdf::GeometryType::BOX) + .value("CONE", sdf::GeometryType::CONE) .value("CYLINDER", sdf::GeometryType::CYLINDER) .value("PLANE", sdf::GeometryType::PLANE) .value("SPHERE", sdf::GeometryType::SPHERE) diff --git a/python/test/pyCollision_TEST.py b/python/test/pyCollision_TEST.py index 8d8d96735..7068c53d2 100644 --- a/python/test/pyCollision_TEST.py +++ b/python/test/pyCollision_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d -from gz_test_deps.sdformat import (Box, Collision, Contact, Cylinder, Error, +from gz_test_deps.sdformat import (Box, Collision, Cone, Contact, Cylinder, Error, Geometry, ParserConfig, Plane, Root, Surface, Sphere, SDFErrorsException) import gz_test_deps.sdformat as sdf @@ -61,6 +61,7 @@ def test_default_construction(self): self.assertNotEqual(None, collision.geometry()) self.assertEqual(sdf.GeometryType.EMPTY, collision.geometry().type()) self.assertEqual(None, collision.geometry().box_shape()) + self.assertEqual(None, collision.geometry().cone_shape()) self.assertEqual(None, collision.geometry().cylinder_shape()) self.assertEqual(None, collision.geometry().plane_shape()) self.assertEqual(None, collision.geometry().sphere_shape()) diff --git a/python/test/pyCone_TEST.py b/python/test/pyCone_TEST.py new file mode 100644 index 000000000..e36e65f63 --- /dev/null +++ b/python/test/pyCone_TEST.py @@ -0,0 +1,114 @@ +# Copyright 2024 CogniPilot Foundation +# Copyright 2024 Open Source Robotics Foundation +# Copyright 2024 Rudis Laboratories + +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at + +# http://www.apache.org/licenses/LICENSE-2.0 + +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import copy + +import math + +from gz_test_deps.sdformat import Cone + +import unittest + + +class ConeTEST(unittest.TestCase): + + def test_default_construction(self): + cone = Cone() + + self.assertEqual(math.pi * math.pow(0.5, 2) * 1.0 / 3.0, + cone.shape().volume()) + + self.assertEqual(0.5, cone.radius()) + self.assertEqual(1.0, cone.length()) + + cone.set_radius(0.5) + cone.set_length(2.3) + + self.assertEqual(0.5, cone.radius()) + self.assertEqual(2.3, cone.length()) + + def test_assignment(self): + cone = Cone() + cone.set_radius(0.2) + cone.set_length(3.0) + self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0 / 3.0, + cone.shape().volume()) + + cone2 = cone + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + self.assertEqual(math.pi * math.pow(0.2, 2) * 3.0 / 3.0, + cone2.shape().volume()) + self.assertEqual(0.2, cone2.shape().radius()) + self.assertEqual(3.0, cone2.shape().length()) + + cone.set_radius(2.0) + cone.set_length(0.3) + + self.assertEqual(2.0, cone.radius()) + self.assertEqual(0.3, cone.length()) + self.assertEqual(2.0, cone2.radius()) + self.assertEqual(0.3, cone2.length()) + + + def test_copy_construction(self): + cone = Cone(); + cone.set_radius(0.2) + cone.set_length(3.0) + + cone2 = Cone(cone) + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + cone.set_radius(2.) + cone.set_length(0.3) + + self.assertEqual(2, cone.radius()) + self.assertEqual(0.3, cone.length()) + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + def test_deepcopy(self): + cone = Cone(); + cone.set_radius(0.2) + cone.set_length(3.0) + + cone2 = copy.deepcopy(cone); + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + cone.set_radius(2.) + cone.set_length(0.3) + + self.assertEqual(2, cone.radius()) + self.assertEqual(0.3, cone.length()) + self.assertEqual(0.2, cone2.radius()) + self.assertEqual(3.0, cone2.length()) + + def test_shape(self): + cone = Cone(); + self.assertEqual(0.5, cone.radius()) + self.assertEqual(1.0, cone.length()) + + cone.shape().set_radius(0.123) + cone.shape().set_length(0.456) + self.assertEqual(0.123, cone.radius()) + self.assertEqual(0.456, cone.length()) + + +if __name__ == '__main__': + unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index c4cd9c511..e76506051 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -13,7 +13,7 @@ # limitations under the License. import copy -from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cylinder, Ellipsoid, +from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid, Mesh, ParserConfig, Plane, Sphere) from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf @@ -33,6 +33,9 @@ def test_default_construction(self): geom.set_type(sdf.GeometryType.CAPSULE) self.assertEqual(sdf.GeometryType.CAPSULE, geom.type()) + geom.set_type(sdf.GeometryType.CONE) + self.assertEqual(sdf.GeometryType.CONE, geom.type()) + geom.set_type(sdf.GeometryType.CYLINDER) self.assertEqual(sdf.GeometryType.CYLINDER, geom.type()) @@ -123,6 +126,21 @@ def test_capsule(self): self.assertEqual(4.56, geom.capsule_shape().length()) + def test_cone(self): + geom = Geometry() + geom.set_type(sdf.GeometryType.CONE) + + coneShape = Cone() + coneShape.set_radius(0.123) + coneShape.set_length(4.56) + geom.set_cone_shape(coneShape) + + self.assertEqual(sdf.GeometryType.CONE, geom.type()) + self.assertNotEqual(None, geom.cone_shape()) + self.assertEqual(0.123, geom.cone_shape().radius()) + self.assertEqual(4.56, geom.cone_shape().length()) + + def test_cylinder(self): geom = Geometry() geom.set_type(sdf.GeometryType.CYLINDER) diff --git a/python/test/pyVisual_TEST.py b/python/test/pyVisual_TEST.py index bedadb0eb..1e07f1412 100644 --- a/python/test/pyVisual_TEST.py +++ b/python/test/pyVisual_TEST.py @@ -65,6 +65,7 @@ def test_default_construction(self): self.assertNotEqual(None, visual.geometry()) self.assertEqual(sdf.GeometryType.EMPTY, visual.geometry().type()) self.assertEqual(None, visual.geometry().box_shape()) + self.assertEqual(None, visual.geometry().cone_shape()) self.assertEqual(None, visual.geometry().cylinder_shape()) self.assertEqual(None, visual.geometry().plane_shape()) self.assertEqual(None, visual.geometry().sphere_shape()) diff --git a/sdf/1.11/CMakeLists.txt b/sdf/1.11/CMakeLists.txt index 2e56bf0a7..fb7f6a38a 100644 --- a/sdf/1.11/CMakeLists.txt +++ b/sdf/1.11/CMakeLists.txt @@ -11,6 +11,7 @@ set (sdfs camera.sdf capsule_shape.sdf collision.sdf + cone_shape.sdf contact.sdf cylinder_shape.sdf ellipsoid_shape.sdf diff --git a/sdf/1.11/cone_shape.sdf b/sdf/1.11/cone_shape.sdf new file mode 100644 index 000000000..5805ba6bb --- /dev/null +++ b/sdf/1.11/cone_shape.sdf @@ -0,0 +1,9 @@ + + Cone shape + + Radius of the cone + + + Length of the cone along the z axis + + diff --git a/sdf/1.11/geometry.sdf b/sdf/1.11/geometry.sdf index 884902afb..447338dbf 100644 --- a/sdf/1.11/geometry.sdf +++ b/sdf/1.11/geometry.sdf @@ -8,6 +8,7 @@ + diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 1ab3d1b18..aaa0cd2dc 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -84,6 +84,7 @@ TEST(DOMcollision, Construction) ASSERT_NE(nullptr, collision.Geom()); EXPECT_EQ(sdf::GeometryType::EMPTY, collision.Geom()->Type()); EXPECT_EQ(nullptr, collision.Geom()->BoxShape()); + EXPECT_EQ(nullptr, collision.Geom()->ConeShape()); EXPECT_EQ(nullptr, collision.Geom()->CylinderShape()); EXPECT_EQ(nullptr, collision.Geom()->PlaneShape()); EXPECT_EQ(nullptr, collision.Geom()->SphereShape()); diff --git a/src/Cone.cc b/src/Cone.cc new file mode 100644 index 000000000..7e094bdd0 --- /dev/null +++ b/src/Cone.cc @@ -0,0 +1,189 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include +#include + +#include +#include +#include "sdf/Cone.hh" +#include "sdf/parser.hh" +#include "Utils.hh" + +using namespace sdf; + +// Private data class +class sdf::Cone::Implementation +{ + // A cone with a length of 1 meter and radius if 0.5 meters. + public: gz::math::Coned cone{1.0, 0.5}; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +///////////////////////////////////////////////// +Cone::Cone() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +///////////////////////////////////////////////// +Errors Cone::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 cone, but the provided SDF " + "element is null."}); + return errors; + } + + // We need a cone child element + if (_sdf->GetName() != "cone") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load a cone geometry, but the provided SDF " + "element is not a ."}); + return errors; + } + + { + std::pair pair = _sdf->Get(errors, "radius", + this->dataPtr->cone.Radius()); + + if (!pair.second) + { + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a radius of " + << this->dataPtr->cone.Radius() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); + } + this->dataPtr->cone.SetRadius(pair.first); + } + + { + std::pair pair = _sdf->Get(errors, "length", + this->dataPtr->cone.Length()); + + if (!pair.second) + { + std::stringstream ss; + ss << "Invalid data for a geometry. " + << "Using a length of " + << this->dataPtr->cone.Length() << "."; + errors.push_back({ErrorCode::ELEMENT_INVALID, ss.str()}); + } + this->dataPtr->cone.SetLength(pair.first); + } + + return errors; +} + +////////////////////////////////////////////////// +double Cone::Radius() const +{ + return this->dataPtr->cone.Radius(); +} + +////////////////////////////////////////////////// +void Cone::SetRadius(double _radius) +{ + this->dataPtr->cone.SetRadius(_radius); +} + +////////////////////////////////////////////////// +double Cone::Length() const +{ + return this->dataPtr->cone.Length(); +} + +////////////////////////////////////////////////// +void Cone::SetLength(double _length) +{ + this->dataPtr->cone.SetLength(_length); +} + +///////////////////////////////////////////////// +sdf::ElementPtr Cone::Element() const +{ + return this->dataPtr->sdf; +} + +///////////////////////////////////////////////// +const gz::math::Coned &Cone::Shape() const +{ + return this->dataPtr->cone; +} + +///////////////////////////////////////////////// +gz::math::Coned &Cone::Shape() +{ + return this->dataPtr->cone; +} + +std::optional Cone::CalculateInertial(double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->cone.SetMat(material); + + auto coneMassMatrix = this->dataPtr->cone.MassMatrix(); + + if (!coneMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald coneInertial; + coneInertial.SetMassMatrix(coneMassMatrix.value()); + coneInertial.SetPose({0, 0, -0.25 * this->dataPtr->cone.Length(), 0, 0, 0}); + return std::make_optional(coneInertial); + } +} + +///////////////////////////////////////////////// +sdf::ElementPtr Cone::ToElement() const +{ + sdf::Errors errors; + auto result = this->ToElement(errors); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +sdf::ElementPtr Cone::ToElement(sdf::Errors &_errors) const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("cone_shape.sdf", elem); + + sdf::ElementPtr radiusElem = elem->GetElement("radius", _errors); + radiusElem->Set(_errors, this->Radius()); + + sdf::ElementPtr lengthElem = elem->GetElement("length", _errors); + lengthElem->Set(_errors, this->Length()); + + return elem; +} diff --git a/src/Cone_TEST.cc b/src/Cone_TEST.cc new file mode 100644 index 000000000..59c5dbf3e --- /dev/null +++ b/src/Cone_TEST.cc @@ -0,0 +1,288 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include "sdf/Cone.hh" +#include "test_utils.hh" +#include +#include +#include +#include + +///////////////////////////////////////////////// +TEST(DOMCone, Construction) +{ + sdf::Cone cone; + EXPECT_EQ(nullptr, cone.Element()); + // A default cone has a length of 1 meter and radius if 0.5 meters. + EXPECT_DOUBLE_EQ(GZ_PI * std::pow(0.5, 2) * 1.0 / 3.0, + cone.Shape().Volume()); + + EXPECT_DOUBLE_EQ(0.5, cone.Radius()); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + + cone.SetRadius(0.5); + cone.SetLength(2.3); + + EXPECT_DOUBLE_EQ(0.5, cone.Radius()); + EXPECT_DOUBLE_EQ(2.3, cone.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, MoveConstructor) +{ + sdf::Cone cone; + cone.SetRadius(0.2); + cone.SetLength(3.0); + + sdf::Cone cone2(std::move(cone)); + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); + + EXPECT_DOUBLE_EQ(GZ_PI * std::pow(0.2, 2) * 3.0 / 3.0, + cone2.Shape().Volume()); + EXPECT_DOUBLE_EQ(0.2, cone2.Shape().Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Shape().Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, CopyConstructor) +{ + sdf::Cone cone; + cone.SetRadius(0.2); + cone.SetLength(3.0); + + sdf::Cone cone2(cone); + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, CopyAssignmentOperator) +{ + sdf::Cone cone; + cone.SetRadius(0.2); + cone.SetLength(3.0); + + sdf::Cone cone2; + cone2 = cone; + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, MoveAssignmentConstructor) +{ + sdf::Cone cone; + cone.SetRadius(0.2); + cone.SetLength(3.0); + + sdf::Cone cone2; + cone2 = std::move(cone); + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, CopyAssignmentAfterMove) +{ + sdf::Cone cone1; + cone1.SetRadius(0.2); + cone1.SetLength(3.0); + + sdf::Cone cone2; + cone2.SetRadius(2); + cone2.SetLength(30.0); + + // This is similar to what std::swap does except it uses std::move for each + // assignment + sdf::Cone tmp = std::move(cone1); + cone1 = cone2; + cone2 = tmp; + + EXPECT_DOUBLE_EQ(2, cone1.Radius()); + EXPECT_DOUBLE_EQ(30, cone1.Length()); + + EXPECT_DOUBLE_EQ(0.2, cone2.Radius()); + EXPECT_DOUBLE_EQ(3.0, cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, Load) +{ + sdf::Cone cone; + sdf::Errors errors; + + // Null element name + errors = cone.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, cone.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = cone.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, cone.Element()); + + // Missing and elements + sdf->SetName("cone"); + errors = cone.Load(sdf); + ASSERT_EQ(2u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[1].Code()); + EXPECT_NE(std::string::npos, errors[1].Message().find("Invalid ")) + << errors[1].Message(); + EXPECT_NE(nullptr, cone.Element()); + + // Add a radius element + sdf::ElementPtr radiusDesc(new sdf::Element()); + radiusDesc->SetName("radius"); + radiusDesc->AddValue("double", "1.0", true, "radius"); + sdf->AddElementDescription(radiusDesc); + sdf::ElementPtr radiusElem = sdf->AddElement("radius"); + radiusElem->Set(2.0); + + // Missing element + sdf->SetName("cone"); + errors = cone.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INVALID, errors[0].Code()); + EXPECT_NE(std::string::npos, errors[0].Message().find("Invalid ")) + << errors[0].Message(); +} + +///////////////////////////////////////////////// +TEST(DOMCone, Shape) +{ + sdf::Cone cone; + EXPECT_DOUBLE_EQ(0.5, cone.Radius()); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + + cone.Shape().SetRadius(0.123); + cone.Shape().SetLength(0.456); + EXPECT_DOUBLE_EQ(0.123, cone.Radius()); + EXPECT_DOUBLE_EQ(0.456, cone.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, CalculateInertial) +{ + sdf::Cone cone; + + // density of aluminium + const double density = 2170; + + // Invalid dimensions leading to std::nullopt return in + // CalculateInertial() + cone.SetLength(-1); + cone.SetRadius(0); + auto invalidConeInertial = cone.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidConeInertial); + + const double l = 2.0; + const double r = 0.1; + + cone.SetLength(l); + cone.SetRadius(r); + + double expectedMass = cone.Shape().Volume() * density; + double ixxIyy = (3 / 80.0) * expectedMass * (4 * r * r + l * l); + double izz = 3.0 * expectedMass * r * r / 10.0; + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixxIyy, ixxIyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose({0, 0, -l / 4.0, 0, 0, 0}); + + auto coneInertial = cone.CalculateInertial(density); + EXPECT_EQ(cone.Shape().Mat().Density(), density); + ASSERT_NE(std::nullopt, coneInertial); + EXPECT_EQ(expectedInertial, *coneInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + coneInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + coneInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), coneInertial->Pose()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, ToElement) +{ + sdf::Cone cone; + + cone.SetRadius(1.2); + cone.SetLength(0.5); + + sdf::ElementPtr elem = cone.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Cone cone2; + cone2.Load(elem); + + EXPECT_DOUBLE_EQ(cone.Radius(), cone2.Radius()); + EXPECT_DOUBLE_EQ(cone.Length(), cone2.Length()); +} + +///////////////////////////////////////////////// +TEST(DOMCone, ToElementErrorOutput) +{ + std::stringstream buffer; + sdf::testing::RedirectConsoleStream redir( + sdf::Console::Instance()->GetMsgStream(), &buffer); + + #ifdef _WIN32 + sdf::Console::Instance()->SetQuiet(false); + sdf::testing::ScopeExit revertSetQuiet( + [] + { + sdf::Console::Instance()->SetQuiet(true); + }); + #endif + + sdf::Cone cone; + sdf::Errors errors; + + cone.SetRadius(1.2); + cone.SetLength(0.5); + + sdf::ElementPtr elem = cone.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Cone cone2; + errors = cone2.Load(elem); + EXPECT_TRUE(errors.empty()); + + EXPECT_DOUBLE_EQ(cone.Radius(), cone2.Radius()); + EXPECT_DOUBLE_EQ(cone.Length(), cone2.Length()); + + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} diff --git a/src/Geometry.cc b/src/Geometry.cc index 9afb20212..456d5e200 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -21,6 +21,7 @@ #include "sdf/Geometry.hh" #include "sdf/Box.hh" #include "sdf/Capsule.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Heightmap.hh" @@ -49,6 +50,9 @@ class sdf::Geometry::Implementation /// \brief Optional capsule. public: std::optional capsule; + /// \brief Optional cone. + public: std::optional cone; + /// \brief Optional cylinder. public: std::optional cylinder; @@ -127,6 +131,14 @@ Errors Geometry::Load(ElementPtr _sdf, const ParserConfig &_config) _sdf->GetElement("capsule", errors)); errors.insert(errors.end(), err.begin(), err.end()); } + else if (_sdf->HasElement("cone")) + { + this->dataPtr->type = GeometryType::CONE; + this->dataPtr->cone.emplace(); + Errors err = this->dataPtr->cone->Load( + _sdf->GetElement("cone", errors)); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (_sdf->HasElement("cylinder")) { this->dataPtr->type = GeometryType::CYLINDER; @@ -240,6 +252,18 @@ void Geometry::SetCapsuleShape(const Capsule &_capsule) this->dataPtr->capsule = _capsule; } +///////////////////////////////////////////////// +const Cone *Geometry::ConeShape() const +{ + return optionalToPointer(this->dataPtr->cone); +} + +///////////////////////////////////////////////// +void Geometry::SetConeShape(const Cone &_cone) +{ + this->dataPtr->cone = _cone; +} + ///////////////////////////////////////////////// const Cylinder *Geometry::CylinderShape() const { @@ -327,6 +351,9 @@ std::optional Geometry::CalculateInertial( case GeometryType::CAPSULE: geomInertial = this->dataPtr->capsule->CalculateInertial(_density); break; + case GeometryType::CONE: + geomInertial = this->dataPtr->cone->CalculateInertial(_density); + break; case GeometryType::CYLINDER: geomInertial = this->dataPtr->cylinder->CalculateInertial(_density); break; @@ -384,6 +411,9 @@ sdf::ElementPtr Geometry::ToElement(sdf::Errors &_errors) const case GeometryType::BOX: elem->InsertElement(this->dataPtr->box->ToElement(_errors), true); break; + case GeometryType::CONE: + elem->InsertElement(this->dataPtr->cone->ToElement(_errors), true); + break; case GeometryType::CYLINDER: elem->InsertElement(this->dataPtr->cylinder->ToElement(_errors), true); break; diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 7b12f878a..05f4d7205 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -18,6 +18,7 @@ #include #include "sdf/Box.hh" #include "sdf/Capsule.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" @@ -49,6 +50,9 @@ TEST(DOMGeometry, Construction) geom.SetType(sdf::GeometryType::CAPSULE); EXPECT_EQ(sdf::GeometryType::CAPSULE, geom.Type()); + geom.SetType(sdf::GeometryType::CONE); + EXPECT_EQ(sdf::GeometryType::CONE, geom.Type()); + geom.SetType(sdf::GeometryType::CYLINDER); EXPECT_EQ(sdf::GeometryType::CYLINDER, geom.Type()); @@ -203,6 +207,23 @@ TEST(DOMGeometry, Capsule) EXPECT_DOUBLE_EQ(4.56, geom.CapsuleShape()->Length()); } +///////////////////////////////////////////////// +TEST(DOMGeometry, Cone) +{ + sdf::Geometry geom; + geom.SetType(sdf::GeometryType::CONE); + + sdf::Cone coneShape; + coneShape.SetRadius(0.123); + coneShape.SetLength(4.56); + geom.SetConeShape(coneShape); + + EXPECT_EQ(sdf::GeometryType::CONE, geom.Type()); + EXPECT_NE(nullptr, geom.ConeShape()); + EXPECT_DOUBLE_EQ(0.123, geom.ConeShape()->Radius()); + EXPECT_DOUBLE_EQ(4.56, geom.ConeShape()->Length()); +} + ///////////////////////////////////////////////// TEST(DOMGeometry, Cylinder) { @@ -395,6 +416,37 @@ TEST(DOMGeometry, CalculateInertial) EXPECT_EQ(expectedInertial.Pose(), capsuleInertial->Pose()); } + // Cone + { + sdf::Cone cone; + const double l = 2.0; + const double r = 0.1; + + cone.SetLength(l); + cone.SetRadius(r); + + expectedMass = cone.Shape().Volume() * density; + double ixxIyy = (3 / 80.0) * expectedMass * (4 * r * r + l * l); + double izz = 3.0 * expectedMass * r * r / 10.0; + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixxIyy, ixxIyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d({0, 0, -l / 4.0, 0, 0, 0})); + + geom.SetType(sdf::GeometryType::CONE); + geom.SetConeShape(cone); + auto coneInertial = geom.CalculateInertial(errors, + sdfParserConfig, density, autoInertiaParams); + + ASSERT_NE(std::nullopt, coneInertial); + EXPECT_EQ(expectedInertial, *coneInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), coneInertial->Pose()); + } + // Cylinder { sdf::Cylinder cylinder; @@ -561,6 +613,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_NE(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -587,6 +640,34 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_NE(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + EXPECT_TRUE(geom2.PolylineShape().empty()); + } + + // Cone + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::CONE); + sdf::Cone cone; + geom.SetConeShape(cone); + + sdf::ElementPtr elem = geom.ToElement(); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + geom2.Load(elem); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_NE(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -613,6 +694,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_NE(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -639,6 +721,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_NE(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -665,6 +748,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_NE(nullptr, geom2.SphereShape()); @@ -691,6 +775,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -717,6 +802,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -743,6 +829,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -769,6 +856,7 @@ TEST(DOMGeometry, ToElement) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -816,6 +904,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_NE(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -844,6 +933,36 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_NE(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); + EXPECT_EQ(nullptr, geom2.CylinderShape()); + EXPECT_EQ(nullptr, geom2.EllipsoidShape()); + EXPECT_EQ(nullptr, geom2.SphereShape()); + EXPECT_EQ(nullptr, geom2.PlaneShape()); + EXPECT_EQ(nullptr, geom2.MeshShape()); + EXPECT_EQ(nullptr, geom2.HeightmapShape()); + EXPECT_TRUE(geom2.PolylineShape().empty()); + } + + // Cone + { + sdf::Geometry geom; + + geom.SetType(sdf::GeometryType::CONE); + sdf::Cone cone; + geom.SetConeShape(cone); + + sdf::ElementPtr elem = geom.ToElement(errors); + EXPECT_TRUE(errors.empty()); + ASSERT_NE(nullptr, elem); + + sdf::Geometry geom2; + errors = geom2.Load(elem); + EXPECT_TRUE(errors.empty()); + + EXPECT_EQ(geom.Type(), geom2.Type()); + EXPECT_EQ(nullptr, geom2.BoxShape()); + EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_NE(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -872,6 +991,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_NE(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -900,6 +1020,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_NE(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -928,6 +1049,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_NE(nullptr, geom2.SphereShape()); @@ -956,6 +1078,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -988,6 +1111,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -1020,6 +1144,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); @@ -1048,6 +1173,7 @@ TEST(DOMGeometry, ToElementErrorOutput) EXPECT_EQ(geom.Type(), geom2.Type()); EXPECT_EQ(nullptr, geom2.BoxShape()); EXPECT_EQ(nullptr, geom2.CapsuleShape()); + EXPECT_EQ(nullptr, geom2.ConeShape()); EXPECT_EQ(nullptr, geom2.CylinderShape()); EXPECT_EQ(nullptr, geom2.EllipsoidShape()); EXPECT_EQ(nullptr, geom2.SphereShape()); diff --git a/src/Visual_TEST.cc b/src/Visual_TEST.cc index 935e99ed6..3e7248445 100644 --- a/src/Visual_TEST.cc +++ b/src/Visual_TEST.cc @@ -68,6 +68,7 @@ TEST(DOMVisual, Construction) ASSERT_NE(nullptr, visual.Geom()); EXPECT_EQ(sdf::GeometryType::EMPTY, visual.Geom()->Type()); EXPECT_EQ(nullptr, visual.Geom()->BoxShape()); + EXPECT_EQ(nullptr, visual.Geom()->ConeShape()); EXPECT_EQ(nullptr, visual.Geom()->CylinderShape()); EXPECT_EQ(nullptr, visual.Geom()->PlaneShape()); EXPECT_EQ(nullptr, visual.Geom()->SphereShape()); diff --git a/test/integration/geometry_dom.cc b/test/integration/geometry_dom.cc index 419a04dc9..927850087 100644 --- a/test/integration/geometry_dom.cc +++ b/test/integration/geometry_dom.cc @@ -21,6 +21,7 @@ #include "sdf/Box.hh" #include "sdf/Capsule.hh" #include "sdf/Collision.hh" +#include "sdf/Cone.hh" #include "sdf/Cylinder.hh" #include "sdf/Element.hh" #include "sdf/Ellipsoid.hh" @@ -94,6 +95,26 @@ TEST(DOMGeometry, Shapes) EXPECT_DOUBLE_EQ(2.1, capsuleVisGeom->Radius()); EXPECT_DOUBLE_EQ(10.2, capsuleVisGeom->Length()); + // Test cone collision + const sdf::Collision *coneCol = link->CollisionByName("cone_col"); + ASSERT_NE(nullptr, coneCol); + ASSERT_NE(nullptr, coneCol->Geom()); + EXPECT_EQ(sdf::GeometryType::CONE, coneCol->Geom()->Type()); + const sdf::Cone *coneColGeom = coneCol->Geom()->ConeShape(); + ASSERT_NE(nullptr, coneColGeom); + EXPECT_DOUBLE_EQ(0.2, coneColGeom->Radius()); + EXPECT_DOUBLE_EQ(0.1, coneColGeom->Length()); + + // Test cone visual + const sdf::Visual *coneVis = link->VisualByName("cone_vis"); + ASSERT_NE(nullptr, coneVis); + ASSERT_NE(nullptr, coneVis->Geom()); + EXPECT_EQ(sdf::GeometryType::CONE, coneVis->Geom()->Type()); + const sdf::Cone *coneVisGeom = coneVis->Geom()->ConeShape(); + ASSERT_NE(nullptr, coneVisGeom); + EXPECT_DOUBLE_EQ(2.1, coneVisGeom->Radius()); + EXPECT_DOUBLE_EQ(10.2, coneVisGeom->Length()); + // Test cylinder collision const sdf::Collision *cylinderCol = link->CollisionByName("cylinder_col"); ASSERT_NE(nullptr, cylinderCol); diff --git a/test/sdf/basic_shapes.sdf b/test/sdf/basic_shapes.sdf index 32e0b416f..93c9fafc1 100644 --- a/test/sdf/basic_shapes.sdf +++ b/test/sdf/basic_shapes.sdf @@ -1,5 +1,5 @@ - + true @@ -155,5 +155,28 @@ + + + 2 0 0 0 0 0 + + + + + 0.2 + 0.1 + + + + + + + + 0.2 + 0.1 + + + + + diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index 37c56277c..5a2e72d4f 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -1,5 +1,5 @@ - + @@ -65,6 +65,24 @@ + + + + 0.2 + 0.1 + + + + + + + + 2.1 + 10.2 + + + + diff --git a/test/sdf/shapes_world.sdf b/test/sdf/shapes_world.sdf index 5eb324490..3a7be8b91 100644 --- a/test/sdf/shapes_world.sdf +++ b/test/sdf/shapes_world.sdf @@ -1,5 +1,5 @@ - + true @@ -147,5 +147,28 @@ + + + 2 0 0 0 0 0 + + + + + 0.2 + 0.1 + + + + + + + + 0.2 + 0.1 + + + + + From f05f4e7ad1a6784f9ff1a6c1b362191677baa70d Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Fri, 21 Jun 2024 02:22:36 +0200 Subject: [PATCH 25/25] Prepare release 14.4.0 (#1441) Signed-off-by: Jose Luis Rivero Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 9 +++++++++ package.xml | 2 +- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 877198921..0eea9fb7b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ if(COMMAND CMAKE_POLICY) CMAKE_POLICY(SET CMP0004 NEW) endif(COMMAND CMAKE_POLICY) -project (sdformat14 VERSION 14.3.0) +project (sdformat14 VERSION 14.4.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software diff --git a/Changelog.md b/Changelog.md index e094e604b..96c4e3e18 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,14 @@ ## libsdformat 14.X +### libsdformat 14.4.0 (2024-06-20) + +1. Add Cone as a primitive parametric shape. + * [Pull request #1415](https://github.com/gazebosim/sdformat/pull/1415) + * Thanks to Benjamin Perseghetti + +1. Add custom attribute to custom element in test + * [Pull request #1406](https://github.com/gazebosim/sdformat/pull/1406) + ### libsdformat 14.3.0 (2024-06-14) 1. Backport voxel_resolution sdf element diff --git a/package.xml b/package.xml index b065874cd..ed841192a 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ sdformat14 - 14.3.0 + 14.4.0 SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications