Add RationalForwardKinematics. (RobotLocomotion#17572)

common/symbolic/trigonometric_polynomial.cc

@@ -410,15 +410,19 @@ bool CheckPolynomialIndeterminatesAreSinCos(
   }
   return e_poly.indeterminates().IsSubsetOf(sin_cos_vars);
 }
+}  // namespace
 
-void SubstituteStereographicProjectionImpl(
+namespace internal {
+symbolic::RationalFunction SubstituteStereographicProjectionImpl(
     const symbolic::Polynomial& e_poly, const std::vector<SinCos>& sin_cos,
     const VectorX<symbolic::Variable>& t, const symbolic::Variables& t_set,
     const VectorX<symbolic::Polynomial>& one_plus_t_angles_squared,
     const VectorX<symbolic::Polynomial>& two_t_angles,
-    const VectorX<symbolic::Polynomial>& one_minus_t_angles_squared,
-    symbolic::RationalFunction* e_rational) {
+    const VectorX<symbolic::Polynomial>& one_minus_t_angles_squared) {
   DRAKE_DEMAND(static_cast<int>(sin_cos.size()) == t.rows());
+  DRAKE_DEMAND(one_plus_t_angles_squared.size() == t.rows());
+  DRAKE_DEMAND(two_t_angles.size() == t.rows());
+  DRAKE_DEMAND(one_minus_t_angles_squared.size() == t.rows());
   DRAKE_DEMAND(CheckPolynomialIndeterminatesAreSinCos(e_poly, sin_cos));
   // First find the angles whose cos or sin appear in the polynomial. This
   // will determine the denominator of the rational function.
@@ -438,9 +442,8 @@ void SubstituteStereographicProjectionImpl(
     }
   }
   if (angle_indices.empty()) {
-    *e_rational = symbolic::RationalFunction(
+    return symbolic::RationalFunction(
         symbolic::Polynomial(e_poly.ToExpression(), t_set));
-    return;
   }
   const symbolic::Monomial monomial_one{};
   symbolic::Polynomial denominator{1};
@@ -476,9 +479,9 @@ void SubstituteStereographicProjectionImpl(
     numerator += numerator_term;
   }
 
-  *e_rational = symbolic::RationalFunction(numerator, denominator);
+  return symbolic::RationalFunction(numerator, denominator);
 }
-}  // namespace
+}  // namespace internal
 
 symbolic::RationalFunction SubstituteStereographicProjection(
     const symbolic::Polynomial& e_poly, const std::vector<SinCos>& sin_cos,
@@ -495,11 +498,8 @@ symbolic::RationalFunction SubstituteStereographicProjection(
         {{monomial_one, 1}, {symbolic::Monomial(t(i), 2), 1}});
   }
   const symbolic::Variables t_set{t};
-  symbolic::RationalFunction e_rational;
-  SubstituteStereographicProjectionImpl(e_poly, sin_cos, t, t_set,
-                                        one_plus_t_square, two_t,
-                                        one_minus_t_square, &e_rational);
-  return e_rational;
+  return internal::SubstituteStereographicProjectionImpl(
+      e_poly, sin_cos, t, t_set, one_plus_t_square, two_t, one_minus_t_square);
 }
 
 symbolic::RationalFunction SubstituteStereographicProjection(

common/symbolic/trigonometric_polynomial.h

@@ -85,6 +85,34 @@ Substitute(const Eigen::MatrixBase<Derived>& m,
   });
 }
 
+namespace internal {
+/*
+ This is the actual implementation of SubstituteStereographicProjection().
+ We expose this function because repeated calls to
+ SubstituteStereographicProjection() require re-constructing the polynomial
+ 1+t², 2t, 1-t²; it is more efficient to construct these polynomials
+ beforehand and then call SubstituteStereographicProjectionImpl() repeatedly.
+ @param e_poly The polynomial before substitution.
+ @param sin_cos The sin and cos variables in e_poly to be replaced.
+ @param t We will replace sin and cos functions as rational functions of t.
+ @param t_set The set of variables @p t.
+ @param one_plus_t_angles_squared 1+t²
+ @param two_t_angles 2t
+ @param one_minus_t_angles_squared 1-t²
+ @retval e_rational The rational polynomial after replacing sin/cos in @p
+ e_poly with rational functions of t.
+ @pre sin_cos, t, t_set, one_plus_t_angles_squared, two_t_angles,
+ one_minus_t_angles_squared all have the same size.
+ @pre The indeterminates in `e_poly` are `sin_cos`.
+ */
+symbolic::RationalFunction SubstituteStereographicProjectionImpl(
+    const symbolic::Polynomial& e_poly, const std::vector<SinCos>& sin_cos,
+    const VectorX<symbolic::Variable>& t, const symbolic::Variables& t_set,
+    const VectorX<symbolic::Polynomial>& one_plus_t_angles_squared,
+    const VectorX<symbolic::Polynomial>& two_t_angles,
+    const VectorX<symbolic::Polynomial>& one_minus_t_angles_squared);
+}  // namespace internal
+
 /**
  * Substitutes the variables representing sine and cosine functions with their
  * stereographic projection.

multibody/rational/BUILD.bazel

@@ -1,16 +1,17 @@
 # -*- python -*-
+# This file contains rules for Bazel; see drake/doc/bazel.rst.
 
 load(
     "@drake//tools/skylark:drake_cc.bzl",
-    "drake_cc_binary",
     "drake_cc_googletest",
     "drake_cc_library",
     "drake_cc_package_library",
-    "drake_cc_test",
 )
 load("//tools/lint:lint.bzl", "add_lint_tests")
 
-package(default_visibility = ["//visibility:public"])
+package(
+    default_visibility = ["//visibility:public"],
+)
 
 drake_cc_package_library(
     name = "rational",
@@ -23,9 +24,11 @@ drake_cc_package_library(
 drake_cc_library(
     name = "rational_forward_kinematics",
     srcs = [
+        "rational_forward_kinematics.cc",
         "rational_forward_kinematics_internal.cc",
     ],
     hdrs = [
+        "rational_forward_kinematics.h",
         "rational_forward_kinematics_internal.h",
     ],
     deps = [
@@ -36,7 +39,6 @@ drake_cc_library(
     ],
 )
 
-# === test/ ===
 drake_cc_library(
     name = "rational_forward_kinematics_test_utilities",
     testonly = 1,
@@ -66,4 +68,15 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "rational_forward_kinematics_test",
+    timeout = "moderate",
+    deps = [
+        ":rational_forward_kinematics",
+        ":rational_forward_kinematics_test_utilities",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:symbolic_test_util",
+    ],
+)
+
 add_lint_tests()