diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 56442a584..6d009a92b 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 000000000..b299c1822 --- /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 000000000..c6c4502ec --- /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 c88ff50ca..559f541ef 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 282845403..a5d11cee1 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 000000000..8cef51c19 --- /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()