diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 53b3e227f..501d15a5a 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -241,11 +241,11 @@ namespace sdf double _density, sdf::ElementPtr _autoInertiaParams); /// \brief Calculate and return the AxisAlignedBox for the Geometry - /// @param _meshAabbCalculator The function to calculate the AABB of a mesh - /// @return std::optional with gz::math::AxisAlignedBox object or + /// \param _meshAabbCalculator The function to calculate the AABB of a mesh + /// \return std::optional with gz::math::AxisAlignedBox object or /// std::nullopt if the geometry type does not support AABB calculation public: std::optional AxisAlignedBox( - Mesh::AxisAlignedBoxCalculator _meshAabbCalculator) const; + const Mesh::AxisAlignedBoxCalculator &_meshAabbCalculator) const; /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Mesh.hh b/include/sdf/Mesh.hh index 44650f04c..286846e1b 100644 --- a/include/sdf/Mesh.hh +++ b/include/sdf/Mesh.hh @@ -211,12 +211,11 @@ namespace sdf const ParserConfig &_config); /// \brief Get the Axis-aligned box for this Mesh. - /// \param[out] _errors A vector of Errors object. Each object - /// would contain an error code and an error message. - /// \param[in] _config Parser Configuration object. + /// \param[in] _meshAabbCalculator A custom function that calculates the + /// AxisAlignedBox for the Mesh. /// \return A gz::math::AxisAlignedBox object. public: std::optional - AxisAlignedBox(AxisAlignedBoxCalculator _meshAabbCalculator) const; + AxisAlignedBox(const AxisAlignedBoxCalculator &_meshAabbCalculator) const; /// \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/python/src/sdf/pyBox.cc b/python/src/sdf/pyBox.cc index 453438119..8ec805075 100644 --- a/python/src/sdf/pyBox.cc +++ b/python/src/sdf/pyBox.cc @@ -45,6 +45,8 @@ void defineBox(pybind11::object module) "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("axis_aligned_box", &sdf::Box::AxisAlignedBox, + "Get the axis-aligned box that contains this 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 50ca4337d..77bec7d43 100644 --- a/python/src/sdf/pyCapsule.cc +++ b/python/src/sdf/pyCapsule.cc @@ -44,6 +44,8 @@ void defineCapsule(pybind11::object module) "Set the capsule's length in meters.") .def("calculate_inertial", &sdf::Capsule::CalculateInertial, "Calculate and return the Inertial values for the Capsule.") + .def("axis_aligned_box", &sdf::Capsule::AxisAlignedBox, + "Get the axis-aligned box that contains this Capsule.") .def( "shape", pybind11::overload_cast<>(&sdf::Capsule::Shape, pybind11::const_), diff --git a/python/src/sdf/pyCone.cc b/python/src/sdf/pyCone.cc index 05cb9cb15..9e7c651be 100644 --- a/python/src/sdf/pyCone.cc +++ b/python/src/sdf/pyCone.cc @@ -48,6 +48,8 @@ void defineCone(pybind11::object module) pybind11::overload_cast<>(&sdf::Cone::Shape, pybind11::const_), pybind11::return_value_policy::reference, "Get a mutable Gazebo Math representation of this Cone.") + .def("axis_aligned_box", &sdf::Cone::AxisAlignedBox, + "Get the axis-aligned box that contains this Cone.") .def("__copy__", [](const sdf::Cone &self) { return sdf::Cone(self); }) diff --git a/python/src/sdf/pyCylinder.cc b/python/src/sdf/pyCylinder.cc index d8515ddc6..5e94f146e 100644 --- a/python/src/sdf/pyCylinder.cc +++ b/python/src/sdf/pyCylinder.cc @@ -49,6 +49,8 @@ void defineCylinder(pybind11::object module) pybind11::overload_cast<>(&sdf::Cylinder::Shape, pybind11::const_), pybind11::return_value_policy::reference, "Get a mutable Gazebo Math representation of this Cylinder.") + .def("axis_aligned_box", &sdf::Cylinder::AxisAlignedBox, + "Get the axis-aligned box that contains this Cylinder.") .def("__copy__", [](const sdf::Cylinder &self) { return sdf::Cylinder(self); }) diff --git a/python/src/sdf/pyEllipsoid.cc b/python/src/sdf/pyEllipsoid.cc index 47ea60c96..aa446a83d 100644 --- a/python/src/sdf/pyEllipsoid.cc +++ b/python/src/sdf/pyEllipsoid.cc @@ -45,6 +45,8 @@ void defineEllipsoid(pybind11::object module) pybind11::overload_cast<>(&sdf::Ellipsoid::Shape, pybind11::const_), pybind11::return_value_policy::reference, "Get a mutable Gazebo Math representation of this Ellipsoid.") + .def("axis_aligned_box", &sdf::Ellipsoid::AxisAlignedBox, + "Get the axis-aligned box that contains this Ellipsoid.") .def("__copy__", [](const sdf::Ellipsoid &self) { return sdf::Ellipsoid(self); }) diff --git a/python/src/sdf/pyGeometry.cc b/python/src/sdf/pyGeometry.cc index 8d7873bc3..a938b27d9 100644 --- a/python/src/sdf/pyGeometry.cc +++ b/python/src/sdf/pyGeometry.cc @@ -18,6 +18,7 @@ #include #include +#include #include "sdf/ParserConfig.hh" @@ -107,6 +108,8 @@ void defineGeometry(pybind11::object module) .def("heightmap_shape", &sdf::Geometry::HeightmapShape, pybind11::return_value_policy::reference, "Get the heightmap geometry.") + .def("axis_aligned_box", &sdf::Geometry::AxisAlignedBox, + "Get the axis-aligned box that contains the Geometry.") .def("__copy__", [](const sdf::Geometry &self) { return sdf::Geometry(self); }) diff --git a/python/src/sdf/pyMesh.cc b/python/src/sdf/pyMesh.cc index b9fdafd17..dda62f719 100644 --- a/python/src/sdf/pyMesh.cc +++ b/python/src/sdf/pyMesh.cc @@ -17,6 +17,8 @@ #include "pyMesh.hh" #include +#include +#include #include "sdf/ParserConfig.hh" #include "sdf/Mesh.hh" @@ -78,6 +80,8 @@ void defineMesh(pybind11::object module) .def("set_center_submesh", &sdf::Mesh::SetCenterSubmesh, "Set whether the submesh should be centered. See CenterSubmesh() " "for more information.") + .def("axis_aligned_box", &sdf::Mesh::AxisAlignedBox, + "Get the axis-aligned box that contains this Mesh.") .def("__copy__", [](const sdf::Mesh &self) { return sdf::Mesh(self); }) diff --git a/python/src/sdf/pySphere.cc b/python/src/sdf/pySphere.cc index ee6197b38..b5565a48f 100644 --- a/python/src/sdf/pySphere.cc +++ b/python/src/sdf/pySphere.cc @@ -45,6 +45,8 @@ void defineSphere(pybind11::object module) pybind11::overload_cast<>(&sdf::Sphere::Shape, pybind11::const_), pybind11::return_value_policy::reference, "Get a mutable Gazebo Math representation of this Sphere.") + .def("axis_aligned_box", &sdf::Sphere::AxisAlignedBox, + "Get the axis-aligned box that contains this Sphere.") .def("__copy__", [](const sdf::Sphere &self) { return sdf::Sphere(self); }) diff --git a/python/test/pyBox_TEST.py b/python/test/pyBox_TEST.py index 84ce40bee..9bb5db704 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 Inertiald, MassMatrix3d, Pose3d, Vector3d +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Box import unittest @@ -89,5 +89,14 @@ def test_calculate_inertial(self): boxInertial.mass_matrix().mass()) self.assertEqual(expectedInertial.pose(), boxInertial.pose()) + def test_axis_aligned_box(self): + box = Box() + box.set_size(Vector3d(1, 2, 3)) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -1, -1.5), Vector3d(0.5, 1, 1.5)), + box.axis_aligned_box()) + + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCapsule_TEST.py b/python/test/pyCapsule_TEST.py index 976e53c7b..e5b535006 100644 --- a/python/test/pyCapsule_TEST.py +++ b/python/test/pyCapsule_TEST.py @@ -16,7 +16,7 @@ import math -from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Capsule import unittest @@ -107,6 +107,7 @@ def test_shape(self): self.assertEqual(0.123, capsule.radius()) self.assertEqual(0.456, capsule.length()) + def test_calculate_inertial(self): capsule = Capsule() @@ -145,5 +146,15 @@ def test_calculate_inertial(self): capsuleInertial.mass_matrix().mass()) self.assertEqual(expectedInertial.pose(), capsuleInertial.pose()) + def test_axis_aligned_box(self): + capsule = Capsule() + capsule.set_radius(0.5) + capsule.set_length(3.0) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)), + capsule.axis_aligned_box()) + + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCone_TEST.py b/python/test/pyCone_TEST.py index e36e65f63..22d3a8191 100644 --- a/python/test/pyCone_TEST.py +++ b/python/test/pyCone_TEST.py @@ -18,6 +18,7 @@ import math +from gz_test_deps.math import AxisAlignedBox, Vector3d from gz_test_deps.sdformat import Cone import unittest @@ -82,6 +83,7 @@ def test_copy_construction(self): self.assertEqual(0.2, cone2.radius()) self.assertEqual(3.0, cone2.length()) + def test_deepcopy(self): cone = Cone(); cone.set_radius(0.2) @@ -109,6 +111,15 @@ def test_shape(self): self.assertEqual(0.123, cone.radius()) self.assertEqual(0.456, cone.length()) + def test_axis_aligned_box(self): + cone = Cone() + cone.set_radius(1.0) + cone.set_length(2.0) + + self.assertEqual( + AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)), + cone.axis_aligned_box()) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyCylinder_TEST.py b/python/test/pyCylinder_TEST.py index b5387dd0b..5fba0b44c 100644 --- a/python/test/pyCylinder_TEST.py +++ b/python/test/pyCylinder_TEST.py @@ -16,7 +16,7 @@ import math -from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d from gz_test_deps.sdformat import Cylinder import unittest @@ -147,5 +147,15 @@ def test_calculate_interial(self): cylinderInertial.mass_matrix().mass()) self.assertEqual(expectedInertial.pose(), cylinderInertial.pose()) + def test_axis_aligned_box(self): + cylinder = Cylinder() + cylinder.set_radius(0.8) + cylinder.set_length(1.8) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.8, -0.8, -0.9), Vector3d(0.8, 0.8, 0.9)), + cylinder.axis_aligned_box()) + + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyEllipsoid_TEST.py b/python/test/pyEllipsoid_TEST.py index c12690ffe..7342bc34f 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 Inertiald, MassMatrix3d, Pose3d, Vector3d +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d import math from gz_test_deps.sdformat import Ellipsoid import unittest @@ -77,6 +77,7 @@ def test_shape(self): ellipsoid.shape().set_radii(expectedradii) self.assertEqual(expectedradii, ellipsoid.radii()) + def test_calculate_inertial(self): ellipsoid = Ellipsoid() @@ -116,5 +117,14 @@ def test_calculate_inertial(self): ellipsoidInertial.mass_matrix().mass()) self.assertEqual(expectedInertial.pose(), ellipsoidInertial.pose()) + def test_axis_aligned_box(self): + ellipsoid = Ellipsoid() + ellipsoid.set_radii(Vector3d(1.0, 2.0, 3.0)) + + self.assertEqual( + AxisAlignedBox(Vector3d(-1.0, -2.0, -3.0), Vector3d(1.0, 2.0, 3.0)), + ellipsoid.axis_aligned_box()) + + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyGeometry_TEST.py b/python/test/pyGeometry_TEST.py index c124f2f71..ffc1359cf 100644 --- a/python/test/pyGeometry_TEST.py +++ b/python/test/pyGeometry_TEST.py @@ -15,7 +15,7 @@ import copy 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 +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d, Vector2d import gz_test_deps.sdformat as sdf import math import unittest @@ -388,5 +388,105 @@ def test_calculate_inertial(self): self.assertEqual(expectedInertial.mass_matrix(), expectedMassMat) self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) + def test_axis_aligned_box(self): + geom = Geometry() + + # Box + geom.set_type(sdf.GeometryType.BOX) + box = Box() + box.set_size(Vector3d(1, 2, 3)) + geom.set_box_shape(box) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -1, -1.5), Vector3d(0.5, 1, 1.5)), + geom.axis_aligned_box(None)) + + # Capsule + geom.set_type(sdf.GeometryType.CAPSULE) + capsule = Capsule() + capsule.set_radius(0.5) + capsule.set_length(3.0) + geom.set_capsule_shape(capsule) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)), + geom.axis_aligned_box(None)) + + # Cone + geom.set_type(sdf.GeometryType.CONE) + cone = Cone() + cone.set_radius(0.5) + cone.set_length(3.0) + geom.set_cone_shape(cone) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)), + geom.axis_aligned_box(None)) + + # Cylinder + geom.set_type(sdf.GeometryType.CYLINDER) + cylinder = Cylinder() + cylinder.set_radius(0.5) + cylinder.set_length(3.0) + geom.set_cylinder_shape(cylinder) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -1.5), Vector3d(0.5, 0.5, 1.5)), + geom.axis_aligned_box(None)) + + # Ellipsoid + geom.set_type(sdf.GeometryType.ELLIPSOID) + ellipsoid = Ellipsoid() + ellipsoid.set_radii(Vector3d(1, 2, 3)) + geom.set_ellipsoid_shape(ellipsoid) + + self.assertEqual( + AxisAlignedBox(Vector3d(-1, -2, -3), Vector3d(1, 2, 3)), + geom.axis_aligned_box(None)) + + # Sphere + geom.set_type(sdf.GeometryType.SPHERE) + sphere = Sphere() + sphere.set_radius(0.5) + geom.set_sphere_shape(sphere) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.5, -0.5, -0.5), Vector3d(0.5, 0.5, 0.5)), + geom.axis_aligned_box(None)) + + # Mesh + geom.set_type(sdf.GeometryType.MESH) + mesh = Mesh() + geom.set_mesh_shape(mesh) + + self.assertEqual(None, geom.axis_aligned_box(None)) + + def mesh_aabb_calulator(sdf_mesh: Mesh) -> AxisAlignedBox: + return AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)) + + self.assertEqual( + AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)), + geom.axis_aligned_box(mesh_aabb_calulator)) + + # Plane - Non volumetric geometry (no AABB) + geom.set_type(sdf.GeometryType.PLANE) + plane = Plane() + geom.set_plane_shape(plane) + + self.assertEqual(None, geom.axis_aligned_box(None)) + + # Heightmap - Non volumetric geometry (no AABB) + geom.set_type(sdf.GeometryType.HEIGHTMAP) + heightmap = Heightmap() + geom.set_heightmap_shape(heightmap) + + self.assertEqual(None, geom.axis_aligned_box(None)) + + # Empty - Non volumetric geometry (no AABB) + geom.set_type(sdf.GeometryType.EMPTY) + + self.assertEqual(None, geom.axis_aligned_box(None)) + + if __name__ == '__main__': unittest.main() diff --git a/python/test/pyMesh_TEST.py b/python/test/pyMesh_TEST.py index 0990095d6..e79b30822 100644 --- a/python/test/pyMesh_TEST.py +++ b/python/test/pyMesh_TEST.py @@ -14,7 +14,7 @@ import copy from gz_test_deps.sdformat import Mesh, ConvexDecomposition -from gz_test_deps.math import Vector3d +from gz_test_deps.math import AxisAlignedBox, Vector3d import gz_test_deps.sdformat as sdf import unittest @@ -116,7 +116,6 @@ def test_deepcopy_construction(self): self.assertTrue(mesh2.center_submesh()) self.assertEqual("/pear", mesh2.file_path()) - def test_set(self): mesh = Mesh() @@ -160,6 +159,28 @@ def test_set(self): mesh.set_file_path("/mypath") self.assertEqual("/mypath", mesh.file_path()) + def test_axis_aligned_box(self): + mesh = Mesh() + self.assertEqual(None, mesh.axis_aligned_box(None)) + + # Customly defined Mesh AABB calculator + def mesh_aabb_calulator(sdf_mesh: Mesh) -> AxisAlignedBox: + if sdf_mesh.uri() == "banana": + # Banana mesh should return invalid AABB + return AxisAlignedBox() + + return AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)) + + mesh.set_uri("banana") + self.assertEqual( + AxisAlignedBox(), + mesh.axis_aligned_box(mesh_aabb_calulator)) + + mesh.set_uri("apple") + self.assertEqual( + AxisAlignedBox(Vector3d(-1, -1, -1), Vector3d(1, 1, 1)), + mesh.axis_aligned_box(mesh_aabb_calulator)) + if __name__ == '__main__': unittest.main() diff --git a/python/test/pySphere_TEST.py b/python/test/pySphere_TEST.py index f687f6815..032081b39 100644 --- a/python/test/pySphere_TEST.py +++ b/python/test/pySphere_TEST.py @@ -15,7 +15,7 @@ import copy import math from gz_test_deps.sdformat import Sphere -from gz_test_deps.math import Inertiald, MassMatrix3d, Pose3d, Vector3d +from gz_test_deps.math import AxisAlignedBox, Inertiald, MassMatrix3d, Pose3d, Vector3d import unittest class SphereTEST(unittest.TestCase): @@ -111,6 +111,14 @@ def test_calculate_inertial(self): sphereInertial.mass_matrix().mass()) self.assertEqual(expectedInertial.pose(), sphereInertial.pose()) + def test_axis_aligned_box(self): + sphere = Sphere() + sphere.set_radius(0.75) + + self.assertEqual( + AxisAlignedBox(Vector3d(-0.75, -0.75, -0.75), Vector3d(0.75, 0.75, 0.75)), + sphere.axis_aligned_box()) + if __name__ == '__main__': unittest.main() diff --git a/src/Geometry.cc b/src/Geometry.cc index 09bd2e846..8883f80ee 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -387,9 +387,9 @@ std::optional Geometry::CalculateInertial( ///////////////////////////////////////////////// std::optional Geometry::AxisAlignedBox( - Mesh::AxisAlignedBoxCalculator _meshAabbCalculator) const + const Mesh::AxisAlignedBoxCalculator &_meshAabbCalculator) const { - // Initialized the AABB to an empty box (zero volume) + // Initialize the AABB to an invalid box (aabb.Min > aabb.Max) std::optional aabb; switch (this->dataPtr->type) diff --git a/src/Mesh.cc b/src/Mesh.cc index 6bf5fea12..ca2e55c3d 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -400,7 +400,7 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, ///////////////////////////////////////////////// std::optional Mesh::AxisAlignedBox( - Mesh::AxisAlignedBoxCalculator _customCalculator) const + const Mesh::AxisAlignedBoxCalculator &_customCalculator) const { if (!_customCalculator) {