Merge branch 'ign-math6' into ahcorde/pybind11/pid

examples/CMakeLists.txt

@@ -7,6 +7,9 @@ find_package(ignition-math${IGN_MATH_VER} REQUIRED)
 add_executable(angle_example angle_example.cc)
 target_link_libraries(angle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 
+add_executable(diff_drive_odometry diff_drive_odometry.cc)
+target_link_libraries(diff_drive_odometry ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
+
 add_executable(gauss_markov_process gauss_markov_process_example.cc)
 target_link_libraries(gauss_markov_process ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 

examples/angle_example.py

@@ -16,7 +16,7 @@
 # installed.
 #
 # Modify the PYTHONPATH environment variable to include the ignition math
-# library install path. For example, if you install to /user:
+# library install path. For example, if you install to /usr:
 #
 # $ export PYTHONPATH=/usr/lib/python:$PYTHONPATH
 #

examples/diff_drive_odometry.cc

@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+//! [complete]
+#include <iostream>
+#include <chrono>
+
+#include <ignition/math/Angle.hh>
+#include <ignition/math/Helpers.hh>
+#include <ignition/math/DiffDriveOdometry.hh>
+
+int main(int argc, char **argv)
+{
+
+//! [Create a DiffDriveOdometry]
+  ignition::math::DiffDriveOdometry odom;
+//! [Create an DiffDriveOdometry]
+
+  double wheelSeparation = 2.0;
+  double wheelRadius = 0.5;
+  double wheelCircumference = 2 * IGN_PI * wheelRadius;
+
+  // This is the linear distance traveled per degree of wheel rotation.
+  double distPerDegree = wheelCircumference / 360.0;
+
+  // Setup the wheel parameters, and initialize
+  odom.SetWheelParams(wheelSeparation, wheelRadius, wheelRadius);
+  auto startTime = std::chrono::steady_clock::now();
+  odom.Init(startTime);
+
+  // Sleep for a little while, then update the odometry with the new wheel
+  // position.
+  std::cout << "--- Rotate both wheels by 1 degree. ---" << '\n';
+  auto time1 = startTime + std::chrono::milliseconds(100);
+  odom.Update(IGN_DTOR(1.0), IGN_DTOR(1.0), time1);
+
+  std::cout << "\tLinear velocity:\t" << distPerDegree / 0.1 << " m/s"
+            << "\n\tOdom linear velocity:\t" << odom.LinearVelocity() << " m/s"
+            << std::endl;
+
+  std::cout << "Angular velocity should be zero since the \"robot\" is traveling\n"
+            << "in a straight line:\n"
+            << "\tOdom angular velocity:\t"
+            << *odom.AngularVelocity() << " rad/s" << std::endl;
+
+  // Sleep again, this time rotate the left wheel by 1 and the right wheel by 2
+  // degrees.
+  std::cout << "--- This time rotate the left wheel by 1 and the right wheel "
+            << "by 2 degrees ---"
+            << std::endl;
+  auto time2 = time1 + std::chrono::milliseconds(100);
+  odom.Update(IGN_DTOR(2.0), IGN_DTOR(3.0), time2);
+
+  std::cout << "The heading should be the arc tangent of the linear distance\n"
+            << "traveled by the right wheel (the left wheel was stationary)\n"
+            << "divided by the wheel separation.\n"
+            << "\tHeading:\t\t" << atan2(distPerDegree, wheelSeparation) << " rad"
+            << "\n\tOdom Heading:\t\t" << *odom.Heading() << " rad" << '\n';
+
+  // The X odom reading should have increased by the sine of the heading *
+  // half the wheel separation.
+  double xDistTraveled =
+    sin(atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5;
+  double prevXPos = distPerDegree * 2.0;
+  std::cout << "\tX distance traveled:\t" << xDistTraveled + prevXPos << " m"
+             << "\n\tOdom X:\t\t" << odom.X() << " m" << std::endl;
+
+  // The Y odom reading should have increased by the cosine of the heading *
+  // half the wheel separation.
+  double yDistTraveled = (wheelSeparation * 0.5) -
+      cos(atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5;
+  double prevYPos = 0.0;
+  std::cout << "\tY distance traveled:\t" << yDistTraveled + prevYPos << " m"
+             << "\n\tOdom Y:\t\t" << odom.Y() << " m" << std::endl;
+
+  std::cout << "Angular velocity should be the difference between the x and y\n"
+            << "distance traveled divided by the wheel separation divided by\n"
+            << "the seconds elapsed.\n"
+            << "\tAngular velocity:\t"
+            << ((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1 << " rad/s"
+            << "\n\tOdom angular velocity:\t" << *odom.AngularVelocity() << " rad/s"
+            << std::endl;
+}
+//! [complete]

examples/diff_drive_odometry.py

@@ -0,0 +1,95 @@
+# Copyright (C) 2021 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http:#www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# This example will only work if the Python interface library was compiled and
+# installed.
+#
+# Modify the PYTHONPATH environment variable to include the ignition math
+# library install path. For example, if you install to /usr:
+#
+# $ export PYTHONPATH=/usr/lib/python:$PYTHONPATH
+
+import datetime
+import math
+
+from ignition.math import Angle, DiffDriveOdometry
+
+odom = DiffDriveOdometry()
+
+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)
+startTime = datetime.datetime.now()
+odom.init(datetime.timedelta())
+
+# Sleep for a little while, then update the odometry with the new wheel
+# position.
+print('--- Rotate both wheels by 1 degree. ---')
+time1 = startTime + datetime.timedelta(milliseconds=100)
+odom.update(Angle(1.0 * math.pi / 180),
+            Angle(1.0 * math.pi / 180),
+            time1 - startTime)
+
+print('\tLinear velocity:\t{} m/s\n\tOdom linear velocity:\t{} m/s'.
+    format(distPerDegree / 0.1, odom.linear_velocity()))
+
+print('Angular velocity should be zero since the "robot" is traveling\n' +
+      'in a straight line:\n' +
+      '\tOdom angular velocity:\t{} rad/s'
+      .format(odom.angular_velocity()))
+
+# Sleep again, this time rotate the left wheel by 1 and the right wheel by 2
+# degrees.
+print('--- This time rotate the left wheel by 1 and the right wheel ' +
+      'by 2 degrees ---');
+time2 = time1 + datetime.timedelta(milliseconds=100)
+odom.update(Angle(2.0 * math.pi / 180),
+            Angle(3.0 * math.pi / 180),
+            time2 - startTime)
+
+print('The heading should be the arc tangent of the linear distance\n' +
+      'traveled by the right wheel (the left wheel was stationary)\n' +
+      'divided by the wheel separation.\n' +
+      '\tHeading:\t\t{} rad\n\tOdom Heading:\t\t{} rad'.format(
+            math.atan2(distPerDegree, wheelSeparation),
+                  odom.heading()))
+
+# The X odom reading should have increased by the sine of the heading *
+# half the wheel separation.
+xDistTraveled = math.sin(
+    math.atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5
+prevXPos = distPerDegree * 2.0
+print('\tX distance traveled:\t{} m\n\tOdom X:\t\t{} m'.format(
+        xDistTraveled + prevXPos, odom.x()))
+
+# The Y odom reading should have increased by the cosine of the heading *
+# half the wheel separation.
+yDistTraveled = (wheelSeparation * 0.5) - math.cos(
+        math.atan2(distPerDegree, wheelSeparation)) * wheelSeparation * 0.5
+prevYPos = 0.0
+print('\tY distance traveled:\t{} m\n\tOdom Y:\t\t{} m'.format(
+        yDistTraveled + prevYPos, odom.y()))
+
+print('Angular velocity should be the difference between the x and y\n' +
+      'distance traveled divided by the wheel separation divided by\n' +
+      'the seconds elapsed.\n' +
+      '\tAngular velocity:\t{} rad/s\n\tOdom angular velocity:\t{} rad/s'.format(
+        ((xDistTraveled - yDistTraveled) / wheelSeparation) / 0.1,
+        odom.angular_velocity()))

examples/gauss_markov_process_example.py

@@ -0,0 +1,44 @@
+# Copyright (C) 2021 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http:#www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Modify the PYTHONPATH environment variable to include the ignition math
+# library install path. For example, if you install to /usr:
+#
+# $ export PYTHONPATH=/usr/lib/python:$PYTHONPATH
+#
+# You can plot the data generated by this program by following these
+# steps.
+#
+# 1. Run this program and save the output to a file:
+#     python3 gauss_markov_process_example.py > plot.data
+#
+# 2. Use gnuplot to create a plot:
+#     gnuplot -e 'set terminal jpeg; plot "plot.data" with lines' > out.jpg
+import datetime
+from ignition.math import GaussMarkovProcess
+
+# Create the process with:
+#   * Start value of 20.2
+#   * Theta (rate at which the process should approach the mean) of 0.1
+#   * Mu (mean value) 0.
+#   * Sigma (volatility) of 0.5.
+gmp = GaussMarkovProcess(20.2, 0.1, 0, 0.5);
+
+dt = datetime.timedelta(milliseconds=100)
+
+# This process should decrease toward the mean value of 0.
+# With noise of 0.5, the process will walk a bit.
+for i in range(1000):
+    value = gmp.update(dt);
+    print(value);

examples/gauss_markov_process_example.rb

@@ -0,0 +1,42 @@
+# Copyright (C) 2021 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http:#www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# Modify the RUBYLIB environment variable to include the ignition math
+# library install path. For example, if you install to /user:
+#
+# $ export RUBYLIB=/usr/lib/ruby:$RUBYLIB
+#
+# You can plot the data generated by this program by following these
+# steps.
+#
+# 1. Run this program and save the output to a file:
+#     ruby gauss_markov_process_example.rb > plot.data
+#
+# 2. Use gnuplot to create a plot:
+#     gnuplot -e 'set terminal jpeg; plot "plot.data" with lines' > out.jpg
+require 'ignition/math'
+
+# Create the process with:
+#   * Start value of 20.2
+#   * Theta (rate at which the process should approach the mean) of 0.1
+#   * Mu (mean value) 0.
+#   * Sigma (volatility) of 0.5.
+gmp = Ignition::Math::GaussMarkovProcess.new(20.2, 0.1, 0, 0.5);
+
+# This process should decrease toward the mean value of 0.
+# With noise of 0.5, the process will walk a bit.
+for i in 0..1000 do
+    value = gmp.Update(0.1);
+    puts(value);
+end

include/ignition/math/AxisAlignedBox.hh

@@ -132,6 +132,16 @@ namespace ignition
       /// \return The new box
       public: AxisAlignedBox operator+(const Vector3d &_v);
 
+      /// \brief Subtract a vector from the min and max values
+      /// \param _v The vector to use during subtraction
+      /// \return The new box
+      public: AxisAlignedBox operator-(const Vector3d &_v) const;
+
+      /// \brief Add a vector to the min and max values
+      /// \param _v The vector to use during addition
+      /// \return The new box
+      public: AxisAlignedBox operator+(const Vector3d &_v) const;
+
       /// \brief Output operator
       /// \param[in] _out Output stream
       /// \param[in] _b AxisAlignedBox to output to the stream

include/ignition/math/Temperature.hh

@@ -151,6 +151,16 @@ namespace ignition
       /// \return Reference to this instance
       public: Temperature &operator=(const Temperature &_temp);
 
+      /// \brief Addition operator
+      /// \param[in] _temp Temperature in Kelvin
+      /// \return Resulting temperature
+      public: Temperature operator+(const double _temp) const;
+
+      /// \brief Addition operator
+      /// \param[in] _temp Temperature object
+      /// \return Resulting temperature
+      public: Temperature operator+(const Temperature &_temp) const;
+
       /// \brief Addition operator
       /// \param[in] _temp Temperature in Kelvin
       /// \return Resulting temperature
@@ -190,6 +200,16 @@ namespace ignition
       /// \return Resulting temperature
       public: Temperature operator-(const Temperature &_temp);
 
+      /// \brief Subtraction operator
+      /// \param[in] _temp Temperature in Kelvin
+      /// \return Resulting temperature
+      public: Temperature operator-(const double _temp) const;
+
+      /// \brief Subtraction operator
+      /// \param[in] _temp Temperature object
+      /// \return Resulting temperature
+      public: Temperature operator-(const Temperature &_temp) const;
+
       /// \brief Subtraction operator for double type.
       /// \param[in] _t Temperature in kelvin
       /// \param[in] _temp Temperature object
@@ -219,6 +239,16 @@ namespace ignition
       /// \return Resulting temperature
       public: Temperature operator*(const Temperature &_temp);
 
+      /// \brief Multiplication operator
+      /// \param[in] _temp Temperature in Kelvin
+      /// \return Resulting temperature
+      public: Temperature operator*(const double _temp) const;
+
+      /// \brief Multiplication operator
+      /// \param[in] _temp Temperature object
+      /// \return Resulting temperature
+      public: Temperature operator*(const Temperature &_temp) const;
+
       /// \brief Multiplication operator for double type.
       /// \param[in] _t Temperature in kelvin
       /// \param[in] _temp Temperature object
@@ -248,6 +278,16 @@ namespace ignition
       /// \return Resulting temperature
       public: Temperature operator/(const Temperature &_temp);
 
+      /// \brief Division operator
+      /// \param[in] _temp Temperature in Kelvin
+      /// \return Resulting temperature
+      public: Temperature operator/(const double _temp) const;
+
+      /// \brief Division operator
+      /// \param[in] _temp Temperature object
+      /// \return Resulting temperature
+      public: Temperature operator/(const Temperature &_temp) const;
+
       /// \brief Division operator for double type.
       /// \param[in] _t Temperature in kelvin
       /// \param[in] _temp Temperature object