From cbc2a1b9a20a2ddde9e4adad95e45ce019657d93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 9 Nov 2021 05:19:12 +0100 Subject: [PATCH] OrientedBox Python interface (#276) Signed-off-by: ahcorde * Rename [XYZ]Length methods to [xyz]_length Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- src/python/CMakeLists.txt | 1 + src/python/OrientedBox.i | 93 ++++++++++ src/python/OrientedBox_TEST.py | 310 +++++++++++++++++++++++++++++++++ src/python/python.i | 1 + 4 files changed, 405 insertions(+) create mode 100644 src/python/OrientedBox.i create mode 100644 src/python/OrientedBox_TEST.py diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index a947a42a9..25c30e743 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -105,6 +105,7 @@ if (PYTHONLIBS_FOUND) Matrix3_TEST Matrix4_TEST MovingWindowFilter_TEST + OrientedBox_TEST PID_TEST Plane_TEST Pose3_TEST diff --git a/src/python/OrientedBox.i b/src/python/OrientedBox.i new file mode 100644 index 000000000..ca32b3121 --- /dev/null +++ b/src/python/OrientedBox.i @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +%module orientedbox +%{ +#include +#include +#include +#include +#include +#include +#include +#include +#include +%} + +namespace ignition +{ + namespace math + { + template + class ignition::math::OrientedBox + { + %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; + public: OrientedBox(); + + public: OrientedBox( + const ignition::math::Vector3 &_size, const ignition::math::Pose3 &_pose); + + public: OrientedBox(const ignition::math::Vector3 &_size, const ignition::math::Pose3 &_pose, + const ignition::math::Material &_mat); + + public: explicit OrientedBox(const ignition::math::Vector3 &_size); + + public: explicit OrientedBox(const ignition::math::Vector3 &_size, + const ignition::math::Material &_mat); + + public: OrientedBox(const ignition::math::OrientedBox &_b); + + public: virtual ~OrientedBox(); + + %rename(x_length) XLength; + public: T XLength() const; + + %rename(y_length) YLength; + public: T YLength() const; + + %rename(z_length) ZLength; + public: T ZLength() const; + + public: const ignition::math::Vector3 &Size() const; + + public: const ignition::math::Pose3 &Pose() const; + + public: void Size(ignition::math::Vector3 &_size); + + public: void Pose(ignition::math::Pose3 &_pose); + + public: bool operator==(const ignition::math::OrientedBox &_b) const; + + public: bool operator!=(const ignition::math::OrientedBox &_b) const; + + public: bool Contains(const ignition::math::Vector3 &_p) const; + + public: const ignition::math::ignition::math::Material &ignition::math::Material() const; + + public: void ignition::math::SetMaterial(const ignition::math::ignition::math::Material &_mat); + + public: T Volume() const; + + public: T DensityFromMass(const T _mass) const; + + public: bool SetDensityFromMass(const T _mass); + + public: bool MassMatrix(MassMatrix3 &_massMat) const; + }; + %template(OrientedBoxd) OrientedBox; + } +} diff --git a/src/python/OrientedBox_TEST.py b/src/python/OrientedBox_TEST.py new file mode 100644 index 000000000..5eb671383 --- /dev/null +++ b/src/python/OrientedBox_TEST.py @@ -0,0 +1,310 @@ +# Copyright (C) 2021 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:#www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import unittest + +from ignition.math import IGN_PI, OrientedBoxd, MassMatrix3d, Material, Pose3d, Vector3d + +g_tolerance = 1e-6 + + +class TestOrientedBox(unittest.TestCase): + + def test_empty_constructor_new(self): + box = OrientedBoxd() + + self.assertEqual(box.size(), Vector3d.ZERO) + self.assertEqual(box.pose(), Pose3d.ZERO) + # self.assertEqual(Material(), box.material()) + + def test_size_only_constructor(self): + box = OrientedBoxd(Vector3d(1, 2, 3)) + self.assertEqual(box.size(), Vector3d(1, 2, 3)) + self.assertEqual(box.pose(), Pose3d.ZERO) + + def test_negative_size_constructor(self): + box = OrientedBoxd(Vector3d(-1, 0, -3)) + self.assertEqual(box.size(), Vector3d(1, 0, 3)) + + def test_size_pose_constructor(self): + box = OrientedBoxd(Vector3d(1, 2, 3), Pose3d(-1, -2, -3, 0, 1, 2)) + self.assertEqual(box.size(), Vector3d(1, 2, 3)) + self.assertEqual(box.pose(), Pose3d(-1, -2, -3, 0, 1, 2)) + + def test_copy_constructor(self): + + box1 = OrientedBoxd(Vector3d(0.1, 0.2, 0.3), + Pose3d(-0.1, -0.2, 0.0, 1.1, 1.2, 1.3)) + box2 = OrientedBoxd(box1) + + self.assertEqual(box2.size(), Vector3d(0.1, 0.2, 0.3)) + self.assertEqual(box2.pose(), Pose3d(-0.1, -0.2, 0.0, 1.1, 1.2, 1.3)) + + def test_length(self): + box = OrientedBoxd(Vector3d(0.1, -2.1, 0.0)) + self.assertEqual(box.x_length(), 0.1) + self.assertEqual(box.y_length(), 2.1) + self.assertEqual(box.z_length(), 0.0) + + def test_operator_equal(self): + box = OrientedBoxd(Vector3d(1, 1, 1)) + box2 = OrientedBoxd(Vector3d(1, 1, 1), + Pose3d(1, 2, 3, 4, 5, 6)) + box3 = OrientedBoxd(Vector3d(0, 0, 0), + Pose3d(1, 2, 3, 4, 5, 6)) + self.assertEqual(box, OrientedBoxd(Vector3d(1, 1, 1))) + self.assertNotEqual(box, box2) + self.assertNotEqual(box3, box) + + def test_constains_zero_box(self): + box = OrientedBoxd() + + self.assertTrue(box.contains(Vector3d(0, 0, 0))) + self.assertFalse(box.contains(Vector3d(0, 0, 0.0001))) + + def test_constains_zero_pose(self): + box = OrientedBoxd(Vector3d(1, 2, 3)) + + # Vertices + self.assertTrue(box.contains(Vector3d(-0.5, -1.0, -1.5))) + self.assertTrue(box.contains(Vector3d(-0.5, -1.0, +1.5))) + self.assertTrue(box.contains(Vector3d(-0.5, +1.0, -1.5))) + self.assertTrue(box.contains(Vector3d(-0.5, +1.0, +1.5))) + + self.assertTrue(box.contains(Vector3d(+0.5, -1.0, -1.5))) + self.assertTrue(box.contains(Vector3d(+0.5, -1.0, +1.5))) + self.assertTrue(box.contains(Vector3d(+0.5, +1.0, -1.5))) + self.assertTrue(box.contains(Vector3d(+0.5, +1.0, +1.5))) + + # Edges + self.assertTrue(box.contains(Vector3d(0.0, -1.0, -1.5))) + self.assertTrue(box.contains(Vector3d(0.0, -1.0, +1.5))) + self.assertTrue(box.contains(Vector3d(0.0, +1.0, -1.5))) + self.assertTrue(box.contains(Vector3d(0.0, +1.0, +1.5))) + + self.assertTrue(box.contains(Vector3d(-0.5, -1.0, 0.0))) + self.assertTrue(box.contains(Vector3d(-0.5, +1.0, 0.0))) + self.assertTrue(box.contains(Vector3d(+0.5, -1.0, 0.0))) + self.assertTrue(box.contains(Vector3d(+0.5, +1.0, 0.0))) + + self.assertTrue(box.contains(Vector3d(-0.5, 0.0, -1.5))) + self.assertTrue(box.contains(Vector3d(-0.5, 0.0, +1.5))) + self.assertTrue(box.contains(Vector3d(+0.5, 0.0, -1.5))) + self.assertTrue(box.contains(Vector3d(+0.5, 0.0, +1.5))) + + # Inside + self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, + -1.0+g_tolerance, + -1.5+g_tolerance))) + self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, + -1.0+g_tolerance, + +1.5-g_tolerance))) + self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, + +1.0-g_tolerance, + -1.5+g_tolerance))) + self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, + +1.0-g_tolerance, + +1.5-g_tolerance))) + + self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, + -1.0+g_tolerance, + -1.5+g_tolerance))) + self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, + -1.0+g_tolerance, + +1.5-g_tolerance))) + self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, + +1.0-g_tolerance, + -1.5+g_tolerance))) + self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, + +1.0-g_tolerance, + +1.5-g_tolerance))) + + # Outside + self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, + -1.0-g_tolerance, + -1.5-g_tolerance))) + self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, + -1.0-g_tolerance, + +1.5+g_tolerance))) + self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, + +1.0+g_tolerance, + -1.5-g_tolerance))) + self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, + +1.0+g_tolerance, + +1.5+g_tolerance))) + + self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, + -1.0-g_tolerance, + -1.5-g_tolerance))) + self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, + -1.0-g_tolerance, + +1.5+g_tolerance))) + self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, + +1.0+g_tolerance, + -1.5-g_tolerance))) + self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, + +1.0+g_tolerance, + +1.5+g_tolerance))) + + def test_contains_oriented_position(self): + + box = OrientedBoxd(Vector3d(1, 2, 3), + Pose3d(10, 20, 30, 0, 0, 0)) + + # Vertices + self.assertTrue(box.contains(Vector3d(10-0.5, 20-1.0, 30-1.5))) + self.assertTrue(box.contains(Vector3d(10-0.5, 20-1.0, 30+1.5))) + self.assertTrue(box.contains(Vector3d(10-0.5, 20+1.0, 30-1.5))) + self.assertTrue(box.contains(Vector3d(10-0.5, 20+1.0, 30+1.5))) + + self.assertTrue(box.contains(Vector3d(10+0.5, 20-1.0, 30-1.5))) + self.assertTrue(box.contains(Vector3d(10+0.5, 20-1.0, 30+1.5))) + self.assertTrue(box.contains(Vector3d(10+0.5, 20+1.0, 30-1.5))) + self.assertTrue(box.contains(Vector3d(10+0.5, 20+1.0, 30+1.5))) + + # Edges + self.assertTrue(box.contains(Vector3d(10.0, 20-1.0, 30-1.5))) + self.assertTrue(box.contains(Vector3d(10.0, 20-1.0, 30+1.5))) + self.assertTrue(box.contains(Vector3d(10.0, 20+1.0, 30-1.5))) + self.assertTrue(box.contains(Vector3d(10.0, 20+1.0, 30+1.5))) + + self.assertTrue(box.contains(Vector3d(10-0.5, 20-1.0, 30.0))) + self.assertTrue(box.contains(Vector3d(10-0.5, 20+1.0, 30.0))) + self.assertTrue(box.contains(Vector3d(10+0.5, 20-1.0, 30.0))) + self.assertTrue(box.contains(Vector3d(10+0.5, 20+1.0, 30.0))) + + self.assertTrue(box.contains(Vector3d(10-0.5, 20.0, 30-1.5))) + self.assertTrue(box.contains(Vector3d(10-0.5, 20.0, 30+1.5))) + self.assertTrue(box.contains(Vector3d(10+0.5, 20.0, 30-1.5))) + self.assertTrue(box.contains(Vector3d(10+0.5, 20.0, 30+1.5))) + + # Inside + self.assertTrue(box.contains(Vector3d(10, 20, 30))) + self.assertTrue(box.contains(Vector3d(10-0.25, 20-0.5, 30-0.75))) + self.assertTrue(box.contains(Vector3d(10+0.25, 20+0.5, 30+0.75))) + + # Outside + self.assertFalse(box.contains(Vector3d(10-1.0, 20-1.0, 30-1.5))) + self.assertFalse(box.contains(Vector3d(10-0.5, 20-2.0, 30-1.5))) + self.assertFalse(box.contains(Vector3d(10-0.5, 20-1.0, 30-2.0))) + + self.assertFalse(box.contains(Vector3d(10+1.0, 20+1.0, 30+1.5))) + self.assertFalse(box.contains(Vector3d(10+0.5, 20+2.0, 30+1.5))) + self.assertFalse(box.contains(Vector3d(10+0.5, 20+1.0, 30+2.0))) + + def test_contains_oriented_rotation(self): + # Rotate PI/2 about +x: swap Z and Y + box = OrientedBoxd(Vector3d(1, 2, 3), Pose3d(0, 0, 0, IGN_PI*0.5, 0, 0)) + + # Doesn't contain non-rotated vertices + self.assertFalse(box.contains(Vector3d(-0.5, -1.0, -1.5))) + self.assertFalse(box.contains(Vector3d(-0.5, -1.0, +1.5))) + self.assertFalse(box.contains(Vector3d(-0.5, +1.0, -1.5))) + self.assertFalse(box.contains(Vector3d(-0.5, +1.0, +1.5))) + + self.assertFalse(box.contains(Vector3d(+0.5, -1.0, -1.5))) + self.assertFalse(box.contains(Vector3d(+0.5, -1.0, +1.5))) + self.assertFalse(box.contains(Vector3d(+0.5, +1.0, -1.5))) + self.assertFalse(box.contains(Vector3d(+0.5, +1.0, +1.5))) + + # Inside + self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, + -1.5+g_tolerance, + -1.0+g_tolerance))) + self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, + -1.5+g_tolerance, + +1.0-g_tolerance))) + self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, + +1.5-g_tolerance, + -1.0+g_tolerance))) + self.assertTrue(box.contains(Vector3d(-0.5+g_tolerance, + +1.5-g_tolerance, + +1.0-g_tolerance))) + + self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, + -1.5+g_tolerance, + -1.0+g_tolerance))) + self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, + -1.5+g_tolerance, + +1.0-g_tolerance))) + self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, + +1.5-g_tolerance, -1.0+g_tolerance))) + self.assertTrue(box.contains(Vector3d(+0.5-g_tolerance, + +1.5-g_tolerance, + +1.0-g_tolerance))) + + # Outside + self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, + -1.5-g_tolerance, + -1.0-g_tolerance))) + self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, + -1.5-g_tolerance, + +1.0+g_tolerance))) + self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, + +1.5+g_tolerance, + -1.0-g_tolerance))) + self.assertFalse(box.contains(Vector3d(-0.5-g_tolerance, + +1.5+g_tolerance, + +1.0+g_tolerance))) + + self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, + -1.5-g_tolerance, + -1.0-g_tolerance))) + self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, + -1.5-g_tolerance, + +1.0+g_tolerance))) + self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, + +1.5+g_tolerance, + -1.0-g_tolerance))) + self.assertFalse(box.contains(Vector3d(+0.5-g_tolerance, + +1.5+g_tolerance, + +1.0+g_tolerance))) + + def test_volume_and_density(self): + mass = 1.0 + box = OrientedBoxd(Vector3d(1.0, 0.1, 10.4)) + expectedVolume = 1.0 * 0.1 * 10.4 + self.assertEqual(expectedVolume, box.volume()) + + expectedDensity = mass / expectedVolume + self.assertEqual(expectedDensity, box.density_from_mass(mass)) + + # Bad density + box2 = OrientedBoxd() + self.assertGreater(0.0, box2.density_from_mass(mass)) + + def test_mass(self): + mass = 2.0 + length = 2.0 + w = 0.1 + h = 34.12 + box = OrientedBoxd(Vector3d(length, w, h)) + box.set_density_from_mass(mass) + + massMat = MassMatrix3d() + ixx = (1.0/12.0) * mass * (w*w + h*h) + iyy = (1.0/12.0) * mass * (length * length + h*h) + izz = (1.0/12.0) * mass * (length * length + w*w) + + expectedMassMat = MassMatrix3d() + expectedMassMat.set_inertia_matrix(ixx, iyy, izz, 0.0, 0.0, 0.0) + expectedMassMat.set_mass(mass) + + box.mass_matrix(massMat) + self.assertEqual(expectedMassMat, massMat) + self.assertEqual(expectedMassMat.mass(), massMat.mass()) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python/python.i b/src/python/python.i index adf54d591..c77889a12 100644 --- a/src/python/python.i +++ b/src/python/python.i @@ -34,6 +34,7 @@ %include AxisAlignedBox.i %include Plane.i %include MassMatrix3.i +%include OrientedBox.i %include Box.i %include Cylinder.i %include Sphere.i