From 62768395a0386e596f5a761fb5f426e0a3f1d050 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 1 Sep 2023 10:53:54 -0500 Subject: [PATCH 01/16] Prepare for 6.15.0 release (#554) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c3e2bcd4..86b4a979 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-math6 VERSION 6.14.0) +project(ignition-math6 VERSION 6.15.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index d578e6c0..3930f045 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,16 @@ ## Gazebo Math 6.x +## Gazebo Math 6.15.0 (2023-09-01) + +1. Fixes for testing in non standard architectures + * [Pull request #546](https://github.com/gazebosim/gz-math/pull/546) + +1. MecanumDriveOdometry to handle odometry estimation of Mecanum wheeled models + * [Pull request #486](https://github.com/gazebosim/gz-math/pull/486) + +1. Infrastructure + * [Pull request #547](https://github.com/gazebosim/gz-math/pull/547) + ## Gazebo Math 6.14.0 (2023-04-14) 1. Disable pybind11 on windows by default From ea9ae349cf87755074b2c97d4241e9270f13dbee Mon Sep 17 00:00:00 2001 From: Bi0T1N <28802083+Bi0T1N@users.noreply.github.com> Date: Fri, 6 Oct 2023 20:57:08 +0200 Subject: [PATCH 02/16] Replace CMake Python variables with new ones from FindPython3 module (#402) Replaces the old variables with the new ones used by the FindPython3 module. Right now IgnPython cannot be used as it only searches for the interpreter but pybind11 also needs the development components, thus it provides a workaround. The Win32 PYTHON_LIBRARIES (PYTHON_DEBUG_LIBRARIES) variable was also removed as there is no equivalent in the new CMake module for getting the debug libraries. --------- Signed-off-by: Bi0T1N Signed-off-by: Addisu Z. Taddese Co-authored-by: Bi0T1N Co-authored-by: Jose Luis Rivero Co-authored-by: Steve Peters Co-authored-by: Addisu Z. Taddese --- CMakeLists.txt | 30 +++++++++++++++++++++++------- src/python_pybind11/CMakeLists.txt | 21 +++++++++++---------- 2 files changed, 34 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 86b4a979..29aef1da 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,16 +95,32 @@ endif() if (SKIP_PYBIND11) message(STATUS "SKIP_PYBIND11 set - disabling python bindings") else() - include(IgnPython) - find_package(PythonLibs QUIET) - if (NOT PYTHONLIBS_FOUND) - IGN_BUILD_WARNING("Python is missing: Python interfaces are disabled.") - message (STATUS "Searching for Python - not found.") + #include(IgnPython) TODO: allow to specify for what it should search and then + # the code below can be removed; e.g. pybind needs Interpreter and Development components + # see https://pybind11.readthedocs.io/en/stable/cmake/index.html#new-findpython-mode + if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + set(IGN_PYTHON_VERSION "3") + find_package(PythonInterp ${IGN_PYTHON_VERSION} QUIET) + if(PYTHONINTERP_FOUND) + set(Python3_FOUND ${PYTHONINTERP_FOUND}) + set(Python3_Interpreter_FOUND ${PYTHONINTERP_FOUND}) + set(Python3_EXECUTABLE ${PYTHON_EXECUTABLE}) + find_package(PythonLibs QUIET) + # we found the interpreter but did we also find the libs? both are required + set(Python3_FOUND ${PYTHONLIBS_FOUND}) + set(Python3_VERSION ${PYTHONLIBS_VERSION_STRING}) + endif() + else() + find_package(Python3 QUIET COMPONENTS Interpreter Development) + endif() + + if (NOT Python3_FOUND) + IGN_BUILD_WARNING("Python3 is missing: Python interfaces are disabled.") + message (STATUS "Searching for Python3 - not found.") else() - message (STATUS "Searching for Python - found version ${PYTHONLIBS_VERSION_STRING}.") + message (STATUS "Searching for Python3 - found version ${Python3_VERSION}.") set(PYBIND11_PYTHON_VERSION 3) - find_package(Python3 QUIET COMPONENTS Interpreter Development) find_package(pybind11 2.2 QUIET) if (${pybind11_FOUND}) diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index e179e138..56442a58 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -1,8 +1,11 @@ -if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") - # pybind11 logic for setting up a debug build when both a debug and release - # python interpreter are present in the system seems to be pretty much broken. - # This works around the issue. - set(PYTHON_LIBRARIES "${PYTHON_DEBUG_LIBRARIES}") +if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + # TODO: remove once the minimum CMake version is increased + if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") + # pybind11 logic for setting up a debug build when both a debug and release + # python interpreter are present in the system seems to be pretty much broken. + # This works around the issue. + set(PYTHON_LIBRARIES "${PYTHON_DEBUG_LIBRARIES}") + endif() endif() message(STATUS "Building pybind11 interfaces") @@ -56,17 +59,15 @@ target_link_libraries(math PRIVATE ) if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) + # Get install variable from Python3 module if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + # Python3_SITEARCH is available via FindPython3 from 3.12 on, workaround if needed execute_process( - COMMAND "${PYTHON_EXECUTABLE}" -c "if True: + COMMAND "${Python3_EXECUTABLE}" -c "if True: from distutils import sysconfig as sc print(sc.get_python_lib(plat_specific=True))" OUTPUT_VARIABLE Python3_SITEARCH OUTPUT_STRIP_TRAILING_WHITESPACE) - else() - # Get install variable from Python3 module - # Python3_SITEARCH is available from 3.12 on, workaround if needed: - find_package(Python3 COMPONENTS Interpreter) endif() if(USE_DIST_PACKAGES_FOR_PYTHON) From 9a5ad69e92c7b01a981482d49af5baf361d03e0e Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 13 Oct 2023 17:07:16 -0500 Subject: [PATCH 03/16] Suppress warnings on MSVC (#564) Fixes #257 Signed-off-by: Michael Carroll --- include/gz/math/Helpers.hh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/include/gz/math/Helpers.hh b/include/gz/math/Helpers.hh index 20222197..62f79b31 100644 --- a/include/gz/math/Helpers.hh +++ b/include/gz/math/Helpers.hh @@ -951,7 +951,10 @@ namespace ignition dayString.erase(dayString.length() - 1); try { - std::stoi(dayString); + // We are only checking if it sucessfully parses, + // and are not concerned with the actual value. + auto day = std::stoi(dayString); + (void) day; } catch (const std::out_of_range &) { From da26a3914516b4e3eb55b79c3a1b40730ddf97d7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 13 Nov 2023 10:16:32 -0600 Subject: [PATCH 04/16] Update github action workflows (#569) * Use on `push` only on stable branches to avoid duplicate runs * Update project automation Signed-off-by: Addisu Z. Taddese --- .github/workflows/ci.yml | 12 +++++++++--- .github/workflows/triage.yml | 9 +++------ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d1d9b2ae..944fb8ff 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,12 @@ name: Ubuntu CI -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'ign-math[0-9]' + - 'gz-math[0-9]' + - 'main' jobs: focal-ci: @@ -8,7 +14,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@focal @@ -20,7 +26,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@jammy diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml index 736670e0..2332244b 100644 --- a/.github/workflows/triage.yml +++ b/.github/workflows/triage.yml @@ -10,10 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Add ticket to inbox - uses: technote-space/create-project-card-action@v1 + uses: actions/add-to-project@v0.5.0 with: - PROJECT: Core development - COLUMN: Inbox - GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} - CHECK_ORG_PROJECT: true - + project-url: https://github.com/orgs/gazebosim/projects/7 + github-token: ${{ secrets.TRIAGE_TOKEN }} From 82ef851a769df89e1b4969e632e4c0311678401f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 21 Dec 2023 09:47:50 -0800 Subject: [PATCH 05/16] Update CI badges in README (#571) Signed-off-by: Ian Chen --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index ca4bb6f2..e1d6e54d 100644 --- a/README.md +++ b/README.md @@ -10,9 +10,9 @@ Build | Status -- | -- Test coverage | [![codecov](https://codecov.io/gh/gazebosim/gz-math/branch/gz-math7/graph/badge.svg)](https://codecov.io/gh/gazebosim/gz-math/branch/gz-math7) -Ubuntu Focal | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_math-ci-gz-math7-focal-amd64)](https://build.osrfoundation.org/job/ignition_math-ci-gz-math7-focal-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_math-ci-gz-math7-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_math-ci-gz-math7-homebrew-amd64) -Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ign_math-ci-win)](https://build.osrfoundation.org/job/ign_math-ci-win) +Ubuntu Jammy | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-ci-gz-math7-jammy-amd64)](https://build.osrfoundation.org/job/gz_math-ci-gz-math7-jammy-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-ci-gz-math7-homebrew-amd64)](https://build.osrfoundation.org/job/gz_math-ci-gz-math7-homebrew-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=gz_math-7-win)](https://build.osrfoundation.org/job/gz_math-7-win) Gazebo Math, a component of [Gazebo](https://gazebosim.org), provides general purpose math classes and functions designed for robotic applications. From c5c3fc4f5f74a4b014834b8bfe4b995929dc2533 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 5 Jan 2024 14:36:33 -0600 Subject: [PATCH 06/16] Prepare for 6.15.1 (#572) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 29aef1da..42dc198e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-math6 VERSION 6.15.0) +project(ignition-math6 VERSION 6.15.1) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 3930f045..c20ffba2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,16 @@ ## Gazebo Math 6.x +## Gazebo Math 6.15.1 (2024-01-05) + +1. Replace CMake Python variables with new ones from FindPython3 module + * [Pull request #402](https://github.com/gazebosim/gz-math/pull/402) + +1. Suppress warnings on MSVC + * [Pull request #564](https://github.com/gazebosim/gz-math/pull/564) + +1. Infrastructure + * [Pull request #569](https://github.com/gazebosim/gz-math/pull/569) + ## Gazebo Math 6.15.0 (2023-09-01) 1. Fixes for testing in non standard architectures From 5b2522aa68be38d52bcd8f84736b4a20649f845a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 13 Feb 2024 23:16:05 +0100 Subject: [PATCH 07/16] Added MecanumDriveOdometry Python wrapper (#549) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/python_pybind11/CMakeLists.txt | 2 + .../src/MecanumDriveOdometry.cc | 74 +++++++++++ .../src/MecanumDriveOdometry.hh | 44 +++++++ .../src/_ignition_math_pybind11.cc | 3 + .../test/DiffDriveOdometry_TEST.py | 17 +-- .../test/MecanumDriveOdometry_TEST.py | 123 ++++++++++++++++++ 6 files changed, 255 insertions(+), 8 deletions(-) create mode 100644 src/python_pybind11/src/MecanumDriveOdometry.cc create mode 100644 src/python_pybind11/src/MecanumDriveOdometry.hh create mode 100644 src/python_pybind11/test/MecanumDriveOdometry_TEST.py diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 56442a58..6d009a92 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -31,6 +31,7 @@ pybind11_add_module(math SHARED src/Matrix3.cc src/Matrix4.cc src/Matrix6.cc + src/MecanumDriveOdometry.cc src/MovingWindowFilter.cc src/PID.cc src/Polynomial3.cc @@ -124,6 +125,7 @@ if (BUILD_TESTING) Matrix3_TEST Matrix4_TEST Matrix6_TEST + MecanumDriveOdometry_TEST MovingWindowFilter_TEST OrientedBox_TEST PID_TEST diff --git a/src/python_pybind11/src/MecanumDriveOdometry.cc b/src/python_pybind11/src/MecanumDriveOdometry.cc new file mode 100644 index 00000000..b299c182 --- /dev/null +++ b/src/python_pybind11/src/MecanumDriveOdometry.cc @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include "MecanumDriveOdometry.hh" + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathMecanumDriveOdometry(py::module &m, const std::string &typestr) +{ + using Class = gz::math::MecanumDriveOdometry; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol()) + .def(py::init(), py::arg("_windowSize") = 10) + .def("init", &Class::Init, "Initialize the odometry") + .def("initialized", &Class::Initialized, "Get whether Init has been called.") + .def("update", + &Class::Update, + "Updates the odometry class with latest wheels and " + "steerings position") + .def("heading", &Class::Heading, "Get the heading.") + .def("x", &Class::X, "Get the X position.") + .def("y", &Class::Y, "Get the Y position.") + .def("linear_velocity", + &Class::LinearVelocity, + "Get the linear velocity.") + .def("angular_velocity", + &Class::AngularVelocity, + "Get the angular velocity.") + .def("lateral_velocity", + &Class::LateralVelocity, + "Get the lateral velocity.") + .def("set_wheel_params", + &Class::SetWheelParams, + "Set the wheel parameters including the radius and separation.") + .def("set_velocity_rolling_window_size", + &Class::SetVelocityRollingWindowSize, + "Set the velocity rolling window size.") + .def("wheel_separation", &Class::WheelSeparation, "Get the wheel separation") + .def("wheel_base", &Class::WheelBase, "Get the wheel base") + .def("left_wheel_radius", + &Class::LeftWheelRadius, + "Get the left wheel radius") + .def("right_wheel_radius", + &Class::RightWheelRadius, + "Get the rightwheel radius"); + +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/MecanumDriveOdometry.hh b/src/python_pybind11/src/MecanumDriveOdometry.hh new file mode 100644 index 00000000..c6c4502e --- /dev/null +++ b/src/python_pybind11/src/MecanumDriveOdometry.hh @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_MATH_PYTHON__MECANUMDRIVEODOMETRY_HH_ +#define GZ_MATH_PYTHON__MECANUMDRIVEODOMETRY_HH_ + +#include +#include +#include + +#include +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an gz::math::MecanumDriveOdometry +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathMecanumDriveOdometry(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace ignition + +#endif // GZ_MATH_PYTHON__MECANUMDRIVEODOMETRY_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index c88ff50c..559f541e 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -36,6 +36,7 @@ #include "Matrix3.hh" #include "Matrix4.hh" #include "Matrix6.hh" +#include "MecanumDriveOdometry.hh" #include "MovingWindowFilter.hh" #include "OrientedBox.hh" #include "PID.hh" @@ -151,6 +152,8 @@ PYBIND11_MODULE(math, m) gz::math::python::defineMathMatrix6(m, "Matrix6"); + gz::math::python::defineMathMecanumDriveOdometry(m, "MecanumDriveOdometry"); + gz::math::python::defineMathTriangle(m, "Triangle"); gz::math::python::defineMathTriangle3(m, "Triangle3"); diff --git a/src/python_pybind11/test/DiffDriveOdometry_TEST.py b/src/python_pybind11/test/DiffDriveOdometry_TEST.py index 28284540..a5d11cee 100644 --- a/src/python_pybind11/test/DiffDriveOdometry_TEST.py +++ b/src/python_pybind11/test/DiffDriveOdometry_TEST.py @@ -14,6 +14,7 @@ import datetime import math +import time import unittest from ignition.math import Angle, DiffDriveOdometry @@ -38,15 +39,15 @@ def test_diff_drive_odometry(self): # Setup the wheel parameters, and initialize odom.set_wheel_params(wheelSeparation, wheelRadius, wheelRadius) - startTime = datetime.datetime.now() - odom.init(datetime.timedelta()) + startTime = datetime.timedelta(time.monotonic()) + odom.init(startTime) # Sleep for a little while, then update the odometry with the new wheel # position. time1 = startTime + datetime.timedelta(milliseconds=100) odom.update(Angle(1.0 * math.pi / 180), Angle(1.0 * math.pi / 180), - time1 - startTime) + time1) self.assertEqual(0.0, odom.heading().radian()) self.assertEqual(distPerDegree, odom.x()) self.assertEqual(0.0, odom.y()) @@ -60,7 +61,7 @@ def test_diff_drive_odometry(self): time2 = time1 + datetime.timedelta(milliseconds=100) odom.update(Angle(2.0 * math.pi / 180), Angle(2.0 * math.pi / 180), - time2 - startTime) + time2) self.assertEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) self.assertEqual(0.0, odom.y()) @@ -71,8 +72,8 @@ def test_diff_drive_odometry(self): self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3) # Initialize again, and odom values should be reset. - startTime = datetime.datetime.now() - odom.init(datetime.timedelta()) + startTime = datetime.timedelta(time.monotonic()) + odom.init(startTime) self.assertEqual(0.0, odom.heading().radian()) self.assertEqual(0.0, odom.x()) self.assertEqual(0.0, odom.y()) @@ -83,7 +84,7 @@ def test_diff_drive_odometry(self): time1 = startTime + datetime.timedelta(milliseconds=100) odom.update(Angle(2.0 * math.pi / 180), Angle(2.0 * math.pi / 180), - time1 - startTime) + time1) self.assertEqual(0.0, odom.heading().radian()) self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) self.assertEqual(0.0, odom.y()) @@ -97,7 +98,7 @@ def test_diff_drive_odometry(self): time2 = time1 + datetime.timedelta(milliseconds=100) odom.update(Angle(2.0 * math.pi / 180), Angle(3.0 * math.pi / 180), - time2 - startTime) + time2) # The heading should be the arc tangent of the linear distance traveled # by the right wheel (the left wheel was stationary) divided by the # wheel separation. diff --git a/src/python_pybind11/test/MecanumDriveOdometry_TEST.py b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py new file mode 100644 index 00000000..8cef51c1 --- /dev/null +++ b/src/python_pybind11/test/MecanumDriveOdometry_TEST.py @@ -0,0 +1,123 @@ +# Copyright (C) 2023 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 datetime +import math +import time +import unittest + +from ignition.math import MecanumDriveOdometry, Angle + + +class TestMecanumDriveOdometry(unittest.TestCase): + + def test_constructor(self): + odom = MecanumDriveOdometry() + self.assertAlmostEqual(0.0, odom.heading().radian()) + self.assertAlmostEqual(0.0, odom.x()) + self.assertAlmostEqual(0.0, odom.y()) + self.assertAlmostEqual(0.0, odom.linear_velocity()) + self.assertAlmostEqual(0.0, odom.lateral_velocity()) + self.assertAlmostEqual(0.0, odom.angular_velocity().radian()) + + wheelSeparation = 2.0 + wheelRadius = 0.5 + wheelCircumference = 2 * math.pi * wheelRadius + + # This is the linear distance traveled per degree of wheel rotation. + distPerDegree = wheelCircumference / 360.0 + + # Setup the wheel parameters, and initialize + odom.set_wheel_params(wheelSeparation, wheelRadius, wheelRadius,wheelRadius) + startTime = datetime.timedelta(time.monotonic()) + odom.init(startTime) + + # Sleep for a little while, then update the odometry with the new wheel + # position. + time1 = startTime + datetime.timedelta(milliseconds=100) + odom.update(Angle(math.radians(1.0)), + Angle(math.radians(1.0)), + Angle(math.radians(1.0)), + Angle(math.radians(1.0)), + time1) + self.assertAlmostEqual(0.0, odom.heading().radian()) + self.assertAlmostEqual(distPerDegree, odom.x()) + self.assertAlmostEqual(0.0, odom.y()) + # Linear velocity should be dist_traveled / time_elapsed. + self.assertAlmostEqual(distPerDegree / 0.1, odom.linear_velocity(), delta=1e-3) + # Angular velocity should be zero since the "robot" is traveling in a + # straight line. + self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3) + + # Sleep again, then update the odometry with the new wheel position. + time2 = time1 + datetime.timedelta(milliseconds=100) + odom.update(Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + time2) + self.assertAlmostEqual(0.0, odom.heading().radian()) + self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) + self.assertAlmostEqual(0.0, odom.y()) + # Linear velocity should be dist_traveled / time_elapsed. + self.assertAlmostEqual(distPerDegree / 0.1, odom.linear_velocity(), delta=1e-3) + # Angular velocity should be zero since the "robot" is traveling in a + # straight line. + self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3) + + # Initialize again, and odom values should be reset. + startTime = datetime.timedelta(time.monotonic()) + odom.init(startTime) + self.assertAlmostEqual(0.0, odom.heading().radian()) + self.assertAlmostEqual(0.0, odom.x()) + self.assertAlmostEqual(0.0, odom.y()) + self.assertAlmostEqual(0.0, odom.linear_velocity()) + self.assertAlmostEqual(0.0, odom.angular_velocity().radian()) + + # Sleep again, this time move 2 degrees in 100ms. + time1 = startTime + datetime.timedelta(milliseconds=100) + odom.update(Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + time1) + self.assertAlmostEqual(0.0, odom.heading().radian()) + self.assertAlmostEqual(distPerDegree * 2.0, odom.x(), delta=3e-6) + self.assertAlmostEqual(0.0, odom.y()) + # Linear velocity should be dist_traveled / time_elapsed. + self.assertAlmostEqual(distPerDegree * 2 / 0.1, odom.linear_velocity(), delta=1e-3) + # Angular velocity should be zero since the "robot" is traveling in a + # straight line. + self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3) + + # Sleep again, this time move 2 degrees in 100ms. + odom.init(startTime) + time1 = startTime + datetime.timedelta(milliseconds=100) + odom.update(Angle(math.radians(-2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(2.0)), + Angle(math.radians(-2.0)), + time1) + self.assertAlmostEqual(0.0, odom.heading().radian()) + self.assertAlmostEqual(distPerDegree * 2.0, odom.y(), delta=3e-6) + # self.assertAlmostEqual(0.0, odom.y()) + # Linear velocity should be dist_traveled / time_elapsed. + self.assertAlmostEqual(distPerDegree * 2 / 0.1, odom.lateral_velocity(), delta=1e-3) + # Angular velocity should be zero since the "robot" is traveling in a + # straight line. + self.assertAlmostEqual(0.0, odom.angular_velocity().radian(), delta=1e-3) + + +if __name__ == '__main__': + unittest.main() From 1649f76ddee22971c10c9c4bd51d16653145f8b9 Mon Sep 17 00:00:00 2001 From: Jagadeeshan S Date: Thu, 22 Feb 2024 12:15:18 +0530 Subject: [PATCH 08/16] Expose non-const reference to edges Signed-off-by: Jagadeeshan S --- include/gz/math/graph/Graph.hh | 21 +++++++++++++++++---- src/graph/GraphUndirected_TEST.cc | 2 ++ 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/include/gz/math/graph/Graph.hh b/include/gz/math/graph/Graph.hh index e5c4395f..8475b541 100644 --- a/include/gz/math/graph/Graph.hh +++ b/include/gz/math/graph/Graph.hh @@ -423,7 +423,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no outgoing edges. - public: const EdgeRef_M IncidentsFrom(const VertexId &_vertex) + public: EdgeRef_M IncidentsFrom(const VertexId &_vertex) const { EdgeRef_M res; @@ -448,7 +448,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no outgoing edges. - public: const EdgeRef_M IncidentsFrom( + public: EdgeRef_M IncidentsFrom( const Vertex &_vertex) const { return this->IncidentsFrom(_vertex.Id()); @@ -459,7 +459,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no incoming edges. - public: const EdgeRef_M IncidentsTo( + public: EdgeRef_M IncidentsTo( const VertexId &_vertex) const { EdgeRef_M res; @@ -484,7 +484,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no incoming edges. - public: const EdgeRef_M IncidentsTo(const Vertex &_vertex) + public: EdgeRef_M IncidentsTo(const Vertex &_vertex) const { return this->IncidentsTo(_vertex.Id()); @@ -681,6 +681,19 @@ namespace graph return iter->second; } + /// \brief Get a mutable reference to an edge using its Id. + /// \param[in] _id The Id of the edge. + /// \return A mutable reference to the edge with Id = _id or NullEdge if + /// not found. + public: EdgeType &EdgeFromId(const EdgeId &_id) + { + auto iter = this->edges.find(_id); + if (iter == this->edges.end()) + return EdgeType::NullEdge; + + return iter->second; + } + /// \brief Stream insertion operator. The output uses DOT graph /// description language. /// \param[out] _out The output stream. diff --git a/src/graph/GraphUndirected_TEST.cc b/src/graph/GraphUndirected_TEST.cc index a49ee433..20f5e920 100644 --- a/src/graph/GraphUndirected_TEST.cc +++ b/src/graph/GraphUndirected_TEST.cc @@ -368,6 +368,8 @@ TEST(UndirectedGraphTest, AddEdge) auto edge = graph.EdgeFromId(e2.Id()); EXPECT_DOUBLE_EQ(5.0, edge.Data()); EXPECT_DOUBLE_EQ(6.0, edge.Weight()); + edge.Data() = 7.0; + EXPECT_DOUBLE_EQ(7.0, edge.Data()); // Check that the edges point to the right vertices. EXPECT_EQ(0u, e0.Vertices().first); From 4db514b722d2c7a59cd1feb7e077883579bb5d79 Mon Sep 17 00:00:00 2001 From: Jagadeeshan S Date: Fri, 1 Mar 2024 11:02:02 +0530 Subject: [PATCH 09/16] Revert changes break ABI Signed-off-by: Jagadeeshan S --- include/gz/math/graph/Graph.hh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/gz/math/graph/Graph.hh b/include/gz/math/graph/Graph.hh index 8475b541..0b48ea2d 100644 --- a/include/gz/math/graph/Graph.hh +++ b/include/gz/math/graph/Graph.hh @@ -423,7 +423,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no outgoing edges. - public: EdgeRef_M IncidentsFrom(const VertexId &_vertex) + public: const EdgeRef_M IncidentsFrom(const VertexId &_vertex) const { EdgeRef_M res; @@ -448,7 +448,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no outgoing edges. - public: EdgeRef_M IncidentsFrom( + public: const EdgeRef_M IncidentsFrom( const Vertex &_vertex) const { return this->IncidentsFrom(_vertex.Id()); @@ -459,7 +459,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no incoming edges. - public: EdgeRef_M IncidentsTo( + public: const EdgeRef_M IncidentsTo( const VertexId &_vertex) const { EdgeRef_M res; @@ -484,7 +484,7 @@ namespace graph /// \return A map of edges, where keys are Ids and values are /// references to the edges. An empty map is returned when the provided /// vertex does not exist, or when there are no incoming edges. - public: EdgeRef_M IncidentsTo(const Vertex &_vertex) + public: const EdgeRef_M IncidentsTo(const Vertex &_vertex) const { return this->IncidentsTo(_vertex.Id()); From 9f5559212059871c9d1e458dcfe623a0c3667110 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 15 Mar 2024 10:16:50 -0500 Subject: [PATCH 10/16] Prepare for 7.4.0 (#583) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 41 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 42 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 64c6b7c5..8e1e0be5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-math7 VERSION 7.3.0) +project(gz-math7 VERSION 7.4.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index f4edd5f4..488ae993 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,46 @@ ## Gazebo Math 7.x +### Gazebo Math 7.4.0 (2024-03-14) + +1. Added MecanumDriveOdometry Python wrapper + * [Pull request #549](https://github.com/gazebosim/gz-math/pull/549) + +1. Update CI badges in README + * [Pull request #571](https://github.com/gazebosim/gz-math/pull/571) + +1. Infrastructure + * [Pull request #569](https://github.com/gazebosim/gz-math/pull/569) + +1. Suppress warnings on MSVC + * [Pull request #564](https://github.com/gazebosim/gz-math/pull/564) + +1. Remove the use of numeric_limits in appendToStream test + * [Pull request #553](https://github.com/gazebosim/gz-math/pull/553) + +1. Replace CMake Python variables with new ones from FindPython3 module + * [Pull request #402](https://github.com/gazebosim/gz-math/pull/402) + +1. Fix `Matrix3_TEST.py` on Windows with conda-forge dependencies + * [Pull request #561](https://github.com/gazebosim/gz-math/pull/561) + +1. Fix small typo cppgetstarted.md + * [Pull request #560](https://github.com/gazebosim/gz-math/pull/560) + +1. Update Ubuntu Binary installation since apt-key is deprecated + * [Pull request #559](https://github.com/gazebosim/gz-math/pull/559) + +1. Update file tree in README to point out pybind11 + * [Pull request #558](https://github.com/gazebosim/gz-math/pull/558) + +1. Update tutorial/color.md + * [Pull request #557](https://github.com/gazebosim/gz-math/pull/557) + +1. ign->gz in README.md + * [Pull request #556](https://github.com/gazebosim/gz-math/pull/556) + +1. Update example_triangle.md + * [Pull request #555](https://github.com/gazebosim/gz-math/pull/555) + ### Gazebo Math 7.3.0 (2023-08-29) 1. Adds a validity check for Sessions created using the `TimeVaryingVolumetricGrid` From 5bf896fd19c09dd5bd9f2c4381758d15af6486cd Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Mon, 1 Apr 2024 15:37:47 -0700 Subject: [PATCH 11/16] Add missing eigen3.hh header for bazel build (#585) Signed-off-by: Shameek Ganguly --- eigen3/BUILD.bazel | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/eigen3/BUILD.bazel b/eigen3/BUILD.bazel index 2b3ba953..4107f59b 100644 --- a/eigen3/BUILD.bazel +++ b/eigen3/BUILD.bazel @@ -1,21 +1,28 @@ load( "@gz//bazel/skylark:build_defs.bzl", - "GZ_FEATURES", "GZ_ROOT", "GZ_VISIBILITY", - "gz_configure_header", - "gz_export_header", "gz_include_header", ) +package(default_applicable_licenses = [GZ_ROOT + "math:license"]) + public_headers = glob([ "include/gz/math/eigen3/*.hh", ]) +gz_include_header( + name = "eigen3_hh_genrule", + out = "include/gz/math/eigen3.hh", + hdrs = public_headers, +) + cc_library( name = "eigen3", srcs = public_headers, - hdrs = public_headers, + hdrs = public_headers + [ + "include/gz/math/eigen3.hh", + ], includes = ["include"], visibility = GZ_VISIBILITY, deps = [ From f0d4e59e66b54949937ceed725e50ba26177d667 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 5 Apr 2024 14:27:26 -0500 Subject: [PATCH 12/16] bazel: correctly export license (#586) Signed-off-by: Michael Carroll --- BUILD.bazel | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/BUILD.bazel b/BUILD.bazel index b89cd10a..968198e9 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -11,13 +11,23 @@ load( "@gz//bazel/lint:lint.bzl", "add_lint_tests", ) +load( + "@rules_license//rules:license.bzl", + "license", +) package( + default_applicable_licenses = [GZ_ROOT + "math:license"], default_visibility = GZ_VISIBILITY, features = GZ_FEATURES, ) -licenses(["notice"]) # Apache-2.0 +license( + name = "license", + package_name = "gz-math", +) + +licenses(["notice"]) exports_files(["LICENSE"]) From a76819bfa99da5dd79b7de224d80c1058e03ae3e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 19 Apr 2024 14:11:47 -0500 Subject: [PATCH 13/16] Add package.xml (#581) * Add workflow Signed-off-by: Addisu Z. Taddese --- .github/workflows/package_xml.yml | 11 +++++++++++ README.md | 2 +- package.xml | 25 +++++++++++++++++++++++++ 3 files changed, 37 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/package_xml.yml create mode 100644 package.xml diff --git a/.github/workflows/package_xml.yml b/.github/workflows/package_xml.yml new file mode 100644 index 00000000..4bd4a9aa --- /dev/null +++ b/.github/workflows/package_xml.yml @@ -0,0 +1,11 @@ +name: Validate package.xml + +on: + pull_request: + +jobs: + package-xml: + runs-on: ubuntu-latest + name: Validate package.xml + steps: + - uses: gazebo-tooling/action-gz-ci/validate_package_xml@jammy diff --git a/README.md b/README.md index e1d6e54d..22163aeb 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Gazebo Math : Math classes and functions for robot applications -**Maintainer:** nate AT openrobotics DOT org +**Maintainer:** scpeters AT openrobotics DOT org [![GitHub open issues](https://img.shields.io/github/issues-raw/gazebosim/gz-math.svg)](https://github.com/gazebosim/gz-math/issues) [![GitHub open pull requests](https://img.shields.io/github/issues-pr-raw/gazebosim/gz-math.svg)](https://github.com/gazebosim/gz-math/pulls) diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..5e6ff7ee --- /dev/null +++ b/package.xml @@ -0,0 +1,25 @@ + + + + gz-math7 + 7.4.0 + Gazebo Math : Math classes and functions for robot applications + Steve Peters + Aditya Pande + Apache License 2.0 + https://github.com/gazebosim/gz-math + + cmake + + gz-cmake3 + pybind11-dev + + eigen + gz-utils2 + + python3-pytest + + + cmake + + From 02e37a63e9e24959424e1b2463a6dbe9195a79bb Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 2 May 2024 10:27:28 -0700 Subject: [PATCH 14/16] Enable 24.04 CI on harmonic (#590) * Don't install python3-distutils on 22.04, 24.04 * Remove old ruby cmake code Signed-off-by: Steve Peters --- .github/ci/packages-focal.apt | 1 + .github/ci/packages.apt | 1 - .github/workflows/ci.yml | 9 +++++++++ src/ruby/CMakeLists.txt | 6 +----- 4 files changed, 11 insertions(+), 6 deletions(-) create mode 100644 .github/ci/packages-focal.apt diff --git a/.github/ci/packages-focal.apt b/.github/ci/packages-focal.apt new file mode 100644 index 00000000..87405495 --- /dev/null +++ b/.github/ci/packages-focal.apt @@ -0,0 +1 @@ +python3-distutils diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 09f6e9c5..87b3bbd5 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -2,7 +2,6 @@ libeigen3-dev libgz-cmake3-dev libgz-utils2-dev libpython3-dev -python3-distutils python3-pybind11 python3-pytest ruby-dev diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 932726d5..93ceece0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -32,3 +32,12 @@ jobs: - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@jammy + noble-ci: + runs-on: ubuntu-latest + name: Ubuntu Noble CI + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Compile and test + id: ci + uses: gazebo-tooling/action-gz-ci@noble diff --git a/src/ruby/CMakeLists.txt b/src/ruby/CMakeLists.txt index 8ad32202..7c541051 100644 --- a/src/ruby/CMakeLists.txt +++ b/src/ruby/CMakeLists.txt @@ -44,11 +44,7 @@ if (RUBY_FOUND) # Create the ruby library set(CMAKE_SWIG_OUTDIR "${CMAKE_BINARY_DIR}/lib/ruby") - if(CMAKE_VERSION VERSION_GREATER 3.8.0) - SWIG_ADD_LIBRARY(${SWIG_RB_LIB} LANGUAGE ruby SOURCES ruby.i ${swig_i_files}) - else() - SWIG_ADD_MODULE(${SWIG_RB_LIB} ruby ruby.i ${swig_i_files}) - endif() + SWIG_ADD_LIBRARY(${SWIG_RB_LIB} LANGUAGE ruby SOURCES ruby.i ${swig_i_files}) # Suppress warnings on SWIG-generated files target_compile_options(${SWIG_RB_LIB} PRIVATE From 3a36b9d41e7d3c4de6d4b679c9ca0dec857fb7ab Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Mon, 17 Jun 2024 15:11:40 -0400 Subject: [PATCH 15/16] Backport: Adding cone primitives. (#594) Signed-off-by: Benjamin Perseghetti --- include/gz/math/Cone.hh | 193 +++++++++++++++++++ include/gz/math/Helpers.hh | 5 + include/gz/math/MassMatrix3.hh | 80 ++++++++ include/gz/math/detail/Cone.hh | 172 +++++++++++++++++ src/Cone_TEST.cc | 147 ++++++++++++++ src/Helpers.i | 2 + src/Helpers_TEST.cc | 3 + src/MassMatrix3_TEST.cc | 51 +++++ src/python_pybind11/CMakeLists.txt | 1 + src/python_pybind11/src/Cone.hh | 125 ++++++++++++ src/python_pybind11/src/Helpers.cc | 23 +++ src/python_pybind11/src/MassMatrix3.hh | 29 +++ src/python_pybind11/src/_gz_math_pybind11.cc | 3 + src/python_pybind11/test/Cone_TEST.py | 126 ++++++++++++ src/python_pybind11/test/Helpers_TEST.py | 2 +- src/ruby/Cone.i | 80 ++++++++ src/ruby/Helpers.i | 3 + src/ruby/MassMatrix3.i | 13 +- 18 files changed, 1056 insertions(+), 2 deletions(-) create mode 100644 include/gz/math/Cone.hh create mode 100644 include/gz/math/detail/Cone.hh create mode 100644 src/Cone_TEST.cc create mode 100644 src/python_pybind11/src/Cone.hh create mode 100644 src/python_pybind11/test/Cone_TEST.py create mode 100644 src/ruby/Cone.i diff --git a/include/gz/math/Cone.hh b/include/gz/math/Cone.hh new file mode 100644 index 00000000..716f2b6b --- /dev/null +++ b/include/gz/math/Cone.hh @@ -0,0 +1,193 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_MATH_CONE_HH_ +#define GZ_MATH_CONE_HH_ + +#include +#include "gz/math/MassMatrix3.hh" +#include "gz/math/Material.hh" +#include "gz/math/Quaternion.hh" + +namespace gz +{ + namespace math + { + // Foward declarations + class ConePrivate; + + // Inline bracket to help doxygen filtering. + inline namespace GZ_MATH_VERSION_NAMESPACE { + // + /// \class Cone Cone.hh gz/math/Cone.hh + /// \brief A representation of a cone. + /// + /// The cone class supports defining a cone with a radius, + /// length, rotational offset, and material properties. Radius and + /// length are in meters. See Material for more on material properties. + /// By default, a cone's length is aligned with the Z axis where the base + /// of the cone is proximal to the origin and vertex points in positive Z. + /// The rotational offset encodes a rotation from the z axis. + template + class Cone + { + /// \brief Default constructor. The default radius and length are both + /// zero. The default rotational offset is + /// Quaternion::Identity. + public: Cone() = default; + + /// \brief Construct a cone with a length, radius, and optionally + /// a rotational offset. + /// \param[in] _length Length of the cone. + /// \param[in] _radius Radius of the cone. + /// \param[in] _rotOffset Rotational offset of the cone. + public: Cone(const Precision _length, const Precision _radius, + const Quaternion &_rotOffset = + Quaternion::Identity); + + /// \brief Construct a cone with a length, radius, material and + /// optionally a rotational offset. + /// \param[in] _length Length of the cone. + /// \param[in] _radius Radius of the cone. + /// \param[in] _mat Material property for the cone. + /// \param[in] _rotOffset Rotational offset of the cone. + public: Cone(const Precision _length, const Precision _radius, + const Material &_mat, + const Quaternion &_rotOffset = + Quaternion::Identity); + + /// \brief Get the radius in meters. + /// \return The radius of the cone in meters. + public: Precision Radius() const; + + /// \brief Set the radius in meters. + /// \param[in] _radius The radius of the cone in meters. + public: void SetRadius(const Precision _radius); + + /// \brief Get the length in meters. + /// \return The length of the cone in meters. + public: Precision Length() const; + + /// \brief Set the length in meters. + /// \param[in] _length The length of the cone in meters. + public: void SetLength(const Precision _length); + + /// \brief Get the rotational offset. By default, a cone's length + /// is aligned with the Z axis. The rotational offset encodes + /// a rotation from the z axis. + /// \return The cone's rotational offset. + /// \sa void SetRotationalOffset(const Quaternion &_rot) + public: Quaternion RotationalOffset() const; + + /// \brief Set the rotation offset. + /// \param[in] _rotOffset rotational offset quaternion. + /// See Quaternion RotationalOffset() for details on the + /// rotational offset. + /// \sa Quaternion RotationalOffset() const + public: void SetRotationalOffset( + const Quaternion &_rotOffset); + + /// \brief Get the material associated with this cone. + /// \return The material assigned to this cone + public: const Material &Mat() const; + + /// \brief Set the material associated with this cone. + /// \param[in] _mat The material assigned to this cone + public: void SetMat(const Material &_mat); + + /// \brief Get the mass matrix for this cone. This function + /// is only meaningful if the cone's radius, length, and material + /// have been set. Optionally, set the rotational offset. + /// \param[out] _massMat The computed mass matrix will be stored + /// here. + /// \return False if computation of the mass matrix failed, which + /// could be due to an invalid radius (<=0), length (<=0), or density + /// (<=0). + public: bool MassMatrix(MassMatrix3d &_massMat) const; + + /// \brief Get the mass matrix for this cone. This function + /// is only meaningful if the cone's radius, length, and material + /// have been set. Optionally, set the rotational offset. + /// \return The computed mass matrix if parameters are valid + /// (radius > 0), (length > 0) and (density > 0). Otherwise + /// std::nullopt is returned. + public: std::optional< MassMatrix3 > MassMatrix() const; + + /// \brief Check if this cone is equal to the provided cone. + /// Radius, length, and material properties will be checked. + public: bool operator==(const Cone &_cone) const; + + /// \brief Get the volume of the cone in m^3. + /// \return Volume of the cone in m^3. + public: Precision Volume() const; + + /// \brief Compute the cone's density given a mass value. The + /// cone is assumed to be solid with uniform density. This + /// function requires the cone's radius and length to be set to + /// values greater than zero. The Material of the cone is ignored. + /// \param[in] _mass Mass of the cone, in kg. This value should be + /// greater than zero. + /// \return Density of the cone in kg/m^3. A negative value is + /// returned if radius, length or _mass is <= 0. + public: Precision DensityFromMass(const Precision _mass) const; + + /// \brief Set the density of this cone based on a mass value. + /// Density is computed using + /// Precision DensityFromMass(const Precision _mass) const. The + /// cone is assumed to be solid with uniform density. This + /// function requires the cone's radius and length to be set to + /// values greater than zero. The existing Material density value is + /// overwritten only if the return value from this true. + /// \param[in] _mass Mass of the cone, in kg. This value should be + /// greater than zero. + /// \return True if the density was set. False is returned if the + /// cone's radius, length, or the _mass value are <= 0. + /// \sa Precision DensityFromMass(const Precision _mass) const + public: bool SetDensityFromMass(const Precision _mass); + + /// \brief Radius of the cone. + private: Precision radius = 0.0; + + /// \brief Length of the cone. + private: Precision length = 0.0; + + /// \brief the cone's material. + private: Material material; + + /// \brief Rotational offset. + private: Quaternion rotOffset = + Quaternion::Identity; + }; + + /// \typedef Cone Conei + /// \brief Cone with integer precision. + typedef Cone Conei; + + /// \typedef Cone Coned + /// \brief Cone with double precision. + typedef Cone Coned; + + /// \typedef Cone Conef + /// \brief Cone with float precision. + typedef Cone Conef; + } + } +} +#include "gz/math/detail/Cone.hh" + +#endif diff --git a/include/gz/math/Helpers.hh b/include/gz/math/Helpers.hh index 609c97ad..f0dc4b5d 100644 --- a/include/gz/math/Helpers.hh +++ b/include/gz/math/Helpers.hh @@ -67,6 +67,11 @@ constexpr T IGN_MASSMATRIX3_DEFAULT_TOLERANCE = T(10); /// \param[in] _radius Sphere radius #define GZ_SPHERE_VOLUME(_radius) (4.0*GZ_PI*std::pow(_radius, 3)/3.0) +/// \brief Compute cone volume +/// \param[in] _r Cone base radius +/// \param[in] _l Cone length +#define GZ_CONE_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2) / 3.0) + /// \brief Compute cylinder volume /// \param[in] _r Cylinder base radius /// \param[in] _l Cylinder length diff --git a/include/gz/math/MassMatrix3.hh b/include/gz/math/MassMatrix3.hh index bd60a236..40b7fb3f 100644 --- a/include/gz/math/MassMatrix3.hh +++ b/include/gz/math/MassMatrix3.hh @@ -18,6 +18,7 @@ #define GZ_MATH_MASSMATRIX3_HH_ #include +#include #include #include #include @@ -930,6 +931,85 @@ namespace gz return this->SetMoi(R * L * R.Transposed()); } + /// \brief Set inertial properties based on a Material and equivalent + /// cone aligned with Z axis. + /// \param[in] _mat Material that specifies a density. Uniform density + /// is used. + /// \param[in] _length Length of cone along Z axis. + /// \param[in] _radius Radius of cone. + /// \param[in] _rot Rotational offset of equivalent cone. + /// \return True if inertial properties were set successfully. + public: bool SetFromConeZ(const Material &_mat, + const T _length, + const T _radius, + const Quaternion &_rot = + Quaternion::Identity) + { + // Check that density, _radius and _length are strictly positive + // and that quaternion is valid + if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + T volume = GZ_PI * _radius * _radius * _length / 3.0; + return this->SetFromConeZ(_mat.Density() * volume, + _length, _radius, _rot); + } + + /// \brief Set inertial properties based on mass and equivalent cone + /// aligned with Z axis. + /// \param[in] _mass Mass to set. + /// \param[in] _length Length of cone along Z axis. + /// \param[in] _radius Radius of cone. + /// \param[in] _rot Rotational offset of equivalent cone. + /// \return True if inertial properties were set successfully. + public: bool SetFromConeZ(const T _mass, + const T _length, + const T _radius, + const Quaternion &_rot = + Quaternion::Identity) + { + // Check that _mass, _radius and _length are strictly positive + // and that quaternion is valid + if (_mass <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + this->SetMass(_mass); + return this->SetFromConeZ(_length, _radius, _rot); + } + + /// \brief Set inertial properties based on equivalent cone + /// aligned with Z axis using the current mass value. + /// \param[in] _length Length of cone along Z axis. + /// \param[in] _radius Radius of cone. + /// \param[in] _rot Rotational offset of equivalent cone. + /// \return True if inertial properties were set successfully. + public: bool SetFromConeZ(const T _length, + const T _radius, + const Quaternion &_rot) + { + // Check that _mass and _size are strictly positive + // and that quaternion is valid + if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 || + _rot == Quaternion::Zero) + { + return false; + } + + // Diagonal matrix L with principal moments + T radius2 = std::pow(_radius, 2); + Matrix3 L; + L(0, 0) = 3.0 * this->mass * (4.0 * radius2 + + std::pow(_length, 2)) / 80.0; + L(1, 1) = L(0, 0); + L(2, 2) = 3.0 * this->mass * radius2 / 10.0; + Matrix3 R(_rot); + return this->SetMoi(R * L * R.Transposed()); + } + /// \brief Set inertial properties based on a Material and equivalent /// cylinder aligned with Z axis. /// \param[in] _mat Material that specifies a density. Uniform density diff --git a/include/gz/math/detail/Cone.hh b/include/gz/math/detail/Cone.hh new file mode 100644 index 00000000..ddd7526a --- /dev/null +++ b/include/gz/math/detail/Cone.hh @@ -0,0 +1,172 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_MATH_DETAIL_CONE_HH_ +#define GZ_MATH_DETAIL_CONE_HH_ + +#include + +namespace gz +{ +namespace math +{ + +////////////////////////////////////////////////// +template +Cone::Cone(const T _length, const T _radius, + const Quaternion &_rotOffset) +{ + this->length = _length; + this->radius = _radius; + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +Cone::Cone(const T _length, const T _radius, + const Material &_mat, const Quaternion &_rotOffset) +{ + this->length = _length; + this->radius = _radius; + this->material = _mat; + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +T Cone::Radius() const +{ + return this->radius; +} + +////////////////////////////////////////////////// +template +void Cone::SetRadius(const T _radius) +{ + this->radius = _radius; +} + +////////////////////////////////////////////////// +template +T Cone::Length() const +{ + return this->length; +} + +////////////////////////////////////////////////// +template +void Cone::SetLength(const T _length) +{ + this->length = _length; +} + +////////////////////////////////////////////////// +template +Quaternion Cone::RotationalOffset() const +{ + return this->rotOffset; +} + +////////////////////////////////////////////////// +template +void Cone::SetRotationalOffset(const Quaternion &_rotOffset) +{ + this->rotOffset = _rotOffset; +} + +////////////////////////////////////////////////// +template +const Material &Cone::Mat() const +{ + return this->material; +} + +////////////////////////////////////////////////// +template +void Cone::SetMat(const Material &_mat) +{ + this->material = _mat; +} + +////////////////////////////////////////////////// +template +bool Cone::operator==(const Cone &_cone) const +{ + return equal(this->radius, _cone.Radius()) && + equal(this->length, _cone.Length()) && + this->material == _cone.Mat(); +} + +////////////////////////////////////////////////// +template +bool Cone::MassMatrix(MassMatrix3d &_massMat) const +{ + return _massMat.SetFromConeZ( + this->material, this->length, + this->radius, this->rotOffset); +} + +////////////////////////////////////////////////// +template +std::optional < MassMatrix3 > Cone::MassMatrix() const +{ + gz::math::MassMatrix3 _massMat; + + if(!_massMat.SetFromConeZ( + this->material, this->length, + this->radius, this->rotOffset)) + { + return std::nullopt; + } + else + { + return std::make_optional(_massMat); + } +} + +////////////////////////////////////////////////// +template +T Cone::Volume() const +{ + return GZ_PI * std::pow(this->radius, 2) * + this->length / 3.0; +} + +////////////////////////////////////////////////// +template +bool Cone::SetDensityFromMass(const T _mass) +{ + T newDensity = this->DensityFromMass(_mass); + if (newDensity > 0) + this->material.SetDensity(newDensity); + return newDensity > 0; +} + +////////////////////////////////////////////////// +template +T Cone::DensityFromMass(const T _mass) const +{ + if (this->radius <= 0 || this->length <=0 || _mass <= 0) + return -1.0; + + return _mass / this->Volume(); +} + +} // namespace math +} // namespace gz +#endif diff --git a/src/Cone_TEST.cc b/src/Cone_TEST.cc new file mode 100644 index 00000000..a70fe206 --- /dev/null +++ b/src/Cone_TEST.cc @@ -0,0 +1,147 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include + +#include "gz/math/Cone.hh" + +using namespace gz; + +///////////////////////////////////////////////// +TEST(ConeTest, Constructor) +{ + // Default constructor + { + math::Coned cone; + EXPECT_DOUBLE_EQ(0.0, cone.Length()); + EXPECT_DOUBLE_EQ(0.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond::Identity, cone.RotationalOffset()); + EXPECT_EQ(math::Material(), cone.Mat()); + + math::Coned cone2; + EXPECT_EQ(cone, cone2); + } + + // Length and radius constructor + { + math::Coned cone(1.0, 2.0); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + EXPECT_DOUBLE_EQ(2.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond::Identity, cone.RotationalOffset()); + EXPECT_EQ(math::Material(), cone.Mat()); + + math::Coned cone2(1.0, 2.0); + EXPECT_EQ(cone, cone2); + } + + // Length, radius, and rot constructor + { + math::Coned cone(1.0, 2.0, math::Quaterniond(0.1, 0.2, 0.3)); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + EXPECT_DOUBLE_EQ(2.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond(0.1, 0.2, 0.3), + cone.RotationalOffset()); + EXPECT_EQ(math::Material(), cone.Mat()); + + math::Coned cone2(1.0, 2.0, math::Quaterniond(0.1, 0.2, 0.3)); + EXPECT_EQ(cone, cone2); + } + + // Length, radius, mat and rot constructor + { + math::Coned cone(1.0, 2.0, + math::Material(math::MaterialType::WOOD), + math::Quaterniond(0.1, 0.2, 0.3)); + EXPECT_DOUBLE_EQ(1.0, cone.Length()); + EXPECT_DOUBLE_EQ(2.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond(0.1, 0.2, 0.3), + cone.RotationalOffset()); + EXPECT_EQ(math::Material(math::MaterialType::WOOD), cone.Mat()); + + math::Coned cone2(1.0, 2.0, + math::Material(math::MaterialType::WOOD), + math::Quaterniond(0.1, 0.2, 0.3)); + EXPECT_EQ(cone, cone2); + } +} + +////////////////////////////////////////////////// +TEST(ConeTest, Mutators) +{ + math::Coned cone; + EXPECT_DOUBLE_EQ(0.0, cone.Length()); + EXPECT_DOUBLE_EQ(0.0, cone.Radius()); + EXPECT_EQ(math::Quaterniond::Identity, cone.RotationalOffset()); + EXPECT_EQ(math::Material(), cone.Mat()); + + cone.SetLength(100.1); + cone.SetRadius(.123); + cone.SetRotationalOffset(math::Quaterniond(1.2, 2.3, 3.4)); + cone.SetMat(math::Material(math::MaterialType::PINE)); + + EXPECT_DOUBLE_EQ(100.1, cone.Length()); + EXPECT_DOUBLE_EQ(.123, cone.Radius()); + EXPECT_EQ(math::Quaterniond(1.2, 2.3, 3.4), + cone.RotationalOffset()); + EXPECT_EQ(math::Material(math::MaterialType::PINE), cone.Mat()); +} + +////////////////////////////////////////////////// +TEST(ConeTest, VolumeAndDensity) +{ + double mass = 1.0; + math::Coned cone(1.0, 0.001); + double expectedVolume = (GZ_PI * std::pow(0.001, 2) * 1.0 / 3.0); + EXPECT_DOUBLE_EQ(expectedVolume, cone.Volume()); + + double expectedDensity = mass / expectedVolume; + EXPECT_DOUBLE_EQ(expectedDensity, cone.DensityFromMass(mass)); + + // Bad density + math::Coned cone2; + EXPECT_GT(0.0, cone2.DensityFromMass(mass)); +} + +////////////////////////////////////////////////// +TEST(ConeTest, Mass) +{ + double mass = 2.0; + double l = 2.0; + double r = 0.1; + math::Coned cone(l, r); + cone.SetDensityFromMass(mass); + + math::MassMatrix3d massMat; + double ixxIyy = (3.0/80.0) * mass * (4*r*r + l*l); + double izz = (3.0/10.0) * mass * r * r; + + math::MassMatrix3d expectedMassMat; + expectedMassMat.SetInertiaMatrix(ixxIyy, ixxIyy, izz, 0.0, 0.0, 0.0); + expectedMassMat.SetMass(mass); + + cone.MassMatrix(massMat); + EXPECT_EQ(expectedMassMat, massMat); + EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMat.Mass()); + + auto massMatOpt = cone.MassMatrix(); + ASSERT_NE(std::nullopt, massMatOpt); + EXPECT_EQ(expectedMassMat, *massMatOpt); + EXPECT_EQ(expectedMassMat.DiagonalMoments(), massMatOpt->DiagonalMoments()); + EXPECT_DOUBLE_EQ(expectedMassMat.Mass(), massMatOpt->Mass()); +} diff --git a/src/Helpers.i b/src/Helpers.i index b49db489..76575cf8 100644 --- a/src/Helpers.i +++ b/src/Helpers.i @@ -38,6 +38,7 @@ constexpr T IGN_MASSMATRIX3_DEFAULT_TOLERANCE = T(10); #define GZ_SQRT2 1.41421356237309504880 #define GZ_SPHERE_VOLUME(_radius) (4.0 * GZ_PI * std::pow(_radius, 3)/3.0) +#define GZ_CONE_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2) / 3.0) #define GZ_CYLINDER_VOLUME(_r, _l) (_l * GZ_PI * std::pow(_r, 2)) #define GZ_BOX_VOLUME(_x, _y, _z) (_x *_y * _z) #define GZ_BOX_VOLUME_V(_v) (_v.X() *_v.Y() * _v.Z()) @@ -50,6 +51,7 @@ constexpr T IGN_MASSMATRIX3_DEFAULT_TOLERANCE = T(10); // TODO(CH3): Deprecated. Remove on tock. #define IGN_SPHERE_VOLUME(_radius) GZ_SPHERE_VOLUME(_radius) +#define IGN_CONE_VOLUME(_r, _l) GZ_CONE_VOLUME(_r, _l) #define IGN_CYLINDER_VOLUME(_r, _l) GZ_CYLINDER_VOLUME(_r, _l) #define IGN_BOX_VOLUME(_x, _y, _z) GZ_BOX_VOLUME(_x, _y, _z) #define IGN_BOX_VOLUME_V(_v) GZ_BOX_VOLUME_V(_v) diff --git a/src/Helpers_TEST.cc b/src/Helpers_TEST.cc index fe09adea..c230889f 100644 --- a/src/Helpers_TEST.cc +++ b/src/Helpers_TEST.cc @@ -429,6 +429,9 @@ TEST(HelpersTest, Volume) EXPECT_DOUBLE_EQ(GZ_SPHERE_VOLUME(0.1), 4.0*GZ_PI*std::pow(.1, 3)/3.0); EXPECT_DOUBLE_EQ(GZ_SPHERE_VOLUME(-1.1), 4.0*GZ_PI*std::pow(-1.1, 3)/3.0); + EXPECT_DOUBLE_EQ(GZ_CONE_VOLUME(0.5, 2.0), 2 * GZ_PI * std::pow(.5, 2) / 3.0); + EXPECT_DOUBLE_EQ(GZ_CONE_VOLUME(1, -1), -1 * GZ_PI * std::pow(1, 2) / 3.0); + EXPECT_DOUBLE_EQ(GZ_CYLINDER_VOLUME(0.5, 2.0), 2 * GZ_PI * std::pow(.5, 2)); EXPECT_DOUBLE_EQ(GZ_CYLINDER_VOLUME(1, -1), -1 * GZ_PI * std::pow(1, 2)); diff --git a/src/MassMatrix3_TEST.cc b/src/MassMatrix3_TEST.cc index b9c91e1b..da9fcc9c 100644 --- a/src/MassMatrix3_TEST.cc +++ b/src/MassMatrix3_TEST.cc @@ -831,6 +831,57 @@ TEST(MassMatrix3dTest, EquivalentBox) } } +///////////////////////////////////////////////// +TEST(MassMatrix3dTest, SetFromConeZ) +{ + const math::Quaterniond q0 = math::Quaterniond::Identity; + + // Default mass matrix with non-positive inertia + { + math::MassMatrix3d m; + + // input is all zeros, so SetFromConeZ should fail + EXPECT_FALSE(m.SetFromConeZ(0, 0, 0, q0)); + EXPECT_FALSE(m.SetFromConeZ(0, 0, q0)); + + // even if some parameters are valid, none should be set if they + // are not all valid + EXPECT_FALSE(m.SetFromConeZ(1, 0, 0, q0)); + EXPECT_FALSE(m.SetFromConeZ(1, 1, 0, q0)); + EXPECT_FALSE(m.SetFromConeZ(1, 0, 1, q0)); + EXPECT_DOUBLE_EQ(m.Mass(), 0.0); + } + + // unit cone with mass 1.0 + { + const double mass = 1.0; + const double length = 1.0; + const double radius = 0.5; + math::MassMatrix3d m; + EXPECT_TRUE(m.SetFromConeZ(mass, length, radius, q0)); + + double ixx = (3.0 / 80.0) * mass * (4.0 * std::pow(radius, 2) + + std::pow(length, 2)); + double iyy = ixx; + double izz = (3.0 / 10.0) * mass * std::pow(radius, 2); + const math::Vector3d ixxyyzz(ixx, iyy, izz); + EXPECT_EQ(m.DiagonalMoments(), ixxyyzz); + EXPECT_EQ(m.OffDiagonalMoments(), math::Vector3d::Zero); + + double density = mass / (GZ_PI * radius * radius * length / 3.0); + math::Material mat(density); + EXPECT_DOUBLE_EQ(density, mat.Density()); + math::MassMatrix3d m1; + EXPECT_FALSE(m1.SetFromConeZ(math::Material(0), length, radius)); + EXPECT_TRUE(m1.SetFromConeZ(mat, length, radius)); + EXPECT_EQ(m, m1); + + // double the length and radius + EXPECT_TRUE(m.SetFromConeZ(mass, 2*length, 2*radius, q0)); + EXPECT_EQ(m.DiagonalMoments(), 4*ixxyyzz); + } +} + ///////////////////////////////////////////////// TEST(MassMatrix3dTest, SetFromCylinderZ) { diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 6c0b8866..c382557a 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -129,6 +129,7 @@ if (BUILD_TESTING) Box_TEST Capsule_TEST Color_TEST + Cone_TEST Cylinder_TEST DiffDriveOdometry_TEST Ellipsoid_TEST diff --git a/src/python_pybind11/src/Cone.hh b/src/python_pybind11/src/Cone.hh new file mode 100644 index 00000000..c2a11d6c --- /dev/null +++ b/src/python_pybind11/src/Cone.hh @@ -0,0 +1,125 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_MATH_PYTHON__CONE_HH_ +#define GZ_MATH_PYTHON__CONE_HH_ + +#include + +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace pybind11::literals; + +namespace gz +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for a gz::math::Cone +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +template +void defineMathCone(py::module &m, const std::string &typestr) +{ + + using Class = gz::math::Cone; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init&>(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rotOffset") = gz::math::Quaternion::Identity) + .def(py::init&>(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_material") = gz::math::Material(), + py::arg("_rotOffset") = gz::math::Quaternion::Identity) + .def(py::self == py::self) + .def("radius", + &Class::Radius, + "Get the radius in meters.") + .def("set_radius", + &Class::SetRadius, + "Set the radius in meters.") + .def("length", + &Class::Length, + "Get the length in meters.") + .def("set_length", + &Class::SetLength, + "Set the length in meters.") + .def("rotational_offset", + &Class::RotationalOffset, + "Get the rotation offset.") + .def("set_rotational_offset", + &Class::SetRotationalOffset, + "Set the rotation offset.") + .def("mat", + &Class::Mat, + "Get the material associated with this box.") + .def("set_mat", + &Class::SetMat, + "Set the material associated with this box.") + .def("volume", + &Class::Volume, + "Get the volume of the box in m^3.") + .def("density_from_mass", + &Class::DensityFromMass, + "Compute the box's density given a mass value.") + .def("set_density_from_mass", + &Class::SetDensityFromMass, + "Set the density of this box based on a mass value.") + .def("mass_matrix", + py::overload_cast<>(&Class::MassMatrix, py::const_), + "Get the mass matrix for this box. This function " + "is only meaningful if the box's size and material " + "have been set.") + .def("mass_matrix", + py::overload_cast&> + (&Class::MassMatrix, py::const_), + "Get the mass matrix for this box. This function " + "is only meaningful if the box's size and material " + "have been set.") + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a); +} + +} // namespace python +} // namespace math +} // namespace gz + +#endif // GZ_MATH_PYTHON__CONE_HH_ diff --git a/src/python_pybind11/src/Helpers.cc b/src/python_pybind11/src/Helpers.cc index 0b27070f..28a872d3 100644 --- a/src/python_pybind11/src/Helpers.cc +++ b/src/python_pybind11/src/Helpers.cc @@ -41,6 +41,15 @@ float SphereVolume(const float _radius) return GZ_SPHERE_VOLUME(_radius); } +/// \brief Compute cone volume +/// \param[in] _r Cone base radius +/// \param[in] _l Cone length +/// \return cone volume +float ConeVolume(const float _r, const float _l) +{ + return GZ_CONE_VOLUME(_r, _l); +} + /// \brief Compute cylinder volume /// \param[in] _r Cylinder base radius /// \param[in] _l Cylinder length @@ -77,6 +86,14 @@ float SphereVolumeDeprecated(const float _radius) return SphereVolume(_radius); } +float ConeVolumeDeprecated(const float _r, const float _l) +{ + std::cerr << "ign_cone_volume is deprecated. " + << "Please use gz_cone_volume instead" + << std::endl; + return ConeVolume(_r, _l); +} + float CylinderVolumeDeprecated(const float _r, const float _l) { std::cerr << "ign_cylinder_volume is deprecated. " @@ -205,6 +222,9 @@ void defineMathHelpers(py::module &m) .def("gz_sphere_volume", &SphereVolume, "Compute sphere volume") + .def("gz_cone_volume", + &ConeVolume, + "Compute cone volume") .def("gz_cylinder_volume", &CylinderVolume, "Compute cylinder volume") @@ -219,6 +239,9 @@ void defineMathHelpers(py::module &m) .def("ign_sphere_volume", &SphereVolumeDeprecated, "[Deprecated] Compute sphere volume") + .def("ign_cone_volume", + &ConeVolumeDeprecated, + "[Deprecated] Compute cone volume") .def("ign_cylinder_volume", &CylinderVolumeDeprecated, "[Deprecated] Compute cylinder volume") diff --git a/src/python_pybind11/src/MassMatrix3.hh b/src/python_pybind11/src/MassMatrix3.hh index dab0a1eb..afe17f71 100644 --- a/src/python_pybind11/src/MassMatrix3.hh +++ b/src/python_pybind11/src/MassMatrix3.hh @@ -114,6 +114,35 @@ void helpDefineMathMassMatrix3(py::module &m, const std::string &typestr) py::arg("_size") = gz::math::Vector3::Zero, py::arg("_rot") = gz::math::Quaternion::Identity, "Set inertial properties based on a Material and equivalent box.") + .def("set_from_cone_z", + py::overload_cast&> + (&Class::SetFromConeZ), + py::arg("_mat") = gz::math::Material(), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rot") = gz::math::Quaternion::Identity, + "Set inertial properties based on a Material and equivalent " + "cone aligned with Z axis.") + .def("set_from_cone_z", + py::overload_cast&> + (&Class::SetFromConeZ), + py::arg("_mass") = 0, + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rot") = gz::math::Quaternion::Identity, + "Set inertial properties based on a Material and equivalent " + "cone aligned with Z axis.") + .def("set_from_cone_z", + py::overload_cast&> + (&Class::SetFromConeZ), + py::arg("_length") = 0, + py::arg("_radius") = 0, + py::arg("_rot") = gz::math::Quaternion::Identity, + "Set inertial properties based on a Material and equivalent " + "cone aligned with Z axis.") .def("set_from_cylinder_z", py::overload_cast&> diff --git a/src/python_pybind11/src/_gz_math_pybind11.cc b/src/python_pybind11/src/_gz_math_pybind11.cc index 0062b2e8..8af6c147 100644 --- a/src/python_pybind11/src/_gz_math_pybind11.cc +++ b/src/python_pybind11/src/_gz_math_pybind11.cc @@ -19,6 +19,7 @@ #include "Box.hh" #include "Capsule.hh" #include "Color.hh" +#include "Cone.hh" #include "Cylinder.hh" #include "DiffDriveOdometry.hh" #include "Ellipsoid.hh" @@ -168,6 +169,8 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) gz::math::python::defineMathSphere(m, "Sphered"); + gz::math::python::defineMathCone(m, "Coned"); + gz::math::python::defineMathCylinder(m, "Cylinderd"); gz::math::python::defineMathInertial(m, "Inertiald"); diff --git a/src/python_pybind11/test/Cone_TEST.py b/src/python_pybind11/test/Cone_TEST.py new file mode 100644 index 00000000..dc305bf1 --- /dev/null +++ b/src/python_pybind11/test/Cone_TEST.py @@ -0,0 +1,126 @@ +# Copyright 2024 CogniPilot Foundation +# Copyright 2024 Open Source Robotics Foundation +# Copyright 2024 Rudis Laboratories +# +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http:#www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import unittest + +import gz +from gz.math7 import Coned, MassMatrix3d, Material, Quaterniond + + +class TestCone(unittest.TestCase): + + def test_constructor(self): + # Default constructor + cone = Coned() + self.assertEqual(0.0, cone.length()) + self.assertEqual(0.0, cone.radius()) + self.assertEqual(Quaterniond.IDENTITY, cone.rotational_offset()) + self.assertEqual(Material(), cone.mat()) + + cone2 = Coned() + self.assertEqual(cone, cone2) + + # Length and radius constructor + cone = Coned(1.0, 2.0) + self.assertEqual(1.0, cone.length()) + self.assertEqual(2.0, cone.radius()) + self.assertEqual(Quaterniond.IDENTITY, cone.rotational_offset()) + self.assertEqual(Material(), cone.mat()) + + cone2 = Coned(1.0, 2.0) + self.assertEqual(cone, cone2) + + # Length, radius, and rot constructor + cone = Coned(1.0, 2.0, Quaterniond(0.1, 0.2, 0.3)) + self.assertEqual(1.0, cone.length()) + self.assertEqual(2.0, cone.radius()) + self.assertEqual(Quaterniond(0.1, 0.2, 0.3), + cone.rotational_offset()) + self.assertEqual(Material(), cone.mat()) + + cone2 = Coned(1.0, 2.0, Quaterniond(0.1, 0.2, 0.3)) + self.assertEqual(cone, cone2) + + # Length, radius, mat and rot constructor + cone = Coned(1.0, 2.0, Material(gz.math7.MaterialType.WOOD), + Quaterniond(0.1, 0.2, 0.3)) + self.assertEqual(1.0, cone.length()) + self.assertEqual(2.0, cone.radius()) + self.assertEqual(Quaterniond(0.1, 0.2, 0.3), cone.rotational_offset()) + self.assertEqual(Material(gz.math7.MaterialType.WOOD), cone.mat()) + + cone2 = Coned(1.0, 2.0, Material(gz.math7.MaterialType.WOOD), + Quaterniond(0.1, 0.2, 0.3)) + self.assertEqual(cone, cone2) + + def test_mutators(self): + cone = Coned() + self.assertEqual(0.0, cone.length()) + self.assertEqual(0.0, cone.radius()) + self.assertEqual(Quaterniond.IDENTITY, cone.rotational_offset()) + self.assertEqual(Material(), cone.mat()) + + cone.set_length(100.1) + cone.set_radius(.123) + cone.set_rotational_offset(Quaterniond(1.2, 2.3, 3.4)) + cone.set_mat(Material(gz.math7.MaterialType.PINE)) + + self.assertEqual(100.1, cone.length()) + self.assertEqual(.123, cone.radius()) + self.assertEqual(Quaterniond(1.2, 2.3, 3.4), cone.rotational_offset()) + self.assertEqual(Material(gz.math7.MaterialType.PINE), cone.mat()) + + def test_volume_and_density(self): + mass = 1.0 + cone = Coned(1.0, 0.001) + expectedVolume = (math.pi * math.pow(0.001, 2) * 1.0 / 3.0) + self.assertEqual(expectedVolume, cone.volume()) + + expectedDensity = mass / expectedVolume + self.assertEqual(expectedDensity, cone.density_from_mass(mass)) + + # Bad density + cone2 = Coned() + self.assertGreater(0.0, cone2.density_from_mass(mass)) + + def test_mass(self): + mass = 2.0 + length = 2.0 + r = 0.1 + cone = Coned(length, r) + cone.set_density_from_mass(mass) + + massMat = MassMatrix3d() + ixxIyy = (3/80.0) * mass * (4*r*r + length*length) + izz = (3/10.0) * mass * r * r + + expectedMassMat = MassMatrix3d() + expectedMassMat.set_inertia_matrix(ixxIyy, ixxIyy, izz, 0.0, 0.0, 0.0) + expectedMassMat.set_mass(mass) + + cone.mass_matrix(massMat) + self.assertEqual(expectedMassMat, massMat) + self.assertEqual(expectedMassMat.mass(), massMat.mass()) + + massMat2 = cone.mass_matrix() + self.assertEqual(expectedMassMat, massMat2) + self.assertEqual(expectedMassMat.diagonal_moments(), massMat2.diagonal_moments()) + self.assertEqual(expectedMassMat.mass(), massMat2.mass()) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/python_pybind11/test/Helpers_TEST.py b/src/python_pybind11/test/Helpers_TEST.py index f2f3e554..64d7dd6c 100644 --- a/src/python_pybind11/test/Helpers_TEST.py +++ b/src/python_pybind11/test/Helpers_TEST.py @@ -16,7 +16,7 @@ import unittest from gz.math7 import (Helpers, gz_box_volume, gz_box_volume_v, gz_cylinder_volume, - gz_sphere_volume, Vector3d, equal, fixnan, + gz_cone_volume, gz_sphere_volume, Vector3d, equal, fixnan, greater_or_near_equal, is_even, is_odd, is_power_of_two, isnan, is_time_string, less_or_near_equal, max, mean, min, parse_float, parse_int, precision, round_up_multiple, diff --git a/src/ruby/Cone.i b/src/ruby/Cone.i new file mode 100644 index 00000000..caee3fa5 --- /dev/null +++ b/src/ruby/Cone.i @@ -0,0 +1,80 @@ +/* + * Copyright 2024 CogniPilot Foundation + * Copyright 2024 Open Source Robotics Foundation + * Copyright 2024 Rudis Laboratories + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +%module cone +%{ +#include +#include +#include +#include +#include +%} + +namespace gz +{ + namespace math + { + template + class Cone + { + %rename("%(undercase)s", %$isfunction, notregexmatch$name="^[A-Z]*$") ""; + + public: Cone() = default; + + public: Cone(const Precision _length, const Precision _radius, + const gz::math::Quaternion &_rotOffset = + gz::math::Quaternion::Identity); + + public: Cone(const Precision _length, const Precision _radius, + const gz::math::Material &_mat, + const gz::math::Quaternion &_rotOffset = + gz::math::Quaternion::Identity); + + public: ~Cone() = default; + + public: Precision Radius() const; + + public: void SetRadius(const Precision _radius); + + public: Precision Length() const; + + public: void SetLength(const Precision _length); + + public: gz::math::Quaternion RotationalOffset() const; + + public: void SetRotationalOffset( + const gz::math::Quaternion &_rotOffset); + + public: const gz::math::Material &Mat() const; + + public: void SetMat(const gz::math::Material &_mat); + + public: bool MassMatrix(gz::math::MassMatrix3 &_massMat) const; + + public: bool operator==(const Cone &_cone) const; + + public: Precision Volume() const; + + public: Precision DensityFromMass(const Precision _mass) const; + + public: bool SetDensityFromMass(const Precision _mass); + }; + %template(Coned) Cone; + } +} diff --git a/src/ruby/Helpers.i b/src/ruby/Helpers.i index 8abbb656..c31240f3 100644 --- a/src/ruby/Helpers.i +++ b/src/ruby/Helpers.i @@ -53,6 +53,9 @@ import math def gz_sphere_volume(_radius): return (4.0*GZ_PI*math.pow(_radius, 3)/3.0) +def gz_cone_volume(_r, _l): + return (_l * GZ_PI * math.pow(_r, 2)/3.0) + def gz_cylinder_volume(_r, _l): return (_l * GZ_PI * math.pow(_r, 2)) diff --git a/src/ruby/MassMatrix3.i b/src/ruby/MassMatrix3.i index 4e13f035..779751b4 100644 --- a/src/ruby/MassMatrix3.i +++ b/src/ruby/MassMatrix3.i @@ -99,10 +99,21 @@ namespace gz public: bool SetFromCylinderZ(const T _mass, const T _length, const T _radius, - const Quaternion &_rot = Quaternion::Identity); + const Quaternion &_rot = Quaternion::Identity); public: bool SetFromCylinderZ(const T _length, const T _radius, const Quaternion &_rot); + public: bool SetFromConeZ(const Material &_mat, + const T _length, + const T _radius, + const Quaternion &_rot = Quaternion::Identity); + public: bool SetFromConeZ(const T _mass, + const T _length, + const T _radius, + const Quaternion &_rot = Quaternion::Identity); + public: bool SetFromConeZ(const T _length, + const T _radius, + const Quaternion &_rot); public: bool SetFromSphere(const Material &_mat, const T _radius); public: bool SetFromSphere(const T _mass, const T _radius); public: bool SetFromSphere(const T _radius); From 7a595ca81b2914c765e09075c656ae08078e9021 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Tue, 18 Jun 2024 15:32:11 -0400 Subject: [PATCH 16/16] Prepare for 7.5.0 (#599) Signed-off-by: Benjamin Perseghetti --- CMakeLists.txt | 2 +- Changelog.md | 20 ++++++++++++++++++++ package.xml | 2 +- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e1e0be5..4ab62586 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(gz-math7 VERSION 7.4.0) +project(gz-math7 VERSION 7.5.0) #============================================================================ # Find gz-cmake diff --git a/Changelog.md b/Changelog.md index 488ae993..cf15b24c 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,25 @@ ## Gazebo Math 7.x +### Gazebo Math 7.5.0 (2024-06-18) + +1. Backport: Adding cone primitives. + * [Pull request #594](https://github.com/gazebosim/gz-math/pull/594) + +1. Enable 24.04 CI on harmonic + * [Pull request #590](https://github.com/gazebosim/gz-math/pull/590) + +1. Add package.xml + * [Pull request #581](https://github.com/gazebosim/gz-math/pull/581) + +1. bazel: correctly export license + * [Pull request #586](https://github.com/gazebosim/gz-math/pull/586) + +1. Add missing eigen3.hh header for bazel build + * [Pull request #585](https://github.com/gazebosim/gz-math/pull/585) + +1. Expose non-const reference to edges + * [Pull request #580](https://github.com/gazebosim/gz-math/pull/580) + ### Gazebo Math 7.4.0 (2024-03-14) 1. Added MecanumDriveOdometry Python wrapper diff --git a/package.xml b/package.xml index 5e6ff7ee..7d94fa1d 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ gz-math7 - 7.4.0 + 7.5.0 Gazebo Math : Math classes and functions for robot applications Steve Peters Aditya Pande