From 54bf59e3160b70ee848822a3d33c92e0afe8e6f0 Mon Sep 17 00:00:00 2001 From: danilo_gsch Date: Thu, 25 Aug 2022 12:18:34 -0300 Subject: [PATCH 01/18] Class ignition::math::MecanumDriveOdometryPrivate to handle Mecanum wheels odometry Signed-off-by: danilo_gsch --- include/ignition/math/MecanumDriveOdometry.hh | 137 +++++++++ src/MecanumDriveOdometry.cc | 284 ++++++++++++++++++ 2 files changed, 421 insertions(+) create mode 100644 include/ignition/math/MecanumDriveOdometry.hh create mode 100644 src/MecanumDriveOdometry.cc diff --git a/include/ignition/math/MecanumDriveOdometry.hh b/include/ignition/math/MecanumDriveOdometry.hh new file mode 100644 index 000000000..337218be0 --- /dev/null +++ b/include/ignition/math/MecanumDriveOdometry.hh @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2019 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 IGNITION_MATH_MECANUMDRIVEODOMETRY_HH_ +#define IGNITION_MATH_MECANUMDRIVEODOMETRY_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Use a steady clock + using clock = std::chrono::steady_clock; + + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // + // Forward declarations. + class MecanumDriveOdometryPrivate; + + /** \class MecanumDriveOdometry MecanumDriveOdometry.hh \ + * ignition/math/MecanumDriveOdometry.hh + **/ + /// \brief Computes odometry values based on a set of kinematic + /// properties and wheel speeds for a Mecanum-drive vehicle. + /// + /// A vehicle with a heading of zero degrees has a local + /// reference frame according to the diagram below. + /// + /// Y + /// ^ + /// | + /// | + /// O--->X(forward) + /// + + + class IGNITION_MATH_VISIBLE MecanumDriveOdometry + { + /// \brief Constructor. + /// \param[in] _windowSize Rolling window size used to compute the + /// velocity mean + public: explicit MecanumDriveOdometry(size_t _windowSize = 10); + + /// \brief Destructor. + public: ~MecanumDriveOdometry(); + + /// \brief Initialize the odometry + /// \param[in] _time Current time. + public: void Init(const clock::time_point &_time); + + /// \brief Get whether Init has been called. + /// \return True if Init has been called, false otherwise. + public: bool Initialized() const; + + /// \brief Updates the odometry class with latest wheels and + /// steerings position + /// \param[in] _frontLeftPos Left wheel position in radians. + /// \param[in] _frontRightPos Right wheel postion in radians. + /// \param[in] _backLeftPos Left wheel position in radians. + /// \param[in] _backRightPos Right wheel postion in radians. + /// \param[in] _time Current time point. + /// \return True if the odometry is actually updated. + public: bool Update(const Angle &_frontLeftPos, const Angle &_frontRightPos, const Angle &_backLeftPos, const Angle &_backRightPos, + const clock::time_point &_time); + + /// \brief Get the heading. + /// \return The heading in radians. + public: const Angle &Heading() const; + + /// \brief Get the X position. + /// \return The X position in meters + public: double X() const; + + /// \brief Get the Y position. + /// \return The Y position in meters. + public: double Y() const; + + /// \brief Get the linear velocity. + /// \return The linear velocity in meter/second. + public: double LinearVelocity() const; + + /// \brief Get the lateral velocity. + /// \return The lateral velocity in meter/second. + public: double LateralVelocity() const; + + /// \brief Get the angular velocity. + /// \return The angular velocity in radian/second. + public: const Angle &AngularVelocity() const; + + /// \brief Set the wheel parameters including the radius and separation. + /// \param[in] _wheelSeparation Distance between left and right wheels. + /// \param[in] _wheelBase Distance between front and back wheels. + /// \param[in] _leftWheelRadius Radius of the left wheel. + /// \param[in] _rightWheelRadius Radius of the right wheel. + public: void SetWheelParams(double _wheelSeparation, double _wheelBase, + double _leftWheelRadius, double _rightWheelRadius); + + /// \brief Set the velocity rolling window size. + /// \param[in] _size The Velocity rolling window size. + public: void SetVelocityRollingWindowSize(size_t _size); + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } + } +} + +#endif diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc new file mode 100644 index 000000000..5ce2c5d70 --- /dev/null +++ b/src/MecanumDriveOdometry.cc @@ -0,0 +1,284 @@ +/* + * Copyright (C) 2019 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 "ignition/math/MecanumDriveOdometry.hh" +#include "ignition/math/RollingMean.hh" + +using namespace ignition; +using namespace math; + +// The implementation was borrowed from: https://github.com/ros-controls/ros_controllers/blob/melodic-devel/diff_drive_controller/src/odometry.cpp +// And these calculations are based on the following references: +// https://robohub.org/drive-kinematics-skid-steer-and-mecanum-ros-twist-included +// https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf + +class ignition::math::MecanumDriveOdometryPrivate +{ + /// \brief Integrates the pose. + /// \param[in] _linear Linear velocity. + /// \param[in] _lateral Lateral velocity. + /// \param[in] _angular Angular velocity. + public: void IntegrateExact(double _linear, double _lateral, double _angular); + public: void IntegrateRungeKutta2(double _linear, double _lateral, double _angular); + + /// \brief Current timestamp. + public: clock::time_point lastUpdateTime; + + /// \brief Current x position in meters. + public: double x{0.0}; + + /// \brief Current y position in meters. + public: double y{0.0}; + + /// \brief Current heading in radians. + public: Angle heading; + + /// \brief Current linear velocity in meter/second. + public: double linearVel{0.0}; + + /// \brief Current lateral velocity in meter/second. + public: double lateralVel{0.0}; + + /// \brief Current angular velocity in radians/second. + public: Angle angularVel; + + /// \brief Left wheel radius in meters. + public: double leftWheelRadius{0.0}; + + /// \brief Right wheel radius in meters. + public: double rightWheelRadius{0.0}; + + /// \brief Wheel separation in meters. + public: double wheelSeparation{1.0}; + + /// \brief Wheel base in meters. + public: double wheelBase{1.0}; + + /// \brief Previous frontleft wheel position/state in radians. + public: double frontLeftWheelOldPos{0.0}; + + /// \brief Previous frontright wheel position/state in radians. + public: double frontRightWheelOldPos{0.0}; + + /// \brief Previous backleft wheel position/state in radians. + public: double backLeftWheelOldPos{0.0}; + + /// \brief Previous backright wheel position/state in radians. + public: double backRightWheelOldPos{0.0}; + + /// \brief Rolling mean accumulators for the linear velocity + public: RollingMean linearMean; + + /// \brief Rolling mean accumulators for the lateral velocity + public: RollingMean lateralMean; + + /// \brief Rolling mean accumulators for the angular velocity + public: RollingMean angularMean; + + /// \brief Initialized flag. + public: bool initialized{false}; +}; + +////////////////////////////////////////////////// +MecanumDriveOdometry::MecanumDriveOdometry(size_t _windowSize) + : dataPtr(new MecanumDriveOdometryPrivate) +{ + this->dataPtr->linearMean.SetWindowSize(_windowSize); + this->dataPtr->lateralMean.SetWindowSize(_windowSize); + this->dataPtr->angularMean.SetWindowSize(_windowSize); +} + +////////////////////////////////////////////////// +MecanumDriveOdometry::~MecanumDriveOdometry() +{ +} + +////////////////////////////////////////////////// +void MecanumDriveOdometry::Init(const clock::time_point &_time) +{ + // Reset accumulators and timestamp. + this->dataPtr->linearMean.Clear(); + this->dataPtr->lateralMean.Clear(); + this->dataPtr->angularMean.Clear(); + this->dataPtr->x = 0.0; + this->dataPtr->y = 0.0; + this->dataPtr->heading = 0.0; + this->dataPtr->linearVel = 0.0; + this->dataPtr->lateralVel = 0.0; + this->dataPtr->angularVel = 0.0; + this->dataPtr->frontLeftWheelOldPos = 0.0; + this->dataPtr->frontRightWheelOldPos = 0.0; + this->dataPtr->backLeftWheelOldPos = 0.0; + this->dataPtr->backRightWheelOldPos = 0.0; + + this->dataPtr->lastUpdateTime = _time; + this->dataPtr->initialized = true; +} + +////////////////////////////////////////////////// +bool MecanumDriveOdometry::Initialized() const +{ + return this->dataPtr->initialized; +} + +////////////////////////////////////////////////// +bool MecanumDriveOdometry::Update(const Angle &_frontLeftPos, const Angle &_frontRightPos, const Angle &_backLeftPos, const Angle &_backRightPos, + const clock::time_point &_time) +{ + // Compute x, y and heading using velocity + const std::chrono::duration dt = + _time - this->dataPtr->lastUpdateTime; + + // Get current wheel joint positions: + const double frontLeftWheelCurPos = *_frontLeftPos * this->dataPtr->leftWheelRadius; + const double frontRightWheelCurPos = *_frontRightPos * this->dataPtr->rightWheelRadius; + const double backLeftWheelCurPos = *_backLeftPos * this->dataPtr->leftWheelRadius; + const double backRightWheelCurPos = *_backRightPos * this->dataPtr->rightWheelRadius; + + // Estimate velocity of wheels using old and current position: + const double frontLeftWheelEstVel = frontLeftWheelCurPos - + this->dataPtr->frontLeftWheelOldPos; + + const double frontRightWheelEstVel = frontRightWheelCurPos - + this->dataPtr->frontRightWheelOldPos; + + const double backLeftWheelEstVel = backLeftWheelCurPos - + this->dataPtr->backLeftWheelOldPos; + + const double backRightWheelEstVel = backRightWheelCurPos - + this->dataPtr->backRightWheelOldPos; + + // Update old position with current + this->dataPtr->frontLeftWheelOldPos = frontLeftWheelCurPos; + this->dataPtr->frontRightWheelOldPos = frontRightWheelCurPos; + this->dataPtr->backLeftWheelOldPos = backLeftWheelCurPos; + this->dataPtr->backRightWheelOldPos = backRightWheelCurPos; + + // constant used in computing target velocities (TODO: support different wheel radius) + const double angularConst = (1 / (4 * (0.5 * (this->dataPtr->wheelSeparation + this->dataPtr->wheelBase)))); + + // Compute linear and angular diff + const double linear = (frontLeftWheelEstVel + frontRightWheelEstVel + backLeftWheelEstVel + backRightWheelEstVel) * 0.25; + const double lateral = (-frontLeftWheelEstVel + frontRightWheelEstVel + backLeftWheelEstVel - backRightWheelEstVel) * 0.25; + const double angular = (-frontLeftWheelEstVel + frontRightWheelEstVel - backLeftWheelEstVel + backRightWheelEstVel) * angularConst; + + this->dataPtr->IntegrateExact(linear, lateral, angular); + + // We cannot estimate the speed if the time interval is zero (or near + // zero). + if (equal(0.0, dt.count())) + return false; + + this->dataPtr->lastUpdateTime = _time; + + // Estimate speeds using a rolling mean to filter them out: + this->dataPtr->linearMean.Push(linear / dt.count()); + this->dataPtr->lateralMean.Push(lateral / dt.count()); + this->dataPtr->angularMean.Push(angular / dt.count()); + + this->dataPtr->linearVel = this->dataPtr->linearMean.Mean(); + this->dataPtr->lateralVel = this->dataPtr->lateralMean.Mean(); + this->dataPtr->angularVel = this->dataPtr->angularMean.Mean(); + + return true; +} + +////////////////////////////////////////////////// +void MecanumDriveOdometry::SetWheelParams(double _wheelSeparation, double _wheelBase, + double _leftWheelRadius, double _rightWheelRadius) +{ + this->dataPtr->wheelSeparation = _wheelSeparation; + this->dataPtr->wheelBase = _wheelBase; + this->dataPtr->leftWheelRadius = _leftWheelRadius; + this->dataPtr->rightWheelRadius = _rightWheelRadius; +} + +////////////////////////////////////////////////// +void MecanumDriveOdometry::SetVelocityRollingWindowSize(size_t _size) +{ + this->dataPtr->linearMean.SetWindowSize(_size); + this->dataPtr->lateralMean.SetWindowSize(_size); + this->dataPtr->angularMean.SetWindowSize(_size); +} + +////////////////////////////////////////////////// +const Angle &MecanumDriveOdometry::Heading() const +{ + return this->dataPtr->heading; +} + +////////////////////////////////////////////////// +double MecanumDriveOdometry::X() const +{ + return this->dataPtr->x; +} + +////////////////////////////////////////////////// +double MecanumDriveOdometry::Y() const +{ + return this->dataPtr->y; +} + +////////////////////////////////////////////////// +double MecanumDriveOdometry::LinearVelocity() const +{ + return this->dataPtr->linearVel; +} + +////////////////////////////////////////////////// +double MecanumDriveOdometry::LateralVelocity() const +{ + return this->dataPtr->lateralVel; +} + +////////////////////////////////////////////////// +const Angle &MecanumDriveOdometry::AngularVelocity() const +{ + return this->dataPtr->angularVel; +} + +////////////////////////////////////////////////// +void MecanumDriveOdometryPrivate::IntegrateRungeKutta2( + double _linear, double _lateral, double _angular) +{ + const double direction = *this->heading + _angular * 0.5; + + // Runge-Kutta 2nd order integration: + this->x += (_linear * std::cos(direction)) - (_lateral * std::sin(direction)); + this->y += _linear * std::sin(direction) + (_lateral * std::cos(direction)); + this->heading += _angular; +} + +////////////////////////////////////////////////// +void MecanumDriveOdometryPrivate::IntegrateExact(double _linear, double _lateral, double _angular) +{ + if (std::fabs(_angular) < 1e-6) + { + this->IntegrateRungeKutta2(_linear, _lateral, _angular); + } + else + { + // Exact integration (should solve problems when angular is zero): + const double headingOld = *this->heading; + const double ratio = _linear / _angular; + const double ratio2 = _lateral / _angular; + this->heading += _angular; + // TODO: Double-check the following equations (based on DiffDriveOdometry): + this->x += (ratio * (std::sin(*this->heading) - std::sin(headingOld))) - (-ratio2 * (std::cos(*this->heading) - std::cos(headingOld))); + this->y += (-ratio * (std::cos(*this->heading) - std::cos(headingOld))) + (ratio2 * (std::sin(*this->heading) - std::sin(headingOld))); + } +} \ No newline at end of file From c4224f91b6c33c233e4ab2649e974e831f11075d Mon Sep 17 00:00:00 2001 From: danilogsch Date: Thu, 25 Aug 2022 13:25:32 -0300 Subject: [PATCH 02/18] Update include/ignition/math/MecanumDriveOdometry.hh MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: danilo_gsch --- include/ignition/math/MecanumDriveOdometry.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/math/MecanumDriveOdometry.hh b/include/ignition/math/MecanumDriveOdometry.hh index 337218be0..ab283003c 100644 --- a/include/ignition/math/MecanumDriveOdometry.hh +++ b/include/ignition/math/MecanumDriveOdometry.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2022 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. From 5e51418d9c352d58bac78da5d6c319caab38d58b Mon Sep 17 00:00:00 2001 From: danilogsch Date: Thu, 25 Aug 2022 13:25:41 -0300 Subject: [PATCH 03/18] Update include/ignition/math/MecanumDriveOdometry.hh MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: danilo_gsch --- include/ignition/math/MecanumDriveOdometry.hh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/ignition/math/MecanumDriveOdometry.hh b/include/ignition/math/MecanumDriveOdometry.hh index ab283003c..228dcf19b 100644 --- a/include/ignition/math/MecanumDriveOdometry.hh +++ b/include/ignition/math/MecanumDriveOdometry.hh @@ -36,9 +36,9 @@ namespace ignition // Forward declarations. class MecanumDriveOdometryPrivate; - /** \class MecanumDriveOdometry MecanumDriveOdometry.hh \ - * ignition/math/MecanumDriveOdometry.hh - **/ + /// \class MecanumDriveOdometry MecanumDriveOdometry.hh \ + /// ignition/math/MecanumDriveOdometry.hh + /// /// \brief Computes odometry values based on a set of kinematic /// properties and wheel speeds for a Mecanum-drive vehicle. /// From 951287546c733965230843d230a44d23dedf6862 Mon Sep 17 00:00:00 2001 From: danilogsch Date: Thu, 25 Aug 2022 13:25:52 -0300 Subject: [PATCH 04/18] Update include/ignition/math/MecanumDriveOdometry.hh MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: danilo_gsch --- include/ignition/math/MecanumDriveOdometry.hh | 1 - 1 file changed, 1 deletion(-) diff --git a/include/ignition/math/MecanumDriveOdometry.hh b/include/ignition/math/MecanumDriveOdometry.hh index 228dcf19b..6dcd1b81e 100644 --- a/include/ignition/math/MecanumDriveOdometry.hh +++ b/include/ignition/math/MecanumDriveOdometry.hh @@ -52,7 +52,6 @@ namespace ignition /// O--->X(forward) /// - class IGNITION_MATH_VISIBLE MecanumDriveOdometry { /// \brief Constructor. From 6ab64a2a15d21312988e37870548dc5f238b6191 Mon Sep 17 00:00:00 2001 From: danilogsch Date: Thu, 25 Aug 2022 13:26:36 -0300 Subject: [PATCH 05/18] Update src/MecanumDriveOdometry.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: danilo_gsch --- src/MecanumDriveOdometry.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc index 5ce2c5d70..78e8b7e43 100644 --- a/src/MecanumDriveOdometry.cc +++ b/src/MecanumDriveOdometry.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 222 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. From 113be1756feab753ad7bad7777602b07f335eee2 Mon Sep 17 00:00:00 2001 From: danilo_gsch Date: Thu, 25 Aug 2022 13:33:53 -0300 Subject: [PATCH 06/18] MecanumDrive doc Signed-off-by: danilo_gsch --- src/MecanumDriveOdometry.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc index 78e8b7e43..fc83b28cc 100644 --- a/src/MecanumDriveOdometry.cc +++ b/src/MecanumDriveOdometry.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 222 Open Source Robotics Foundation + * Copyright (C) 2022 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. @@ -281,4 +281,4 @@ void MecanumDriveOdometryPrivate::IntegrateExact(double _linear, double _lateral this->x += (ratio * (std::sin(*this->heading) - std::sin(headingOld))) - (-ratio2 * (std::cos(*this->heading) - std::cos(headingOld))); this->y += (-ratio * (std::cos(*this->heading) - std::cos(headingOld))) + (ratio2 * (std::sin(*this->heading) - std::sin(headingOld))); } -} \ No newline at end of file +} From 1ac66f565b37303af0a0d249347a68096e1ea190 Mon Sep 17 00:00:00 2001 From: danilo_gsch Date: Thu, 25 Aug 2022 13:38:41 -0300 Subject: [PATCH 07/18] MecanumDrive doc Signed-off-by: danilo_gsch --- src/MecanumDriveOdometry.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc index fc83b28cc..72648387b 100644 --- a/src/MecanumDriveOdometry.cc +++ b/src/MecanumDriveOdometry.cc @@ -33,6 +33,11 @@ class ignition::math::MecanumDriveOdometryPrivate /// \param[in] _lateral Lateral velocity. /// \param[in] _angular Angular velocity. public: void IntegrateExact(double _linear, double _lateral, double _angular); + + /// \brief Integrates the poseusing second order Runge-Kuffa aproximation. + /// \param[in] _linear Linear velocity. + /// \param[in] _lateral Lateral velocity. + /// \param[in] _angular Angular velocity. public: void IntegrateRungeKutta2(double _linear, double _lateral, double _angular); /// \brief Current timestamp. From 320424fd0a503dc14f2fa66e052d21dff43eb681 Mon Sep 17 00:00:00 2001 From: danilogsch Date: Wed, 30 Nov 2022 09:22:45 -0300 Subject: [PATCH 08/18] Update MecanumDriveOdometry.cc line length corrections --- src/MecanumDriveOdometry.cc | 53 +++++++++++++++++++++++-------------- 1 file changed, 33 insertions(+), 20 deletions(-) diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc index 72648387b..5b1efbc26 100644 --- a/src/MecanumDriveOdometry.cc +++ b/src/MecanumDriveOdometry.cc @@ -33,12 +33,13 @@ class ignition::math::MecanumDriveOdometryPrivate /// \param[in] _lateral Lateral velocity. /// \param[in] _angular Angular velocity. public: void IntegrateExact(double _linear, double _lateral, double _angular); - - /// \brief Integrates the poseusing second order Runge-Kuffa aproximation. + + /// \brief Integrates the pose using second order Runge-Kuffa aproximation. /// \param[in] _linear Linear velocity. /// \param[in] _lateral Lateral velocity. /// \param[in] _angular Angular velocity. - public: void IntegrateRungeKutta2(double _linear, double _lateral, double _angular); + public: void IntegrateRungeKutta2(double _linear, double _lateral, + double _angular); /// \brief Current timestamp. public: clock::time_point lastUpdateTime; @@ -141,18 +142,23 @@ bool MecanumDriveOdometry::Initialized() const } ////////////////////////////////////////////////// -bool MecanumDriveOdometry::Update(const Angle &_frontLeftPos, const Angle &_frontRightPos, const Angle &_backLeftPos, const Angle &_backRightPos, - const clock::time_point &_time) +bool MecanumDriveOdometry::Update(const Angle &_frontLeftPos, + const Angle &_frontRightPos, const Angle &_backLeftPos, + const Angle &_backRightPos, const clock::time_point &_time) { // Compute x, y and heading using velocity const std::chrono::duration dt = _time - this->dataPtr->lastUpdateTime; // Get current wheel joint positions: - const double frontLeftWheelCurPos = *_frontLeftPos * this->dataPtr->leftWheelRadius; - const double frontRightWheelCurPos = *_frontRightPos * this->dataPtr->rightWheelRadius; - const double backLeftWheelCurPos = *_backLeftPos * this->dataPtr->leftWheelRadius; - const double backRightWheelCurPos = *_backRightPos * this->dataPtr->rightWheelRadius; + const double frontLeftWheelCurPos = + *_frontLeftPos * this->dataPtr->leftWheelRadius; + const double frontRightWheelCurPos = + *_frontRightPos * this->dataPtr->rightWheelRadius; + const double backLeftWheelCurPos = + *_backLeftPos * this->dataPtr->leftWheelRadius; + const double backRightWheelCurPos = + *_backRightPos * this->dataPtr->rightWheelRadius; // Estimate velocity of wheels using old and current position: const double frontLeftWheelEstVel = frontLeftWheelCurPos - @@ -173,13 +179,18 @@ bool MecanumDriveOdometry::Update(const Angle &_frontLeftPos, const Angle &_fron this->dataPtr->backLeftWheelOldPos = backLeftWheelCurPos; this->dataPtr->backRightWheelOldPos = backRightWheelCurPos; - // constant used in computing target velocities (TODO: support different wheel radius) - const double angularConst = (1 / (4 * (0.5 * (this->dataPtr->wheelSeparation + this->dataPtr->wheelBase)))); + // constant used in computing target velocities + // TODO(danilogsch): support different wheel radius + const double angularConst = + (1/(4*(0.5*(this->dataPtr->wheelSeparation + this->dataPtr->wheelBase)))); // Compute linear and angular diff - const double linear = (frontLeftWheelEstVel + frontRightWheelEstVel + backLeftWheelEstVel + backRightWheelEstVel) * 0.25; - const double lateral = (-frontLeftWheelEstVel + frontRightWheelEstVel + backLeftWheelEstVel - backRightWheelEstVel) * 0.25; - const double angular = (-frontLeftWheelEstVel + frontRightWheelEstVel - backLeftWheelEstVel + backRightWheelEstVel) * angularConst; + const double linear = (frontLeftWheelEstVel + frontRightWheelEstVel + + backLeftWheelEstVel + backRightWheelEstVel) * 0.25; + const double lateral = (-frontLeftWheelEstVel + frontRightWheelEstVel + + backLeftWheelEstVel - backRightWheelEstVel) * 0.25; + const double angular = (-frontLeftWheelEstVel + frontRightWheelEstVel + - backLeftWheelEstVel + backRightWheelEstVel) * angularConst; this->dataPtr->IntegrateExact(linear, lateral, angular); @@ -203,8 +214,8 @@ bool MecanumDriveOdometry::Update(const Angle &_frontLeftPos, const Angle &_fron } ////////////////////////////////////////////////// -void MecanumDriveOdometry::SetWheelParams(double _wheelSeparation, double _wheelBase, - double _leftWheelRadius, double _rightWheelRadius) +void MecanumDriveOdometry::SetWheelParams(double _wheelSeparation, + double _wheelBase, double _leftWheelRadius, double _rightWheelRadius) { this->dataPtr->wheelSeparation = _wheelSeparation; this->dataPtr->wheelBase = _wheelBase; @@ -269,7 +280,8 @@ void MecanumDriveOdometryPrivate::IntegrateRungeKutta2( } ////////////////////////////////////////////////// -void MecanumDriveOdometryPrivate::IntegrateExact(double _linear, double _lateral, double _angular) +void MecanumDriveOdometryPrivate::IntegrateExact(double _linear, + double _lateral, double _angular) { if (std::fabs(_angular) < 1e-6) { @@ -282,8 +294,9 @@ void MecanumDriveOdometryPrivate::IntegrateExact(double _linear, double _lateral const double ratio = _linear / _angular; const double ratio2 = _lateral / _angular; this->heading += _angular; - // TODO: Double-check the following equations (based on DiffDriveOdometry): - this->x += (ratio * (std::sin(*this->heading) - std::sin(headingOld))) - (-ratio2 * (std::cos(*this->heading) - std::cos(headingOld))); - this->y += (-ratio * (std::cos(*this->heading) - std::cos(headingOld))) + (ratio2 * (std::sin(*this->heading) - std::sin(headingOld))); + this->x += (ratio * (std::sin(*this->heading) - std::sin(headingOld))) + - (-ratio2 * (std::cos(*this->heading) - std::cos(headingOld))); + this->y += (-ratio * (std::cos(*this->heading) - std::cos(headingOld))) + + (ratio2 * (std::sin(*this->heading) - std::sin(headingOld))); } } From 7e54f8d5a03fcfb3c552284594eb08fa8c25125a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 13 Feb 2023 23:09:02 +0100 Subject: [PATCH 09/18] Update include/ignition/math/MecanumDriveOdometry.hh --- include/ignition/math/MecanumDriveOdometry.hh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/ignition/math/MecanumDriveOdometry.hh b/include/ignition/math/MecanumDriveOdometry.hh index 6dcd1b81e..c929c9d3c 100644 --- a/include/ignition/math/MecanumDriveOdometry.hh +++ b/include/ignition/math/MecanumDriveOdometry.hh @@ -78,7 +78,8 @@ namespace ignition /// \param[in] _backRightPos Right wheel postion in radians. /// \param[in] _time Current time point. /// \return True if the odometry is actually updated. - public: bool Update(const Angle &_frontLeftPos, const Angle &_frontRightPos, const Angle &_backLeftPos, const Angle &_backRightPos, + public: bool Update(const Angle &_frontLeftPos, const Angle &_frontRightPos, + const Angle &_backLeftPos, const Angle &_backRightPos, const clock::time_point &_time); /// \brief Get the heading. From aa97e312b09e8ae72b9d5c33b5fae9cbef2bfafc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Dec 2022 21:25:14 -0800 Subject: [PATCH 10/18] CI workflow: use checkout v3 (#519) Version v2 of the actions/checkout workflow is deprecated, so switch to v3. Part of gazebo-tooling/release-tools#862. Signed-off-by: Steve Peters --- .github/workflows/ci.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 7f4849d30..7a8b7b39b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -8,7 +8,7 @@ jobs: name: Ubuntu Bionic CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@bionic @@ -20,7 +20,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@focal @@ -29,7 +29,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@jammy From 1b1c6b8905cde8b24b480cb412eff54d42ff699e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 6 Feb 2023 00:54:44 -0800 Subject: [PATCH 11/18] Rename COPYING to LICENSE (#521) The LICENSE file contained a copy of the stanze used at the top of source code files, while the actual license was in the COPYING file. So remove the stanza and put the actual Apache 2.0 license text in LICENSE. Signed-off-by: Steve Peters --- COPYING | 178 ----------------------------------------------------- LICENSE | 185 ++++++++++++++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 174 insertions(+), 189 deletions(-) delete mode 100644 COPYING diff --git a/COPYING b/COPYING deleted file mode 100644 index 4909afd04..000000000 --- a/COPYING +++ /dev/null @@ -1,178 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - diff --git a/LICENSE b/LICENSE index bd33c4376..4909afd04 100644 --- a/LICENSE +++ b/LICENSE @@ -1,15 +1,178 @@ -Software License Agreement (Apache License) + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -Copyright 2012 Open Source Robotics Foundation + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION -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 + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS - 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. From ea6f2fb4a0f2d89b75278d225ba92e012f5e3a31 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 14 Mar 2023 11:29:16 -0700 Subject: [PATCH 12/18] Custom PID error rate (#525) * Custom PID error rate Signed-off-by: Nate Koenig * added test Signed-off-by: Nate Koenig --------- Signed-off-by: Nate Koenig --- include/gz/math/PID.hh | 15 +++++++++++++++ src/PID.cc | 25 +++++++++++++++++++------ src/PID_TEST.cc | 9 +++++++++ src/python_pybind11/src/PID.cc | 8 +++++++- src/ruby/PID.i | 5 +++++ 5 files changed, 55 insertions(+), 7 deletions(-) diff --git a/include/gz/math/PID.hh b/include/gz/math/PID.hh index bf40356c8..1317caf7d 100644 --- a/include/gz/math/PID.hh +++ b/include/gz/math/PID.hh @@ -157,6 +157,21 @@ namespace ignition /// \return The offset value public: double CmdOffset() const; + /// \brief Update the Pid loop with nonuniform time step size. + /// \param[in] _error Error since last call (p_state - p_target). + /// \param[in] _errorRate Estimate of error rate, that can be used + /// when a smoother estimate is available than the finite difference + /// used by Update(const double _error, + /// const std::chrono::duration &_dt) + /// \param[in] _dt Change in time since last update call. + /// Normally, this is called at every time step, + /// The return value is an updated command to be passed + /// to the object being controlled. + /// \return the command value + public: double Update(const double _error, + double _errorRate, + const std::chrono::duration &_dt); + /// \brief Update the Pid loop with nonuniform time step size. /// \param[in] _error Error since last call (p_state - p_target). /// \param[in] _dt Change in time since last update call. diff --git a/src/PID.cc b/src/PID.cc index c80989121..4e873182a 100644 --- a/src/PID.cc +++ b/src/PID.cc @@ -141,6 +141,22 @@ double PID::Update(const double _error, return 0.0; } + // Pass in the derivative error + return this->Update(_error, (_error - this->pErrLast) / _dt.count(), _dt); +} + +///////////////////////////////////////////////// +double PID::Update(const double _error, + double _errorRate, + const std::chrono::duration &_dt) +{ + if (_dt == std::chrono::duration(0) || + isnan(_error) || std::isinf(_error) || + isnan(_errorRate) || std::isinf(_errorRate)) + { + return 0.0; + } + double pTerm, dTerm; this->pErr = _error; @@ -156,12 +172,9 @@ double PID::Update(const double _error, if (this->iMax >= this->iMin) this->iErr = clamp(this->iErr, this->iMin, this->iMax); - // Calculate the derivative error - if (_dt != std::chrono::duration(0)) - { - this->dErr = (this->pErr - this->pErrLast) / _dt.count(); - this->pErrLast = this->pErr; - } + // Use the provided error rate + this->dErr = _errorRate; + this->pErrLast = this->pErr; // Calculate derivative contribution to command dTerm = this->dGain * this->dErr; diff --git a/src/PID_TEST.cc b/src/PID_TEST.cc index ff58a25bf..e31b672db 100644 --- a/src/PID_TEST.cc +++ b/src/PID_TEST.cc @@ -172,6 +172,15 @@ TEST(PidTest, Update) EXPECT_DOUBLE_EQ(pe, 5); EXPECT_DOUBLE_EQ(ie, 1.4); EXPECT_DOUBLE_EQ(de, 0.0); + + pid.Reset(); + pid.SetIGain(0.0); + pid.SetIMin(0.0); + result = pid.Update(5.0, 1.0, std::chrono::duration(10.0)); + EXPECT_DOUBLE_EQ(result, -5.5); + + result = pid.Update(5.0, 1.0, std::chrono::duration(0.0)); + EXPECT_DOUBLE_EQ(result, 0); } ///////////////////////////////////////////////// diff --git a/src/python_pybind11/src/PID.cc b/src/python_pybind11/src/PID.cc index 9e273d1a6..f3b81bef2 100644 --- a/src/python_pybind11/src/PID.cc +++ b/src/python_pybind11/src/PID.cc @@ -102,7 +102,13 @@ void defineMathPID(py::module &m, const std::string &typestr) &Class::CmdOffset, "Get the offset value for the command.") .def("update", - &Class::Update, + py::overload_cast &>(&Class::Update), + "Update the Pid loop with nonuniform time step size, and custom error " + "rate.") + .def("update", + py::overload_cast &>(&Class::Update), "Update the Pid loop with nonuniform time step size.") .def("set_cmd", &Class::SetCmd, diff --git a/src/ruby/PID.i b/src/ruby/PID.i index 7861cfcc2..b38be1d94 100644 --- a/src/ruby/PID.i +++ b/src/ruby/PID.i @@ -83,6 +83,11 @@ namespace ignition double Update(const double error, const double dt) { return (*$self).Update(error, std::chrono::duration(dt)); } + double Update(const double error, double errorRate, const double dt) { + return (*$self).Update(error, errorRate, + std::chrono::duration(dt)); + } + } } } From 8e77c39607adacb185f9cd6c0de1cfe3b660957c Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 9 Aug 2022 21:26:09 -0500 Subject: [PATCH 13/18] Add option to skip pybind11 and SWIG (#480) Signed-off-by: Michael Carroll --- CMakeLists.txt | 91 +++++++++++++++++++++++++++------------------- src/CMakeLists.txt | 7 +++- 2 files changed, 59 insertions(+), 39 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c00ac5051..1f59135b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,17 +25,26 @@ ign_configure_project( # Set project-specific options #============================================================================ -option(USE_SYSTEM_PATHS_FOR_RUBY_INSTALLATION - "Install ruby modules in standard system paths in the system" +option(SKIP_SWIG + "Skip generating ruby bindings via Swig" OFF) -option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION - "Install python modules in standard system paths in the system" +option(SKIP_PYBIND11 + "Skip generating Python bindings via pybind11" OFF) -option(USE_DIST_PACKAGES_FOR_PYTHON +include(CMakeDependentOption) +cmake_dependent_option(USE_SYSTEM_PATHS_FOR_RUBY_INSTALLATION + "Install ruby modules in standard system paths in the system" + OFF "NOT SKIP_SWIG" OFF) + +cmake_dependent_option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION + "Install python modules in standard system paths in the system" + OFF "NOT SKIP_PYBIND11" OFF) + +cmake_dependent_option(USE_DIST_PACKAGES_FOR_PYTHON "Use dist-packages instead of site-package to install python modules" - OFF) + OFF "NOT SKIP_PYBIND11" OFF) #============================================================================ # Search for project-specific dependencies @@ -51,46 +60,54 @@ ign_find_package( ######################################## # Include swig -find_package(SWIG QUIET) -if (NOT SWIG_FOUND) - IGN_BUILD_WARNING("Swig is missing: Language interfaces are disabled.") - message (STATUS "Searching for swig - not found.") +if (SKIP_SWIG) + message(STATUS "SKIP_SWIG set - disabling SWIG Ruby support") else() - message (STATUS "Searching for swig - found version ${SWIG_VERSION}.") -endif() - -# Include other languages if swig was found -if (SWIG_FOUND) - ######################################## - # Include ruby - find_package(Ruby 1.9 QUIET) - if (NOT RUBY_FOUND) - IGN_BUILD_WARNING("Ruby is missing: Install ruby-dev to enable ruby interface to ignition math.") - message (STATUS "Searching for Ruby - not found.") + find_package(SWIG QUIET) + if (NOT SWIG_FOUND) + IGN_BUILD_WARNING("Swig is missing: Language interfaces are disabled.") + message (STATUS "Searching for swig - not found.") else() - message (STATUS "Searching for Ruby - found version ${RUBY_VERSION}.") + message (STATUS "Searching for swig - found version ${SWIG_VERSION}.") + endif() + + # Include other languages if swig was found + if (SWIG_FOUND) + ######################################## + # Include ruby + find_package(Ruby 1.9 QUIET) + if (NOT RUBY_FOUND) + IGN_BUILD_WARNING("Ruby is missing: Install ruby-dev to enable ruby interface to ignition math.") + message (STATUS "Searching for Ruby - not found.") + else() + message (STATUS "Searching for Ruby - found version ${RUBY_VERSION}.") + endif() endif() endif() ######################################## # Python bindings -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.") +if (SKIP_PYBIND11) + message(STATUS "SKIP_PYBIND11 set - disabling python bindings") else() - message (STATUS "Searching for Python - found version ${PYTHONLIBS_VERSION_STRING}.") - - set(PYBIND11_PYTHON_VERSION 3) - find_package(Python3 QUIET COMPONENTS Interpreter Development) - find_package(pybind11 2.2 QUIET) - - if (${pybind11_FOUND}) - message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + 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.") else() - IGN_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") - message (STATUS "Searching for pybind11 - not found.") + message (STATUS "Searching for Python - found version ${PYTHONLIBS_VERSION_STRING}.") + + set(PYBIND11_PYTHON_VERSION 3) + find_package(Python3 QUIET COMPONENTS Interpreter Development) + find_package(pybind11 2.2 QUIET) + + if (${pybind11_FOUND}) + message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + else() + IGN_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") + message (STATUS "Searching for pybind11 - not found.") + endif() endif() endif() diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index edbf8e591..e098b3bbe 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,7 +13,10 @@ ign_build_tests(TYPE UNIT SOURCES ${gtest_sources}) add_subdirectory(graph) # Bindings subdirectories -if (${pybind11_FOUND}) +if (pybind11_FOUND AND NOT SKIP_PYBIND11) add_subdirectory(python_pybind11) endif() -add_subdirectory(ruby) + +if (SWIG_FOUND AND NOT SKIP_SWIG) + add_subdirectory(ruby) +endif() From e57cc85a79ff7b91309c8e569a006cc872c63491 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 6 Apr 2023 13:37:19 -0500 Subject: [PATCH 14/18] Disable pybind11 on windows by default (#529) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f59135b1..c1481b0a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,9 +29,14 @@ option(SKIP_SWIG "Skip generating ruby bindings via Swig" OFF) +set(skip_pybind11_default_value OFF) +if (MSVC) + set(skip_pybind11_default_value ON) +endif() + option(SKIP_PYBIND11 "Skip generating Python bindings via pybind11" - OFF) + ${skip_pybind11_default_value}) include(CMakeDependentOption) cmake_dependent_option(USE_SYSTEM_PATHS_FOR_RUBY_INSTALLATION From 8f2c940e02e11e579897c22f2706c404886dd23d Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 14 Apr 2023 12:53:50 -0500 Subject: [PATCH 15/18] =?UTF-8?q?=F0=9F=8E=88=206.14.0=20(#531)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 15 +++++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c1481b0a2..c3e2bcd4f 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.13.0) +project(ignition-math6 VERSION 6.14.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 563e38956..d578e6c0a 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,20 @@ ## Gazebo Math 6.x +## Gazebo Math 6.14.0 (2023-04-14) + +1. Disable pybind11 on windows by default + * [Pull request #529](https://github.com/gazebosim/gz-math/pull/529) + +1. Add option to skip pybind11 and SWIG + * [Pull request #480](https://github.com/gazebosim/gz-math/pull/480) + +1. Custom PID error rate + * [Pull request #525](https://github.com/gazebosim/gz-math/pull/525) + +1. Infrastructure + * [Pull request #521](https://github.com/gazebosim/gz-math/pull/521) + * [Pull request #519](https://github.com/gazebosim/gz-math/pull/519) + ## Gazebo Math 6.13.0 (2022-08-31) 1. Support migration and migrate headers From 241951bcdd232c7e4bcbb67b547341ce75670017 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 15 Aug 2023 20:20:51 +0000 Subject: [PATCH 16/18] Lint Signed-off-by: Michael Carroll --- include/ignition/math/MecanumDriveOdometry.hh | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/include/ignition/math/MecanumDriveOdometry.hh b/include/ignition/math/MecanumDriveOdometry.hh index c929c9d3c..4490aa3f7 100644 --- a/include/ignition/math/MecanumDriveOdometry.hh +++ b/include/ignition/math/MecanumDriveOdometry.hh @@ -27,9 +27,6 @@ namespace ignition { namespace math { - // Use a steady clock - using clock = std::chrono::steady_clock; - // Inline bracket to help doxygen filtering. inline namespace IGNITION_MATH_VERSION_NAMESPACE { // @@ -54,6 +51,9 @@ namespace ignition class IGNITION_MATH_VISIBLE MecanumDriveOdometry { + // Use a steady clock + public: using clock = std::chrono::steady_clock; + /// \brief Constructor. /// \param[in] _windowSize Rolling window size used to compute the /// velocity mean @@ -78,9 +78,10 @@ namespace ignition /// \param[in] _backRightPos Right wheel postion in radians. /// \param[in] _time Current time point. /// \return True if the odometry is actually updated. - public: bool Update(const Angle &_frontLeftPos, const Angle &_frontRightPos, - const Angle &_backLeftPos, const Angle &_backRightPos, - const clock::time_point &_time); + public: bool Update( + const Angle &_frontLeftPos, const Angle &_frontRightPos, + const Angle &_backLeftPos, const Angle &_backRightPos, + const clock::time_point &_time); /// \brief Get the heading. /// \return The heading in radians. @@ -130,8 +131,7 @@ namespace ignition #pragma warning(pop) #endif }; - } - } -} - -#endif + } // namespace IGNITION_MATH_VERSION_NAMESPACE + } // namespace math +} // namespace ignition +#endif // IGNITION_MATH_MECANUMDRIVEODOMETRY_HH_ From b3d4d74cb2a5e4a5360bd5d86bf39d2538da1495 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Tue, 15 Aug 2023 20:56:29 +0000 Subject: [PATCH 17/18] Port changes and tests from muttistefano Co-authored-by: muttistefano Signed-off-by: Michael Carroll --- include/gz/math/MecanumDriveOdometry.hh | 134 ++++++++++++++++++ include/ignition/math/MecanumDriveOdometry.hh | 122 +--------------- src/MecanumDriveOdometry.cc | 10 +- src/MecanumDriveOdometry_TEST.cc | 108 ++++++++++++++ 4 files changed, 249 insertions(+), 125 deletions(-) create mode 100644 include/gz/math/MecanumDriveOdometry.hh create mode 100644 src/MecanumDriveOdometry_TEST.cc diff --git a/include/gz/math/MecanumDriveOdometry.hh b/include/gz/math/MecanumDriveOdometry.hh new file mode 100644 index 000000000..79c4bd06b --- /dev/null +++ b/include/gz/math/MecanumDriveOdometry.hh @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2022 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_MECANUMDRIVEODOMETRY_HH_ +#define GZ_MATH_MECANUMDRIVEODOMETRY_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + // Forward declarations. + class MecanumDriveOdometryPrivate; + + /// \class MecanumDriveOdometry MecanumDriveOdometry.hh + /// gz/math/MecanumDriveOdometry.hh + /// + /// \brief Computes odometry values based on a set of kinematic + /// properties and wheel speeds for a Mecanum-drive vehicle. + /// + /// A vehicle with a heading of zero degrees has a local + /// reference frame according to the diagram below. + /// + /// Y + /// ^ + /// | + /// | + /// O--->X(forward) + class IGNITION_MATH_VISIBLE MecanumDriveOdometry + { + // Use a steady clock + public: using clock = std::chrono::steady_clock; + + /// \brief Constructor. + /// \param[in] _windowSize Rolling window size used to compute the + /// velocity mean + public: explicit MecanumDriveOdometry(size_t _windowSize = 10); + + /// \brief Destructor. + public: ~MecanumDriveOdometry(); + + /// \brief Initialize the odometry + /// \param[in] _time Current time. + public: void Init(const clock::time_point &_time); + + /// \brief Get whether Init has been called. + /// \return True if Init has been called, false otherwise. + public: bool Initialized() const; + + /// \brief Updates the odometry class with latest wheels and + /// steerings position + /// \param[in] _frontLeftPos Left wheel position in radians. + /// \param[in] _frontRightPos Right wheel postion in radians. + /// \param[in] _backLeftPos Left wheel position in radians. + /// \param[in] _backRightPos Right wheel postion in radians. + /// \param[in] _time Current time point. + /// \return True if the odometry is actually updated. + public: bool Update( + const Angle &_frontLeftPos, const Angle &_frontRightPos, + const Angle &_backLeftPos, const Angle &_backRightPos, + const clock::time_point &_time); + + /// \brief Get the heading. + /// \return The heading in radians. + public: const Angle &Heading() const; + + /// \brief Get the X position. + /// \return The X position in meters + public: double X() const; + + /// \brief Get the Y position. + /// \return The Y position in meters. + public: double Y() const; + + /// \brief Get the linear velocity. + /// \return The linear velocity in meter/second. + public: double LinearVelocity() const; + + /// \brief Get the lateral velocity. + /// \return The lateral velocity in meter/second. + public: double LateralVelocity() const; + + /// \brief Get the angular velocity. + /// \return The angular velocity in radian/second. + public: const Angle &AngularVelocity() const; + + /// \brief Set the wheel parameters including the radius and separation. + /// \param[in] _wheelSeparation Distance between left and right wheels. + /// \param[in] _wheelBase Distance between front and back wheels. + /// \param[in] _leftWheelRadius Radius of the left wheel. + /// \param[in] _rightWheelRadius Radius of the right wheel. + public: void SetWheelParams(double _wheelSeparation, double _wheelBase, + double _leftWheelRadius, double _rightWheelRadius); + + /// \brief Set the velocity rolling window size. + /// \param[in] _size The Velocity rolling window size. + public: void SetVelocityRollingWindowSize(size_t _size); + +#ifdef _WIN32 +// Disable warning C4251 which is triggered by +// std::unique_ptr +#pragma warning(push) +#pragma warning(disable: 4251) +#endif + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; +#ifdef _WIN32 +#pragma warning(pop) +#endif + }; + } // namespace IGNITION_MATH_VERSION_NAMESPACE + } // namespace math +} // namespace ignition +#endif // GZ_MATH_MECANUMDRIVEODOMETRY_HH_ diff --git a/include/ignition/math/MecanumDriveOdometry.hh b/include/ignition/math/MecanumDriveOdometry.hh index 4490aa3f7..a07bf8b28 100644 --- a/include/ignition/math/MecanumDriveOdometry.hh +++ b/include/ignition/math/MecanumDriveOdometry.hh @@ -13,125 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ -#ifndef IGNITION_MATH_MECANUMDRIVEODOMETRY_HH_ -#define IGNITION_MATH_MECANUMDRIVEODOMETRY_HH_ + */ -#include -#include -#include -#include +#include #include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - // - // Forward declarations. - class MecanumDriveOdometryPrivate; - - /// \class MecanumDriveOdometry MecanumDriveOdometry.hh \ - /// ignition/math/MecanumDriveOdometry.hh - /// - /// \brief Computes odometry values based on a set of kinematic - /// properties and wheel speeds for a Mecanum-drive vehicle. - /// - /// A vehicle with a heading of zero degrees has a local - /// reference frame according to the diagram below. - /// - /// Y - /// ^ - /// | - /// | - /// O--->X(forward) - /// - - class IGNITION_MATH_VISIBLE MecanumDriveOdometry - { - // Use a steady clock - public: using clock = std::chrono::steady_clock; - - /// \brief Constructor. - /// \param[in] _windowSize Rolling window size used to compute the - /// velocity mean - public: explicit MecanumDriveOdometry(size_t _windowSize = 10); - - /// \brief Destructor. - public: ~MecanumDriveOdometry(); - - /// \brief Initialize the odometry - /// \param[in] _time Current time. - public: void Init(const clock::time_point &_time); - - /// \brief Get whether Init has been called. - /// \return True if Init has been called, false otherwise. - public: bool Initialized() const; - - /// \brief Updates the odometry class with latest wheels and - /// steerings position - /// \param[in] _frontLeftPos Left wheel position in radians. - /// \param[in] _frontRightPos Right wheel postion in radians. - /// \param[in] _backLeftPos Left wheel position in radians. - /// \param[in] _backRightPos Right wheel postion in radians. - /// \param[in] _time Current time point. - /// \return True if the odometry is actually updated. - public: bool Update( - const Angle &_frontLeftPos, const Angle &_frontRightPos, - const Angle &_backLeftPos, const Angle &_backRightPos, - const clock::time_point &_time); - - /// \brief Get the heading. - /// \return The heading in radians. - public: const Angle &Heading() const; - - /// \brief Get the X position. - /// \return The X position in meters - public: double X() const; - - /// \brief Get the Y position. - /// \return The Y position in meters. - public: double Y() const; - - /// \brief Get the linear velocity. - /// \return The linear velocity in meter/second. - public: double LinearVelocity() const; - - /// \brief Get the lateral velocity. - /// \return The lateral velocity in meter/second. - public: double LateralVelocity() const; - - /// \brief Get the angular velocity. - /// \return The angular velocity in radian/second. - public: const Angle &AngularVelocity() const; - - /// \brief Set the wheel parameters including the radius and separation. - /// \param[in] _wheelSeparation Distance between left and right wheels. - /// \param[in] _wheelBase Distance between front and back wheels. - /// \param[in] _leftWheelRadius Radius of the left wheel. - /// \param[in] _rightWheelRadius Radius of the right wheel. - public: void SetWheelParams(double _wheelSeparation, double _wheelBase, - double _leftWheelRadius, double _rightWheelRadius); - - /// \brief Set the velocity rolling window size. - /// \param[in] _size The Velocity rolling window size. - public: void SetVelocityRollingWindowSize(size_t _size); - -#ifdef _WIN32 -// Disable warning C4251 which is triggered by -// std::unique_ptr -#pragma warning(push) -#pragma warning(disable: 4251) -#endif - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; -#ifdef _WIN32 -#pragma warning(pop) -#endif - }; - } // namespace IGNITION_MATH_VERSION_NAMESPACE - } // namespace math -} // namespace ignition -#endif // IGNITION_MATH_MECANUMDRIVEODOMETRY_HH_ diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc index 5b1efbc26..b28fa168e 100644 --- a/src/MecanumDriveOdometry.cc +++ b/src/MecanumDriveOdometry.cc @@ -15,10 +15,10 @@ * */ #include -#include "ignition/math/MecanumDriveOdometry.hh" -#include "ignition/math/RollingMean.hh" +#include "gz/math/MecanumDriveOdometry.hh" +#include "gz/math/RollingMean.hh" -using namespace ignition; +using namespace gz; using namespace math; // The implementation was borrowed from: https://github.com/ros-controls/ros_controllers/blob/melodic-devel/diff_drive_controller/src/odometry.cpp @@ -26,7 +26,7 @@ using namespace math; // https://robohub.org/drive-kinematics-skid-steer-and-mecanum-ros-twist-included // https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf -class ignition::math::MecanumDriveOdometryPrivate +class gz::math::MecanumDriveOdometryPrivate { /// \brief Integrates the pose. /// \param[in] _linear Linear velocity. @@ -42,7 +42,7 @@ class ignition::math::MecanumDriveOdometryPrivate double _angular); /// \brief Current timestamp. - public: clock::time_point lastUpdateTime; + public: MecanumDriveOdometry::clock::time_point lastUpdateTime; /// \brief Current x position in meters. public: double x{0.0}; diff --git a/src/MecanumDriveOdometry_TEST.cc b/src/MecanumDriveOdometry_TEST.cc new file mode 100644 index 000000000..ce168f5cf --- /dev/null +++ b/src/MecanumDriveOdometry_TEST.cc @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2019 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 "gz/math/Angle.hh" +#include "gz/math/Helpers.hh" +#include "gz/math/MecanumDriveOdometry.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(MecanumDriveOdometryTest, MecanumDriveOdometry) +{ + math::MecanumDriveOdometry odom; + EXPECT_DOUBLE_EQ(0.0, *odom.Heading()); + EXPECT_DOUBLE_EQ(0.0, odom.X()); + EXPECT_DOUBLE_EQ(0.0, odom.Y()); + EXPECT_DOUBLE_EQ(0.0, odom.LinearVelocity()); + EXPECT_DOUBLE_EQ(0.0, odom.LateralVelocity()); + EXPECT_DOUBLE_EQ(0.0, *odom.AngularVelocity()); + + 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,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. + auto time1 = startTime + std::chrono::milliseconds(100); + odom.Update(IGN_DTOR(1.0), IGN_DTOR(1.0), IGN_DTOR(1.0), IGN_DTOR(1.0), time1); + EXPECT_DOUBLE_EQ(0.0, *odom.Heading()); + EXPECT_DOUBLE_EQ(distPerDegree, odom.X()); + EXPECT_DOUBLE_EQ(0.0, odom.Y()); + // Linear velocity should be dist_traveled / time_elapsed. + EXPECT_NEAR(distPerDegree / 0.1, odom.LinearVelocity(), 1e-3); + // Angular velocity should be zero since the "robot" is traveling in a + // straight line. + EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3); + + // Sleep again, then update the odometry with the new wheel position. + auto time2 = time1 + std::chrono::milliseconds(100); + odom.Update(IGN_DTOR(2.0), IGN_DTOR(2.0), IGN_DTOR(2.0), IGN_DTOR(2.0), time2); + EXPECT_DOUBLE_EQ(0.0, *odom.Heading()); + EXPECT_NEAR(distPerDegree * 2.0, odom.X(), 3e-6); + EXPECT_DOUBLE_EQ(0.0, odom.Y()); + // Linear velocity should be dist_traveled / time_elapsed. + EXPECT_NEAR(distPerDegree / 0.1, odom.LinearVelocity(), 1e-3); + // Angular velocity should be zero since the "robot" is traveling in a + // straight line. + EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3); + + // Initialize again, and odom values should be reset. + startTime = std::chrono::steady_clock::now(); + odom.Init(startTime); + EXPECT_DOUBLE_EQ(0.0, *odom.Heading()); + EXPECT_DOUBLE_EQ(0.0, odom.X()); + EXPECT_DOUBLE_EQ(0.0, odom.Y()); + EXPECT_DOUBLE_EQ(0.0, odom.LinearVelocity()); + EXPECT_DOUBLE_EQ(0.0, *odom.AngularVelocity()); + + // Sleep again, this time move 2 degrees in 100ms. + time1 = startTime + std::chrono::milliseconds(100); + odom.Update(IGN_DTOR(2.0), IGN_DTOR(2.0), IGN_DTOR(2.0), IGN_DTOR(2.0), time1); + EXPECT_DOUBLE_EQ(0.0, *odom.Heading()); + EXPECT_NEAR(distPerDegree * 2.0, odom.X(), 3e-6); + EXPECT_DOUBLE_EQ(0.0, odom.Y()); + // Linear velocity should be dist_traveled / time_elapsed. + EXPECT_NEAR(distPerDegree * 2 / 0.1, odom.LinearVelocity(), 1e-3); + // Angular velocity should be zero since the "robot" is traveling in a + // straight line. + EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3); + + // Sleep again, this time move 2 degrees in 100ms. + odom.Init(startTime); + time1 = startTime + std::chrono::milliseconds(100); + odom.Update(IGN_DTOR(-2.0), IGN_DTOR(2.0), IGN_DTOR(2.0), IGN_DTOR(-2.0), time1); + EXPECT_DOUBLE_EQ(0.0, *odom.Heading()); + EXPECT_NEAR(distPerDegree * 2.0, odom.Y(), 3e-6); + // EXPECT_DOUBLE_EQ(0.0, odom.Y()); + // Linear velocity should be dist_traveled / time_elapsed. + EXPECT_NEAR(distPerDegree * 2 / 0.1, odom.LateralVelocity(), 1e-3); + // Angular velocity should be zero since the "robot" is traveling in a + // straight line. + EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3); +} From 6a2af359c24b979a3cf0e441911de19d04000e54 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 16 Aug 2023 20:41:11 +0000 Subject: [PATCH 18/18] Add accessor and TODO note Signed-off-by: Michael Carroll --- include/gz/math/MecanumDriveOdometry.hh | 21 ++++++++++++++++++++- src/MecanumDriveOdometry.cc | 24 ++++++++++++++++++++++++ 2 files changed, 44 insertions(+), 1 deletion(-) diff --git a/include/gz/math/MecanumDriveOdometry.hh b/include/gz/math/MecanumDriveOdometry.hh index 79c4bd06b..d6ce3c632 100644 --- a/include/gz/math/MecanumDriveOdometry.hh +++ b/include/gz/math/MecanumDriveOdometry.hh @@ -38,6 +38,9 @@ namespace ignition /// \brief Computes odometry values based on a set of kinematic /// properties and wheel speeds for a Mecanum-drive vehicle. /// + /// Note: when computing velocity the math currently assumes that + /// all wheels have a radius of 1.0. + /// /// A vehicle with a heading of zero degrees has a local /// reference frame according to the diagram below. /// @@ -111,11 +114,27 @@ namespace ignition /// \param[in] _rightWheelRadius Radius of the right wheel. public: void SetWheelParams(double _wheelSeparation, double _wheelBase, double _leftWheelRadius, double _rightWheelRadius); - /// \brief Set the velocity rolling window size. /// \param[in] _size The Velocity rolling window size. public: void SetVelocityRollingWindowSize(size_t _size); + /// \brief Get the wheel separation + /// \return Distance between left and right wheels in meters. + public: double WheelSeparation() const; + + /// \brief Get the wheel base + /// \return Distance between front and back wheels in meters. + public: double WheelBase() const; + + /// \brief Get the left wheel radius + /// \return Left wheel radius in meters. + public: double LeftWheelRadius() const; + + /// \brief Get the rightwheel radius + /// \return Right wheel radius in meters. + public: double RightWheelRadius() const; + + #ifdef _WIN32 // Disable warning C4251 which is triggered by // std::unique_ptr diff --git a/src/MecanumDriveOdometry.cc b/src/MecanumDriveOdometry.cc index b28fa168e..54012a02a 100644 --- a/src/MecanumDriveOdometry.cc +++ b/src/MecanumDriveOdometry.cc @@ -267,6 +267,30 @@ const Angle &MecanumDriveOdometry::AngularVelocity() const return this->dataPtr->angularVel; } +////////////////////////////////////////////////// +double MecanumDriveOdometry::WheelSeparation() const +{ + return this->dataPtr->wheelSeparation; +} + +////////////////////////////////////////////////// +double MecanumDriveOdometry::WheelBase() const +{ + return this->dataPtr->wheelBase; +} + +////////////////////////////////////////////////// +double MecanumDriveOdometry::LeftWheelRadius() const +{ + return this->dataPtr->leftWheelRadius; +} + +////////////////////////////////////////////////// +double MecanumDriveOdometry::RightWheelRadius() const +{ + return this->dataPtr->rightWheelRadius; +} + ////////////////////////////////////////////////// void MecanumDriveOdometryPrivate::IntegrateRungeKutta2( double _linear, double _lateral, double _angular)