From 3c58f768a481bf8595c871fe7f505fe21ed5bbf8 Mon Sep 17 00:00:00 2001 From: Jarek Karwowski Date: Wed, 4 Oct 2023 00:39:10 +0200 Subject: [PATCH] `hubero_interfaces` - `NavigationBase`: velocity transformations support holonomic actors [#31] --- .../hubero_interfaces/navigation_base.h | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/hubero_interfaces/include/hubero_interfaces/navigation_base.h b/hubero_interfaces/include/hubero_interfaces/navigation_base.h index 94ddf298..519822cc 100644 --- a/hubero_interfaces/include/hubero_interfaces/navigation_base.h +++ b/hubero_interfaces/include/hubero_interfaces/navigation_base.h @@ -169,15 +169,29 @@ class NavigationBase { * @brief Transforms local velocity (typically received as velocity command) to a global coordinate system */ static Vector3 convertCommandToGlobalCs(const double& yaw_actor, const Vector3& cmd_vel_local) { - // slide 38 at https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture3-RobotMotion.pdf + // Ref: 19-local-to-global-matrix-form at + // https://automaticaddison.com/how-to-describe-the-rotation-of-a-robot-in-2d/ ignition::math::Matrix3d r( - cos(yaw_actor), 0.0, 0.0, - sin(yaw_actor), 0.0, 0.0, - 0.0, 0.0, 1.0 + +std::cos(yaw_actor), -std::sin(yaw_actor), +0.0, + +std::sin(yaw_actor), +std::cos(yaw_actor), +0.0, + +0.0, +0.0, +1.0 ); return r * cmd_vel_local; } + /** + * @brief Transforms a global velocity to a local/base coordinate system + */ + static Vector3 convertCommandToLocalCs(const double& yaw_actor, const Vector3& vel_global) { + // Ref: last equation in https://automaticaddison.com/how-to-describe-the-rotation-of-a-robot-in-2d/ + ignition::math::Matrix3d r( + +std::cos(yaw_actor), +std::sin(yaw_actor), +0.0, + -std::sin(yaw_actor), +std::cos(yaw_actor), +0.0, + +0.0, +0.0, +1.0 + ); + return r * vel_global; + } + protected: /// @brief Stores initialization indicator flag bool initialized_;