Add constructors to RollPitchYaw that takes RotationMatrix and Quater…

…nion (RobotLocomotion#8721)

automotive/agent_trajectory.h

@@ -53,7 +53,8 @@ class PoseVelocity final {
   /// Accesses the projection of the pose of frame A to the x-y components of
   /// translation and the z-component of rotation.
   Eigen::Vector3d pose3() const {
-    const double w_z = math::QuaternionToSpaceXYZ(rotation_).z();
+    const math::RollPitchYaw<double> rpy(rotation_);
+    const double w_z = rpy.get_yaw_angle();
     return Eigen::Vector3d{translation_.x(), translation_.y(), w_z};
   }
 

automotive/maliput/api/lane_data.h

@@ -11,6 +11,7 @@
 #include "drake/common/eigen_types.h"
 #include "drake/common/extract_double.h"
 #include "drake/math/quaternion.h"
+#include "drake/math/roll_pitch_yaw.h"
 #include "drake/math/rotation_matrix.h"
 
 namespace drake {
@@ -92,22 +93,22 @@ class Rotation {
 
   /// Provides a representation of rotation as a vector of angles
   /// `[roll, pitch, yaw]` (in radians).
-  Vector3<double> rpy() const {
-    return math::QuaternionToSpaceXYZ(quaternion_);
+  math::RollPitchYaw<double> rpy() const {
+    return math::RollPitchYaw<double>(quaternion_);
   }
 
   // TODO([email protected])  Deprecate and/or remove roll()/pitch()/yaw(),
   //                          since they hide the call to rpy(), and since
   //                          most call-sites should probably be using something
   //                          else (e.g., quaternion) anyway.
   /// Returns the roll component of the Rotation (in radians).
-  double roll() const { return rpy().x(); }
+  double roll() const { return rpy().get_roll_angle(); }
 
   /// Returns the pitch component of the Rotation (in radians).
-  double pitch() const { return rpy().y(); }
+  double pitch() const { return rpy().get_pitch_angle(); }
 
   /// Returns the yaw component of the Rotation (in radians).
-  double yaw() const { return rpy().z(); }
+  double yaw() const { return rpy().get_yaw_angle(); }
 
  private:
   explicit Rotation(const Quaternion<double>& quat) : quaternion_(quat) {}

automotive/maliput/api/test/lane_data_test.cc

@@ -209,7 +209,7 @@ const double kRotationTolerance = 1e-15;
     EXPECT_TRUE(CompareMatrices(dut.quat().coeffs(),                    \
                                 Vector4<double>(_x, _y, _z, _w),        \
                                 kRotationTolerance));                   \
-    EXPECT_TRUE(CompareMatrices(dut.rpy(),                              \
+    EXPECT_TRUE(CompareMatrices(dut.rpy().vector(),                     \
                                 Vector3<double>(_ro, _pi, _ya),         \
                                 kRotationTolerance));                   \
     EXPECT_NEAR(dut.roll(), _ro, kRotationTolerance);                   \

automotive/test/agent_trajectory_test.cc

@@ -16,7 +16,6 @@ namespace {
 static constexpr double kTol = 1e-12;
 
 using math::IsQuaternionValid;
-using math::RollPitchYaw;
 using multibody::SpatialVelocity;
 using Eigen::Isometry3d;
 using Eigen::Quaternion;
@@ -43,20 +42,21 @@ GTEST_TEST(PoseVelocityTest, Accessors) {
   using std::sqrt;
 
   const Translation<double, 3> translation{1., 2., 3.};
-  const Vector3d rpy{0.4, 0.5, 0.6};
-  const Quaternion<double> rotation = RollPitchYaw<double>(rpy).ToQuaternion();
+  const math::RollPitchYaw<double> rpy(0.4, 0.5, 0.6);
+  const Quaternion<double> quaternion = rpy.ToQuaternion();
   const Vector3d w{8., 9., 10.};
   const Vector3d v{11., 12., 13.};
   const SpatialVelocity<double> velocity{w, v};
-  const PoseVelocity actual(rotation, translation, velocity);
+  const PoseVelocity actual(quaternion, translation, velocity);
 
   EXPECT_TRUE(
       CompareMatrices(actual.translation().vector(), translation.vector()));
   EXPECT_TRUE(
-      CompareMatrices(actual.rotation().matrix(), rotation.matrix(), kTol));
+      CompareMatrices(actual.rotation().matrix(), quaternion.matrix(), kTol));
   EXPECT_TRUE(CompareMatrices(actual.velocity().rotational(), w));
   EXPECT_TRUE(CompareMatrices(actual.velocity().translational(), v));
-  const Vector3d expected_pose3{translation.x(), translation.y(), rpy.z()};
+  const Vector3d expected_pose3{translation.x(), translation.y(),
+                                rpy.get_yaw_angle()};
   EXPECT_TRUE(CompareMatrices(actual.pose3(), expected_pose3));
   EXPECT_EQ(actual.speed(), sqrt(pow(v(0), 2) + pow(v(1), 2) + pow(v(2), 2)));
 }

automotive/test/pose_selector_test.cc

@@ -836,7 +836,7 @@ void AddToTrafficPosesAt(int index,
 
   const Rotation traffic_rotation =
       traffic_lane->GetOrientation(srh);
-  Vector3<double> rpy = traffic_rotation.rpy();
+  Vector3<double> rpy = traffic_rotation.rpy().vector();
   rpy.x() = (traffic_polarity == LanePolarity::kWithS) ? rpy.x() : -rpy.x();
   rpy.y() = (traffic_polarity == LanePolarity::kWithS) ? rpy.y() : -rpy.y();
   rpy.z() -= (traffic_polarity == LanePolarity::kWithS) ? 0. : M_PI;
@@ -875,7 +875,7 @@ void SetDefaultOnrampPoses(const Lane* ego_lane,
   const double ego_yaw =
       ego_rotation.yaw() - ((ego_polarity == LanePolarity::kWithS) ? 0. : M_PI);
   const Rotation new_rotation = Rotation::FromRpy(ego_roll, ego_pitch, ego_yaw);
-  const Vector3<double> new_rpy = new_rotation.rpy();
+  const Vector3<double> new_rpy = new_rotation.rpy().vector();
   ego_pose->set_rotation(math::RollPitchYaw<double>(new_rpy).ToQuaternion());
 
   const Eigen::Matrix3d ego_rotmat = math::rpy2rotmat(new_rpy);

examples/kuka_iiwa_arm/pick_and_place/pick_and_place_configuration_parsing.cc

@@ -47,8 +47,9 @@ Isometry3<double> ParsePose(const proto::Pose& pose) {
   }
   if (!pose.rpy().empty()) {
     DRAKE_THROW_UNLESS(pose.rpy_size() == 3);
-    X.linear() =
-        rpy2rotmat(Vector3<double>(pose.rpy(0), pose.rpy(1), pose.rpy(2)));
+    const math::RollPitchYaw<double> rpy(pose.rpy(0), pose.rpy(1), pose.rpy(2));
+    const math::RotationMatrix<double> R(rpy);
+    X.linear() = R.matrix();
   }
   return X;
 }

examples/kuka_iiwa_arm/pick_and_place/test/pick_and_place_configuration_parsing_test.cc

@@ -18,7 +18,6 @@ namespace kuka_iiwa_arm {
 namespace pick_and_place {
 namespace {
 
-using math::rpy2rotmat;
 using pick_and_place::PlannerConfiguration;
 using pick_and_place::SimulatedPlantConfiguration;
 using pick_and_place::OptitrackConfiguration;

math/BUILD.bazel

@@ -89,7 +89,6 @@ drake_cc_library(
         "random_rotation.h",
         "roll_pitch_yaw.h",
         "roll_pitch_yaw_not_using_quaternion.h",
-        "roll_pitch_yaw_using_quaternion.h",
         "rotation_matrix.h",
         "rotation_matrix_deprecated.h",
         "transform.h",

math/quaternion.h

@@ -201,155 +201,6 @@ Matrix3<typename Derived::Scalar> quat2rotmat(
   return R.matrix();
 }
 
-/**
- * Computes SpaceXYZ Euler angles (roll-pitch-yaw) from a quaternion.
- * @param quaternion unit quaternion with elements [e0=w, e1=x, e2=y, e3=z].
- * @return 3x1 SpaceXYZ Euler angles (called roll-pitch-yaw by ROS).
- *
- * This accurate algorithm avoids numerical round-off issues encountered by
- * some algorithms when pitch angle is within 1E-6 of PI/2 or -PI/2.
- *
- * Note: SpaceXYZ roll-pitch-yaw is equivalent to BodyZYX yaw-pitch-roll.
- * http://answers.ros.org/question/58863/incorrect-rollpitch-yaw-values-using-getrpy/
- *
- * <h3>Theory</h3>
- *
- * This algorithm was created October 2016 by Paul Mitiguy for TRI (Toyota).
- * We believe this is a new algorithm (not previously published).
- * Some of the theory/formulation of this algorithm are provided below.
- *
- * <pre>
- * Notation: Angles q1, q2, q3 designate SpaceXYZ "roll, pitch, yaw" angles.
- *           Symbols e0, e1, e2, e3 are elements of the passed-in quaternion.
- *           e0 = cos(theta/2), e1 = L1*sin(theta/2), e2 = L2*sin(theta/2), ...
- *
- * Step 1.  Convert the quaternion to a 3x3 rotation matrix R.
- *          This is done solely to provide an accurate computation of pitch-
- *          angle q2, which is calculated with the atan2 function and only 5
- *          elements of what is interpretated as a SpaceXYZ rotation matrix.
- *          Since only 5 elements of R are used, perhaps the algorithm could
- *          be improved by only calculating those 5 elements -- or manipulating
- *          those 5 elements to reduce calculations involving e0, e1, e2, e3.
- *
- * Step 2.  Realize the quaternion passed to the function can be regarded as
- *          resulting from multiplication of certain 4x4 and 4x1 matrices, or
- *          multiplying three rotation quaternions (Hamilton product), to give:
- *          e0 = sin(q1/2)*sin(q2/2)*sin(q3/2) + cos(q1/2)*cos(q2/2)*cos(q3/2)
- *          e1 = sin(q3/2)*cos(q1/2)*cos(q2/2) - sin(q1/2)*sin(q2/2)*cos(q3/2)
- *          e2 = sin(q1/2)*sin(q3/2)*cos(q2/2) + sin(q2/2)*cos(q1/2)*cos(q3/2)
- *          e3 = sin(q1/2)*cos(q2/2)*cos(q3/2) - sin(q2/2)*sin(q3/2)*cos(q1/2)
- *
- *          Reference for step 2:
- * https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
- *
- * Step 3.  Since q2 has already been calculated (in Step 1), substitute
- *          cos(q2/2) = A and sin(q2/2) = f*A.
- *          Note: The final results are independent of A and f = tan(q2/2).
- *          Note: -pi/2 <= q2 <= pi/2  so -0.707 <= [A = cos(q2/2)] <= 0.707...
- *          and  -1 <= [f = tan(q2/2)] <= 1.
- *
- * Step 4.  Referring to Step 2 form: (1+f)*e1 + (1+f)*e3 and rearrange to:
- *          sin(q1/2+q3/2) = (e1+e3)/(A*(1-f))
- *
- *          Referring to Step 2 form: (1+f)*e0 - (1+f)*e2 and rearrange to:
- *          cos(q1/2+q3/2) = (e0-e2)/(A*(1-f))
- *
- *          Combine the two previous results to produce:
- *          1/2*( q1 + q3 ) = atan2( e1+e3, e0-e2 )
- *
- * Step 5.  Referring to Step 2 form: (1-f)*e1 - (1-f)*e3 and rearrange to:
- *          sin(q1/5-q3/5) = -(e1-e3)/(A*(1+f))
- *
- *          Referring to Step 2 form: (1-f)*e0 + (1-f)*e2 and rearrange to:
- *          cos(q1/2-q3/2) = (e0+e2)/(A*(1+f))
- *
- *          Combine the two previous results to produce:
- *          1/2*( q1 - q3 ) = atan2( e3-e1, e0+e2 )
- *
- * Step 6.  Combine Steps 4 and 5 and solve the linear equations for q1, q3.
- *          Use zA, zB to handle case in which both atan2 arguments are 0.
- *          zA = (e1+e3==0  &&  e0-e2==0) ? 0 : atan2( e1+e3, e0-e2 );
- *          zB = (e3-e1==0  &&  e0+e2==0) ? 0 : atan2( e3-e1, e0+e2 );
- *          Solve: 1/2*( q1 + q3 ) = zA     To produce:  q1 = zA + zB
- *                 1/2*( q1 - q3 ) = zB                  q3 = zA - zB
- *
- * Step 7.  As necessary, modify angles by 2*PI to return angles in range:
- *          -pi   <= q1 <= pi
- *          -pi/2 <= q2 <= pi/2
- *          -pi   <= q3 <= pi
- *
- * Textbook reference: Mitiguy, Paul, Advanced Dynamics and Motion Simulation,
- *                     For professional engineers and scientists (2017).
- *                     Section 8.2, Euler rotation angles, pg 60.
- *                     Available at www.MotionGenesis.com
- * </pre>
- * @author Paul Mitiguy
-**/
-template <typename T>
-Vector3<T> QuaternionToSpaceXYZ(const Eigen::Quaternion<T>& quaternion) {
-  const Eigen::Matrix3d R = RotationMatrix<T>(quaternion).matrix();
-
-  using std::atan2;
-  using std::sqrt;
-  using std::abs;
-
-  // This algorithm is specific to SpaceXYZ order, including the calculation
-  // of q2, the formulas for xA,yA, xB,yB, and values of q1, q3.
-  // It is easily modified for other SpaceIJK and BodyIJI rotation sequences.
-
-  // Calculate q2 using lots of information in the rotation matrix.
-  // Rsum = abs( cos(q2) ) is inherently non-negative.
-  // R20 = -sin(q2) may be negative, zero, or positive.
-  const T R22 = R(2, 2);
-  const T R21 = R(2, 1);
-  const T R10 = R(1, 0);
-  const T R00 = R(0, 0);
-  const T Rsum = sqrt((R22 * R22 + R21 * R21 + R10 * R10 + R00 * R00) / 2);
-  const T R20 = R(2, 0);
-  const T q2 = atan2(-R20, Rsum);
-
-  // Calculate q1 and q3 from Steps 2-6 (documented above).
-  const T e0 = quaternion.w(), e1 = quaternion.x();
-  const T e2 = quaternion.y(), e3 = quaternion.z();
-  const T yA = e1 + e3, xA = e0 - e2;
-  const T yB = e3 - e1, xB = e0 + e2;
-  const T epsilon = Eigen::NumTraits<T>::epsilon();
-  const bool isSingularA = abs(yA) <= epsilon && abs(xA) <= epsilon;
-  const bool isSingularB = abs(yB) <= epsilon && abs(xB) <= epsilon;
-  const T zA = isSingularA ? 0.0 : atan2(yA, xA);
-  const T zB = isSingularB ? 0.0 : atan2(yB, xB);
-  T q1 = zA - zB;  // First angle in rotation sequence.
-  T q3 = zA + zB;  // Third angle in rotation sequence.
-
-  // If necessary, modify angles q1 and/or q3 to be between -pi and pi.
-  if (q1 > M_PI) q1 = q1 - 2 * M_PI;
-  if (q1 < -M_PI) q1 = q1 + 2 * M_PI;
-  if (q3 > M_PI) q3 = q3 - 2 * M_PI;
-  if (q3 < -M_PI) q3 = q3 + 2 * M_PI;
-
-  // Return in Drake/ROS conventional SpaceXYZ q1, q2, q3 (roll-pitch-yaw) order
-  // (which is equivalent to BodyZYX q3, q2, q1 order).
-  Vector3<T> spaceXYZ_angles(q1, q2, q3);
-
-#ifdef DRAKE_ASSERT_IS_ARMED
-  // This algorithm converts from quaternion to SpaceXYZ.
-  // Test this algorithm by converting the quaternion to a rotation matrix
-  // and converting the SpaceXYZ angles to a rotation matrix and ensuring
-  // these rotation matrices are within epsilon of each other.
-  // Assuming sine, cosine are accurate to 4*(standard double-precision epsilon
-  // = 2.22E-16) and there are two sets of two multiplies and one addition for
-  // each rotation matrix element, I decided to test with 20 * epsilon:
-  // (1+4*eps)*(1+4*eps)*(1+4*eps) = 1 + 3*(4*eps) + 3*(4*eps)^2 + (4*eps)^3.
-  // Each + or * or sqrt rounds-off, which can introduce 1/2 eps for each.
-  // Use: (12*eps) + (4 mults + 1 add) * 1/2 eps = 17.5 eps.
-  const Matrix3<T> R_quaternion = RotationMatrix<T>(quaternion).matrix();
-  const Matrix3<T> R_spaceXYZ = rpy2rotmat(spaceXYZ_angles);
-  DRAKE_ASSERT(R_quaternion.isApprox(R_spaceXYZ, 20 * epsilon));
-#endif
-
-  return spaceXYZ_angles;
-}
-
 /**
  * This function tests whether a quaternion is in "canonical form" meaning that
  * it tests whether the quaternion [w, x, y, z] has a non-negative w value.

math/random_rotation.h

@@ -7,6 +7,7 @@
 #include "drake/common/constants.h"
 #include "drake/common/eigen_types.h"
 #include "drake/math/quaternion.h"
+#include "drake/math/roll_pitch_yaw.h"
 
 namespace drake {
 namespace math {
@@ -59,7 +60,8 @@ template <class Generator>
 Eigen::Vector3d UniformlyRandomRPY(Generator* generator) {
   DRAKE_DEMAND(generator != nullptr);
   const Eigen::Quaterniond q = UniformlyRandomQuaternion(generator);
-  return QuaternionToSpaceXYZ(q);
+  const RollPitchYaw<double> rpy(q);
+  return rpy.vector();
 }
 
 }  // namespace math