Refactors the "cylinder torque free" benchmark (RobotLocomotion#7218)

* First pass

* Adds MultibodyAcrobotPlant to the acrobot example

* WIP

* Enables using TypeSafeIndex as key in STL containers

* Adds BUILD file

* Start implementation of MultibodyAcrobotPlant<T>::BuildMultibodyModeler()

* Adds code for mobilizers

* Increases tolerance for checks in CouldBePhysicallyValid()

* Fixes bug in AreMomentsOfInertiaNearPositiveAndSatisfyTriangleInequality()

* Adds output vector of strings to report failures in RotationalInertia::CouldBePhysicallyValid()

* Implements Acrobot's DoCalcTimeDerivatives()

* Moved multibody acrobot model into subdirectory acrobot/multibody/

* Refactors to AcrobotMultibodyPlant

* Renmaes files

* Fixes some issues but energy is not preserved

* Adds logger to acrobot_run_dynamics.cc

* Fixes bug in mass matrix computation

* Implemenets force element

* Updates inverse dynamics to allow force elements and external forces

* Implements unit tests

* DOcuments CalcInverseDynamics()

* Makes the double pendulum test more interesting by defining link2's frame to be coincident with Eo

* Adds some doc

* Allows force elements clonning

* Refactors to UniformGravityFieldElement

* Splits UniformGravityFieldElement into its own file

* Adds doc to ForceElement

* Adds energy budget tracking methods

* Fixes using applied forces when of zero size

* Get rid of a TODO

* Updates CMake files

* Removes lint

* Documents/cleanup new Body methods

* Implements RodElement

* Implements CosseratRodPlant in examples/cosserat_rod

* Implements a Kelvin–Voigt type viscous damping.

* Creates Cantilever example

* Moves RodElement into examples/cosserat_rod

* Makes cosserat rod plant independent of Modeler

* Adds CalcMassMatrixViaInverseDynamics() and CalcBiasTerm() to MBT

* RodPlant builds and runs. Results are NaN

* Adds DoPublish() method to write poses

* Adds some prints

* Run example and verified with analytical solution for coarse grid, n = 11.

* Changes to an implicit Euler integrator

* Faster log(R) implementation. It needs debugging.

* Implements BallMobilizer

* Replaces .rotation() with .linear()

* Implements MapQDotToVelocity()

* Implements BallMobilizer

* Simplify CosseratRodPlant run to a double pendulum

* Adds visualization. Not tested

* Make visualization work

* Introduces ProjectQ()

* Implement RollPitchYawMobilizer

* Creates spatial_double_pendulum_test.cc

* Implements variable radius constructor.

* Creates poking demo

* Implements variable mass elements

* Fixes area as a function of the material coordinate

* Introduces LCMGL capability.

* Draws force vector using LCMGL

* Fixes bug in BodyNode::CalcInverseDynamics_TipToBase()

* Creates twising and bending case using a "spring" constraint at the bottom to keep the rod moving upwards along its original axis.

* Adds doc to RollPitchYawMobilizer

* Implements map in terms of E_F(q)

* Refactors TorqueFreeCylinderExactSolution into its own library, independent of RBT

* Creates ball_joint_test.cc

* Verifies that the ball_joint_test test works

* Add TODO's to ball_joint_test.cc for things to verify before pushing

* Fixes after merge

* PARTIAL MERGE with mbt_joint

* PARTIAL MERGE with mbt_joint

* Fixes after merge

* Adds mbt_joint changes to mbt_dev

* Fixes after the merge

* Fixes ball_joint_test.cc after merge

* Implements FreeBodyPlant::DoMapQDotToVelocity() and DoMapVelocityToQDot()

* Cleansup free_body_plant.h/.cc

* Refactors test name

* Cleansup free_body_test.cc

* Revert unwanted changes

* Reverts in multibody_tree

* Document MBT new methods

* Adds some doc

* Removes unwatend chantges

* Removes lint

* Disables long line lint error in BUILD file

* Updates BUILD for the benchmark

* Removes lint

* Refactor to torque_free_analytical_solution --> free_body

* Referts RPY PR changes

* Updates doc

* Addresses reviews

* Addresses reviews

* Remove using-directive from free_body.h

* Fixes doc

drake/multibody/benchmarks/cylinder_torque_free_analytical_solution/BUILD.bazel → drake/multibody/benchmarks/free_body/BUILD.bazel

@@ -17,13 +17,28 @@ filegroup(
     ]),
 )
 
+drake_cc_library(
+    name = "free_body",
+    srcs = [
+        "free_body.cc",
+    ],
+    hdrs = [
+        "free_body.h",
+    ],
+    deps = [
+        "//drake/common",
+        "//drake/math:geometric_transform",
+    ],
+)
+
 # === test/ ===
 
 drake_cc_googletest(
-    name = "cylinder_torque_free_analytical_solution",
+    name = "rigid_body_plant_free_body_test",
     size = "medium",
     data = [":models"],
     deps = [
+        ":free_body",
         "//drake/common:find_resource",
         "//drake/common/test_utilities:eigen_matrix_compare",
         "//drake/math:autodiff",

drake/multibody/benchmarks/free_body/free_body.cc

@@ -0,0 +1,146 @@
+#include "drake/multibody/benchmarks/free_body/free_body.h"
+
+#include <cmath>
+#include <tuple>
+
+#include "drake/common/eigen_types.h"
+#include "drake/math/quaternion.h"
+
+namespace drake {
+namespace benchmarks {
+namespace free_body {
+
+using Eigen::Vector3d;
+using Eigen::Vector4d;
+using Eigen::VectorXd;
+using Eigen::Quaterniond;
+
+std::tuple<Quaterniond, Vector3d, Vector3d>
+FreeBody::CalculateExactRotationalSolutionABInitiallyAligned(
+    const double t) const {
+  // Constant values of moments of inertia.
+  const double I = get_I();
+  const double J = get_J();
+
+  // Initial values of wx, wy, wz.
+  const Vector3d& w_NB_B_initial = get_w_NB_B_initial();
+  const double wx0 = w_NB_B_initial[0];
+  const double wy0 = w_NB_B_initial[1];
+  const double wz0 = w_NB_B_initial[2];
+
+  // Intermediate calculations for quaternion solution.
+  const double p =
+      std::sqrt(wx0 * wx0 + wy0 * wy0 + (wz0 * J / I) * (wz0 * J / I));
+  const double s = (I - J) / I * wz0;
+  const double coef = wz0 * (J / (I * p));
+  const double spt2 = std::sin(p * t / 2);
+  const double cpt2 = std::cos(p * t / 2);
+  const double sst2 = std::sin(s * t / 2);
+  const double cst2 = std::cos(s * t / 2);
+
+  // Kane's analytical solution is for quaternion quat_AB that relates A to B,
+  // where A is a set Ax, Ay, Az of right-handed orthogonal unit vectors which
+  // are fixed in N (Newtonian frame/World) and initially aligned to Bx, By, Bz,
+  // where Bz is parallel to B's symmetry axis.
+  // Kane produced an analytical solution for quat_AB [eAB0, eAB1, eAB2, eAB3],
+  // which allows for direct calculation of the R_AB rotation matrix.
+  const double eAB1 = spt2 / p * (wx0 * cst2 + wy0 * sst2);
+  const double eAB2 = spt2 / p * (-wx0 * sst2 + wy0 * cst2);
+  const double eAB3 = coef * spt2 * cst2 + cpt2 * sst2;
+  const double eAB0 = -coef * spt2 * sst2 + cpt2 * cst2;
+  const Quaterniond quat_AB(eAB0, eAB1, eAB2, eAB3);
+
+  // Analytical solution for wx(t), wy(t), wz(t).
+  const double wx = wx0 * std::cos(s * t) + wy0 * std::sin(s * t);
+  const double wy = -wx0 * std::sin(s * t) + wy0 * std::cos(s * t);
+  const double wz = wz0;
+
+  // Analytical solution for time-derivatives of wx, wy, wz.
+  const double wxDt = (1 - J / I) * wy * wz;
+  const double wyDt = (-1 + J / I) * wx * wz;
+  const double wzDt = 0.0;
+
+  return std::make_tuple(
+      quat_AB, Vector3d(wx, wy, wz), Vector3d(wxDt, wyDt, wzDt));
+}
+
+std::tuple<Quaterniond, Vector4d, Vector3d, Vector3d>
+FreeBody::CalculateExactRotationalSolutionNB(const double t) const {
+  // Kane's analytical solution is for quaternion quat_AB that relates A to B,
+  // where A is another set Ax, Ay, Az of right-handed orthogonal unit vectors
+  // which are fixed in N (Newtonian frame) but initially aligned to Bx, By, Bz.
+  Quaterniond quat_AB;
+  Vector3d w_NB_B, wDt_NB_B;
+  std::tie(quat_AB, w_NB_B, wDt_NB_B) =
+      CalculateExactRotationalSolutionABInitiallyAligned(t);
+
+  // Define A basis as World-fixed basis aligned with B's initial orientation.
+  // Multiply (Hamilton product) the quaternions quat_NA * quat_AB to produce
+  // quat_NB, which is analogous to multiplying rotation matrices R_NA * R_AB
+  // to produce the rotation matrix R_NB.
+  // In other words, account for quat_NB_initial (which is quat_NA) to calculate
+  // the quaternion quat_NB that is analogous to the R_NB rotation matrix.
+  const Quaterniond& quat_NA = get_quat_NB_initial();
+  const Quaterniond quat_NB = quat_NA * quat_AB;
+
+  // Analytical solution for time-derivative quaternion in B.
+  const Vector4d quatDt =
+      math::CalculateQuaternionDtFromAngularVelocityExpressedInB(quat_NB,
+                                                                 w_NB_B);
+
+  // Create a tuple to package for returning.
+  std::tuple<Quaterniond, Vector4d, Vector3d, Vector3d> returned_tuple;
+  std::get<0>(returned_tuple) = quat_NB;
+  std::get<1>(returned_tuple) = quatDt;
+  std::get<2>(returned_tuple) = w_NB_B;
+  std::get<3>(returned_tuple) = wDt_NB_B;
+
+  return returned_tuple;
+}
+
+std::tuple<Vector3d, Vector3d, Vector3d>
+FreeBody::CalculateExactTranslationalSolution(const double t) const {
+  // Initial values of x, y, z and ẋ, ẏ, ż.
+  const Vector3d& xyz_initial = get_p_NoBcm_N_initial();
+  const Vector3d& xyzDt_initial =
+      GetInitialVelocityOfBcmInWorldExpressedInWorld();
+  const double x0 = xyz_initial[0];
+  const double y0 = xyz_initial[1];
+  const double z0 = xyz_initial[2];
+  const double xDt0 = xyzDt_initial[0];
+  const double yDt0 = xyzDt_initial[1];
+  const double zDt0 = xyzDt_initial[2];
+
+  // Analytical solution for ẋ, ẏ, z̈ (only gravitational forces on body).
+  const Vector3d& gravity = get_uniform_gravity_expressed_in_world();
+  const double x_acceleration = gravity[0];
+  const double y_acceleration = gravity[1];
+  const double z_acceleration = gravity[2];
+
+  // Analytical solution for ẋ, ẏ, ż (since acceleration is constant).
+  const double xDt = xDt0 + x_acceleration * t;
+  const double yDt = yDt0 + y_acceleration * t;
+  const double zDt = zDt0 + z_acceleration * t;
+
+  // Analytical solution for x, y, z (since acceleration is constant).
+  const double x = x0 + xDt0 * t + 0.5 * x_acceleration * t * t;
+  const double y = y0 + yDt0 * t + 0.5 * y_acceleration * t * t;
+  const double z = z0 + zDt0 * t + 0.5 * z_acceleration * t * t;
+
+  // Create a tuple to package for returning.
+  std::tuple<Vector3d, Vector3d, Vector3d> returned_tuple;
+  Vector3d& xyz = std::get<0>(returned_tuple);
+  Vector3d& xyzDt = std::get<1>(returned_tuple);
+  Vector3d& xyzDDt = std::get<2>(returned_tuple);
+
+  // Fill returned_tuple with results.
+  xyz << x, y, z;
+  xyzDt << xDt, yDt, zDt;
+  xyzDDt << x_acceleration, y_acceleration, z_acceleration;
+
+  return returned_tuple;
+}
+
+}  // namespace free_body
+}  // namespace benchmarks
+}  // namespace drake

drake/multibody/benchmarks/free_body/free_body.h

@@ -0,0 +1,206 @@
+#pragma once
+
+#include <cmath>
+#include <tuple>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
+#include "drake/math/quaternion.h"
+
+namespace drake {
+namespace benchmarks {
+namespace free_body {
+
+/// The purpose of the %FreeBody class is to provide the data (initial values
+/// and gravity) and methods for calculating the exact analytical solution for
+/// the translational and rotational motion of a toque-free rigid body B with
+/// axially symmetric inertia, in a Newtonian frame (World) N. Examples of
+/// bodies with axially symmetric inertia include cylinders, rods or bars with a
+/// circular or square cross section and spinning tops.
+/// Since the only external forces on B are uniform gravitational forces, there
+/// exists an exact closed-form analytical solution for B's motion. The closed-
+/// form rotational solution is available since B is "torque-free", i.e., the
+/// moment of all forces about B's mass center is zero.
+/// This class calculates the body B's quaternion, angular velocity and angular
+/// acceleration expressed in B (body-frame) as well as the position, velocity,
+/// acceleration of Bcm (B's center of mass) in N (World).
+/// Algorithm from [Kane, 1983] Sections 1.13 and 3.1, Pages 60-62 and 159-169.
+///
+/// - [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983.
+///   (with P. W. Likins and D. A. Levinson).  Available for free .pdf download:
+///   https:///ecommons.cornell.edu/handle/1813/637
+class FreeBody {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FreeBody)
+
+  FreeBody(const Eigen::Quaterniond& quat_NB_initial,
+           const Eigen::Vector3d& w_NB_B_initial,
+           const Eigen::Vector3d& p_NoBcm_N_initial,
+           const Eigen::Vector3d& v_NBcm_B_initial,
+           const Eigen::Vector3d& gravity_N)
+      : quat_NB_initial_(quat_NB_initial),
+        w_NB_B_initial_(w_NB_B_initial),
+        p_NoBcm_N_initial_(p_NoBcm_N_initial),
+        v_NBcm_B_initial_(v_NBcm_B_initial),
+        uniform_gravity_expressed_in_world_(gravity_N) {}
+
+  ~FreeBody() = default;
+
+  /// Returns the body's moment of inertia about an axis perpendicular to its
+  /// axis of rotation and passing through its center of mass.
+  /// For example, for a cylinder of radius r, length h and uniformly
+  /// distributed mass m with its rotational axis aligined along its body frame
+  /// z-axis this would be: <pre>
+  ///   I = Ixx = Iyy = m / 12 (3 r² + h²)
+  /// </pre>
+  double get_I() const { return 0.04; }
+
+  /// Returns body's moment of inertia about its axis of rotation.
+  /// For example, for a cylinder of radius r, length h and uniformly
+  /// distributed mass  m with its rotational axis aligined along its body frame
+  /// z-axis this would be: <pre>
+  ///   J = Izz = m r² / 2
+  /// </pre>
+  double get_J() const { return 0.02; }
+
+  // Get methods for initial values and gravity.
+  const Eigen::Quaterniond& get_quat_NB_initial() const {
+    return quat_NB_initial_;
+  }
+  const Eigen::Vector3d& get_w_NB_B_initial() const { return w_NB_B_initial_; }
+  const Eigen::Vector3d& get_p_NoBcm_N_initial() const {
+    return p_NoBcm_N_initial_;
+  }
+  const Eigen::Vector3d& get_v_NBcm_B_initial() const {
+    return v_NBcm_B_initial_;
+  }
+  const Eigen::Vector3d& get_uniform_gravity_expressed_in_world() const {
+    return uniform_gravity_expressed_in_world_;
+  }
+  Eigen::Vector3d GetInitialVelocityOfBcmInWorldExpressedInWorld() const {
+    const Eigen::Matrix3d R_NB_initial = quat_NB_initial_.toRotationMatrix();
+    return R_NB_initial * v_NBcm_B_initial_;
+  }
+
+  // Set methods for initial values and gravity.
+  void set_quat_NB_initial(const Eigen::Quaterniond& quat_NB_initial) {
+    quat_NB_initial_ = quat_NB_initial;
+  }
+  void set_w_NB_B_initial(const Eigen::Vector3d& w_NB_B_initial) {
+    w_NB_B_initial_ = w_NB_B_initial;
+  }
+  void set_p_NoBcm_N_initial(const Eigen::Vector3d& p_NoBcm_N_initial) {
+    p_NoBcm_N_initial_ = p_NoBcm_N_initial;
+  }
+  void set_v_NBcm_B_initial(const Eigen::Vector3d& v_NBcm_B_initial) {
+    v_NBcm_B_initial_ = v_NBcm_B_initial;
+  }
+  void SetUniformGravityExpressedInWorld(const Eigen::Vector3d& gravity) {
+    uniform_gravity_expressed_in_world_ = gravity;
+  }
+
+  /// Calculates exact solutions for quaternion and angular velocity expressed
+  /// in body-frame, and their time derivatives for torque-free rotational
+  /// motion of axis-symmetric rigid body B in Newtonian frame (World) N,
+  /// where torque-free means the moment of forces about B's mass center is
+  /// zero. The quaternion characterizes the orientation between
+  /// right-handed orthogonal unit vectors Nx, Ny, Nz fixed in N and
+  /// right-handed orthogonal unit vectors Bx, By, Bz fixed in B, where Bz is
+  /// parallel to B's symmetry axis.
+  ///
+  /// @note CalculateExactRotationalSolutionABInitiallyAligned() implements the
+  /// algorithm from [Kane, 1983] Sections 1.13 and 3.1, Pages 60-62 and
+  /// 159-169.
+  ///
+  /// @param t Current value of time.
+  /// @returns Machine-precision values at time t are returned as defined below.
+  ///
+  /// @note This function allows for initial misalignment of Nx, Ny, Nz and
+  /// Bx, By, Bz.
+  ///
+  /// std::tuple | Description
+  /// -----------|-------------------------------------------------
+  /// quat_NB    | Quaternion relating frame N to frame B: [e0, e1, e2, e3]
+  ///            | Note: quat_NB is analogous to the rotation matrix R_NB.
+  /// quatDt     | Time-derivative of `quat_NB', i.e., [ė0, ė1, ė2, ė3].
+  /// w_NB_B     | B's angular velocity in N, expressed in B.
+  /// alpha_NB_B | B's angular acceleration in N, expressed in B.
+  ///
+  /// - [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York,
+  ///   1983. (with P. W. Likins and D. A. Levinson).  Available for free .pdf
+  ///   download: https:///ecommons.cornell.edu/handle/1813/637
+  std::tuple<Eigen::Quaterniond, Eigen::Vector4d, Eigen::Vector3d,
+             Eigen::Vector3d>
+  CalculateExactRotationalSolutionNB(const double t) const;
+
+  /// Calculates exact solutions for translational motion of an arbitrary rigid
+  /// body B in a Newtonian frame (world) N.  Algorithm from high-school
+  /// physics.
+  ///
+  /// @param t Current value of time.
+  /// @returns Machine-precision values at time t are returned as defined below.
+  ///
+  /// std::tuple | Description
+  /// -----------|-----------------------------------------------------------
+  /// xyz        | Vector3d [x, y, z], Bcm's position from No, expressed in N.
+  /// xyzDt      | Vector3d [ẋ, ẏ, ż]  Bcm's velocity in N, expressed in N.
+  /// xyzDDt     | Vector3d [ẍ  ÿ  z̈], Bcm's acceleration in N, expressed in N.
+  std::tuple<Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d>
+  CalculateExactTranslationalSolution(const double t) const;
+
+ private:
+  // This "helper" method calculates quat_NB, w_NB_B, and alpha_NB_B at time t.
+  // More precisely, this method calculates exact solutions for quaternion,
+  // angular velocity, and angular acceleration expressed in body-frame, for
+  // torque-free rotational motion of an axis-symmetric rigid body B in
+  // Newtonian frame (World) N, where torque-free means the moment of forces
+  // about B's mass center is zero.
+  // Right-handed orthogonal unit vectors Nx, Ny, Nz fixed in N are initially
+  // equal to right-handed orthogonal unit vectors Bx, By, Bz fixed in B,
+  // where Bz is parallel to B's symmetry axis.
+  // Note: The function CalculateExactRotationalSolutionNB() is a more general
+  // solution that allows for initial misalignment of Bx, By, Bz.
+  // Algorithm from [Kane, 1983] Sections 1.13 and 3.1, Pages 60-62 and 159-169.
+  // @param t Current value of time.
+  // @returns Machine-precision values at time t are returned as defined below.
+  //
+  // std::tuple | Description
+  // -----------|-------------------------------------------------
+  // quat_NB    | Quaternion relating Nx, Ny, Nz to Bx, By, Bz.
+  //            | Note: quat_NB is analogous to the rotation matrix R_NB.
+  // w_NB_B     | B's angular velocity in N, expressed in B, e.g., [wx, wy, wz].
+  // alpha_NB_B | B's angular acceleration in N, expressed in B, [ẇx, ẇy, ẇz].
+  //
+  // - [Kane, 1983] "Spacecraft Dynamics," McGraw-Hill Book Co., New York, 1983.
+  //   (with P. W. Likins and D. A. Levinson).  Available for free .pdf
+  // download: https://ecommons.cornell.edu/handle/1813/637
+  std::tuple<Eigen::Quaterniond, Eigen::Vector3d, Eigen::Vector3d>
+  CalculateExactRotationalSolutionABInitiallyAligned(const double t) const;
+
+  // quat_NB_initial_ is the initial (t=0) value of the quaternion that relates
+  // unit vectors Nx, Ny, Nz fixed in World N (e.g., Nz vertically upward) to
+  // unit vectors Bx, By, Bz fixed in body B (Bz parallel to symmetry axis)
+  // Note: The quaternion should already be normalized before it is set.
+  // Note: quat_NB_initial is analogous to the initial rotation matrix R_NB.
+  Eigen::Quaterniond quat_NB_initial_;
+
+  // w_NB_B_initial_ is B's initial angular velocity in N, expressed in B.
+  Eigen::Vector3d w_NB_B_initial_;
+
+  // p_NoBcm_N_initial_ is Bcm's initial position from No, expressed in N, i.e.,
+  // x, y, z, the Nx, Ny, Nz measures of Bcm's position vector from point No
+  // (World origin).  Note: Bcm (B's center of mass) is coincident with Bo.
+  Eigen::Vector3d p_NoBcm_N_initial_;
+
+  // v_NBcm_B_initial_ is Bcm's initial velocity in N, expressed in B.
+  // Note: v_NBcm_B is not (in general) the time-derivative of ẋ, ẏ, ż.
+  Eigen::Vector3d v_NBcm_B_initial_;
+
+  // uniform_gravity_expressed_in_world_ is the local planet's (e.g., Earth)
+  // uniform gravitational acceleration, expressed in World (e.g., Earth).
+  Eigen::Vector3d uniform_gravity_expressed_in_world_;
+};
+
+}  // namespace free_body
+}  // namespace benchmarks
+}  // namespace drake