From 9101e86d3f03cdd32d3e73b3a9a7a1cb96696f59 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 20 Mar 2024 01:58:37 -0700 Subject: [PATCH 01/47] 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 2799934dc3429b2ce9234043756756ee52847c3c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 28 Mar 2024 17:20:15 -0700 Subject: [PATCH 02/47] 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 e9a2c01d7fef44d32b26e3d2c87bfd1a7bbf745c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 10 May 2024 03:03:24 -0500 Subject: [PATCH 03/47] 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 c9779cb7f19bac92b512e213b7fd50dbcf2c8c8d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 May 2024 23:00:20 +0200 Subject: [PATCH 04/47] 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 05/47] 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 06/47] 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 07/47] 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 08/47] =?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 09/47] 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 10/47] 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 11/47] 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 12/47] 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 13/47] 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 783106382967bfff979e42e289da13c20307f32a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 14 Jun 2024 09:41:11 -0700 Subject: [PATCH 14/47] Added World::ActorByName: (#1436) Signed-off-by: Nate Koenig --- include/sdf/World.hh | 14 ++++++++++++++ python/src/sdf/pyWorld.cc | 16 +++++++++++++--- src/World.cc | 19 ++++++++++++++++--- src/World_TEST.cc | 15 +++++++++++++++ 4 files changed, 58 insertions(+), 6 deletions(-) diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 0afc85472..22a61862d 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -278,6 +278,20 @@ namespace sdf /// \return True if there exists an actor with the given name. public: bool ActorNameExists(const std::string &_name) const; + /// \brief Get an actor based on a name. + /// \param[in] _name Name of the actor. + /// \return Pointer to the actor. Nullptr if an actor with the given name + /// does not exist. + /// \sa bool ActorNameExists(const std::string &_name) const + public: const Actor *ActorByName(const std::string &_name) const; + + /// \brief Get a mutable actor based on a name. + /// \param[in] _name Name of the actor. + /// \return Pointer to the actor. Nullptr if an actor with the given name + /// does not exist. + /// \sa bool ActorNameExists(const std::string &_name) const + public: Actor *ActorByName(const std::string &_name); + /// \brief Get the number of explicit frames that are immediate (not nested) /// children of this World object. /// \remark FrameByName() can find explicit frames that are not immediate diff --git a/python/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index 41b930fc4..c215033ba 100644 --- a/python/src/sdf/pyWorld.cc +++ b/python/src/sdf/pyWorld.cc @@ -98,13 +98,13 @@ void defineWorld(pybind11::object module) pybind11::overload_cast( &sdf::World::ModelByIndex), pybind11::return_value_policy::reference_internal, - "Get a mutable immediate (not nested) child joint model on an index.") + "Get a mutable immediate (not nested) model based on an index.") .def("model_by_name", pybind11::overload_cast( &sdf::World::ModelByName), pybind11::return_value_policy::reference_internal, - "Get a mutable immediate (not nested) mutable child model based on an " - "index.") + "Get a mutable immediate (not nested) mutable child model based on a " + "name.") .def("model_name_exists", &sdf::World::ModelNameExists, "Get whether a model name exists.") .def("name_exists_in_frame_attached_to_graph", @@ -137,6 +137,16 @@ void defineWorld(pybind11::object module) "Remove all joints.") .def("actor_count", &sdf::World::ActorCount, "Get the number of actors.") + .def("actor_by_index", + pybind11::overload_cast( + &sdf::World::ActorByIndex), + pybind11::return_value_policy::reference_internal, + "Get a mutable actor based on an index.") + .def("actor_by_name", + pybind11::overload_cast( + &sdf::World::ActorByName), + pybind11::return_value_policy::reference_internal, + "Get a mutable actor based on a name.") .def("frame_count", &sdf::World::FrameCount, "Get the number of explicit frames that are immediate (not nested) " "children of this World object.") diff --git a/src/World.cc b/src/World.cc index 09bb82a76..47706a03e 100644 --- a/src/World.cc +++ b/src/World.cc @@ -757,14 +757,27 @@ Actor *World::ActorByIndex(uint64_t _index) ///////////////////////////////////////////////// bool World::ActorNameExists(const std::string &_name) const { - for (auto const &a : this->dataPtr->actors) + return nullptr != this->ActorByName(_name); +} + +///////////////////////////////////////////////// +const Actor *World::ActorByName(const std::string &_name) const +{ + for (const Actor &a : this->dataPtr->actors) { if (a.Name() == _name) { - return true; + return &a; } } - return false; + return nullptr; +} + +///////////////////////////////////////////////// +Actor *World::ActorByName(const std::string &_name) +{ + return const_cast( + static_cast(this)->ActorByName(_name)); } ////////////////////////////////////////////////// diff --git a/src/World_TEST.cc b/src/World_TEST.cc index b1f93b92e..3083aca3c 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -58,6 +58,9 @@ TEST(DOMWorld, Construction) EXPECT_EQ(nullptr, world.ModelByName("a::b::c")); EXPECT_EQ(nullptr, world.ModelByName("::::")); + EXPECT_EQ(nullptr, world.ActorByName("")); + EXPECT_EQ(nullptr, world.ActorByName("default")); + EXPECT_EQ(0u, world.FrameCount()); EXPECT_EQ(nullptr, world.FrameByIndex(0)); EXPECT_EQ(nullptr, world.FrameByIndex(1)); @@ -452,15 +455,27 @@ TEST(DOMWorld, AddActor) EXPECT_EQ(1u, world.ActorCount()); EXPECT_FALSE(world.AddActor(actor)); EXPECT_EQ(1u, world.ActorCount()); + EXPECT_NE(nullptr, world.ActorByName("actor1")); world.ClearActors(); EXPECT_EQ(0u, world.ActorCount()); + EXPECT_EQ(nullptr, world.ActorByName("actor1")); EXPECT_TRUE(world.AddActor(actor)); EXPECT_EQ(1u, world.ActorCount()); const sdf::Actor *actorFromWorld = world.ActorByIndex(0); ASSERT_NE(nullptr, actorFromWorld); EXPECT_EQ(actorFromWorld->Name(), actor.Name()); + + const sdf::Actor *actorFromWorldByName = world.ActorByName("actor1"); + ASSERT_NE(nullptr, actorFromWorldByName); + EXPECT_EQ(actorFromWorldByName->Name(), actor.Name()); + + sdf::Actor *mutableActorFromWorldByName = world.ActorByName("actor1"); + ASSERT_NE(nullptr, mutableActorFromWorldByName); + EXPECT_EQ(mutableActorFromWorldByName->Name(), actor.Name()); + mutableActorFromWorldByName->SetName("new_name"); + EXPECT_NE(mutableActorFromWorldByName->Name(), actor.Name()); } ///////////////////////////////////////////////// From 37ca7fcaf96e93c2cf26bd6bf4fdc62e51aa08cc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 14 Jun 2024 12:55:53 -0700 Subject: [PATCH 15/47] 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 e2c81f2d7358c308617d8342ad3801a997ed2ea2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 18 Jun 2024 12:27:47 -0700 Subject: [PATCH 16/47] Remove Cone shape from particle emitters (#1434) Signed-off-by: Ian Chen --- include/sdf/ParticleEmitter.hh | 5 +---- python/src/sdf/pyParticleEmitter.cc | 1 - python/test/pyParticleEmitter_TEST.py | 2 -- sdf/1.11/particle_emitter.sdf | 4 +--- sdf/1.12/particle_emitter.sdf | 4 +--- src/ParticleEmitter.cc | 5 ++--- src/ParticleEmitter_TEST.cc | 2 -- 7 files changed, 5 insertions(+), 18 deletions(-) diff --git a/include/sdf/ParticleEmitter.hh b/include/sdf/ParticleEmitter.hh index c2e09dec5..0326f34c2 100644 --- a/include/sdf/ParticleEmitter.hh +++ b/include/sdf/ParticleEmitter.hh @@ -51,10 +51,7 @@ namespace sdf CYLINDER = 2, /// \brief An ellipsoid emitter. - ELLIPSOID = 3, - - /// \brief An cone emitter. - CONE = 4, + ELLIPSOID = 3 }; /// \brief A description of a particle emitter, which can be attached diff --git a/python/src/sdf/pyParticleEmitter.cc b/python/src/sdf/pyParticleEmitter.cc index 42d70cd43..0e9873c57 100644 --- a/python/src/sdf/pyParticleEmitter.cc +++ b/python/src/sdf/pyParticleEmitter.cc @@ -154,7 +154,6 @@ void defineParticleEmitter(pybind11::object module) pybind11::enum_(particleEmitterModule, "ParticleEmitterType") .value("POINT", sdf::ParticleEmitterType::POINT) .value("BOX", sdf::ParticleEmitterType::BOX) - .value("CONE", sdf::ParticleEmitterType::CONE) .value("CYLINDER", sdf::ParticleEmitterType::CYLINDER) .value("ELLIPSOID", sdf::ParticleEmitterType::ELLIPSOID); } diff --git a/python/test/pyParticleEmitter_TEST.py b/python/test/pyParticleEmitter_TEST.py index dc44557bf..9995eb66d 100644 --- a/python/test/pyParticleEmitter_TEST.py +++ b/python/test/pyParticleEmitter_TEST.py @@ -34,8 +34,6 @@ def test_default_construction(self): self.assertTrue(emitter.set_type("box")) self.assertEqual("box", emitter.type_str()) self.assertEqual(ParticleEmitter.ParticleEmitterType.BOX, emitter.type()) - emitter.set_type(ParticleEmitter.ParticleEmitterType.CONE) - self.assertEqual("cone", emitter.type_str()) emitter.set_type(ParticleEmitter.ParticleEmitterType.CYLINDER) self.assertEqual("cylinder", emitter.type_str()) diff --git a/sdf/1.11/particle_emitter.sdf b/sdf/1.11/particle_emitter.sdf index 1be49ccc8..d6a2fdf7e 100755 --- a/sdf/1.11/particle_emitter.sdf +++ b/sdf/1.11/particle_emitter.sdf @@ -7,7 +7,7 @@ - The type of a particle emitter. One of "box", "cylinder", "cone", "ellipsoid", or "point". + The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". @@ -26,8 +26,6 @@ depending on the emmiter type: - point: The area is ignored. - box: The area is interpreted as width X height X depth. - - cone: The area is interpreted as the bounding box of the - cone. The cone is oriented along the Z-axis. - cylinder: The area is interpreted as the bounding box of the cylinder. The cylinder is oriented along the Z-axis. - ellipsoid: The area is interpreted as the bounding box of an diff --git a/sdf/1.12/particle_emitter.sdf b/sdf/1.12/particle_emitter.sdf index aec7b90cc..d6a2fdf7e 100755 --- a/sdf/1.12/particle_emitter.sdf +++ b/sdf/1.12/particle_emitter.sdf @@ -7,7 +7,7 @@ - The type of a particle emitter. One of "box", "cone", "cylinder", "ellipsoid", or "point". + The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". @@ -26,8 +26,6 @@ depending on the emmiter type: - point: The area is ignored. - box: The area is interpreted as width X height X depth. - - cone: The area is interpreted as the bounding box of the - cone. The cone is oriented along the Z-axis. - cylinder: The area is interpreted as the bounding box of the cylinder. The cylinder is oriented along the Z-axis. - ellipsoid: The area is interpreted as the bounding box of an diff --git a/src/ParticleEmitter.cc b/src/ParticleEmitter.cc index 75c7ac438..b0d07a205 100644 --- a/src/ParticleEmitter.cc +++ b/src/ParticleEmitter.cc @@ -34,13 +34,12 @@ using namespace sdf; /// Particle emitter type strings. These should match the data in /// `enum class ParticleEmitterType` located in ParticleEmitter.hh, and the size /// template parameter should match the number of elements as well. -constexpr std::array kEmitterTypeStrs = +constexpr std::array kEmitterTypeStrs = { "point", "box", "cylinder", - "ellipsoid", - "cone", + "ellipsoid" }; class sdf::ParticleEmitter::Implementation diff --git a/src/ParticleEmitter_TEST.cc b/src/ParticleEmitter_TEST.cc index a8970b9be..121cf2808 100644 --- a/src/ParticleEmitter_TEST.cc +++ b/src/ParticleEmitter_TEST.cc @@ -35,8 +35,6 @@ TEST(DOMParticleEmitter, Construction) EXPECT_TRUE(emitter.SetType("box")); EXPECT_EQ("box", emitter.TypeStr()); EXPECT_EQ(sdf::ParticleEmitterType::BOX, emitter.Type()); - emitter.SetType(sdf::ParticleEmitterType::CONE); - EXPECT_EQ("cone", emitter.TypeStr()); emitter.SetType(sdf::ParticleEmitterType::CYLINDER); EXPECT_EQ("cylinder", emitter.TypeStr()); From 44cc342d7eea1be5747128cf0be5e8cc05e5a189 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 18 Jun 2024 21:32:59 -0700 Subject: [PATCH 17/47] 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 18/47] 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 19/47] 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 From b43b83f27d9f0174be67584ec6a48c4c3424838f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 24 Jun 2024 12:35:45 -0700 Subject: [PATCH 20/47] Backport #1367 to Garden: Fix find Python3 logic (#1370) * Find Python3 with find_package instead of GzPython, adapting the approach from gz-sim. Signed-off-by: Steve Peters --- CMakeLists.txt | 49 +++++++++++++++++++++++-------------------------- 1 file changed, 23 insertions(+), 26 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 308478f8a..b1e078444 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,9 +78,30 @@ if (BUILD_SDF) # available during build time set(GZ_TOOLS_VER 2) + ################################################# + # Find python + if (SKIP_PYBIND11) + message(STATUS "SKIP_PYBIND11 set - disabling python bindings") + find_package(Python3 COMPONENTS Interpreter) + else() + find_package(Python3 COMPONENTS Interpreter Development) + if (NOT Python3_Development_FOUND) + GZ_BUILD_WARNING("Python development libraries are missing: Python interfaces are disabled.") + else() + set(PYBIND11_PYTHON_VERSION 3) + find_package(pybind11 2.4 CONFIG QUIET) + + if (pybind11_FOUND) + message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + else() + GZ_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") + message (STATUS "Searching for pybind11 - not found.") + endif() + endif() + endif() + ################################################# # Copied from catkin/cmake/empy.cmake - include(GzPython) function(find_python_module module) # cribbed from http://www.cmake.org/pipermail/cmake/2011-January/041666.html string(TOUPPER ${module} module_upper) @@ -130,30 +151,6 @@ if (BUILD_SDF) gz_find_package(gz-utils2 REQUIRED COMPONENTS cli) set(GZ_UTILS_VER ${gz-utils2_VERSION_MAJOR}) - ######################################## - # Python interfaces - if (NOT PYTHON3_FOUND) - GZ_BUILD_WARNING("Python is missing: Python interfaces are disabled.") - message (STATUS "Searching for Python - not found.") - else() - message (STATUS "Searching for Python - found version ${Python3_VERSION}.") - - if (SKIP_PYBIND11) - message(STATUS "SKIP_PYBIND11 set - disabling python bindings") - else() - set(PYBIND11_PYTHON_VERSION 3) - find_package(pybind11 2.4 QUIET) - - if (${pybind11_FOUND}) - find_package(Python3 ${GZ_PYTHON_VERSION} REQUIRED COMPONENTS Development) - message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") - else() - GZ_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") - message (STATUS "Searching for pybind11 - not found.") - endif() - endif() - endif() - gz_configure_build(HIDE_SYMBOLS_BY_DEFAULT QUIT_IF_BUILD_ERRORS) gz_create_packages() @@ -162,7 +159,7 @@ if (BUILD_SDF) add_subdirectory(conf) add_subdirectory(doc) if (pybind11_FOUND AND NOT SKIP_PYBIND11) - add_subdirectory(python) + add_subdirectory(python) endif() endif(BUILD_SDF) From 023cf4ec26b3234c7b8618ed90f7a53d77036416 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 25 Jun 2024 10:04:49 -0700 Subject: [PATCH 21/47] Prepare for 13.8.0 release (#1443) * Prepare for 13.8.0 release Signed-off-by: Nate Koenig --------- Signed-off-by: Nate Koenig --- CMakeLists.txt | 2 +- Changelog.md | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b1e078444..4c85726fb 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.7.0) +project (sdformat13 VERSION 13.8.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 605aefd81..668d228e4 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,13 @@ ## libsdformat 13.X +### libsdformat 13.8.0 (2024-06-25) + +1. Added `World::ActorByName` + * [Pull request #1436](https://github.com/gazebosim/sdformat/pull/1436) + +1. Backport #1367 to Garden: Fix find Python3 logic. + * [Pull request #1370](https://github.com/gazebosim/sdformat/pull/1370) + ### libsdformat 13.7.0 (2024-06-13) 1. Add support for no gravity link From 405369597bd11f67a8d44e05c8f2058fcf7e8cc3 Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Thu, 27 Jun 2024 04:42:38 +0800 Subject: [PATCH 22/47] SDF.cc update calls to use sdf::Errors output (#1295) Adds missing sdf::Errors structure as an option to report errors on SDF. Makes sure that no errors are printed and all are reported through the structure when using the functions that include it as parameter. --------- Signed-off-by: Marco A. Gutierrez Signed-off-by: Steve Peters Co-authored-by: Marco A. Gutierrez Co-authored-by: Steve Peters Co-authored-by: Addisu Z. Taddese --- include/sdf/SDFImpl.hh | 94 +++++++++++++++++++++++++++++++ src/SDF.cc | 124 ++++++++++++++++++++++++++++++++++++----- src/SDF_TEST.cc | 40 +++++++++++++ 3 files changed, 244 insertions(+), 14 deletions(-) diff --git a/include/sdf/SDFImpl.hh b/include/sdf/SDFImpl.hh index 20e1ec19f..277d45188 100644 --- a/include/sdf/SDFImpl.hh +++ b/include/sdf/SDFImpl.hh @@ -80,6 +80,34 @@ namespace sdf bool _searchLocalPath = true, bool _useCallback = false); + /// \brief Find the absolute path of a file. + /// + /// The search order in the function is as follows: + /// 1. Using the global URI path map, search in paths associated with the URI + /// scheme of the input. + /// 2. Seach in the path defined by the macro `SDF_SHARE_PATH`. + /// 3. Search in the the libsdformat install path. The path is formed by + /// has the pattern `SDF_SHARE_PATH/sdformat//` + /// 4. Directly check if the input path exists in the filesystem. + /// 5. Seach in the path defined by the environment variable `SDF_PATH`. + /// 6. If enabled via _searchLocalPath, prepend the input with the current + /// working directory and check if the result path exists. + /// 7. If enabled via _useCallback and the global callback function is set, + /// invoke the function and return its result. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Name of the file to find. + /// \param[in] _searchLocalPath True to search for the file in the current + /// working directory. + /// \param[in] _useCallback True to find a file based on a registered + /// callback if the file is not found via the normal mechanism. + /// \return File's full path. + SDFORMAT_VISIBLE + std::string findFile(sdf::Errors &_errors, + const std::string &_filename, + bool _searchLocalPath = true, + bool _useCallback = false); + /// \brief Find the absolute path of a file. /// /// This overload uses the URI path map and and the callback function @@ -99,6 +127,26 @@ namespace sdf bool _useCallback, const ParserConfig &_config); + /// \brief Find the absolute path of a file. + /// + /// This overload uses the URI path map and and the callback function + /// configured in the input ParserConfig object instead of their global + /// counterparts. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Name of the file to find. + /// \param[in] _searchLocalPath True to search for the file in the current + /// working directory. + /// \param[in] _useCallback True to find a file based on a registered + /// callback if the file is not found via the normal mechanism. + /// \param[in] _config Custom parser configuration. + /// \return File's full path. + SDFORMAT_VISIBLE + std::string findFile(sdf::Errors &_errors, + const std::string &_filename, + bool _searchLocalPath, + bool _useCallback, + const ParserConfig &_config); /// \brief Associate paths to a URI. /// Example paramters: "model://", "/usr/share/models:~/.gazebo/models" @@ -121,22 +169,45 @@ namespace sdf /// \brief Destructor public: ~SDF(); public: void PrintDescription(); + public: void PrintDescription(sdf::Errors &_errors); public: void PrintDoc(); public: void Write(const std::string &_filename); + public: void Write(sdf::Errors &_errors, const std::string &_filename); /// \brief Output SDF's values to stdout. /// \param[in] _config Configuration for printing the values. public: void PrintValues(const PrintConfig &_config = PrintConfig()); + /// \brief Output SDF's values to stdout. + /// \param[out] _errors Vector of errrors. + /// \param[in] _config Configuration for printing the values. + public: void PrintValues(sdf::Errors &_errors, + const PrintConfig &_config = PrintConfig()); + + /// \brief Convert the SDF values to a string representation. + /// \param[in] _config Configuration for printing the values. + /// \return The string representation. + public: std::string ToString( + const PrintConfig &_config = PrintConfig()) const; + /// \brief Convert the SDF values to a string representation. + /// \param[out] _errors Vector of errors. /// \param[in] _config Configuration for printing the values. /// \return The string representation. public: std::string ToString( + sdf::Errors &_errors, const PrintConfig &_config = PrintConfig()) const; /// \brief Set SDF values from a string + /// \param[in] sdfData String with the values to load. public: void SetFromString(const std::string &_sdfData); + /// \brief Set SDF values from a string + /// \param[out] _errors Vector of errors. + /// \param[in] sdfData String with the values to load. + public: void SetFromString(sdf::Errors &_Errors, + const std::string &_sdfData); + /// \brief Clear the data in this object. public: void Clear(); @@ -182,6 +253,13 @@ namespace sdf /// \return a wrapped clone of the SDF element public: static ElementPtr WrapInRoot(const ElementPtr &_sdf); + /// \brief wraps the SDF element into a root element with the version info. + /// \param[out] _errors Vector of errors. + /// \param[in] _sdf the sdf element. Will be cloned by this function. + /// \return a wrapped clone of the SDF element + public: static ElementPtr WrapInRoot(sdf::Errors &_errors, + const ElementPtr &_sdf); + /// \brief Get a string representation of an SDF specification file. /// This function uses a built-in version of a .sdf file located in /// the sdf directory. The parser.cc code uses this function, which avoids @@ -198,6 +276,22 @@ namespace sdf public: static const std::string &EmbeddedSpec( const std::string &_filename, const bool _quiet); + /// \brief Get a string representation of an SDF specification file. + /// This function uses a built-in version of a .sdf file located in + /// the sdf directory. The parser.cc code uses this function, which avoids + /// touching the filesystem. + /// + /// Most people should not use this function. + /// + /// \param[out] _errors Vector of errors. + /// \param[in] _filename Base name of the SDF specification file to + /// load. For example "root.sdf" or "world.sdf". + /// \return A string that contains the contents of the specified + /// _filename. An empty string is returned if the _filename could not be + /// found. + public: static const std::string &EmbeddedSpec( + sdf::Errors &_errors, const std::string &_filename); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/SDF.cc b/src/SDF.cc index 7a43d0f9a..e033aac2b 100644 --- a/src/SDF.cc +++ b/src/SDF.cc @@ -27,11 +27,13 @@ #include "sdf/parser.hh" #include "sdf/Assert.hh" #include "sdf/Console.hh" +#include "sdf/Error.hh" #include "sdf/Filesystem.hh" #include "sdf/SDFImpl.hh" #include "SDFImplPrivate.hh" #include "sdf/sdf_config.h" #include "EmbeddedSdf.hh" +#include "Utils.hh" #include @@ -61,14 +63,40 @@ void setFindCallback(std::function _cb) ///////////////////////////////////////////////// std::string findFile( const std::string &_filename, bool _searchLocalPath, bool _useCallback) +{ + sdf::Errors errors; + std::string result = findFile(errors, _filename, _searchLocalPath, + _useCallback, ParserConfig::GlobalConfig()); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +std::string findFile( + sdf::Errors &_errors, const std::string &_filename, bool _searchLocalPath, + bool _useCallback) { return findFile( - _filename, _searchLocalPath, _useCallback, ParserConfig::GlobalConfig()); + _errors, _filename, _searchLocalPath, + _useCallback, ParserConfig::GlobalConfig()); } ///////////////////////////////////////////////// std::string findFile(const std::string &_filename, bool _searchLocalPath, bool _useCallback, const ParserConfig &_config) +{ + sdf::Errors errors; + std::string result = findFile(errors, _filename, _searchLocalPath, + _useCallback, _config); + sdf::throwOrPrintErrors(errors); + return result; +} + + +///////////////////////////////////////////////// +std::string findFile(sdf::Errors &_errors, const std::string &_filename, + bool _searchLocalPath, bool _useCallback, + const ParserConfig &_config) { // Check to see if _filename is URI. If so, resolve the URI path. for (const auto &[uriScheme, paths] : _config.URIPathMap()) @@ -162,8 +190,9 @@ std::string findFile(const std::string &_filename, bool _searchLocalPath, { if (!_config.FindFileCallback()) { - sdferr << "Tried to use callback in sdf::findFile(), but the callback " - "is empty. Did you call sdf::setFindCallback()?\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Tried to use callback in sdf::findFile(), but the callback " + "is empty. Did you call sdf::setFindCallback()?"}); return std::string(); } else @@ -195,13 +224,29 @@ SDF::~SDF() ///////////////////////////////////////////////// void SDF::PrintDescription() { - this->Root()->PrintDescription(""); + sdf::Errors errors; + this->PrintDescription(errors); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::PrintDescription(sdf::Errors &_errors) +{ + this->Root()->PrintDescription(_errors, ""); } ///////////////////////////////////////////////// void SDF::PrintValues(const PrintConfig &_config) { - this->Root()->PrintValues("", _config); + sdf::Errors errors; + this->PrintValues(errors, _config); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::PrintValues(sdf::Errors &_errors, const PrintConfig &_config) +{ + this->Root()->PrintValues(_errors, "", _config); } ///////////////////////////////////////////////// @@ -319,13 +364,22 @@ void SDF::PrintDoc() ///////////////////////////////////////////////// void SDF::Write(const std::string &_filename) { - std::string string = this->Root()->ToString(""); + sdf::Errors errors; + this->Write(errors, _filename); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::Write(sdf::Errors &_errors, const std::string &_filename) +{ + std::string string = this->Root()->ToString(_errors, ""); std::ofstream out(_filename.c_str(), std::ios::out); if (!out) { - sdferr << "Unable to open file[" << _filename << "] for writing\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Unable to open file[" + _filename + "] for writing."}); return; } out << string; @@ -334,6 +388,16 @@ void SDF::Write(const std::string &_filename) ///////////////////////////////////////////////// std::string SDF::ToString(const PrintConfig &_config) const +{ + sdf::Errors errors; + std::string result = this->ToString(errors, _config); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +std::string SDF::ToString(sdf::Errors &_errors, + const PrintConfig &_config) const { std::ostringstream stream; @@ -343,7 +407,7 @@ std::string SDF::ToString(const PrintConfig &_config) const stream << "\n"; } - stream << this->Root()->ToString("", _config); + stream << this->Root()->ToString(_errors, "", _config); if (this->Root()->GetName() != "sdf") { @@ -355,11 +419,21 @@ std::string SDF::ToString(const PrintConfig &_config) const ///////////////////////////////////////////////// void SDF::SetFromString(const std::string &_sdfData) +{ + sdf::Errors errors; + this->SetFromString(errors, _sdfData); + sdf::throwOrPrintErrors(errors); +} + +///////////////////////////////////////////////// +void SDF::SetFromString(sdf::Errors &_errors, + const std::string &_sdfData) { sdf::initFile("root.sdf", this->Root()); - if (!sdf::readString(_sdfData, this->Root())) + if (!sdf::readString(_sdfData, this->Root(), _errors)) { - sdferr << "Unable to parse sdf string[" << _sdfData << "]\n"; + _errors.push_back({sdf::ErrorCode::PARSING_ERROR, + "Unable to parse sdf string[" + _sdfData + "]"}); } } @@ -429,12 +503,21 @@ void SDF::Version(const std::string &_version) ///////////////////////////////////////////////// ElementPtr SDF::WrapInRoot(const ElementPtr &_sdf) +{ + sdf::Errors errors; + ElementPtr result = SDF::WrapInRoot(errors, _sdf); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +ElementPtr SDF::WrapInRoot(sdf::Errors &_errors, const ElementPtr &_sdf) { ElementPtr root(new Element); root->SetName("sdf"); std::stringstream v; v << Version(); - root->AddAttribute("version", "string", v.str(), true, "version"); + root->AddAttribute("version", "string", v.str(), true, _errors, "version"); root->InsertElement(_sdf->Clone()); return root; } @@ -442,6 +525,19 @@ ElementPtr SDF::WrapInRoot(const ElementPtr &_sdf) ///////////////////////////////////////////////// const std::string &SDF::EmbeddedSpec( const std::string &_filename, const bool _quiet) +{ + sdf::Errors errors; + const std::string &result = EmbeddedSpec(errors, _filename); + if (!_quiet) + { + sdf::throwOrPrintErrors(errors); + } + return result; +} + +///////////////////////////////////////////////// +const std::string &SDF::EmbeddedSpec( + sdf::Errors &_errors, const std::string &_filename) { try { @@ -450,9 +546,9 @@ const std::string &SDF::EmbeddedSpec( } catch(const std::out_of_range &) { - if (!_quiet) - sdferr << "Unable to find SDF filename[" << _filename << "] with " - << "version " << SDF::Version() << "\n"; + _errors.push_back({sdf::ErrorCode::FILE_READ, + "Unable to find SDF filename[" + _filename + "] with " + + "version " + SDF::Version()}); } // An empty SDF string is returned if a query into the embeddedSdf map fails. diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc index 44bb9f284..8c8289a9d 100644 --- a/src/SDF_TEST.cc +++ b/src/SDF_TEST.cc @@ -742,6 +742,46 @@ bool create_new_temp_dir(std::string &_new_temp_path) return true; } +///////////////////////////////////////////////// +TEST(SDF, ErrorOutput) +{ + 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::Errors errors; + + // Test findFile + EXPECT_EQ(sdf::findFile(errors, "adfjialkas31", false, true), ""); + EXPECT_EQ(errors.size(), 1) << errors; + EXPECT_NE(std::string::npos, + errors[0].Message().find("Tried to use callback in sdf::findFile(), " + "but the callback is empty. Did you call " + "sdf::setFindCallback()?")) + << errors[0].Message(); + errors.clear(); + + sdf::SDF sdf; + sdf.SetFromString(errors, "banana"); + EXPECT_EQ(errors.size(), 2) << errors; + EXPECT_NE(std::string::npos, + errors[0].Message().find("Error parsing XML from string")) + << errors[0].Message(); + EXPECT_NE(std::string::npos, + errors[1].Message().find("Unable to parse sdf string[banana]")) + << errors[1].Message(); + // Check nothing has been printed + EXPECT_TRUE(buffer.str().empty()) << buffer.str(); +} + ///////////////////////////////////////////////// bool g_findFileCbCalled = false; std::string findFileCb(const std::string &) From d428889265360893fff5ec27e37e74f4729777af Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 27 Jun 2024 15:09:18 -0700 Subject: [PATCH 23/47] workflows/ci.yml fix push branch regex (#1445) The current pattern matches the following: "sdf", "sdf0", "sdf1", ..., "sdf8", "sdf9" of which "sdf" is incorrect, and only "sdf9" is currently supported. This updates the pattern to match single digits "sdf1" - "sdf9" and double digits "sdf10" to "sdf99". Signed-off-by: Steve Peters --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8a0a9947a..aa170f7fe 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -4,7 +4,7 @@ on: pull_request: push: branches: - - 'sdf[0-9]?' + - 'sdf[1-9]?[0-9]' - 'main' jobs: From 0ecfcc3eb2f0acec9fc281697ff2d3a27315cd0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 28 Jun 2024 20:58:39 +0200 Subject: [PATCH 24/47] Added SetHeightMap and Heighmap to Geometry Python binding (#1440) Signed-off-by: Alejandro Hernandez Cordero --- python/src/sdf/pyGeometry.cc | 6 ++++++ python/test/pyGeometry_TEST.py | 19 ++++++++++++++++++- 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index a30d3106f..8d7873bc3 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -27,6 +27,7 @@ #include "sdf/Cylinder.hh" #include "sdf/Ellipsoid.hh" #include "sdf/Geometry.hh" +#include "sdf/Heightmap.hh" #include "sdf/Mesh.hh" #include "sdf/Plane.hh" #include "sdf/Sphere.hh" @@ -101,6 +102,11 @@ void defineGeometry(pybind11::object module) "geometry is not a mesh.") .def("set_mesh_shape", &sdf::Geometry::SetMeshShape, "Set the mesh shape.") + .def("set_heightmap_shape", &sdf::Geometry::SetHeightmapShape, + "Set the heightmap shape.") + .def("heightmap_shape", &sdf::Geometry::HeightmapShape, + pybind11::return_value_policy::reference, + "Get the heightmap geometry.") .def("__copy__", [](const sdf::Geometry &self) { return sdf::Geometry(self); }) diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index e76506051..c124f2f71 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid, - Mesh, ParserConfig, Plane, Sphere) + Heightmap, Mesh, ParserConfig, Plane, Sphere) from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf import math @@ -188,6 +188,23 @@ def test_mesh(self): self.assertEqual("orange", geom.mesh_shape().submesh()) self.assertTrue(geom.mesh_shape().center_submesh()) + def test_heighmap(self): + geom = Geometry() + + geom.set_type(sdf.GeometryType.HEIGHTMAP) + heightmap = Heightmap() + geom.set_heightmap_shape(heightmap) + + self.assertEqual(sdf.GeometryType.HEIGHTMAP, geom.type()) + self.assertEqual(None, geom.box_shape()) + self.assertEqual(None, geom.capsule_shape()) + self.assertEqual(None, geom.cylinder_shape()) + self.assertEqual(None, geom.ellipsoid_shape()) + self.assertEqual(None, geom.sphere_shape()) + self.assertEqual(None, geom.plane_shape()) + self.assertEqual(None, geom.mesh_shape()) + self.assertNotEqual(None, geom.heightmap_shape()) + def test_plane(self): geom = Geometry() From 8e7b267b4b52ec892e53cd9d1a1032c3eee5f82a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 28 Jun 2024 16:02:45 -0500 Subject: [PATCH 25/47] Disable latex and class hierarchy generation (#1447) We don't use the latex output anywhere, but it brings in a big dependency when building documentation or deb packages. The class hierarchy is also not that useful since there is not much inheritance used in the codebase. This also brings the doxygen parameters of sdformat closer to other Gazebo libraries. Signed-off-by: Addisu Z. Taddese --- doc/CMakeLists.txt | 3 --- doc/sdf.in | 6 +++--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 66d4caa6a..46a775b84 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -13,9 +13,6 @@ if (DOXYGEN_FOUND) ${CMAKE_BINARY_DIR}/doxygen/html COMMAND cp ${CMAKE_SOURCE_DIR}/doc/search.js ${CMAKE_BINARY_DIR}/doxygen/html/search - COMMAND make -C ${CMAKE_BINARY_DIR}/doxygen/latex - COMMAND mv ${CMAKE_BINARY_DIR}/doxygen/latex/refman.pdf - ${CMAKE_BINARY_DIR}/doxygen/latex/sdf-${PROJECT_VERSION_FULL}.pdf COMMENT "Generating API documentation with Doxygen" VERBATIM) endif() diff --git a/doc/sdf.in b/doc/sdf.in index 9865bc35b..30298aef3 100644 --- a/doc/sdf.in +++ b/doc/sdf.in @@ -1234,7 +1234,7 @@ SERVER_BASED_SEARCH = NO # If the GENERATE_LATEX tag is set to YES (the default) Doxygen will # generate Latex output. -GENERATE_LATEX = YES +GENERATE_LATEX = NO # The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be @@ -1595,7 +1595,7 @@ PERL_PATH = /usr/bin/perl # this option also works with HAVE_DOT disabled, but it is recommended to # install and use dot, since it yields more powerful graphs. -CLASS_DIAGRAMS = YES +CLASS_DIAGRAMS = NO # You can define message sequence charts within doxygen comments using the \msc # command. Doxygen will then run the mscgen tool (see @@ -1617,7 +1617,7 @@ HIDE_UNDOC_RELATIONS = YES # toolkit from AT&T and Lucent Bell Labs. The other options in this section # have no effect if this option is set to NO (the default) -HAVE_DOT = YES +HAVE_DOT = NO # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is # allowed to run in parallel. When set to 0 (the default) doxygen will From d80af036589f5306e992e1eed6ce5121e879c8ee Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 28 Jun 2024 16:02:45 -0500 Subject: [PATCH 26/47] Disable latex and class hierarchy generation (#1447) We don't use the latex output anywhere, but it brings in a big dependency when building documentation or deb packages. The class hierarchy is also not that useful since there is not much inheritance used in the codebase. This also brings the doxygen parameters of sdformat closer to other Gazebo libraries. Signed-off-by: Addisu Z. Taddese (cherry picked from commit 8e7b267b4b52ec892e53cd9d1a1032c3eee5f82a) --- doc/CMakeLists.txt | 3 --- doc/sdf.in | 6 +++--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 66d4caa6a..46a775b84 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -13,9 +13,6 @@ if (DOXYGEN_FOUND) ${CMAKE_BINARY_DIR}/doxygen/html COMMAND cp ${CMAKE_SOURCE_DIR}/doc/search.js ${CMAKE_BINARY_DIR}/doxygen/html/search - COMMAND make -C ${CMAKE_BINARY_DIR}/doxygen/latex - COMMAND mv ${CMAKE_BINARY_DIR}/doxygen/latex/refman.pdf - ${CMAKE_BINARY_DIR}/doxygen/latex/sdf-${PROJECT_VERSION_FULL}.pdf COMMENT "Generating API documentation with Doxygen" VERBATIM) endif() diff --git a/doc/sdf.in b/doc/sdf.in index 9865bc35b..30298aef3 100644 --- a/doc/sdf.in +++ b/doc/sdf.in @@ -1234,7 +1234,7 @@ SERVER_BASED_SEARCH = NO # If the GENERATE_LATEX tag is set to YES (the default) Doxygen will # generate Latex output. -GENERATE_LATEX = YES +GENERATE_LATEX = NO # The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be @@ -1595,7 +1595,7 @@ PERL_PATH = /usr/bin/perl # this option also works with HAVE_DOT disabled, but it is recommended to # install and use dot, since it yields more powerful graphs. -CLASS_DIAGRAMS = YES +CLASS_DIAGRAMS = NO # You can define message sequence charts within doxygen comments using the \msc # command. Doxygen will then run the mscgen tool (see @@ -1617,7 +1617,7 @@ HIDE_UNDOC_RELATIONS = YES # toolkit from AT&T and Lucent Bell Labs. The other options in this section # have no effect if this option is set to NO (the default) -HAVE_DOT = YES +HAVE_DOT = NO # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is # allowed to run in parallel. When set to 0 (the default) doxygen will From 68147636b8b81f705cbf798e7097d1a661fc7699 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Tue, 2 Jul 2024 13:45:15 -0700 Subject: [PATCH 27/47] Disable latex and class hierarchy generation (#1447) We don't use the latex output anywhere, but it brings in a big dependency when building documentation or deb packages. The class hierarchy is also not that useful since there is not much inheritance used in the codebase. This also brings the doxygen parameters of sdformat closer to other Gazebo libraries. (cherry picked from commit 8e7b267b4b52ec892e53cd9d1a1032c3eee5f82a) Signed-off-by: Addisu Z. Taddese Co-authored-by: Addisu Z. Taddese --- doc/CMakeLists.txt | 3 --- doc/sdf.in | 6 +++--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 019149959..46a775b84 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -13,9 +13,6 @@ if (DOXYGEN_FOUND) ${CMAKE_BINARY_DIR}/doxygen/html COMMAND cp ${CMAKE_SOURCE_DIR}/doc/search.js ${CMAKE_BINARY_DIR}/doxygen/html/search - COMMAND make -C ${CMAKE_BINARY_DIR}/doxygen/latex - COMMAND mv ${CMAKE_BINARY_DIR}/doxygen/latex/refman.pdf - ${CMAKE_BINARY_DIR}/doxygen/latex/sdf-${SDF_VERSION_FULL}.pdf COMMENT "Generating API documentation with Doxygen" VERBATIM) endif() diff --git a/doc/sdf.in b/doc/sdf.in index 9865bc35b..30298aef3 100644 --- a/doc/sdf.in +++ b/doc/sdf.in @@ -1234,7 +1234,7 @@ SERVER_BASED_SEARCH = NO # If the GENERATE_LATEX tag is set to YES (the default) Doxygen will # generate Latex output. -GENERATE_LATEX = YES +GENERATE_LATEX = NO # The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. # If a relative path is entered the value of OUTPUT_DIRECTORY will be @@ -1595,7 +1595,7 @@ PERL_PATH = /usr/bin/perl # this option also works with HAVE_DOT disabled, but it is recommended to # install and use dot, since it yields more powerful graphs. -CLASS_DIAGRAMS = YES +CLASS_DIAGRAMS = NO # You can define message sequence charts within doxygen comments using the \msc # command. Doxygen will then run the mscgen tool (see @@ -1617,7 +1617,7 @@ HIDE_UNDOC_RELATIONS = YES # toolkit from AT&T and Lucent Bell Labs. The other options in this section # have no effect if this option is set to NO (the default) -HAVE_DOT = YES +HAVE_DOT = NO # The DOT_NUM_THREADS specifies the number of dot invocations doxygen is # allowed to run in parallel. When set to 0 (the default) doxygen will From dcd0964ccbb7954d801aea83ae5776b7477d2dde Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 8 Jul 2024 10:21:50 -0700 Subject: [PATCH 28/47] Add //sensor/frame_id to SDF spec 1.12 (#1454) Signed-off-by: Ian Chen --- Migration.md | 12 ++++++++++++ include/sdf/Camera.hh | 4 ++-- include/sdf/Sensor.hh | 8 ++++++++ python/src/sdf/pyCamera.cc | 5 +++++ python/src/sdf/pySensor.cc | 4 ++++ python/test/pySensor_TEST.py | 7 +++++++ sdf/1.12/camera.sdf | 2 +- sdf/1.12/sensor.sdf | 4 ++++ src/Camera.cc | 7 +++++++ src/Camera_TEST.cc | 13 +++++++++++++ src/Sensor.cc | 19 ++++++++++++++++++- src/Sensor_TEST.cc | 19 +++++++++++++++++++ 12 files changed, 100 insertions(+), 4 deletions(-) diff --git a/Migration.md b/Migration.md index 410c4931b..0976e95f0 100644 --- a/Migration.md +++ b/Migration.md @@ -30,6 +30,18 @@ but with improved human-readability.. matches `"0"`, `"1"`, `"true"`, or `"false"` and returns `false` otherwise. +### Deprecations + +- **sdf/Camera.hh**: + + The `//sensor/camera/optical_frame_id` SDF element and corresponding functions + in the Camera DOM class are deprecated. Please specify camera frame using + the `//sensor/frame_id` SDF element instead. + + ***Deprecation:*** std::string OpticalFrameId() const + + ***Replacement:*** std::string Sensor::FrameId() const + + ***Deprecation:*** void SetOpticalFrameId(const std::string &) + + ***Replacement:*** void Sensor::SetFrameId(const std::string &) + + ## libsdformat 13.x to 14.x ### Additions diff --git a/include/sdf/Camera.hh b/include/sdf/Camera.hh index 5c46827c8..bb564c057 100644 --- a/include/sdf/Camera.hh +++ b/include/sdf/Camera.hh @@ -363,12 +363,12 @@ namespace sdf /// The Camera sensor assumes that the color and depth images are captured /// at the same frame_id. /// \return The name of the frame this camera uses in its camera_info topic. - public: const std::string OpticalFrameId() const; + public: const std::string GZ_DEPRECATED(15) OpticalFrameId() const; /// \brief Set the name of the coordinate frame relative to which this /// object's camera_info is expressed. /// \param[in] _frame The frame this camera uses in its camera_info topic. - public: void SetOpticalFrameId(const std::string &_frame); + public: void GZ_DEPRECATED(15) SetOpticalFrameId(const std::string &_frame); /// \brief Get the lens type. This is the type of the lens mapping. /// Supported values are gnomonical, stereographic, equidistant, diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 7b62572ec..49bc336f9 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -160,6 +160,14 @@ namespace sdf /// \param[in] _name Name of the sensor. public: void SetName(const std::string &_name); + /// \brief Get the frame ID of the sensor. + /// \return Sensor frame ID. + public: std::string FrameId() const; + + /// \brief Set the frame ID of the sensor. + /// \param[in] _frameId Frame ID to set. + public: void SetFrameId(const std::string &_frameId); + /// \brief Get the topic on which sensor data should be published. /// \return Topic for this sensor's data. public: std::string Topic() const; diff --git a/python/src/sdf/pyCamera.cc b/python/src/sdf/pyCamera.cc index e91683d5e..11c78f97b 100644 --- a/python/src/sdf/pyCamera.cc +++ b/python/src/sdf/pyCamera.cc @@ -32,6 +32,9 @@ namespace python ///////////////////////////////////////////////// void defineCamera(pybind11::object module) { + // \todo(iche033) OpticalFrameId and SetOpticalFrameId are deprecated + // Remove sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION pybind11::class_ cameraModule(module, "Camera"); cameraModule .def(pybind11::init<>()) @@ -296,6 +299,8 @@ void defineCamera(pybind11::object module) .value("BAYER_BGGR8", sdf::PixelFormatType::BAYER_BGGR8) .value("BAYER_GBRG8", sdf::PixelFormatType::BAYER_GBRG8) .value("BAYER_GRBG8", sdf::PixelFormatType::BAYER_GRBG8); + + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } } // namespace python } // namespace SDF_VERSION_NAMESPACE diff --git a/python/src/sdf/pySensor.cc b/python/src/sdf/pySensor.cc index 063830def..db788aa5c 100644 --- a/python/src/sdf/pySensor.cc +++ b/python/src/sdf/pySensor.cc @@ -51,6 +51,10 @@ void defineSensor(pybind11::object module) "Get the name of the sensor.") .def("set_name", &sdf::Sensor::SetName, "Set the name of the sensor.") + .def("frame_id", &sdf::Sensor::FrameId, + "Get the frame id of the sensor.") + .def("set_frame_id", &sdf::Sensor::SetFrameId, + "Set the frame id of the sensor.") .def("topic", &sdf::Sensor::Topic, "Get the topic on which sensor data should be published.") .def("set_topic", &sdf::Sensor::SetTopic, diff --git a/python/test/pySensor_TEST.py b/python/test/pySensor_TEST.py index 4a7f28415..204bd4adf 100644 --- a/python/test/pySensor_TEST.py +++ b/python/test/pySensor_TEST.py @@ -60,12 +60,14 @@ def test_default_construction(self): self.assertAlmostEqual(0.0, sensor.update_rate()) + self.assertFalse(sensor.frame_id()) self.assertFalse(sensor.topic()) self.assertFalse(sensor == sensor2) self.assertTrue(sensor != sensor2) def test_copy_construction(self): sensor = Sensor() + sensor.set_frame_id("test_frame_id") sensor.set_raw_pose(Pose3d(1, 2, 3, 0, 0, 0)) sensor.set_type(sdf.Sensortype.MAGNETOMETER) sensor.set_pose_relative_to("a_frame") @@ -79,6 +81,7 @@ def test_copy_construction(self): sensor2 = Sensor(sensor) + self.assertEqual("test_frame_id", sensor.frame_id()) self.assertEqual(sdf.Sensortype.MAGNETOMETER, sensor.type()) self.assertEqual(Pose3d(1, 2, 3, 0, 0, 0), sensor.raw_pose()) self.assertEqual("a_frame", sensor.pose_relative_to()) @@ -86,6 +89,7 @@ def test_copy_construction(self): self.assertAlmostEqual(mag.x_noise().mean(), sensor.magnetometer_sensor().x_noise().mean()) + self.assertEqual("test_frame_id", sensor2.frame_id()) self.assertEqual(sdf.Sensortype.MAGNETOMETER, sensor2.type()) self.assertEqual(Pose3d(1, 2, 3, 0, 0, 0), sensor2.raw_pose()) self.assertEqual("a_frame", sensor2.pose_relative_to()) @@ -96,6 +100,7 @@ def test_copy_construction(self): def test_deepcopy(self): sensor = Sensor() + sensor.set_frame_id("test_frame_id") sensor.set_raw_pose(Pose3d(1, 2, 3, 0, 0, 0)) sensor.set_type(sdf.Sensortype.MAGNETOMETER) sensor.set_pose_relative_to("a_frame") @@ -108,6 +113,7 @@ def test_deepcopy(self): sensor2 = copy.deepcopy(sensor) + self.assertEqual("test_frame_id", sensor.frame_id()) self.assertEqual(sdf.Sensortype.MAGNETOMETER, sensor.type()) self.assertEqual(Pose3d(1, 2, 3, 0, 0, 0), sensor.raw_pose()) self.assertEqual("a_frame", sensor.pose_relative_to()) @@ -115,6 +121,7 @@ def test_deepcopy(self): self.assertAlmostEqual(mag.x_noise().mean(), sensor.magnetometer_sensor().x_noise().mean()) + self.assertEqual("test_frame_id", sensor2.frame_id()) self.assertEqual(sdf.Sensortype.MAGNETOMETER, sensor2.type()) self.assertEqual(Pose3d(1, 2, 3, 0, 0, 0), sensor2.raw_pose()) self.assertEqual("a_frame", sensor2.pose_relative_to()) diff --git a/sdf/1.12/camera.sdf b/sdf/1.12/camera.sdf index 8fd218caf..cf959397b 100644 --- a/sdf/1.12/camera.sdf +++ b/sdf/1.12/camera.sdf @@ -222,7 +222,7 @@ - + An optional frame id name to be used in the camera_info message header. diff --git a/sdf/1.12/sensor.sdf b/sdf/1.12/sensor.sdf index 78954738b..4e0876fa7 100644 --- a/sdf/1.12/sensor.sdf +++ b/sdf/1.12/sensor.sdf @@ -59,6 +59,10 @@ If true, the sensor will publish performance metrics + + An optional frame id which indicates the sensor's frame of reference. + + diff --git a/src/Camera.cc b/src/Camera.cc index 2f2ea7a2b..184fbcead 100644 --- a/src/Camera.cc +++ b/src/Camera.cc @@ -752,6 +752,9 @@ void Camera::SetSaveFramesPath(const std::string &_path) ////////////////////////////////////////////////// bool Camera::operator==(const Camera &_cam) const { + + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION return this->Name() == _cam.Name() && this->HorizontalFov() == _cam.HorizontalFov() && this->ImageWidth() == _cam.ImageWidth() && @@ -764,6 +767,7 @@ bool Camera::operator==(const Camera &_cam) const this->ImageNoise() == _cam.ImageNoise() && this->VisibilityMask() == _cam.VisibilityMask() && this->OpticalFrameId() == _cam.OpticalFrameId(); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } ////////////////////////////////////////////////// @@ -1332,8 +1336,11 @@ sdf::ElementPtr Camera::ToElement() const this->SegmentationType()); } + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION elem->GetElement("optical_frame_id")->Set( this->OpticalFrameId()); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION return elem; } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 68b61beac..e8f62c3fa 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -134,9 +134,12 @@ TEST(DOMCamera, Construction) cam.SetPoseRelativeTo("/frame"); EXPECT_EQ("/frame", cam.PoseRelativeTo()); + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION EXPECT_TRUE(cam.OpticalFrameId().empty()); cam.SetOpticalFrameId("/optical_frame"); EXPECT_EQ("/optical_frame", cam.OpticalFrameId()); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION EXPECT_EQ("stereographic", cam.LensType()); cam.SetLensType("custom"); @@ -273,7 +276,12 @@ TEST(DOMCamera, ToElement) cam.SetPoseRelativeTo("/frame"); cam.SetSaveFrames(true); cam.SetSaveFramesPath("/tmp"); + + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION cam.SetOpticalFrameId("/optical_frame"); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + cam.SetCameraInfoTopic("/camera_info_test"); cam.SetTriggerTopic("/trigger_topic_test"); cam.SetTriggered(true); @@ -296,7 +304,12 @@ TEST(DOMCamera, ToElement) EXPECT_EQ("/frame", cam2.PoseRelativeTo()); EXPECT_TRUE(cam2.SaveFrames()); EXPECT_EQ("/tmp", cam2.SaveFramesPath()); + + // \todo(iche033) Remove in sdformat16 + GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION EXPECT_EQ("/optical_frame", cam2.OpticalFrameId()); + GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + EXPECT_EQ("/camera_info_test", cam2.CameraInfoTopic()); EXPECT_EQ("/trigger_topic_test", cam2.TriggerTopic()); EXPECT_TRUE(cam2.Triggered()); diff --git a/src/Sensor.cc b/src/Sensor.cc index 78ac7087b..8f3b12055 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -82,6 +82,9 @@ class sdf::Sensor::Implementation /// \brief Name of the sensor. public: std::string name = ""; + /// \brief Sensor frame ID + public: std::string frameId{""}; + /// \brief Sensor data topic. public: std::string topic = ""; @@ -243,7 +246,8 @@ Errors Sensor::Load(ElementPtr _sdf) "The supplied sensor name [" + this->dataPtr->name + "] is reserved."}); } - + this->dataPtr->frameId = _sdf->Get("frame_id", + this->dataPtr->frameId).first; this->dataPtr->updateRate = _sdf->Get("update_rate", this->dataPtr->updateRate).first; this->dataPtr->topic = _sdf->Get("topic"); @@ -439,6 +443,18 @@ void Sensor::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +std::string Sensor::FrameId() const +{ + return this->dataPtr->frameId; +} + +///////////////////////////////////////////////// +void Sensor::SetFrameId(const std::string &_frameId) +{ + this->dataPtr->frameId = _frameId; +} + ///////////////////////////////////////////////// std::string Sensor::Topic() const { @@ -750,6 +766,7 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const } poseElem->Set(this->RawPose()); + elem->GetElement("frame_id")->Set(this->FrameId()); elem->GetElement("topic")->Set(this->Topic()); elem->GetElement("update_rate")->Set(this->UpdateRate()); elem->GetElement("enable_metrics")->Set(this->EnableMetrics()); diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 8596cabce..482c6f1f9 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -60,6 +60,9 @@ TEST(DOMSensor, Construction) EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } + EXPECT_TRUE(sensor.FrameId().empty()); + sensor.SetFrameId("new_frame_id"); + EXPECT_EQ("new_frame_id", sensor.FrameId()); EXPECT_DOUBLE_EQ(0.0, sensor.UpdateRate()); @@ -76,6 +79,7 @@ TEST(DOMSensor, MoveConstructor) sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); sensor.SetUpdateRate(0.123); + sensor.SetFrameId("new_frame_id"); sdf::Noise noise; noise.SetMean(0.1); @@ -88,6 +92,7 @@ TEST(DOMSensor, MoveConstructor) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor2.FrameId()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); @@ -102,6 +107,7 @@ TEST(DOMSensor, CopyConstructor) sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); sensor.SetUpdateRate(0.123); + sensor.SetFrameId("new_frame_id"); sdf::Noise noise; noise.SetMean(0.1); @@ -114,6 +120,7 @@ TEST(DOMSensor, CopyConstructor) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); EXPECT_EQ("a_frame", sensor.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor.FrameId()); ASSERT_TRUE(nullptr != sensor.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor.MagnetometerSensor()->XNoise().Mean()); @@ -121,6 +128,7 @@ TEST(DOMSensor, CopyConstructor) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor2.FrameId()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); @@ -134,6 +142,7 @@ TEST(DOMSensor, MoveAssignment) sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); + sensor.SetFrameId("new_frame_id"); sdf::Noise noise; noise.SetMean(0.1); @@ -147,6 +156,7 @@ TEST(DOMSensor, MoveAssignment) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor2.FrameId()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); @@ -159,6 +169,7 @@ TEST(DOMSensor, CopyAssignment) sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); + sensor.SetFrameId("new_frame_id"); sdf::Noise noise; noise.SetMean(0.1); @@ -172,6 +183,7 @@ TEST(DOMSensor, CopyAssignment) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor.RawPose()); EXPECT_EQ("a_frame", sensor.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor.FrameId()); ASSERT_TRUE(nullptr != sensor.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor.MagnetometerSensor()->XNoise().Mean()); @@ -179,6 +191,7 @@ TEST(DOMSensor, CopyAssignment) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor.FrameId()); ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); @@ -191,11 +204,13 @@ TEST(DOMSensor, CopyAssignmentAfterMove) sensor1.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor1.SetType(sdf::SensorType::MAGNETOMETER); sensor1.SetPoseRelativeTo("frame1"); + sensor1.SetFrameId("new_frame_id"); sdf::Sensor sensor2; sensor2.SetRawPose(gz::math::Pose3d(4, 5, 6, 0, 0, 0)); sensor2.SetType(sdf::SensorType::CAMERA); sensor2.SetPoseRelativeTo("frame2"); + sensor2.SetFrameId("new_frame_id2"); // This is similar to what std::swap does except it uses std::move for each // assignment @@ -206,10 +221,12 @@ TEST(DOMSensor, CopyAssignmentAfterMove) EXPECT_EQ(sdf::SensorType::CAMERA, sensor1.Type()); EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 0), sensor1.RawPose()); EXPECT_EQ("frame2", sensor1.PoseRelativeTo()); + EXPECT_EQ("new_frame_id2", sensor1.FrameId()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("frame1", sensor2.PoseRelativeTo()); + EXPECT_EQ("new_frame_id", sensor2.FrameId()); } ///////////////////////////////////////////////// @@ -509,6 +526,7 @@ TEST(DOMSensor, ToElement) // test calling ToElement on a DOM object constructed without calling Load sdf::Sensor sensor; sensor.SetName("my_sensor"); + sensor.SetFrameId("my_sensor_frame_id"); sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetPoseRelativeTo("a_frame"); @@ -534,6 +552,7 @@ TEST(DOMSensor, ToElement) sensor2.Load(sensorElem); EXPECT_EQ("my_sensor", sensor2.Name()); + EXPECT_EQ("my_sensor_frame_id", sensor2.FrameId()); EXPECT_EQ(sdf::SensorType::MAGNETOMETER, sensor2.Type()); EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), sensor2.RawPose()); EXPECT_EQ("a_frame", sensor2.PoseRelativeTo()); From 1779253c733cec32edbecae0ba2fc1b6e9066158 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 8 Jul 2024 18:01:27 -0700 Subject: [PATCH 29/47] Remove deprecated APIs (#1456) * Joint.hh: *LinkName setters and getters * parser.hh: checkJointParentChildLinkNames(const sdf::Root *) * SDFImpl.hh: Root(const ElementPtr) * Types.hh: kSdfScopeDelimiter, SdfScopeDelimiter(), Inertia Signed-off-by: Steve Peters --- Migration.md | 17 +++++++++++++++++ include/sdf/Joint.hh | 20 -------------------- include/sdf/SDFImpl.hh | 5 ----- include/sdf/Types.hh | 20 -------------------- include/sdf/parser.hh | 11 ----------- src/Joint.cc | 24 ------------------------ src/Joint_TEST.cc | 16 ---------------- src/SDF.cc | 6 ------ src/SDF_TEST.cc | 8 -------- src/Types.cc | 8 -------- src/Types_TEST.cc | 8 -------- src/parser.cc | 6 ------ 12 files changed, 17 insertions(+), 132 deletions(-) diff --git a/Migration.md b/Migration.md index 0976e95f0..f339080fa 100644 --- a/Migration.md +++ b/Migration.md @@ -41,6 +41,23 @@ but with improved human-readability.. + ***Deprecation:*** void SetOpticalFrameId(const std::string &) + ***Replacement:*** void Sensor::SetFrameId(const std::string &) +### Removals + +- **sdf/Joint.hh**: + + `const std::string &ChildLinkName() const` (use `ChildName()` instead) + + `const std::string &ParentLinkName() const` (use `ParentName()` instead) + + `void SetChildLinkName(const std::string &)` (use `SetChildName()` instead) + + `void SetParentLinkName(const std::string &)` (use `SetParentName()` instead) + +- **sdf/SDFImpl.hh**: + + `void Root(const ElementPtr)` (use `SetRoot(const ElementPtr)` instead) + +- **sdf/Types.hh**: + + `const std::string &kSdfScopeDelimiter` (use `kScopeDelimiter` instead) + + `const std::string &SdfScopeDelimiter()` (use `kScopeDelimiter` instead) + +- **sdf/parser.hh**: + + `bool checkJointParentChildLinkNames(const sdf::Root *)` (use `checkJointParentChildNames(const sdf::Root *)` instead) ## libsdformat 13.x to 14.x diff --git a/include/sdf/Joint.hh b/include/sdf/Joint.hh index 26c4b226f..596e0a398 100644 --- a/include/sdf/Joint.hh +++ b/include/sdf/Joint.hh @@ -129,26 +129,6 @@ namespace sdf /// \param[in] _name Name of the child frame. public: void SetChildName(const std::string &_name); - /// \brief Get the name of this joint's parent link. - /// \return The name of the parent link. - /// \deprecated Use ParentName. - public: GZ_DEPRECATED(13) const std::string &ParentLinkName() const; - - /// \brief Set the name of the parent link. - /// \param[in] _name Name of the parent link. - /// \deprecated Use SetParentName. - public: GZ_DEPRECATED(13) void SetParentLinkName(const std::string &_name); - - /// \brief Get the name of this joint's child link. - /// \return The name of the child link. - /// \deprecated Use ChildName. - public: GZ_DEPRECATED(13) const std::string &ChildLinkName() const; - - /// \brief Set the name of the child link. - /// \param[in] _name Name of the child link. - /// \deprecated Use SetChildName. - public: GZ_DEPRECATED(13) void SetChildLinkName(const std::string &_name); - /// \brief Resolve the name of the child link from the /// FrameAttachedToGraph. /// \param[out] _body Name of child link of this joint. diff --git a/include/sdf/SDFImpl.hh b/include/sdf/SDFImpl.hh index 20e1ec19f..a868b4b4a 100644 --- a/include/sdf/SDFImpl.hh +++ b/include/sdf/SDFImpl.hh @@ -148,11 +148,6 @@ namespace sdf /// \param[in] _root Root element public: void SetRoot(const ElementPtr _root); - /// \brief Set the root pointer - /// \param[in] _root Root element - /// \deprecated Use SetRoot - public: GZ_DEPRECATED(13) void Root(const ElementPtr _root); - /// \brief Get the path to the SDF document on disk. /// \return The full path to the SDF document. public: std::string FilePath() const; diff --git a/include/sdf/Types.hh b/include/sdf/Types.hh index f2f19248c..26e25cdac 100644 --- a/include/sdf/Types.hh +++ b/include/sdf/Types.hh @@ -36,22 +36,8 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { // - namespace internal - { - /// \brief Initializes the scope delimiter as a function-local static - /// variable so it can be used to initialize kSdfScopeDelimiter. - /// \note This should not be used directly in user code. It will likely be - /// removed in libsdformat 15 with kSdfScopeDelimiter. - SDFORMAT_VISIBLE const std::string &SdfScopeDelimiter(); - } // namespace internal - constexpr std::string_view kScopeDelimiter{"::"}; - // Deprecated because it violates the Google Style Guide as it is not - // trivially destructible. Please use sdf::kScopeDelimiter instead. - GZ_DEPRECATED(14) - inline const std::string &kSdfScopeDelimiter = internal::SdfScopeDelimiter(); - /// \brief The source path replacement if it was parsed from a string, /// instead of a file. constexpr char kSdfStringSource[] = ""; @@ -158,12 +144,6 @@ namespace sdf public: int32_t nsec; }; - /// \brief A class for inertial information about a link. - class SDFORMAT_VISIBLE GZ_DEPRECATED(13) Inertia - { - public: double mass; - }; - /// \brief Transforms a string to its lowercase equivalent /// \param[in] _in String to convert to lowercase /// \return Lowercase equilvalent of _in. diff --git a/include/sdf/parser.hh b/include/sdf/parser.hh index a7dee740a..a96b484e3 100644 --- a/include/sdf/parser.hh +++ b/include/sdf/parser.hh @@ -503,17 +503,6 @@ namespace sdf SDFORMAT_VISIBLE bool checkJointParentChildNames(const sdf::Root *_root); - /// \brief Check that all joints in contained models specify parent - /// and child link names that match the names of sibling links. - /// This checks recursively and should check the files exhaustively - /// rather than terminating early when the first error is found. - /// \param[in] _root SDF Root object to check recursively. - /// \return True if all models have joints with valid parent and child - /// link names. - /// \deprecated Use checkJointParentChildNames. - SDFORMAT_VISIBLE GZ_DEPRECATED(13) - bool checkJointParentChildLinkNames(const sdf::Root *_root); - /// \brief Check that all joints in contained models specify parent /// and child names that match the names of sibling links, joints, /// models, or frames. diff --git a/src/Joint.cc b/src/Joint.cc index 2b8fd8282..93cc5900c 100644 --- a/src/Joint.cc +++ b/src/Joint.cc @@ -306,30 +306,6 @@ void Joint::SetChildName(const std::string &_name) this->dataPtr->childName = _name; } -///////////////////////////////////////////////// -const std::string &Joint::ParentLinkName() const -{ - return this->ParentName(); -} - -///////////////////////////////////////////////// -void Joint::SetParentLinkName(const std::string &_name) -{ - this->SetParentName(_name); -} - -///////////////////////////////////////////////// -const std::string &Joint::ChildLinkName() const -{ - return this->ChildName(); -} - -///////////////////////////////////////////////// -void Joint::SetChildLinkName(const std::string &_name) -{ - this->SetChildName(_name); -} - ///////////////////////////////////////////////// const JointAxis *Joint::Axis(const unsigned int _index) const { diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc index f4c23abf9..83217b744 100644 --- a/src/Joint_TEST.cc +++ b/src/Joint_TEST.cc @@ -30,10 +30,6 @@ TEST(DOMJoint, Construction) EXPECT_TRUE(joint.ParentName().empty()); EXPECT_TRUE(joint.ChildName().empty()); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - EXPECT_TRUE(joint.ParentLinkName().empty()); - EXPECT_TRUE(joint.ChildLinkName().empty()); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION EXPECT_EQ(gz::math::Pose3d::Zero, joint.RawPose()); EXPECT_TRUE(joint.PoseRelativeTo().empty()); @@ -65,18 +61,6 @@ TEST(DOMJoint, Construction) joint.SetName("test_joint"); EXPECT_EQ("test_joint", joint.Name()); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - joint.SetParentLinkName("parent"); - EXPECT_EQ("parent", joint.ParentLinkName()); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - EXPECT_EQ("parent", joint.ParentName()); - - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - joint.SetChildLinkName("child"); - EXPECT_EQ("child", joint.ChildLinkName()); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - EXPECT_EQ("child", joint.ChildName()); - joint.SetParentName("parent2"); EXPECT_EQ("parent2", joint.ParentName()); diff --git a/src/SDF.cc b/src/SDF.cc index 7a43d0f9a..22fcaef3d 100644 --- a/src/SDF.cc +++ b/src/SDF.cc @@ -383,12 +383,6 @@ void SDF::SetRoot(const ElementPtr _root) this->dataPtr->root = _root; } -///////////////////////////////////////////////// -void SDF::Root(const ElementPtr _root) -{ - this->SetRoot(_root); -} - ///////////////////////////////////////////////// std::string SDF::FilePath() const { diff --git a/src/SDF_TEST.cc b/src/SDF_TEST.cc index 44bb9f284..5538406d3 100644 --- a/src/SDF_TEST.cc +++ b/src/SDF_TEST.cc @@ -290,14 +290,6 @@ TEST(SDF, SetRoot) elem->AddValue("bool", "true", false, "description"); s.SetRoot(elem); EXPECT_EQ(elem, s.Root()); - - // Test deprecated setter, remove in libsdformat14 - s.SetRoot(nullptr); - EXPECT_EQ(nullptr, s.Root()); - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - s.Root(elem); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION - EXPECT_EQ(elem, s.Root()); } //////////////////////////////////////////////////// diff --git a/src/Types.cc b/src/Types.cc index 6e202b936..5b363a383 100644 --- a/src/Types.cc +++ b/src/Types.cc @@ -144,13 +144,5 @@ std::string JoinName( else return _scopeName + std::string(kScopeDelimiter) + _localName; } - -///////////////////////////////////////////////// -const std::string &internal::SdfScopeDelimiter() -{ - static const gz::utils::NeverDestroyed delimiter{ - kScopeDelimiter}; - return delimiter.Access(); -} } } diff --git a/src/Types_TEST.cc b/src/Types_TEST.cc index 37b951739..3d04947be 100644 --- a/src/Types_TEST.cc +++ b/src/Types_TEST.cc @@ -200,11 +200,3 @@ TEST(Types, JoinName) EXPECT_EQ(joinedName, ""); } } - -///////////////////////////////////////////////// -TEST(Types, ScopeDelimiters) -{ - GZ_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - EXPECT_EQ(sdf::kScopeDelimiter, sdf::kSdfScopeDelimiter); - GZ_UTILS_WARN_RESUME__DEPRECATED_DECLARATION -} diff --git a/src/parser.cc b/src/parser.cc index 84a7593d6..97603cfa1 100644 --- a/src/parser.cc +++ b/src/parser.cc @@ -2724,12 +2724,6 @@ bool checkPoseRelativeToGraph(sdf::Errors &_errors, const sdf::Root *_root) return result; } -////////////////////////////////////////////////// -bool checkJointParentChildLinkNames(const sdf::Root *_root) -{ - return checkJointParentChildNames(_root); -} - ////////////////////////////////////////////////// bool checkJointParentChildNames(const sdf::Root *_root) { From e40331e67301f88580dcdcdeba012832d3b239ab Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 15 Jul 2024 11:39:05 -0700 Subject: [PATCH 30/47] FrameSemantics: fix NullVertex warnings (#1458) Follow-up to gz-math#606. Signed-off-by: Steve Peters --- src/FrameSemantics.cc | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index e4d0aedad..40df3c4c4 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -77,6 +77,8 @@ FindSourceVertex(const ScopedGraph &_graph, using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = gz::math::graph::VertexId; + using VertexType = typename ScopedGraph::VertexType; + using gz::math::graph::NullVertex; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -86,7 +88,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "Unable to resolve pose, invalid vertex[" + std::to_string(_id) + "] " "in PoseRelativeToGraph."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } if (_id == _graph.ScopeVertexId()) @@ -106,7 +108,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: multiple incoming edges to " "current vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } auto const &edge = incidentsTo.begin()->second; vertex = _graph.Graph().VertexFromId(edge.get().Vertices().first); @@ -116,7 +118,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, "PoseRelativeToGraph cycle detected, already visited vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } if (vertex.get().Id() == _graph.ScopeVertexId()) { @@ -129,7 +131,7 @@ FindSourceVertex(const ScopedGraph &_graph, if (vertex.get().Id() != _graph.ScopeVertexId()) { // Error, the root vertex is not the same as the the source - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } return PairType(vertex, edges); @@ -157,6 +159,8 @@ FindSinkVertex( using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = gz::math::graph::VertexId; + using VertexType = typename ScopedGraph::VertexType; + using gz::math::graph::NullVertex; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -166,7 +170,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "Invalid vertex[" + std::to_string(_id) + "] " "in FrameAttachedToGraph."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } std::set visited; @@ -180,7 +184,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: multiple outgoing edges from " "current vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } auto const &edge = incidentsFrom.begin()->second; vertex = _graph.Graph().VertexFromId(edge.get().Vertices().second); @@ -190,7 +194,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_CYCLE, "FrameAttachedToGraph cycle detected, already visited vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } visited.insert(vertex.get().Id()); incidentsFrom = _graph.Graph().IncidentsFrom(vertex); From 2e86f7b00167928a178648b104d6ca7466320cff Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 16 Jul 2024 18:42:24 -0700 Subject: [PATCH 31/47] Revert "FrameSemantics: fix NullVertex warnings (#1458)" This reverts commit e40331e67301f88580dcdcdeba012832d3b239ab. Signed-off-by: Steve Peters --- src/FrameSemantics.cc | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index 40df3c4c4..e4d0aedad 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -77,8 +77,6 @@ FindSourceVertex(const ScopedGraph &_graph, using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = gz::math::graph::VertexId; - using VertexType = typename ScopedGraph::VertexType; - using gz::math::graph::NullVertex; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -88,7 +86,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "Unable to resolve pose, invalid vertex[" + std::to_string(_id) + "] " "in PoseRelativeToGraph."}); - return PairType(NullVertex(), EdgesType()); + return PairType(Vertex::NullVertex, EdgesType()); } if (_id == _graph.ScopeVertexId()) @@ -108,7 +106,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: multiple incoming edges to " "current vertex [" + vertex.get().Name() + "]."}); - return PairType(NullVertex(), EdgesType()); + return PairType(Vertex::NullVertex, EdgesType()); } auto const &edge = incidentsTo.begin()->second; vertex = _graph.Graph().VertexFromId(edge.get().Vertices().first); @@ -118,7 +116,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, "PoseRelativeToGraph cycle detected, already visited vertex [" + vertex.get().Name() + "]."}); - return PairType(NullVertex(), EdgesType()); + return PairType(Vertex::NullVertex, EdgesType()); } if (vertex.get().Id() == _graph.ScopeVertexId()) { @@ -131,7 +129,7 @@ FindSourceVertex(const ScopedGraph &_graph, if (vertex.get().Id() != _graph.ScopeVertexId()) { // Error, the root vertex is not the same as the the source - return PairType(NullVertex(), EdgesType()); + return PairType(Vertex::NullVertex, EdgesType()); } return PairType(vertex, edges); @@ -159,8 +157,6 @@ FindSinkVertex( using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = gz::math::graph::VertexId; - using VertexType = typename ScopedGraph::VertexType; - using gz::math::graph::NullVertex; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -170,7 +166,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "Invalid vertex[" + std::to_string(_id) + "] " "in FrameAttachedToGraph."}); - return PairType(NullVertex(), EdgesType()); + return PairType(Vertex::NullVertex, EdgesType()); } std::set visited; @@ -184,7 +180,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: multiple outgoing edges from " "current vertex [" + vertex.get().Name() + "]."}); - return PairType(NullVertex(), EdgesType()); + return PairType(Vertex::NullVertex, EdgesType()); } auto const &edge = incidentsFrom.begin()->second; vertex = _graph.Graph().VertexFromId(edge.get().Vertices().second); @@ -194,7 +190,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_CYCLE, "FrameAttachedToGraph cycle detected, already visited vertex [" + vertex.get().Name() + "]."}); - return PairType(NullVertex(), EdgesType()); + return PairType(Vertex::NullVertex, EdgesType()); } visited.insert(vertex.get().Id()); incidentsFrom = _graph.Graph().IncidentsFrom(vertex); From fe071fa3904a8075b557f2776cda0c23e21e8acb Mon Sep 17 00:00:00 2001 From: "Marco A. Gutierrez" Date: Tue, 30 Jul 2024 03:11:28 +0800 Subject: [PATCH 32/47] Adding Errors structure to XmlUtils (#1296) Signed-off-by: Marco A. Gutierrez Signed-off-by: Steve Peters Co-authored-by: Marco A. Gutierrez Co-authored-by: Steve Peters Co-authored-by: Addisu Z. Taddese --- include/sdf/Error.hh | 3 +++ src/Converter.cc | 10 +++++---- src/Converter.hh | 4 +++- src/Converter_TEST.cc | 21 ++++++++++++----- src/ParamPassing.cc | 52 ++++++++++++++++++++++--------------------- src/XmlUtils.cc | 30 ++++++++++++++++++++----- src/XmlUtils.hh | 18 ++++++++++++++- src/XmlUtils_TEST.cc | 49 +++++++++++++++++++++++++++++++++++++++- 8 files changed, 143 insertions(+), 44 deletions(-) diff --git a/include/sdf/Error.hh b/include/sdf/Error.hh index f100ebb58..c8c37394c 100644 --- a/include/sdf/Error.hh +++ b/include/sdf/Error.hh @@ -179,6 +179,9 @@ namespace sdf /// \brief Generic error during parsing. PARSING_ERROR, + + /// \brief Error at the XML level. + XML_ERROR, }; class SDFORMAT_VISIBLE Error diff --git a/src/Converter.cc b/src/Converter.cc index 8947d3333..41d550df1 100644 --- a/src/Converter.cc +++ b/src/Converter.cc @@ -264,7 +264,7 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, } else if (name == "copy") { - Move(_elem, childElem, true); + Move(_elem, childElem, true, _errors); } else if (name == "map") { @@ -272,7 +272,7 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem, } else if (name == "move") { - Move(_elem, childElem, false); + Move(_elem, childElem, false, _errors); } else if (name == "add") { @@ -923,7 +923,8 @@ void Converter::Map(tinyxml2::XMLElement *_elem, ///////////////////////////////////////////////// void Converter::Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy) + const bool _copy, + sdf::Errors &_errors) { SDF_ASSERT(_elem != NULL, "SDF element is NULL"); SDF_ASSERT(_moveElem != NULL, "Move element is NULL"); @@ -1024,7 +1025,8 @@ void Converter::Move(tinyxml2::XMLElement *_elem, if (toElemStr && !toAttrStr) { - tinyxml2::XMLNode *cloned = DeepClone(moveFrom->GetDocument(), moveFrom); + tinyxml2::XMLNode *cloned = DeepClone(_errors, moveFrom->GetDocument(), + moveFrom); tinyxml2::XMLElement *moveTo = static_cast(cloned); moveTo->SetValue(toName); diff --git a/src/Converter.hh b/src/Converter.hh index e992b124c..cfba5413f 100644 --- a/src/Converter.hh +++ b/src/Converter.hh @@ -108,9 +108,11 @@ namespace sdf /// \param[in] _moveElem A 'convert' element that describes the move /// operation. /// \param[in] _copy True to copy the element + /// \param[out] _errors Vector of errors. private: static void Move(tinyxml2::XMLElement *_elem, tinyxml2::XMLElement *_moveElem, - const bool _copy); + const bool _copy, + sdf::Errors &_errors); /// \brief Add an element or attribute to an element. /// \param[in] _elem The element to receive the value. diff --git a/src/Converter_TEST.cc b/src/Converter_TEST.cc index 443774017..d56cba30a 100644 --- a/src/Converter_TEST.cc +++ b/src/Converter_TEST.cc @@ -3370,7 +3370,8 @@ TEST(Converter, World_17_to_18) ASSERT_TRUE(errors.empty()); // Compare converted xml with expected - std::string convertedXmlStr = ElementToString(xmlDoc.RootElement()); + std::string convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); std::string expectedXmlStr = R"( @@ -3393,7 +3394,9 @@ TEST(Converter, World_17_to_18) tinyxml2::XMLDocument expectedXmlDoc; expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // Check some basic elements tinyxml2::XMLElement *convertedElem = xmlDoc.FirstChildElement(); @@ -3509,7 +3512,8 @@ TEST(Converter, World_17_to_18) ASSERT_TRUE(errors.empty()); // Compare converted xml with expected - convertedXmlStr = ElementToString(xmlDoc.RootElement()); + convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); expectedXmlStr = R"( @@ -3577,7 +3581,9 @@ TEST(Converter, World_17_to_18) expectedXmlDoc.Clear(); expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // ------- Another flattened world in 1.7 format @@ -3616,7 +3622,8 @@ TEST(Converter, World_17_to_18) EXPECT_TRUE(buffer.str().empty()) << buffer.str(); // Compare converted xml with expected - convertedXmlStr = ElementToString(xmlDoc.RootElement()); + convertedXmlStr = ElementToString(errors, xmlDoc.RootElement()); + ASSERT_TRUE(errors.empty()); ASSERT_FALSE(convertedXmlStr.empty()); expectedXmlStr = R"( @@ -3655,7 +3662,9 @@ TEST(Converter, World_17_to_18) expectedXmlDoc.Clear(); expectedXmlDoc.Parse(expectedXmlStr.c_str()); - EXPECT_EQ(ElementToString(expectedXmlDoc.RootElement()), convertedXmlStr); + EXPECT_EQ(ElementToString(errors, expectedXmlDoc.RootElement()), + convertedXmlStr); + ASSERT_TRUE(errors.empty()); // Check some basic elements convertedElem = xmlDoc.FirstChildElement(); diff --git a/src/ParamPassing.cc b/src/ParamPassing.cc index 1b80d09cf..5bf0c97a9 100644 --- a/src/ParamPassing.cc +++ b/src/ParamPassing.cc @@ -52,7 +52,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_MISSING, "Element identifier requires an element_id attribute, but the " "element_id is not set. Skipping element alteration:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -67,7 +67,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Missing name after double colons in element identifier. " "Skipping element alteration:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -83,7 +83,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Action [" + actionStr + "] is not a valid action. Skipping " - "element alteration:\n" + ElementToString(childElemXml) + "element alteration:\n" + ElementToString(_errors, childElemXml) }); continue; } @@ -102,7 +102,7 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ATTRIBUTE_MISSING, "Element to be added is missing a 'name' attribute. " "Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -112,7 +112,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "The 'name' attribute can not be empty. Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -129,7 +129,7 @@ void updateParams(const ParserConfig &_config, + " element_id='" + childElemXml->Attribute("element_id") + "'> because element already exists in included model. " + "Skipping element addition:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -157,7 +157,8 @@ void updateParams(const ParserConfig &_config, _errors.push_back({ErrorCode::ELEMENT_MISSING, "Could not find element <" + std::string(childElemXml->Name()) + " element_id='" + childElemXml->Attribute("element_id") + "'>. " + - "Skipping element modification:\n" + ElementToString(childElemXml) + "Skipping element modification:\n" + + ElementToString(_errors, childElemXml) }); continue; } @@ -194,7 +195,7 @@ void updateParams(const ParserConfig &_config, { _errors.push_back({ErrorCode::ELEMENT_INVALID, "Unable to convert XML to SDF. Skipping element replacement:\n" - + ElementToString(childElemXml) + + ElementToString(_errors, childElemXml) }); continue; } @@ -355,7 +356,7 @@ ElementPtr initElementDescription(const tinyxml2::XMLElement *_xml, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Element [" + std::string(_xml->Name()) + "] is not a defined " "SDF element. Skipping element alteration\n: " - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); return nullptr; } @@ -384,7 +385,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Missing an action attribute. Skipping child element modification " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -397,7 +398,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "child element modification with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -411,7 +412,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element removal " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); } else @@ -430,7 +431,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element modification " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); } else @@ -455,7 +456,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "child element modification with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -468,7 +469,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Unable to convert XML to SDF. Skipping child element alteration " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -486,7 +487,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Could not find element. Skipping child element replacement " "with parent <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) + "'>:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } @@ -498,7 +499,7 @@ void handleIndividualChildActions(const ParserConfig &_config, "Replacement element is missing a 'name' attribute. " "Skipping element replacement <" + std::string(_childrenXml->Name()) + " element_id='" + std::string(_childrenXml->Attribute("element_id")) - + "'>:\n" + ElementToString(xmlChild) + + "'>:\n" + ElementToString(_errors, xmlChild) }); continue; } @@ -525,7 +526,7 @@ void add(const ParserConfig &_config, const std::string &_source, { _errors.push_back({ErrorCode::ELEMENT_INVALID, "Unable to convert XML to SDF. Skipping element addition:\n" - + ElementToString(_childXml) + + ElementToString(_errors, _childXml) }); } } @@ -557,7 +558,8 @@ void modifyAttributes(tinyxml2::XMLElement *_xml, { _errors.push_back({ErrorCode::ATTRIBUTE_INVALID, "Attribute [" + attrName + "] is invalid. " - "Skipping attribute modification in:\n" + ElementToString(_xml) + "Skipping attribute modification in:\n" + + ElementToString(_errors, _xml) }); continue; } @@ -582,7 +584,7 @@ void modifyChildren(tinyxml2::XMLElement *_xml, { _errors.push_back({ErrorCode::ELEMENT_MISSING, "Could not find element [" + elemName + "]. " - "Skipping modification for:\n" + ElementToString(_xml) + "Skipping modification for:\n" + ElementToString(_errors, _xml) }); continue; } @@ -599,7 +601,7 @@ void modifyChildren(tinyxml2::XMLElement *_xml, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Value [" + std::string(xmlChild->GetText()) + "] for element [" + elemName + "] is invalid. Skipping modification for:\n" - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); continue; } @@ -620,9 +622,9 @@ void modifyChildren(tinyxml2::XMLElement *_xml, // sdf has child elements but no children were specified in xml std::stringstream ss; ss << "No modifications for element " - << ElementToString(xmlChild) + << ElementToString(_errors, xmlChild) << " provided, skipping modification for:\n" - << ElementToString(_xml); + << ElementToString(_errors, _xml); Error err(ErrorCode::WARNING, ss.str()); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), err, _errors); @@ -650,7 +652,7 @@ void modify(tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, _errors.push_back({ErrorCode::ELEMENT_INVALID, "Value [" + std::string(_xml->GetText()) + "] for element [" + std::string(_xml->Name()) + "] is invalid. Skipping modification for:\n" - + ElementToString(_xml) + + ElementToString(_errors, _xml) }); } } @@ -688,7 +690,7 @@ void remove(const tinyxml2::XMLElement *_xml, const sdf::ParserConfig &_config, + std::string(xmlParent->Name()) + " element_id='" + std::string(xmlParent->Attribute("element_id")) + "'> with parent <" + std::string(_xml->Name()) + ">:\n" - + ElementToString(xmlChild) + + ElementToString(_errors, xmlChild) }); continue; } diff --git a/src/XmlUtils.cc b/src/XmlUtils.cc index 4dbceab71..5d3709707 100644 --- a/src/XmlUtils.cc +++ b/src/XmlUtils.cc @@ -15,7 +15,9 @@ * */ #include +#include +#include "Utils.hh" #include "XmlUtils.hh" #include "sdf/Console.hh" @@ -27,27 +29,41 @@ inline namespace SDF_VERSION_NAMESPACE { ///////////////////////////////////////////////// tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, const tinyxml2::XMLNode *_src) +{ + sdf::Errors errors; + tinyxml2::XMLNode *result = DeepClone(errors, _doc, _src); + sdf::throwOrPrintErrors(errors); + return result; +} + +///////////////////////////////////////////////// +tinyxml2::XMLNode *DeepClone(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + const tinyxml2::XMLNode *_src) { if (_src == nullptr) { - sdferr << "Pointer to XML node _src is NULL\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Pointer to XML node _src is NULL"}); return nullptr; } tinyxml2::XMLNode *copy = _src->ShallowClone(_doc); if (copy == nullptr) { - sdferr << "Failed to clone node " << _src->Value() << "\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Failed to clone node " + std::string(_src->Value())}); return nullptr; } for (const tinyxml2::XMLNode *node = _src->FirstChild(); node != nullptr; node = node->NextSibling()) { - auto *childCopy = DeepClone(_doc, node); + auto *childCopy = DeepClone(_errors, _doc, node); if (childCopy == nullptr) { - sdferr << "Failed to clone child " << node->Value() << "\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Failed to clone child " + std::string(node->Value())}); return nullptr; } copy->InsertEndChild(childCopy); @@ -57,11 +73,13 @@ tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, } ///////////////////////////////////////////////// -std::string ElementToString(const tinyxml2::XMLElement *_elem) +std::string ElementToString(sdf::Errors &_errors, + const tinyxml2::XMLElement *_elem) { if (_elem == nullptr) { - sdferr << "Pointer to XML Element _elem is nullptr\n"; + _errors.push_back({sdf::ErrorCode::XML_ERROR, + "Pointer to XML Element _elem is nullptr"}); return ""; } diff --git a/src/XmlUtils.hh b/src/XmlUtils.hh index 7ad0c30d4..522458aad 100644 --- a/src/XmlUtils.hh +++ b/src/XmlUtils.hh @@ -41,10 +41,26 @@ namespace sdf tinyxml2::XMLNode *DeepClone(tinyxml2::XMLDocument *_doc, const tinyxml2::XMLNode *_src); + /// \brief Perform a deep copy of an XML Node + /// + /// This copies an XMLNode _src and all of its decendants + /// into a specified XMLDocument. + /// + /// \param[out] _errors Vector of errors + /// \param[in] _doc Document in which to place the copied node + /// \param[in] _src The node to deep copy + /// \returns The newly copied node upon success OR + /// nullptr if an error occurs. + tinyxml2::XMLNode *DeepClone(sdf::Errors &_errors, + tinyxml2::XMLDocument *_doc, + const tinyxml2::XMLNode *_src); + /// \brief Converts the XML Element to a string + /// \param[out] _errors Vector of errors /// \param[in] _elem Element to be converted /// \return The string representation - std::string ElementToString(const tinyxml2::XMLElement *_elem); + std::string ElementToString(sdf::Errors &_errors, + const tinyxml2::XMLElement *_elem); } } #endif diff --git a/src/XmlUtils_TEST.cc b/src/XmlUtils_TEST.cc index aef79262d..4190b058b 100644 --- a/src/XmlUtils_TEST.cc +++ b/src/XmlUtils_TEST.cc @@ -37,7 +37,9 @@ TEST(XMLUtils, DeepClone) ASSERT_EQ(tinyxml2::XML_SUCCESS, ret); auto root = oldDoc.FirstChild(); - auto newRoot = sdf::DeepClone(&newDoc, root); + sdf::Errors errors; + auto newRoot = sdf::DeepClone(errors, &newDoc, root); + EXPECT_TRUE(errors.empty()) << errors; EXPECT_STREQ("document", newRoot->ToElement()->Name()); @@ -53,3 +55,48 @@ TEST(XMLUtils, DeepClone) auto childB_text = newChildB->ToElement()->GetText(); EXPECT_STREQ("Hello World", childB_text); } + +///////////////////////////////////////////////// +TEST(XMLUtils, InvalidDeepClone) +{ + sdf::Errors errors; + auto newRoot = sdf::DeepClone(errors, nullptr, nullptr); + EXPECT_EQ(nullptr, newRoot); + EXPECT_EQ(1u, errors.size()) << errors; + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::XML_ERROR); +} + +///////////////////////////////////////////////// +TEST(XMLUtils, ElementToString) +{ + tinyxml2::XMLDocument doc; + + std::string docXml = +R"( + + Hello World + + +)"; + + auto ret = doc.Parse(docXml.c_str()); + ASSERT_EQ(tinyxml2::XML_SUCCESS, ret); + + auto root = doc.FirstChild(); + sdf::Errors errors; + std::string docString = sdf::ElementToString(errors, root->ToElement()); + EXPECT_TRUE(errors.empty()) << errors; + EXPECT_EQ(docXml, docString); +} + +///////////////////////////////////////////////// +TEST(XMLUtils, InvalidElementToString) +{ + sdf::Errors errors; + std::string docString = sdf::ElementToString(errors, nullptr); + EXPECT_TRUE(docString.empty()); + EXPECT_EQ(1u, errors.size()) << errors; + ASSERT_FALSE(errors.empty()); + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::XML_ERROR); +} From fd81923d11919f17931be113e44d00a0e566aa04 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 1 Aug 2024 16:55:49 -0700 Subject: [PATCH 33/47] FrameSemantics: fix NullVertex warnings (#1460) Follow-up to gz-math#606, retry of #1458. Signed-off-by: Steve Peters --- src/FrameSemantics.cc | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc index e4d0aedad..40df3c4c4 100644 --- a/src/FrameSemantics.cc +++ b/src/FrameSemantics.cc @@ -77,6 +77,8 @@ FindSourceVertex(const ScopedGraph &_graph, using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = gz::math::graph::VertexId; + using VertexType = typename ScopedGraph::VertexType; + using gz::math::graph::NullVertex; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -86,7 +88,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID, "Unable to resolve pose, invalid vertex[" + std::to_string(_id) + "] " "in PoseRelativeToGraph."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } if (_id == _graph.ScopeVertexId()) @@ -106,7 +108,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR, "PoseRelativeToGraph error: multiple incoming edges to " "current vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } auto const &edge = incidentsTo.begin()->second; vertex = _graph.Graph().VertexFromId(edge.get().Vertices().first); @@ -116,7 +118,7 @@ FindSourceVertex(const ScopedGraph &_graph, _errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE, "PoseRelativeToGraph cycle detected, already visited vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } if (vertex.get().Id() == _graph.ScopeVertexId()) { @@ -129,7 +131,7 @@ FindSourceVertex(const ScopedGraph &_graph, if (vertex.get().Id() != _graph.ScopeVertexId()) { // Error, the root vertex is not the same as the the source - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } return PairType(vertex, edges); @@ -157,6 +159,8 @@ FindSinkVertex( using DirectedEdge = typename ScopedGraph::Edge; using Vertex = typename ScopedGraph::Vertex; using VertexId = gz::math::graph::VertexId; + using VertexType = typename ScopedGraph::VertexType; + using gz::math::graph::NullVertex; using EdgesType = std::vector; using PairType = std::pair; EdgesType edges; @@ -166,7 +170,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID, "Invalid vertex[" + std::to_string(_id) + "] " "in FrameAttachedToGraph."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } std::set visited; @@ -180,7 +184,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR, "FrameAttachedToGraph error: multiple outgoing edges from " "current vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } auto const &edge = incidentsFrom.begin()->second; vertex = _graph.Graph().VertexFromId(edge.get().Vertices().second); @@ -190,7 +194,7 @@ FindSinkVertex( _errors.push_back({ErrorCode::FRAME_ATTACHED_TO_CYCLE, "FrameAttachedToGraph cycle detected, already visited vertex [" + vertex.get().Name() + "]."}); - return PairType(Vertex::NullVertex, EdgesType()); + return PairType(NullVertex(), EdgesType()); } visited.insert(vertex.get().Id()); incidentsFrom = _graph.Graph().IncidentsFrom(vertex); From 6483c252e3c74dfe5ba8d84954a01547a75189dc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 2 Aug 2024 11:56:47 -0700 Subject: [PATCH 34/47] Expand SDF description on link kinematic property (#1462) Signed-off-by: Ian Chen --- sdf/1.12/link.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sdf/1.12/link.sdf b/sdf/1.12/link.sdf index b518ff3c5..856629a37 100644 --- a/sdf/1.12/link.sdf +++ b/sdf/1.12/link.sdf @@ -19,7 +19,7 @@ - If true, the link is kinematic only + If true, the link is kinematic only. A kinematic link does not react to forces such as gravity, applied force / torque, or influences from other dynamic bodies. The link can be animated by changing its position or velocity. Kinematic links can also be connected by joints, and moved by joint position or velocity commands. From 86db7db7e0c95ebd544ca03f57bbed7b37061ba8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 5 Aug 2024 15:16:23 -0700 Subject: [PATCH 35/47] Prepare for sdformat 14.5.0 release (#1466) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 20 ++++++++++++++++++++ package.xml | 2 +- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0eea9fb7b..93b4c4101 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.4.0) +project (sdformat14 VERSION 14.5.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 08b9858d4..65579f27d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,25 @@ ## libsdformat 14.X +### libsdformat 14.5.0 (2024-08-05) + +1. Adding Errors structure to XmlUtils + * [Pull request #1296](https://github.com/gazebosim/sdformat/pull/1296) + +1. Disable latex and class hierarchy generation + * [Pull request #1447](https://github.com/gazebosim/sdformat/pull/1447) + +1. Added SetHeightMap and Heighmap to Geometry Python binding + * [Pull request #1440](https://github.com/gazebosim/sdformat/pull/1440) + +1. workflows/ci.yml fix push branch regex + * [Pull request #1445](https://github.com/gazebosim/sdformat/pull/1445) + +1. SDF.cc update calls to use sdf::Errors output + * [Pull request #1295](https://github.com/gazebosim/sdformat/pull/1295) + +1. Added World::ActorByName + * [Pull request #1436](https://github.com/gazebosim/sdformat/pull/1436) + ### libsdformat 14.4.0 (2024-06-20) 1. Add Cone as a primitive parametric shape. diff --git a/package.xml b/package.xml index ed841192a..d5e40891f 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ sdformat14 - 14.4.0 + 14.5.0 SDFormat is an XML file format that describes environments, objects, and robots in a manner suitable for robotic applications From 7404b55218a566a78241839bb75d3e3ff8396a51 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 16 Aug 2024 20:38:47 +0200 Subject: [PATCH 36/47] Add optional binary relocatability (#1414) This PR uses the changes introduced in gz-cmake3 in gazebosim/gz-cmake#334 to support the cmake installation directory to be moved after the make install prefix, and continue to work without the need to set any special environment variable, as long as the library is compiled as shared. To avoid regressions and problems in Ubuntu Focal due to the use of std::filesystem, this new behaviour is only activated if the GZ_ENABLE_RELOCATABLE_INSTALL option is enabled, and its default value is OFF . In particular, this PR defines a sdf::getSharePath() function that should be used in place of the SDF_SHARE_PATH macro to ensure that the library is relocatable. Signed-off-by: Silvio Traversaro Co-authored-by: Steve Peters Co-authored-by: Ian Chen --- CMakeLists.txt | 7 ++ include/sdf/InstallationDirectories.hh | 40 +++++++ include/sdf/config.hh.in | 4 +- src/CMakeLists.txt | 25 ++++ src/InstallationDirectories.cc | 151 +++++++++++++++++++++++++ src/SDF.cc | 15 ++- test/integration/CMakeLists.txt | 1 + test/performance/CMakeLists.txt | 5 +- 8 files changed, 240 insertions(+), 8 deletions(-) create mode 100644 include/sdf/InstallationDirectories.hh create mode 100644 src/InstallationDirectories.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 93b4c4101..b81c2acf0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,13 @@ if (BUILD_SDF) # Find tinyxml2. gz_find_package(TINYXML2 REQUIRED) + ################################################# + # Find DL if doing relocatable installation + if (GZ_ENABLE_RELOCATABLE_INSTALL) + gz_find_package(DL REQUIRED) + endif() + + ################################################ # Find urdfdom parser. Logic: # diff --git a/include/sdf/InstallationDirectories.hh b/include/sdf/InstallationDirectories.hh new file mode 100644 index 000000000..be3b0c678 --- /dev/null +++ b/include/sdf/InstallationDirectories.hh @@ -0,0 +1,40 @@ +/* + * 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 SDF_INSTALLATION_DIRECTORIES_HH_ +#define SDF_INSTALLATION_DIRECTORIES_HH_ + +#include + +#include +#include + +namespace sdf +{ + inline namespace SDF_VERSION_NAMESPACE { + + /// \brief getInstallPrefix return the install prefix of the library + /// i.e. CMAKE_INSTALL_PREFIX unless the library has been moved + SDFORMAT_VISIBLE std::string getInstallPrefix(); + + /// \brief getSharePath return the share directory used by sdformat + SDFORMAT_VISIBLE std::string getSharePath(); + + } +} + +#endif diff --git a/include/sdf/config.hh.in b/include/sdf/config.hh.in index 6a32cb8bc..456eb12cc 100644 --- a/include/sdf/config.hh.in +++ b/include/sdf/config.hh.in @@ -48,11 +48,11 @@ #cmakedefine SDFORMAT_DISABLE_CONSOLE_LOGFILE 1 #ifndef SDF_SHARE_PATH -#define SDF_SHARE_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" +#define SDF_SHARE_PATH _Pragma ("GCC warning \"'SDF_SHARE_PATH' macro is deprecated, use sdf::getSharePath() function instead. \"") "${CMAKE_INSTALL_FULL_DATAROOTDIR}/" #endif #ifndef SDF_VERSION_PATH -#define SDF_VERSION_PATH "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/${PROJECT_VERSION}" +#define SDF_VERSION_PATH _Pragma ("GCC warning \"'SDF_VERSION_PATH' macro is deprecated and should not be used. \"") "${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/${PROJECT_VERSION}" #endif #endif // #ifndef SDF_CONFIG_HH_ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index dfd569186..57dab8757 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -23,6 +23,24 @@ gz_create_core_library(SOURCES ${sources} CXX_STANDARD 17 LEGACY_PROJECT_PREFIX SDFormat ) +gz_add_get_install_prefix_impl(GET_INSTALL_PREFIX_FUNCTION sdf::getInstallPrefix + GET_INSTALL_PREFIX_HEADER sdf/InstallationDirectories.hh + OVERRIDE_INSTALL_PREFIX_ENV_VARIABLE SDF_INSTALL_PREFIX) + +# CMAKE_INSTALL_DATAROOTDIR may be an absolute path, let's make sure to use the +# relative version +if(IS_ABSOLUTE "${CMAKE_INSTALL_DATAROOTDIR}") + file(RELATIVE_PATH CMAKE_INSTALL_RELATIVE_DATAROOTDIR "${CMAKE_INSTALL_PREFIX}" "${CMAKE_INSTALL_DATAROOTDIR}") +else() + set(CMAKE_INSTALL_RELATIVE_DATAROOTDIR "${CMAKE_INSTALL_DATAROOTDIR}") +endif() + +set_property( + SOURCE InstallationDirectories.cc + PROPERTY COMPILE_DEFINITIONS + CMAKE_INSTALL_RELATIVE_DATAROOTDIR="${CMAKE_INSTALL_RELATIVE_DATAROOTDIR}" +) + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PUBLIC @@ -91,6 +109,11 @@ if (BUILD_TESTING) -DGZ_SDFORMAT_STATIC_DEFINE ) + if(WIN32) + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} + PRIVATE shlwapi) + endif() + gz_build_tests( TYPE UNIT SOURCES ${gtest_sources} @@ -99,6 +122,8 @@ if (BUILD_TESTING) ${PROJECT_SOURCE_DIR}/test LIB_DEPS library_for_tests + ENVIRONMENT + SDF_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) if (TARGET UNIT_gz_TEST) diff --git a/src/InstallationDirectories.cc b/src/InstallationDirectories.cc new file mode 100644 index 000000000..167df10eb --- /dev/null +++ b/src/InstallationDirectories.cc @@ -0,0 +1,151 @@ +/* + * 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 + +#include +#include + +#ifdef _WIN32 +#include +#endif + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { + +// We locally import the gz::common::joinPaths function +// See https://github.com/gazebosim/gz-physics/pull/507#discussion_r1186919267 +// for more details + +// Function imported from +// https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/FilesystemBoost.cc#L507 +#ifndef WIN32 +static const char preferred_separator = '/'; +#else // Windows +static const char preferred_separator = '\\'; +#endif +const std::string separator(const std::string &_p) +{ + return _p + preferred_separator; +} + +// Function imported from +// https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/Filesystem.cc#L227 +std::string checkWindowsPath(const std::string _path) +{ + if (_path.empty()) + return _path; + + // Check if this is a http or https, if so change backslashes generated by + // jointPaths to '/' + if ((_path.size() > 7 && 0 == _path.compare(0, 7, "http://")) || + (_path.size() > 8 && 0 == _path.compare(0, 8, "https://"))) + { + return std::regex_replace(_path, std::regex(R"(\\)"), "/"); + } + + // This is a Windows path, convert all '/' into backslashes + std::string result = std::regex_replace(_path, std::regex(R"(/)"), "\\"); + std::string drive_letters; + + // only Windows contains absolute paths starting with drive letters + if (result.length() > 3 && 0 == result.compare(1, 2, ":\\")) + { + drive_letters = result.substr(0, 3); + result = result.substr(3); + } + result = drive_letters + std::regex_replace( + result, std::regex("[<>:\"|?*]"), ""); + return result; +} + +// Function imported from +// https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/Filesystem.cc#L256 +std::string joinPaths(const std::string &_path1, + const std::string &_path2) +{ + + /// This function is used to avoid duplicated path separators at the + /// beginning/end of the string, and between the two paths being joined. + /// \param[in] _path This is the string to sanitize. + /// \param[in] _stripLeading True if the leading separator should be + /// removed. + auto sanitizeSlashes = [](const std::string &_path, + bool _stripLeading = false) + { + // Shortcut + if (_path.empty()) + return _path; + + std::string result = _path; + + // Use the appropriate character for each platform. +#ifndef _WIN32 + char replacement = '/'; +#else + char replacement = '\\'; +#endif + + // Sanitize the start of the path. + size_t index = 0; + size_t leadingIndex = _stripLeading ? 0 : 1; + for (; index < result.length() && result[index] == replacement; ++index) + { + } + if (index > leadingIndex) + result.erase(leadingIndex, index-leadingIndex); + + // Sanitize the end of the path. + index = result.length()-1; + for (; index < result.length() && result[index] == replacement; --index) + { + } + index += 1; + if (index < result.length()-1) + result.erase(index+1); + return result; + }; + + std::string path; +#ifndef _WIN32 + path = sanitizeSlashes(sanitizeSlashes(separator(_path1)) + + sanitizeSlashes(_path2, true)); +#else // _WIN32 + std::string path1 = sanitizeSlashes(checkWindowsPath(_path1)); + std::string path2 = sanitizeSlashes(checkWindowsPath(_path2), true); + std::vector combined(path1.length() + path2.length() + 2); + if (::PathCombineA(combined.data(), path1.c_str(), path2.c_str()) != NULL) + { + path = sanitizeSlashes(checkWindowsPath(std::string(combined.data()))); + } + else + { + path = sanitizeSlashes(checkWindowsPath(separator(path1) + path2)); + } +#endif // _WIN32 + return path; +} + +std::string getSharePath() +{ + return sdf::joinPaths( + getInstallPrefix(), CMAKE_INSTALL_RELATIVE_DATAROOTDIR); +} + +} +} diff --git a/src/SDF.cc b/src/SDF.cc index e033aac2b..e090ed097 100644 --- a/src/SDF.cc +++ b/src/SDF.cc @@ -29,6 +29,7 @@ #include "sdf/Console.hh" #include "sdf/Error.hh" #include "sdf/Filesystem.hh" +#include "sdf/InstallationDirectories.hh" #include "sdf/SDFImpl.hh" #include "SDFImplPrivate.hh" #include "sdf/sdf_config.h" @@ -47,11 +48,15 @@ std::string SDF::version = SDF_VERSION; // NOLINT(runtime/string) std::string sdfSharePath() { -#ifdef SDF_SHARE_PATH - if (std::string(SDF_SHARE_PATH) != "/") - return SDF_SHARE_PATH; -#endif - return ""; + std::string sharePath = sdf::getSharePath(); + if (sharePath != "/") + { + return sharePath; + } + else + { + return ""; + } } ///////////////////////////////////////////////// diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index b7d846e95..e7e4b2c93 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -77,6 +77,7 @@ endif() gz_build_tests(TYPE ${TEST_TYPE} SOURCES ${tests} INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test + ENVIRONMENT SDF_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index c809bf32e..fe3e17931 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -4,4 +4,7 @@ set(tests parser_urdf.cc ) -gz_build_tests(TYPE ${TEST_TYPE} SOURCES ${tests} INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test) +gz_build_tests(TYPE ${TEST_TYPE} + SOURCES ${tests} + INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/test + ENVIRONMENT SDF_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) From c8c4a6ff2764e7aec02019f909ea9039afecd25b Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 18 Aug 2024 14:11:28 -0700 Subject: [PATCH 37/47] Add _state suffix to //state subelements (#1455) Also move definition of //joint_state to its own file and include joint_state.sdf from model_state.sdf and state.sdf for //world/joint states. Signed-off-by: Steve Peters --- Migration.md | 17 ++++++++++ sdf/1.12/CMakeLists.txt | 1 + sdf/1.12/joint_state.sdf | 19 +++++++++++ sdf/1.12/light_state.sdf | 7 ++-- sdf/1.12/link_state.sdf | 10 ++++-- sdf/1.12/model_state.sdf | 26 +++++---------- sdf/1.12/state.sdf | 14 ++++++-- test/integration/frame.cc | 24 +++++++------- test/integration/nested_model.cc | 57 +++++++++++++++++--------------- 9 files changed, 111 insertions(+), 64 deletions(-) create mode 100644 sdf/1.12/joint_state.sdf diff --git a/Migration.md b/Migration.md index aedddd29b..341816cf2 100644 --- a/Migration.md +++ b/Migration.md @@ -628,6 +628,23 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. ## SDFormat specification 1.11 to 1.12 +### Modifications + +1. **state.sdf**, **model_state.sdf**, **joint_state.sdf**, **link_state.sdf**, + **light_state.sdf**: A `_state` suffix has been added to state element names + to match the `.sdf` file names and for consistency. + + `//state/light` renamed to `//state/light_state` + + `//state/model` renamed to `//state/model_state` + + `//state/model/joint` renamed to `//state/model_state/joint_state` + + `//state/model/light` renamed to `//state/model_state/light_state` + + `//state/model/link` renamed to `//state/model_state/link_state` + + `//state/model/model` renamed to `//state/model_state/model_state` + + `//state/model/link/collision` renamed to `//state/model_state/link_state/collision_state` + +1. **state.sdf**: `//state/joint_state` has been added to represent the state of a + `//world/joint` and `//state/insertions/joint` can represent inserted + `//world/joint` elements. + ## SDFormat specification 1.10 to 1.11 ### Additions diff --git a/sdf/1.12/CMakeLists.txt b/sdf/1.12/CMakeLists.txt index 08b4bf240..82d9ebf71 100644 --- a/sdf/1.12/CMakeLists.txt +++ b/sdf/1.12/CMakeLists.txt @@ -26,6 +26,7 @@ set (sdfs imu.sdf inertial.sdf joint.sdf + joint_state.sdf lidar.sdf light.sdf light_state.sdf diff --git a/sdf/1.12/joint_state.sdf b/sdf/1.12/joint_state.sdf new file mode 100644 index 000000000..2b64d832c --- /dev/null +++ b/sdf/1.12/joint_state.sdf @@ -0,0 +1,19 @@ + + + + The joint state element encapsulates variables within a joint that may + change over time, currently limited to the joint angle. + + + + Name of the joint + + + + + Index of the axis. + + + Angle of an axis + + diff --git a/sdf/1.12/light_state.sdf b/sdf/1.12/light_state.sdf index 673efcdb4..54bebb02f 100644 --- a/sdf/1.12/light_state.sdf +++ b/sdf/1.12/light_state.sdf @@ -1,6 +1,9 @@ - - Light state + + + The light state element encapsulates variables within a light that may + change over time, currently limited to its pose. + Name of the light diff --git a/sdf/1.12/link_state.sdf b/sdf/1.12/link_state.sdf index 28fff465d..e59d9e0f1 100644 --- a/sdf/1.12/link_state.sdf +++ b/sdf/1.12/link_state.sdf @@ -1,6 +1,10 @@ - - Link state + + + The link state element encapsulates variables within a link that may + change over time, including pose, velocity, acceleration, applied wrench, + and the state of attached collisions. + Name of the link @@ -27,7 +31,7 @@ - + Collision state diff --git a/sdf/1.12/model_state.sdf b/sdf/1.12/model_state.sdf index 3fd296ff2..cbbbfd0de 100644 --- a/sdf/1.12/model_state.sdf +++ b/sdf/1.12/model_state.sdf @@ -1,28 +1,18 @@ - - Model state + + + The model state element encapsulates variables within a model that may + change over time, including object poses, the states of its nested models + and links and joints, and changes in model scale. + Name of the model - - Joint angle + - - Name of the joint - - - - - Index of the axis. - - - Angle of an axis - - - - + A nested model state element Name of the model. diff --git a/sdf/1.12/state.sdf b/sdf/1.12/state.sdf index 6a13e28bf..30fbd4e42 100644 --- a/sdf/1.12/state.sdf +++ b/sdf/1.12/state.sdf @@ -1,5 +1,12 @@ + + The state element encapsulates variables within a world that may change + over time, including object poses, dynamic states such as velocity and + acceleration, a description of objects added to the world, and a list + of objects deleted from the world. + + Name of the world this state applies to @@ -22,13 +29,14 @@ - A list containing the entire description of entities inserted. + A list containing the entire description of entities inserted into the world. + - A list of names of deleted entities/ + A list of names of entities deleted from the world./ The name of a deleted entity. @@ -38,4 +46,6 @@ + + diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 1ed67cd3a..b080ae5d5 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -213,18 +213,18 @@ TEST(Frame, StateFrame) sdfStr << "" << "" << "" - << "" + << "" << " " << " 1 0 2 0 0 0" << " " << " 3 3 9 0 0 0" - << " " + << " " << " 111 3 0 0 0 0" - << " " - << "" - << "" + << " " + << "" + << "" << " 99 0 22 0 0 0" - << "" + << "" << "" << "" << ""; @@ -239,8 +239,8 @@ TEST(Frame, StateFrame) EXPECT_TRUE(worldElem->HasElement("state")); sdf::ElementPtr stateElem = worldElem->GetElement("state"); - EXPECT_TRUE(stateElem->HasElement("model")); - sdf::ElementPtr modelStateElem = stateElem->GetElement("model"); + EXPECT_TRUE(stateElem->HasElement("model_state")); + sdf::ElementPtr modelStateElem = stateElem->GetElement("model_state"); // model EXPECT_TRUE(modelStateElem->HasAttribute("name")); @@ -271,8 +271,8 @@ TEST(Frame, StateFrame) } // link - EXPECT_TRUE(modelStateElem->HasElement("link")); - sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link"); + EXPECT_TRUE(modelStateElem->HasElement("link_state")); + sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link_state"); EXPECT_TRUE(linkStateElem->HasAttribute("name")); EXPECT_EQ(linkStateElem->Get("name"), "my_link"); @@ -286,8 +286,8 @@ TEST(Frame, StateFrame) gz::math::Pose3d(111, 3, 0, 0, 0, 0)); } - EXPECT_TRUE(stateElem->HasElement("light")); - sdf::ElementPtr lightStateElem = stateElem->GetElement("light"); + EXPECT_TRUE(stateElem->HasElement("light_state")); + sdf::ElementPtr lightStateElem = stateElem->GetElement("light_state"); // light EXPECT_TRUE(lightStateElem->HasAttribute("name")); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 0c63cbc43..813d04021 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -114,33 +114,33 @@ TEST(NestedModel, State) sdfStr << "" << "" << "" - << "" + << "" << " 0 0 0.5 0 0 0" - << " " + << " " << " 0 0 0.5 0 0 0" << " 0.001 0 0 0 0 0" << " 0 0.006121 0 0.012288 0 0.001751" << " 0 0.006121 0 0 0 0" - << " " - << " " + << " " + << " " << " 1 0 0.5 0 0 0" - << " " + << " " << " 1.25 0 0.5 0 0 0" << " 0 -0.001 0 0 0 0" << " 0 0.000674 0 -0.001268 0 0" << " 0 0.000674 0 0 0 0" - << " " - << " " + << " " + << " " << " 1 1 0.5 0 0 0" - << " " + << " " << " 1.25 1 0.5 0 0 0" << " 0 0 0.001 0 0 0" << " 0 0 0 0 0 0" << " 0 0 0 0 0 0" - << " " - << " " - << " " - << "" + << " " + << " " + << " " + << "" << "" << "" << ""; @@ -154,9 +154,9 @@ TEST(NestedModel, State) sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world"); EXPECT_TRUE(worldElem->HasElement("state")); sdf::ElementPtr stateElem = worldElem->GetElement("state"); - EXPECT_TRUE(stateElem->HasElement("model")); + EXPECT_TRUE(stateElem->HasElement("model_state")); - sdf::ElementPtr modelStateElem = stateElem->GetElement("model"); + sdf::ElementPtr modelStateElem = stateElem->GetElement("model_state"); // model sdf EXPECT_TRUE(modelStateElem->HasAttribute("name")); @@ -164,11 +164,12 @@ TEST(NestedModel, State) EXPECT_TRUE(modelStateElem->HasElement("pose")); EXPECT_EQ(modelStateElem->Get("pose"), gz::math::Pose3d(0, 0, 0.5, 0, 0, 0)); - EXPECT_TRUE(!modelStateElem->HasElement("joint")); + EXPECT_FALSE(modelStateElem->HasElement("joint")); + EXPECT_FALSE(modelStateElem->HasElement("joint_state")); // link sdf - EXPECT_TRUE(modelStateElem->HasElement("link")); - sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link"); + EXPECT_TRUE(modelStateElem->HasElement("link_state")); + sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link_state"); EXPECT_TRUE(linkStateElem->HasAttribute("name")); EXPECT_EQ(linkStateElem->Get("name"), "link_00"); EXPECT_TRUE(linkStateElem->HasElement("pose")); @@ -185,20 +186,21 @@ TEST(NestedModel, State) gz::math::Pose3d(0, 0.006121, 0, 0, 0, 0)); // nested model sdf - EXPECT_TRUE(modelStateElem->HasElement("model")); + EXPECT_TRUE(modelStateElem->HasElement("model_state")); sdf::ElementPtr nestedModelStateElem = - modelStateElem->GetElement("model"); + modelStateElem->GetElement("model_state"); EXPECT_TRUE(nestedModelStateElem->HasAttribute("name")); EXPECT_EQ(nestedModelStateElem->Get("name"), "model_01"); EXPECT_TRUE(nestedModelStateElem->HasElement("pose")); EXPECT_EQ(nestedModelStateElem->Get("pose"), gz::math::Pose3d(1, 0, 0.5, 0, 0, 0)); - EXPECT_TRUE(!nestedModelStateElem->HasElement("joint")); + EXPECT_FALSE(nestedModelStateElem->HasElement("joint")); + EXPECT_FALSE(nestedModelStateElem->HasElement("joint_state")); // nested model's link sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("link")); + EXPECT_TRUE(nestedModelStateElem->HasElement("link_state")); sdf::ElementPtr nestedLinkStateElem = - nestedModelStateElem->GetElement("link"); + nestedModelStateElem->GetElement("link_state"); EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name")); EXPECT_EQ(nestedLinkStateElem->Get("name"), "link_01"); EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); @@ -215,18 +217,19 @@ TEST(NestedModel, State) gz::math::Pose3d(0, 0.000674, 0, 0, 0, 0)); // double nested model sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("model")); - nestedModelStateElem = nestedModelStateElem->GetElement("model"); + EXPECT_TRUE(nestedModelStateElem->HasElement("model_state")); + nestedModelStateElem = nestedModelStateElem->GetElement("model_state"); EXPECT_TRUE(nestedModelStateElem->HasAttribute("name")); EXPECT_EQ(nestedModelStateElem->Get("name"), "model_02"); EXPECT_TRUE(nestedModelStateElem->HasElement("pose")); EXPECT_EQ(nestedModelStateElem->Get("pose"), gz::math::Pose3d(1, 1, 0.5, 0, 0, 0)); - EXPECT_TRUE(!nestedModelStateElem->HasElement("joint")); + EXPECT_FALSE(nestedModelStateElem->HasElement("joint")); + EXPECT_FALSE(nestedModelStateElem->HasElement("joint_state")); // double nested model's link sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("link")); - nestedLinkStateElem = nestedModelStateElem->GetElement("link"); + EXPECT_TRUE(nestedModelStateElem->HasElement("link_state")); + nestedLinkStateElem = nestedModelStateElem->GetElement("link_state"); EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name")); EXPECT_EQ(nestedLinkStateElem->Get("name"), "link_02"); EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); From 01c28aacec7356d967c45698cd64275aef3dc996 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 19 Aug 2024 15:46:31 -0700 Subject: [PATCH 38/47] Decouple linking to shlwapi from BUILD_TESTING (#1468) The windows builds were broken with -DBUILD_TESTING=OFF, so move the target_link_libraries call outside the `if (BUILD_TESTING)` logical block. Signed-off-by: Steve Peters --- src/CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 57dab8757..6a7be941e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -49,6 +49,11 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE TINYXML2::TINYXML2) +if (WIN32) + target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} + PRIVATE shlwapi) +endif() + if (USE_INTERNAL_URDF) target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/urdf) @@ -109,11 +114,6 @@ if (BUILD_TESTING) -DGZ_SDFORMAT_STATIC_DEFINE ) - if(WIN32) - target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} - PRIVATE shlwapi) - endif() - gz_build_tests( TYPE UNIT SOURCES ${gtest_sources} From ba1bf8fedc0b6063d4312ad312e515e28f71cfb9 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 20 Aug 2024 13:52:19 -0700 Subject: [PATCH 39/47] Update joinPaths implementation in InstallationDirectories (#1469) Signed-off-by: Ian Chen --- src/CMakeLists.txt | 5 -- src/InstallationDirectories.cc | 89 +++++++++++++--------------------- 2 files changed, 34 insertions(+), 60 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6a7be941e..3ffa05625 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -49,11 +49,6 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE TINYXML2::TINYXML2) -if (WIN32) - target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} - PRIVATE shlwapi) -endif() - if (USE_INTERNAL_URDF) target_include_directories(${PROJECT_LIBRARY_TARGET_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/urdf) diff --git a/src/InstallationDirectories.cc b/src/InstallationDirectories.cc index 167df10eb..a260a32b4 100644 --- a/src/InstallationDirectories.cc +++ b/src/InstallationDirectories.cc @@ -15,15 +15,12 @@ * */ +#include #include #include #include -#ifdef _WIN32 -#include -#endif - namespace sdf { inline namespace SDF_VERSION_NAMESPACE { @@ -75,70 +72,52 @@ std::string checkWindowsPath(const std::string _path) } // Function imported from -// https://github.com/gazebosim/gz-common/blob/ignition-common4_4.6.2/src/Filesystem.cc#L256 +// https://github.com/gazebosim/gz-common/blob/gz-common5_5.6.0/src/Filesystem.cc#L142 std::string joinPaths(const std::string &_path1, const std::string &_path2) { + namespace fs = std::filesystem; + fs::path p1{_path1}; + fs::path p2{_path2}; - /// This function is used to avoid duplicated path separators at the - /// beginning/end of the string, and between the two paths being joined. - /// \param[in] _path This is the string to sanitize. - /// \param[in] _stripLeading True if the leading separator should be - /// removed. - auto sanitizeSlashes = [](const std::string &_path, - bool _stripLeading = false) + if (p1.empty()) { - // Shortcut - if (_path.empty()) - return _path; + p1 = std::string{fs::path::preferred_separator}; + } - std::string result = _path; + bool is_url = false; - // Use the appropriate character for each platform. -#ifndef _WIN32 - char replacement = '/'; -#else - char replacement = '\\'; -#endif + if (_path1.find("://") == std::string::npos) + p1 = p1.lexically_normal(); + else + is_url = true; + + // TODO(mjcarroll) Address the case that path2 is also a URI. + // It's likely not a valid scenario, but not currently covered by our test + // suite and doesn't return an error. + if (_path2.find("://") == std::string::npos) + p2 = p2.lexically_normal(); + else + is_url = true; + + if (p2.string()[0] == fs::path::preferred_separator) + { + p2 = fs::path{p2.string().substr(1)}; + } + + auto ret = (p1 / p2); - // Sanitize the start of the path. - size_t index = 0; - size_t leadingIndex = _stripLeading ? 0 : 1; - for (; index < result.length() && result[index] == replacement; ++index) - { - } - if (index > leadingIndex) - result.erase(leadingIndex, index-leadingIndex); - - // Sanitize the end of the path. - index = result.length()-1; - for (; index < result.length() && result[index] == replacement; --index) - { - } - index += 1; - if (index < result.length()-1) - result.erase(index+1); - return result; - }; - - std::string path; -#ifndef _WIN32 - path = sanitizeSlashes(sanitizeSlashes(separator(_path1)) + - sanitizeSlashes(_path2, true)); -#else // _WIN32 - std::string path1 = sanitizeSlashes(checkWindowsPath(_path1)); - std::string path2 = sanitizeSlashes(checkWindowsPath(_path2), true); - std::vector combined(path1.length() + path2.length() + 2); - if (::PathCombineA(combined.data(), path1.c_str(), path2.c_str()) != NULL) + if (is_url) { - path = sanitizeSlashes(checkWindowsPath(std::string(combined.data()))); + std::string path = ret.string(); + std::replace(path.begin(), path.end(), + static_cast(fs::path::preferred_separator), '/'); + return path; } else { - path = sanitizeSlashes(checkWindowsPath(separator(path1) + path2)); + return ret.lexically_normal().string(); } -#endif // _WIN32 - return path; } std::string getSharePath() From 51bfd3b3b8b76d02b491c18fa2d88ea64a32b351 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 24 Aug 2024 17:30:57 -0700 Subject: [PATCH 40/47] Ionic Changelog (#1471) Signed-off-by: Steve Peters --- Changelog.md | 53 +++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index 879b6052c..0d74e90df 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,57 @@ ## libsdformat 15.X -### libsdformat 15.X.X (202X-XX-XX) +### libsdformat 15.0.0 (2024-09-XX) + +1. Spec 1.12: add `_state` suffix to //state subelements + * [Pull request #1455](https://github.com/gazebosim/sdformat/pull/1455) + +1. Add optional binary relocatability + * [Pull request #1414](https://github.com/gazebosim/sdformat/pull/1414) + * [Pull request #1468](https://github.com/gazebosim/sdformat/pull/1468) + * [Pull request #1469](https://github.com/gazebosim/sdformat/pull/1469) + +1. FrameSemantics: fix NullVertex warnings + * [Pull request #1460](https://github.com/gazebosim/sdformat/pull/1460) + * [Pull request #1459](https://github.com/gazebosim/sdformat/pull/1459) + * [Pull request #1458](https://github.com/gazebosim/sdformat/pull/1458) + +1. Remove deprecated APIs + * [Pull request #1456](https://github.com/gazebosim/sdformat/pull/1456) + +1. Spec 1.12: add `//sensor/frame_id` + * [Pull request #1454](https://github.com/gazebosim/sdformat/pull/1454) + +1. Disable latex and class hierarchy generation + * [Pull request #1447](https://github.com/gazebosim/sdformat/pull/1447) + +1. Print auto inertial values with `gz sdf --print --expand-auto-inertials` + * [Pull request #1422](https://github.com/gazebosim/sdformat/pull/1422) + +1. Enable 24.04 CI, remove distutils dependency + * [Pull request #1408](https://github.com/gazebosim/sdformat/pull/1408) + +1. Change behavior of `Param::Get` + * [Pull request #1397](https://github.com/gazebosim/sdformat/pull/1397) + +1. Parse kinematic property in Link, expand spec documentation of property + * [Pull request #1390](https://github.com/gazebosim/sdformat/pull/1390) + * [Pull request #1399](https://github.com/gazebosim/sdformat/pull/1399) + * [Pull request #1462](https://github.com/gazebosim/sdformat/pull/1462) + +1. Copy 1.11 spec to 1.12 for libsdformat15 + * [Pull request #1375](https://github.com/gazebosim/sdformat/pull/1375) + +1. Fix find Python3 logic and macOS workflow + * [Pull request #1367](https://github.com/gazebosim/sdformat/pull/1367) + +1. Remove `HIDE_SYMBOLS_BY_DEFAULT`: replace by a default configuration in gz-cmake. + * [Pull request #1355](https://github.com/gazebosim/sdformat/pull/1355) + +1. Dependency version bumps in ionic: use gz-cmake4, gz-utils3, gz-math8 + * [Pull request #1340](https://github.com/gazebosim/sdformat/pull/1340) + +1. Bump major version to 15 + * [Pull request #1338](https://github.com/gazebosim/sdformat/pull/1338) ## libsdformat 14.X From 39292269626ac2aad76d217d69517a780c718fb0 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 26 Aug 2024 15:42:03 -0700 Subject: [PATCH 41/47] README: update badges for sdf15 (#1472) Signed-off-by: Steve Peters --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 373f69fee..2dbbfeffd 100644 --- a/README.md +++ b/README.md @@ -10,10 +10,10 @@ Note: The branch name in the codecov URL & library version should be updated whe --> Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/gazebosim/sdformat/tree/main/graph/badge.svg)](https://codecov.io/gh/gazebosim/sdformat/tree/main) -Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-jammy-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-jammy-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/sdformat-ci-main-homebrew-amd64) -Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-main-win)](https://build.osrfoundation.org/job/sdformat-main-win) +Test coverage | [![codecov](https://codecov.io/gh/gazebosim/sdformat/tree/sdf15/graph/badge.svg)](https://codecov.io/gh/gazebosim/sdformat/tree/sdf15) +Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-sdf15-noble-amd64)](https://build.osrfoundation.org/job/sdformat-ci-sdf15-noble-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-ci-sdf15-homebrew-amd64)](https://build.osrfoundation.org/job/sdformat-ci-sdf15-homebrew-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=sdformat-sdf15-win)](https://build.osrfoundation.org/job/sdformat-sdf15-win) SDFormat is an XML file format that describes environments, objects, and robots From cba2108d1bebf694f73a2430143c02ea32e71f85 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 26 Aug 2024 19:07:30 -0700 Subject: [PATCH 42/47] Prepare for Ionic prerelease (#1473) * Bump to 15.0.0~pre1 --------- Signed-off-by: Steve Peters Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f1eed1eaa..be1d5c3d5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,7 @@ if (BUILD_SDF) gz_configure_project( NO_PROJECT_PREFIX REPLACE_INCLUDE_PATH sdf - VERSION_SUFFIX) + VERSION_SUFFIX pre1) ################################################# # Find tinyxml2. diff --git a/Changelog.md b/Changelog.md index 0d74e90df..1e073ee0d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,14 @@ ### libsdformat 15.0.0 (2024-09-XX) +1. **Baseline:** this includes all changes from 14.5.0 and earlier. + +1. README: update badges for sdf15 + * [Pull request #1472](https://github.com/gazebosim/sdformat/pull/1472) + +1. Ionic Changelog + * [Pull request #1471](https://github.com/gazebosim/sdformat/pull/1471) + 1. Spec 1.12: add `_state` suffix to //state subelements * [Pull request #1455](https://github.com/gazebosim/sdformat/pull/1455) @@ -27,6 +35,10 @@ 1. Print auto inertial values with `gz sdf --print --expand-auto-inertials` * [Pull request #1422](https://github.com/gazebosim/sdformat/pull/1422) +1. Add cone shape to SDFormat spec + * [Pull request #1418](https://github.com/gazebosim/sdformat/pull/1418) + * [Pull request #1434](https://github.com/gazebosim/sdformat/pull/1434) + 1. Enable 24.04 CI, remove distutils dependency * [Pull request #1408](https://github.com/gazebosim/sdformat/pull/1408) @@ -38,6 +50,11 @@ * [Pull request #1399](https://github.com/gazebosim/sdformat/pull/1399) * [Pull request #1462](https://github.com/gazebosim/sdformat/pull/1462) +1. Spec 1.11+: add `//mesh/@optimization`, `//mesh/convex_decomposition` + * [Pull request #1382](https://github.com/gazebosim/sdformat/pull/1382) + * [Pull request #1386](https://github.com/gazebosim/sdformat/pull/1386) + * [Pull request #1403](https://github.com/gazebosim/sdformat/pull/1403) + 1. Copy 1.11 spec to 1.12 for libsdformat15 * [Pull request #1375](https://github.com/gazebosim/sdformat/pull/1375) From c306c57186afd97454b68a9e9c4f0810e4e9b649 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Tue, 27 Aug 2024 21:32:34 +0200 Subject: [PATCH 43/47] Fix symbol checking test when compiled with debug symbols (#1474) * Fix symbol checking test The test that checks for prefixed binary symbols was broken when compiled with DebWithRelInfo since it was checking debugging symbols that broke the heuristics used. The commit fixes it doing a couple of actions: - Include the length of the namespace sdf: 3sdf - Check only dynamic symbols being exported Signed-off-by: Jose Luis Rivero * Update test/integration/all_symbols_have_version.bash.in Co-authored-by: Steve Peters Signed-off-by: Jose Luis Rivero --------- Signed-off-by: Jose Luis Rivero Co-authored-by: Steve Peters (cherry picked from commit 904706c2c02c5c333e916717e0a4ea6b3c74947e) --- test/integration/all_symbols_have_version.bash.in | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/test/integration/all_symbols_have_version.bash.in b/test/integration/all_symbols_have_version.bash.in index d69a58442..94912d34d 100644 --- a/test/integration/all_symbols_have_version.bash.in +++ b/test/integration/all_symbols_have_version.bash.in @@ -4,7 +4,13 @@ LIBPATH=$1 VERSIONED_NS=v@PROJECT_VERSION_MAJOR@ # Sanity check - there should be at least one symbol -NUM_SYMBOLS=$(nm $LIBPATH | grep -e "sdf" | wc -l) + +# nm options: +# -D to get only dynamic symbols exported +# 3 before the sdf is used by +# mangled symbols in C++ to check for the +# sdf namespace +NUM_SYMBOLS=$(nm -D $LIBPATH | grep -e "3sdf" | wc -l) if [ $NUM_SYMBOLS -eq 0 ] then @@ -13,7 +19,7 @@ then fi # There must be no unversioned symbols -UNVERSIONED_SYMBOLS=$(nm $LIBPATH | grep -e "sdf" | grep -e "$VERSIONED_NS" -v) +UNVERSIONED_SYMBOLS=$(nm -D $LIBPATH | grep -e "3sdf" | grep -e "$VERSIONED_NS" -v) UNVERSIONED_SYMBOL_CHARS=$(printf "$UNVERSIONED_SYMBOLS" | wc -m) if [ $UNVERSIONED_SYMBOL_CHARS -ne 0 ] From d9b26df3a029be399e72e21daee482f018009981 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Aug 2024 15:42:32 -0700 Subject: [PATCH 44/47] Prepare for 15.0.0~pre2, update changelog (#1477) Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index be1d5c3d5..14e63da0c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,7 @@ if (BUILD_SDF) gz_configure_project( NO_PROJECT_PREFIX REPLACE_INCLUDE_PATH sdf - VERSION_SUFFIX pre1) + VERSION_SUFFIX pre2) ################################################# # Find tinyxml2. diff --git a/Changelog.md b/Changelog.md index 1e073ee0d..246b22eab 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,9 @@ 1. **Baseline:** this includes all changes from 14.5.0 and earlier. +1. Fix symbol checking test when compiled with debug symbols + * [Pull request #1474](https://github.com/gazebosim/sdformat/pull/1474) + 1. README: update badges for sdf15 * [Pull request #1472](https://github.com/gazebosim/sdformat/pull/1472) From c2c4f918ea57c9a274407b65d99ca22b9a69274c Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 28 Aug 2024 13:54:04 -0700 Subject: [PATCH 45/47] Spec 1.12: link_state, joint_state changes (#1461) This makes several changes to the specification for the //state element in SDFormat 1.12. * //link_state changes: use pairs of vector3 datatypes to represent twist, spatial acceleration, and wrenches instead of a pose datatype, which distorts the angular values by interpreting them as Euler angles. The new elements for velocity and acceleration have names starting with linear_ or angular_, while the replacement for wrench is force and torque. * //joint_state changes: refactor //joint_state/angle and //joint_state/angle/@axis to //joint_state/axis_state/position and //joint_state/axis2_state/position. Also add velocity, acceleration, and effort to joint axis states. * degrees attribute: add a boolean degrees attribute to the //link_state/angular_* elements and joint axis position, velocity, and acceleration elements. When true, this causes data with units involving radians to interpret it as involving degrees instead. The attribute does not affect prismatic joints. * Specify model state within a model using //model/model_state or when including a model with //model/include/model_state and //state/include/model_state. Signed-off-by: Steve Peters --- Migration.md | 41 +++++++++++ sdf/1.12/joint_state.sdf | 116 ++++++++++++++++++++++++++++- sdf/1.12/link_state.sdf | 54 +++++++++++++- sdf/1.12/model.sdf | 3 + sdf/1.12/world.sdf | 1 + test/integration/nested_model.cc | 121 ++++++++++++++++++++++--------- 6 files changed, 296 insertions(+), 40 deletions(-) diff --git a/Migration.md b/Migration.md index 341816cf2..5d036b71b 100644 --- a/Migration.md +++ b/Migration.md @@ -628,6 +628,33 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. ## SDFormat specification 1.11 to 1.12 +### Additions + +1. **joint_state.sdf**: + + `//joint_state/axis_state/position` + + `//joint_state/axis_state/velocity` + + `//joint_state/axis_state/acceleration` + + `//joint_state/axis_state/effort` + + `//joint_state/axis2_state/position` + + `//joint_state/axis2_state/velocity` + + `//joint_state/axis2_state/acceleration` + + `//joint_state/axis2_state/effort` + +1. **link_state.sdf**: + + `//link_state/linear_velocity` + + `//link_state/angular_velocity` + + `//link_state/linear_acceleration` + + `//link_state/angular_acceleration` + + `//link_state/force` + + `//link_state/torque` + +1. **model.sdf**: + + `//model/model_state` + + `//model/include/model_state` + +1. **world.sdf**: + + `//world/include/model_state` + ### Modifications 1. **state.sdf**, **model_state.sdf**, **joint_state.sdf**, **link_state.sdf**, @@ -645,6 +672,20 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. `//world/joint` and `//state/insertions/joint` can represent inserted `//world/joint` elements. +### Deprecations + +1. **joint_state.sdf**: + + `//joint_state/angle` is deprecated in favor of `//joint_state/axis_state/position` + and `//joint_state/axis2_state/position`. + +1. **link_state.sdf**: + + `//link_state/velocity` is deprecated in favor of `//link_state/angular_velocity` + and `//link_state/linear_velocity`. + + `//link_state/acceleration` is deprecated in favor of `//link_state/angular_acceleration` + and `//link_state/linear_acceleration`. + + `//link_state/wrench` is deprecated in favor of `//link_state/torque` + and `//link_state/force`. + ## SDFormat specification 1.10 to 1.11 ### Additions diff --git a/sdf/1.12/joint_state.sdf b/sdf/1.12/joint_state.sdf index 2b64d832c..096b56965 100644 --- a/sdf/1.12/joint_state.sdf +++ b/sdf/1.12/joint_state.sdf @@ -9,11 +9,125 @@ Name of the joint - + Index of the axis. Angle of an axis + + + + Contains the state of the first joint axis. + + + + The position of the first joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint position is expressed in units of degrees [deg], + otherwise it is expressed in radians [rad]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters [m] regardless of the value of + this attribute. + + + + + + The velocity of the first joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint velocity is expressed in units of degrees per + second [deg/s], otherwise it is expressed in radians per second + [rad/s]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters per second [m/s] regardless of + the value of this attribute. + + + + + + The acceleration of the first joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint acceleration is expressed in units of degrees per + second per second [deg/s^2], otherwise it is expressed in radians per + second per second [rad/s^2]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters per second per second [m/s^2] + regardless of the value of this attribute. + + + + + + The effort applied at the first joint axis. + + + + + + Contains the state of the second joint axis. + + + + The position of the second joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint position is expressed in units of degrees [deg], + otherwise it is expressed in radians [rad]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters [m] regardless of the value of + this attribute. + + + + + + The velocity of the second joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint velocity is expressed in units of degrees per + second [deg/s], otherwise it is expressed in radians per second + [rad/s]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters per second [m/s] regardless of + the value of this attribute. + + + + + + The acceleration of the second joint axis. + + + + If this is a rotational axis and this attribute is true, + the joint acceleration is expressed in units of degrees per + second per second [deg/s^2], otherwise it is expressed in radians per + second per second [rad/s^2]. + If this axis is translational (such as a prismatic joint), the + units will be interpreted in meters per second per second [m/s^2] + regardless of the value of this attribute. + + + + + + The effort applied at the second joint axis. + + diff --git a/sdf/1.12/link_state.sdf b/sdf/1.12/link_state.sdf index e59d9e0f1..d086e4856 100644 --- a/sdf/1.12/link_state.sdf +++ b/sdf/1.12/link_state.sdf @@ -10,21 +10,69 @@ Name of the link - + + Angular velocity of the link frame relative to the world frame. + + + + + If true, the angular velocity is expressed in units of degrees per + second [deg/s], otherwise it is expressed in radians per second [rad/s]. + + + + + + Linear velocity of the link frame relative to the world frame. + + + + Velocity of the link. The x, y, z components of the pose correspond to the linear velocity of the link, and the roll, pitch, yaw components correspond to the angular velocity of the link - + + + Angular acceleration of the link frame relative to the world frame. + + + + + If true, the angular acceleration is expressed in units of degrees per + second per second [deg/s^2], otherwise it is expressed in radians per + second per second [rad/s^2]. + + + + + + + Linear acceleration of the link frame relative to the world frame. + + + + Acceleration of the link. The x, y, z components of the pose correspond to the linear acceleration of the link, and the roll, pitch, yaw components correspond to the angular acceleration of the link - + + Torque acting on the link relative to the world frame. + + + + + + Force acting on the link at the link frame relative to the world frame. + + + + Force and torque applied to the link. The x, y, z components of the pose correspond to the force applied to the link, and the roll, pitch, yaw components correspond to the torque applied to the link diff --git a/sdf/1.12/model.sdf b/sdf/1.12/model.sdf index 57dc83a01..de8b43130 100644 --- a/sdf/1.12/model.sdf +++ b/sdf/1.12/model.sdf @@ -62,6 +62,7 @@ URI to a resource, such as a model + @@ -85,6 +86,8 @@ + + If set to true, all links in the model will be affected by the wind. Can be overriden by the link wind property. diff --git a/sdf/1.12/world.sdf b/sdf/1.12/world.sdf index 6235e193b..d39ad8819 100644 --- a/sdf/1.12/world.sdf +++ b/sdf/1.12/world.sdf @@ -40,6 +40,7 @@ Override the static value of the included entity. + diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 813d04021..54859c95e 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -118,25 +118,34 @@ TEST(NestedModel, State) << " 0 0 0.5 0 0 0" << " " << " 0 0 0.5 0 0 0" - << " 0.001 0 0 0 0 0" - << " 0 0.006121 0 0.012288 0 0.001751" - << " 0 0.006121 0 0 0 0" + << " 0.001 0 0" + << " -0.1 5.0 -0.1" + << " 0 0.006121 0" + << " 0.012288 0 0.001751" + << " 0 0.006121 0" + << " 0 0 0" << " " << " " << " 1 0 0.5 0 0 0" << " " << " 1.25 0 0.5 0 0 0" - << " 0 -0.001 0 0 0 0" - << " 0 0.000674 0 -0.001268 0 0" - << " 0 0.000674 0 0 0 0" + << " 0 -0.001 0" + << " 0 0 0" + << " 0 0.000674 0" + << " -0.001268 0 0" + << " 0 0.000674 0" + << " 0 0 0" << " " << " " << " 1 1 0.5 0 0 0" << " " << " 1.25 1 0.5 0 0 0" - << " 0 0 0.001 0 0 0" - << " 0 0 0 0 0 0" - << " 0 0 0 0 0 0" + << " 0 0 0.001" + << " 0 0 0" + << " 0 0 0" + << " 0 0 0" + << " 0 0 0" + << " 0 0 0" << " " << " " << " " @@ -175,15 +184,27 @@ TEST(NestedModel, State) EXPECT_TRUE(linkStateElem->HasElement("pose")); EXPECT_EQ(linkStateElem->Get("pose"), gz::math::Pose3d(0, 0, 0.5, 0, 0, 0)); - EXPECT_TRUE(linkStateElem->HasElement("velocity")); - EXPECT_EQ(linkStateElem->Get("velocity"), - gz::math::Pose3d(0.001, 0, 0, 0, 0, 0)); - EXPECT_TRUE(linkStateElem->HasElement("acceleration")); - EXPECT_EQ(linkStateElem->Get("acceleration"), - gz::math::Pose3d(0, 0.006121, 0, 0.012288, 0, 0.001751)); - EXPECT_TRUE(linkStateElem->HasElement("wrench")); - EXPECT_EQ(linkStateElem->Get("wrench"), - gz::math::Pose3d(0, 0.006121, 0, 0, 0, 0)); + EXPECT_FALSE(linkStateElem->HasElement("velocity")); + EXPECT_TRUE(linkStateElem->HasElement("angular_velocity")); + EXPECT_TRUE(linkStateElem->HasElement("linear_velocity")); + EXPECT_EQ(linkStateElem->Get("linear_velocity"), + gz::math::Vector3d(0.001, 0, 0)); + EXPECT_EQ(linkStateElem->Get("angular_velocity"), + gz::math::Vector3d(-0.1, 5.0, -0.1)); + EXPECT_FALSE(linkStateElem->HasElement("acceleration")); + EXPECT_TRUE(linkStateElem->HasElement("angular_acceleration")); + EXPECT_TRUE(linkStateElem->HasElement("linear_acceleration")); + EXPECT_EQ(linkStateElem->Get("linear_acceleration"), + gz::math::Vector3d(0, 0.006121, 0)); + EXPECT_EQ(linkStateElem->Get("angular_acceleration"), + gz::math::Vector3d(0.012288, 0, 0.001751)); + EXPECT_FALSE(linkStateElem->HasElement("wrench")); + EXPECT_TRUE(linkStateElem->HasElement("torque")); + EXPECT_TRUE(linkStateElem->HasElement("force")); + EXPECT_EQ(linkStateElem->Get("force"), + gz::math::Vector3d(0, 0.006121, 0)); + EXPECT_EQ(linkStateElem->Get("torque"), + gz::math::Vector3d(0, 0, 0)); // nested model sdf EXPECT_TRUE(modelStateElem->HasElement("model_state")); @@ -206,15 +227,29 @@ TEST(NestedModel, State) EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); EXPECT_EQ(nestedLinkStateElem->Get("pose"), gz::math::Pose3d(1.25, 0, 0.5, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("velocity")); - EXPECT_EQ(nestedLinkStateElem->Get("velocity"), - gz::math::Pose3d(0, -0.001, 0, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("acceleration")); - EXPECT_EQ(nestedLinkStateElem->Get("acceleration"), - gz::math::Pose3d(0, 0.000674, 0, -0.001268, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("wrench")); - EXPECT_EQ(nestedLinkStateElem->Get("wrench"), - gz::math::Pose3d(0, 0.000674, 0, 0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("velocity")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("angular_velocity")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("linear_velocity")); + EXPECT_EQ(nestedLinkStateElem->Get("linear_velocity"), + gz::math::Vector3d(0, -0.001, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("angular_velocity"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("acceleration")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("angular_acceleration")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("linear_acceleration")); + EXPECT_EQ( + nestedLinkStateElem->Get("linear_acceleration"), + gz::math::Vector3d(0, 0.000674, 0)); + EXPECT_EQ( + nestedLinkStateElem->Get("angular_acceleration"), + gz::math::Vector3d(-0.001268, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("wrench")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("torque")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("force")); + EXPECT_EQ(nestedLinkStateElem->Get("force"), + gz::math::Vector3d(0, 0.000674, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("torque"), + gz::math::Vector3d(0, 0, 0)); // double nested model sdf EXPECT_TRUE(nestedModelStateElem->HasElement("model_state")); @@ -235,15 +270,29 @@ TEST(NestedModel, State) EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); EXPECT_EQ(nestedLinkStateElem->Get("pose"), gz::math::Pose3d(1.25, 1, 0.5, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("velocity")); - EXPECT_EQ(nestedLinkStateElem->Get("velocity"), - gz::math::Pose3d(0, 0, 0.001, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("acceleration")); - EXPECT_EQ(nestedLinkStateElem->Get("acceleration"), - gz::math::Pose3d(0, 0, 0, 0, 0, 0)); - EXPECT_TRUE(nestedLinkStateElem->HasElement("wrench")); - EXPECT_EQ(nestedLinkStateElem->Get("wrench"), - gz::math::Pose3d(0, 0, 0, 0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("velocity")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("angular_velocity")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("linear_velocity")); + EXPECT_EQ(nestedLinkStateElem->Get("linear_velocity"), + gz::math::Vector3d(0, 0, 0.001)); + EXPECT_EQ(nestedLinkStateElem->Get("angular_velocity"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("acceleration")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("angular_acceleration")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("linear_acceleration")); + EXPECT_EQ( + nestedLinkStateElem->Get("linear_acceleration"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_EQ( + nestedLinkStateElem->Get("angular_acceleration"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_FALSE(nestedLinkStateElem->HasElement("wrench")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("torque")); + EXPECT_TRUE(nestedLinkStateElem->HasElement("force")); + EXPECT_EQ(nestedLinkStateElem->Get("force"), + gz::math::Vector3d(0, 0, 0)); + EXPECT_EQ(nestedLinkStateElem->Get("torque"), + gz::math::Vector3d(0, 0, 0)); } //////////////////////////////////////// From 8b5d5b5e985b0ebe3d5b680b10c3a31a5aae708f Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Fri, 13 Sep 2024 20:22:10 +0200 Subject: [PATCH 46/47] Use colcon for Windows building compilation (#1481) Signed-off-by: Jose Luis Rivero --- README.md | 53 ++++++++++++++++++++++++++++------------------------- 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index 2dbbfeffd..0b6dfc324 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ sudo apt-get update sudo apt install libsdformat<#>-dev libsdformat<#> ``` -Be sure to replace `<#>` with a number value, such as 2 or 3, depending on +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on which version you need, or leave it empty for version 1. ### macOS @@ -82,7 +82,7 @@ Install sdformat: brew install sdformat<#> ``` -Be sure to replace `<#>` with a number value, such as 1 or 2, depending on +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on which version you need. ### Windows @@ -126,7 +126,7 @@ Clone the repository ```sh git clone https://github.com/gazebosim/sdformat -b sdf<#> ``` -Be sure to replace `<#>` with a number value, such as 1 or 2, depending on +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on which version you need. ### Build from Source @@ -168,7 +168,7 @@ Clone the repository ```sh git clone https://github.com/gazebosim/sdformat -b sdf<#> ``` -Be sure to replace `<#>` with a number value, such as 1 or 2, depending on +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on which version you need. Install dependencies @@ -213,34 +213,37 @@ conda activate gz-ws Install prerequisites: ``` -conda install tinyxml2 urdfdom --channel conda-forge +conda install cmake git vcstool colcon-common-extensions ^ +tinyxml2 urdfdom pybind11 -channel conda-forge ``` -Install Gazebo dependencies: +### Getting the sources and building from Source -You can view lists of dependencies: -``` -conda search libsdformat --channel conda-forge --info -``` +Be sure to replace `<#>` with a number value, such as 14 or 15, depending on +which version you need. + +1. Getting the sources -Install dependencies, replacing `<#>` with the desired versions: ``` -conda install libgz-cmake<#> libgz-math<#> libgz-tools<#> libgz-utils<#> --channel conda-forge +mkdir ws\src +cd ws +vcs import src --input https://raw.githubusercontent.com/gazebo-tooling/gazebodistro/master/sdformat<#>.yaml ``` -### Build from Source +2. Build from source -This assumes you have created and activated a Conda environment while installing the Prerequisites. +Note: the Gazebo library dependencies are going to be compiled from source +with sdformat although it should be possible to install them from +conda-forge on stable Gazebo releases using the standard conda install +command. -1. Configure and build - ``` - mkdir build - cd build - cmake .. -DBUILD_TESTING=OFF # Optionally, -DCMAKE_INSTALL_PREFIX=path\to\install - cmake --build . --config Release - ``` +Build the gazebo libraries needed as dependencies (skip testing to speed up the compilation) +using the [colcon](https://colcon.readthedocs.io/en/released/) tool: +``` + colcon build --cmake-args -DBUILD_TESTING=OFF --merge-install --packages-skip sdformat<#> +``` -2. Install - ``` - cmake --install . --config Release - ``` +Build sdformat with its test suite: +``` + colcon build --cmake-args -DBUILD_TESTING=ON --merge-install --packages-select sdformat<#> +``` From 551cb998e8bd1af84e0196b337098ff686ea47a0 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 26 Sep 2024 13:35:45 -0500 Subject: [PATCH 47/47] Prepare for 15.0.0 (#1483) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 14e63da0c..f1eed1eaa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,7 @@ if (BUILD_SDF) gz_configure_project( NO_PROJECT_PREFIX REPLACE_INCLUDE_PATH sdf - VERSION_SUFFIX pre2) + VERSION_SUFFIX) ################################################# # Find tinyxml2. diff --git a/Changelog.md b/Changelog.md index 246b22eab..211119ff2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,9 +1,15 @@ ## libsdformat 15.X -### libsdformat 15.0.0 (2024-09-XX) +### libsdformat 15.0.0 (2024-09-25) 1. **Baseline:** this includes all changes from 14.5.0 and earlier. +1. Use colcon for Windows building compilation + * [Pull request #1481](https://github.com/gazebosim/sdformat/pull/1481) + +1. Spec 1.12: link_state, joint_state changes + * [Pull request #1461](https://github.com/gazebosim/sdformat/pull/1461) + 1. Fix symbol checking test when compiled with debug symbols * [Pull request #1474](https://github.com/gazebosim/sdformat/pull/1474)