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: diff --git a/Changelog.md b/Changelog.md index 44167531a..e14050e4d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,47 @@ ## 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 + * [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 @@ -132,6 +173,50 @@ ## 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 + * [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 @@ -594,6 +679,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 diff --git a/Migration.md b/Migration.md index f339080fa..aedddd29b 100644 --- a/Migration.md +++ b/Migration.md @@ -69,6 +69,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. diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 3f433598d..dfd02f69c 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -408,12 +408,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 Check if this link is kinematic only /// \return true if the link is kinematic only, false otherwise. public: bool Kinematic() const; diff --git a/include/sdf/SDFImpl.hh b/include/sdf/SDFImpl.hh index a868b4b4a..fb7e96ea0 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(); @@ -177,6 +248,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 @@ -193,6 +271,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/include/sdf/Surface.hh b/include/sdf/Surface.hh index 02fd98be5..caec7d0cd 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 GZ_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. + GZ_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. + GZ_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/include/sdf/World.hh b/include/sdf/World.hh index 7e449abab..757f4f45d 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/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index 6103541d1..80892100e 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -84,6 +84,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); @@ -136,6 +137,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/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 be54d3ac0..8d7873bc3 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" @@ -26,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" @@ -46,6 +48,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, @@ -98,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/src/sdf/pyLink.cc b/python/src/sdf/pyLink.cc index 8fd3b6a33..0431cb437 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, @@ -168,6 +178,13 @@ void defineLink(pybind11::object module) .def("set_kinematic", &sdf::Link::SetKinematic, "Set whether this link is kinematic only.") + .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/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/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/src/sdf/pyWorld.cc b/python/src/sdf/pyWorld.cc index 41b930fc4..5eb400630 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.") @@ -98,13 +100,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 +139,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/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/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/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 b4275d038..7068c53d2 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, Cone, 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()) @@ -131,5 +135,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 6eea3f5f4..c124f2f71 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, Cone, Cylinder, Ellipsoid, - Mesh, Plane, Sphere) -from gz_test_deps.math import Vector3d, Vector2d +from gz_test_deps.sdformat import (Element, Error, Geometry, Box, Capsule, Cone, Cylinder, Ellipsoid, + 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 import unittest @@ -124,6 +125,7 @@ def test_capsule(self): self.assertEqual(0.123, geom.capsule_shape().radius()) self.assertEqual(4.56, geom.capsule_shape().length()) + def test_cone(self): geom = Geometry() geom.set_type(sdf.GeometryType.CONE) @@ -138,6 +140,7 @@ def test_cone(self): 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) @@ -185,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() @@ -201,5 +221,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 e7a7a77a2..154b085b9 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,18 @@ 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.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.assertFalse(link.kinematic()) link.set_kinematic(True) self.assertTrue(link.kinematic()) @@ -317,8 +329,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() @@ -380,7 +392,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()) @@ -389,12 +401,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/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() 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() diff --git a/src/Camera.cc b/src/Camera.cc index 184fbcead..88c0e584b 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 e8f62c3fa..bfcda9261 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -217,7 +217,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()); 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/Link.cc b/src/Link.cc index 06de2fff8..19c3569fe 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -87,6 +87,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 True if this link is kinematic only public: bool kinematic = false; @@ -316,6 +319,9 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->enableWind = _sdf->Get("enable_wind", this->dataPtr->enableWind).first; + this->dataPtr->enableGravity = _sdf->Get("gravity", + this->dataPtr->enableGravity).first; + this->dataPtr->kinematic = _sdf->Get("kinematic", this->dataPtr->kinematic).first; @@ -865,12 +871,24 @@ bool Link::EnableWind() const return this->dataPtr->enableWind; } +///////////////////////////////////////////////// +bool Link::EnableGravity() const +{ + return this->dataPtr->enableGravity; +} + ///////////////////////////////////////////////// void Link::SetEnableWind(const bool _enableWind) { this->dataPtr->enableWind = _enableWind; } +///////////////////////////////////////////////// +void Link::SetEnableGravity(bool _enableGravity) +{ + this->dataPtr->enableGravity = _enableGravity; +} + ///////////////////////////////////////////////// bool Link::Kinematic() const { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index ed4853fe8..7a9207dd0 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -72,6 +72,10 @@ TEST(DOMLink, Construction) link.SetEnableWind(true); EXPECT_TRUE(link.EnableWind()); + EXPECT_TRUE(link.EnableGravity()); + link.SetEnableGravity(false); + EXPECT_FALSE(link.EnableGravity()); + EXPECT_FALSE(link.Kinematic()); link.SetKinematic(true); EXPECT_TRUE(link.Kinematic()); diff --git a/src/SDF.cc b/src/SDF.cc index 22fcaef3d..712d21b81 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 + "]"}); } } @@ -423,12 +497,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; } @@ -436,6 +519,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 { @@ -444,9 +540,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 5538406d3..6b81db9f9 100644 --- a/src/SDF_TEST.cc +++ b/src/SDF_TEST.cc @@ -734,6 +734,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 &) diff --git a/src/Surface.cc b/src/Surface.cc index 5bc84970d..0d10e4e9e 100644 --- a/src/Surface.cc +++ b/src/Surface.cc @@ -15,6 +15,9 @@ * */ +#include +#include + #include "sdf/Element.hh" #include "sdf/parser.hh" #include "sdf/Surface.hh" @@ -64,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 @@ -85,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() @@ -235,6 +507,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; } @@ -256,6 +542,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()) @@ -429,5 +739,34 @@ sdf::ElementPtr Surface::ToElement(sdf::Errors &_errors) const ode->GetElement("fdir1", _errors)->Set( _errors, 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 8498be8f1..a7b2341d6 100644 --- a/src/Surface_TEST.cc +++ b/src/Surface_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include "sdf/Surface.hh" #include "test_utils.hh" @@ -150,6 +151,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); @@ -168,6 +182,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()); } ///////////////////////////////////////////////// @@ -287,13 +313,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()); } ///////////////////////////////////////////////// @@ -308,6 +360,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); @@ -315,6 +382,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()); } ///////////////////////////////////////////////// @@ -329,6 +407,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); @@ -336,6 +429,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()); } ///////////////////////////////////////////////// @@ -359,6 +463,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; @@ -375,34 +509,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()); } ///////////////////////////////////////////////// @@ -515,3 +641,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/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/World.cc b/src/World.cc index d94ff685a..e17005c9f 100644 --- a/src/World.cc +++ b/src/World.cc @@ -773,14 +773,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 3100ce166..ad1e0d2d5 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -64,6 +64,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)); @@ -458,15 +461,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()); } ///////////////////////////////////////////////// diff --git a/src/parser.cc b/src/parser.cc index 97603cfa1..fea9f1ff6 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..e936450e1 100644 --- a/test/integration/custom_elems_attrs.sdf +++ b/test/integration/custom_elems_attrs.sdf @@ -4,13 +4,13 @@ Description of this world - + L1 L2 - + transmission_interface/SimpleTransmission EffortJointInterface 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..ef04256b9 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(); @@ -125,7 +139,7 @@ TEST(SDFParser, ReloadCustomElements) ASSERT_NE(nullptr, customElem2); const std::string customElemStr = -R"( +R"( transmission_interface/SimpleTransmission EffortJointInterface diff --git a/test/integration/surface_dom.cc b/test/integration/surface_dom.cc index 9b3cf83aa..f1f7faf94 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/basic_shapes.sdf b/test/sdf/basic_shapes.sdf index 64d4a7091..93c9fafc1 100644 --- a/test/sdf/basic_shapes.sdf +++ b/test/sdf/basic_shapes.sdf @@ -1,5 +1,5 @@ - + true diff --git a/test/sdf/shapes.sdf b/test/sdf/shapes.sdf index cd5b31e41..5a2e72d4f 100644 --- a/test/sdf/shapes.sdf +++ b/test/sdf/shapes.sdf @@ -1,5 +1,5 @@ - + @@ -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 + + diff --git a/test/sdf/shapes_world.sdf b/test/sdf/shapes_world.sdf index b0858ec75..3a7be8b91 100644 --- a/test/sdf/shapes_world.sdf +++ b/test/sdf/shapes_world.sdf @@ -1,5 +1,5 @@ - + true