Switched the input/output of PositionTransform for SPHERICAL frame fr…

…om radians to degrees.

This change is made to unify the units that are inputs/outputs to the coordinate transforms.

Signed-off-by: Martin Pecka <[email protected]>

Migration.md

@@ -5,6 +5,13 @@ Deprecated code produces compile-time warnings. These warning serve as
 notification to users that their code should be upgraded. The next major
 release will remove the deprecated code.
 
+## Gazebo Math 7.X to 8.X
+
+### Breaking Changes
+
+  1. `SphericalCoordinates::PositionTransform()` now uses degrees for `SPHERICAL` frame instead of radians (both for input and output coordinates).
+     This change was made so that its output is consistent with `SphericalFromLocal` etc.
+
 ## Gazebo Math 6.X to 7.X
 
 ### Breaking Changes

include/gz/math/SphericalCoordinates.hh

@@ -281,7 +281,7 @@ namespace gz
       public: void UpdateTransformationMatrix();
 
       /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame
-      /// Spherical coordinates use radians, while the other frames use meters.
+      /// Spherical coordinates use degrees, while the other frames use meters.
       /// \param[in] _pos Position vector in frame defined by parameter _in
       /// \param[in] _in  CoordinateType for input
       /// \param[in] _out CoordinateType for output
@@ -290,8 +290,9 @@ namespace gz
               PositionTransform(const gz::math::Vector3d &_pos,
                   const CoordinateType &_in, const CoordinateType &_out) const;
 
-      /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame
-      /// Spherical coordinates use radians, while the other frames use meters.
+      /// \brief Convert between velocity in ECEF/LOCAL/GLOBAL frame
+      /// Velocity should not be expressed in SPHERICAL frame (such values will
+      /// be passed through). All other frames use meters.
       /// \param[in] _vel Velocity vector in frame defined by parameter _in
       /// \param[in] _in  CoordinateType for input
       /// \param[in] _out CoordinateType for output

src/SphericalCoordinates.cc

@@ -377,21 +377,14 @@ void SphericalCoordinates::SetHeadingOffset(const Angle &_angle)
 Vector3d SphericalCoordinates::SphericalFromLocalPosition(
     const Vector3d &_xyz) const
 {
-  Vector3d result =
-    this->PositionTransform(_xyz, LOCAL, SPHERICAL);
-  result.X(GZ_RTOD(result.X()));
-  result.Y(GZ_RTOD(result.Y()));
-  return result;
+  return this->PositionTransform(_xyz, LOCAL, SPHERICAL);
 }
 
 //////////////////////////////////////////////////
 Vector3d SphericalCoordinates::LocalFromSphericalPosition(
     const Vector3d &_xyz) const
 {
-  Vector3d result = _xyz;
-  result.X(GZ_DTOR(result.X()));
-  result.Y(GZ_DTOR(result.Y()));
-  return this->PositionTransform(result, SPHERICAL, LOCAL);
+  return this->PositionTransform(_xyz, SPHERICAL, LOCAL);
 }
 
 //////////////////////////////////////////////////
@@ -520,8 +513,8 @@ void SphericalCoordinates::UpdateTransformationMatrix()
 
   // Cache the ECEF coordinate of the origin
   this->dataPtr->origin = Vector3d(
-    this->dataPtr->latitudeReference.Radian(),
-    this->dataPtr->longitudeReference.Radian(),
+    this->dataPtr->latitudeReference.Degree(),
+    this->dataPtr->longitudeReference.Degree(),
     this->dataPtr->elevationReference);
   this->dataPtr->origin =
     this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF);
@@ -535,10 +528,12 @@ Vector3d SphericalCoordinates::PositionTransform(
   Vector3d tmp = _pos;
 
   // Cache trig results
-  double cosLat = cos(_pos.X());
-  double sinLat = sin(_pos.X());
-  double cosLon = cos(_pos.Y());
-  double sinLon = sin(_pos.Y());
+  const double latRad = GZ_DTOR(_pos.X());
+  const double lonRad = GZ_DTOR(_pos.Y());
+  double cosLat = cos(latRad);
+  double sinLat = sin(latRad);
+  double cosLon = cos(lonRad);
+  double sinLon = sin(lonRad);
 
   // Radius of planet curvature (meters)
   double curvature = 1.0 -
@@ -619,8 +614,8 @@ Vector3d SphericalCoordinates::PositionTransform(
           std::pow(sin(lat), 2);
         nCurvature = this->dataPtr->ellA / sqrt(nCurvature);
 
-        tmp.X(lat);
-        tmp.Y(lon);
+        tmp.X(GZ_RTOD(lat));
+        tmp.Y(GZ_RTOD(lon));
 
         // Now calculate Z
         tmp.Z(p/cos(lat) - nCurvature);

src/SphericalCoordinates_TEST.cc

@@ -304,13 +304,13 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms)
       // > -1510.88 3766.64 (EAST,NORTH)
       math::Vector3d vec(-1510.88, 3766.64, -3.29);
 
-      // Convert degrees to radians
-      osrf_s.X() *= 0.0174532925;
-      osrf_s.Y() *= 0.0174532925;
+      math::Angle osrf_s_lat;
+      math::Angle osrf_s_lon;
+      osrf_s_lat.SetDegree(osrf_s.X());
+      osrf_s_lon.SetDegree(osrf_s.Y());
 
       // Set the ORIGIN to be the Open Source Robotics Foundation
-      math::SphericalCoordinates sc2(st, math::Angle(osrf_s.X()),
-          math::Angle(osrf_s.Y()), osrf_s.Z(),
+      math::SphericalCoordinates sc2(st, osrf_s_lat, osrf_s_lon, osrf_s.Z(),
           math::Angle::Zero);
 
       // Check that SPHERICAL -> ECEF works

src/python_pybind11/src/SphericalCoordinates.cc

@@ -134,13 +134,13 @@ void defineMathSphericalCoordinates(py::module &m, const std::string &typestr)
     .def("position_transform",
          &Class::PositionTransform,
          "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame "
-         "Spherical coordinates use radians, while the other frames use "
+         "Spherical coordinates use degrees, while the other frames use "
          "meters.")
     .def("velocity_transform",
          &Class::VelocityTransform,
          "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame "
-         "Spherical coordinates use radians, while the other frames use "
-         "meters.");
+         "Velocity should not be expressed in SPHERICAL frame (such values "
+         "will be passed through). All other frames use meters.");
 
    py::enum_<Class::CoordinateType>(sphericalCoordinates, "CoordinateType")
        .value("SPHERICAL", Class::CoordinateType::SPHERICAL)

src/python_pybind11/test/SphericalCoordinates_TEST.py

@@ -262,13 +262,14 @@ def test_coordinate_transforms(self):
         # > -1510.88 3766.64 (EAST,NORTH)
         vec = Vector3d(-1510.88, 3766.64, -3.29)
 
-        # Convert degrees to radians
-        osrf_s.x(osrf_s.x() * 0.0174532925)
-        osrf_s.y(osrf_s.y() * 0.0174532925)
+        osrf_s_lat = Angle()
+        osrf_s_lon = Angle()
+        osrf_s_lat.set_degree(osrf_s.x())
+        osrf_s_lon.set_degree(osrf_s.y())
 
         # Set the ORIGIN to be the Open Source Robotics Foundation
-        sc2 = SphericalCoordinates(st, Angle(osrf_s.x()),
-                                   Angle(osrf_s.y()), osrf_s.z(), Angle.ZERO)
+        sc2 = SphericalCoordinates(st, osrf_s_lat, osrf_s_lon, osrf_s.z(),
+                                   Angle.ZERO)
 
         # Check that SPHERICAL -> ECEF works
         tmp = sc2.position_transform(osrf_s, SphericalCoordinates.SPHERICAL,