rod2d.h

#pragma once

#include <memory>
#include <utility>
#include <vector>

#include "drake/examples/rod2d/gen/rod2d_state_vector.h"
#include "drake/multibody/constraint/constraint_problem_data.h"
#include "drake/multibody/constraint/constraint_solver.h"
#include "drake/solvers/moby_lcp_solver.h"
#include "drake/systems/framework/leaf_system.h"

namespace drake {
namespace examples {
namespace rod2d {

// TODO(edrumwri): Track energy and add a test to check it.
/** Dynamical system representation of a rod contacting a half-space in
two dimensions.

<h3>Notation</h3>
In the discussion below and in code comments, we will use the 2D analog of our
standard multibody notation as described in detail here:
@ref multibody_notation.
<!-- https://drake.mit.edu/doxygen_cxx/group__multibody__notation.html -->

For a quick summary and translation to 2D:

- When we combine rotational and translational quantities into a single
  quantity in 3D, we call those "spatial" quantities. In 2D those combined
  quantities are actually planar, but we will continue to refer to them as
  "spatial" to keep the notation analogous and promote easy extension of 2D
  pedagogical examples to 3D.
- We use capital letters to represent bodies and coordinate frames. Frame F has
  an origin point Fo, and a basis formed by orthogonal unit vector axes Fx and
  Fy, with an implicit `Fz=Fx × Fy` always pointing out of the screen for a 2D
  system. The inertial frame World is W, and the rod frame is R.
- We also use capitals to represent points, and we allow a frame name F to be
  used where a point is expected to represent its origin Fo.
- We use `p_CD` to represent the position vector from point C to point D. Note
  that if A and B are frames, `p_AB` means `p_AoBo`.
- If we need to be explicit about the expressed-in frame F for any quantity, we
  add the suffix `_F` to its symbol. So the position vector from C to D,
  expressed in W, is `p_CD_W`.
- R_AB is the rotation matrix giving frame B's orientation in frame A.
- X_AB is the transformation matrix giving frame B's pose in frame A, combining
  both a rotation and a translation; this is conventionally called a
  "transform". A transform is a spatial quantity.

In 2D, with frames A and B the above quantities are (conceptually) matrices
with the indicated dimensions: <pre>
    p_AB = Bo-Ao = |x|      R_AB=| cθ -sθ |       X_AB=| R_AB p_AB |
                   |y|₂ₓ₁        | sθ  cθ |₂ₓ₂         | 0  0   1  |₃ₓ₃
</pre>
where x,y are B's Cartesian coordinates in the A frame, and θ is the
counterclockwise angle from Ax to Bx, measured about Az (and Bz). In practice,
2D rotations are represented just by the scalar angle θ, and 2D transforms are
represented by (x,y,θ).

We use v for translational velocity of a point and w (ω) for rotational
velocity of a frame. The symbols are:

- `v_AP` is point P's velocity in frame A, expressed in frame A if no
  other frame is given as a suffix.
- `w_AB` is frame B's angular velocity in frame A, expressed in frame A
  if no other frame is given as a suffix.
- `V_AB` is frame B's spatial velocity in A, meaning `v_ABo` and `w_AB`.


These quantities are conceptually: <pre>
    v_AB = |vx|      w_AB=|0|       V_AB=| w_AB |
           |vy|           |0|            | v_AB |₆ₓ₁
           | 0|₃ₓ₁        |ω|₃ₓ₁
</pre>
but in 2D we represent translational velocity with just (vx,vy), angular
velocity with just the scalar w=ω=@f$\dot{\theta}@f$ (that is, d/dt θ), and
spatial velocity as (vx,vy,ω).

Forces f and torques τ are represented similarly:

- `f_P` is an in-plane force applied to a point P fixed to some rigid body.
- `t_A` is an in-plane torque applied to frame A (meaning it is about Az).
- `F_A` is a spatial force including both `f_Ao` and `t_A`.

The above symbols can be suffixed with an expressed-in frame if the frame is
not already obvious, so `F_A_W` is a spatial force applied to frame A (at Ao)
but expressed in W. These quantities are conceptually: <pre>
    f_A = |fx|      t_A=|0|       F_A=| t_A |
          |fy|          |0|           | f_A |₆ₓ₁
          | 0|₃ₓ₁       |τ|₃ₓ₁
</pre>
but in 2D we represent translational force with just (fx,fy), torque with just
the scalar t=τ, and spatial force as (fx,fy,τ).

<h3>The 2D rod model</h3>
The rod's coordinate frame R is placed at the rod's center point Ro, which
is also its center of mass Rcm. R's planar pose is given by a planar
transform X_WR=(x,y,θ). When X_WR=0 (identity transform), R is coincident
with the World frame W, and aligned horizontally as shown: <pre>

       +Wy                                  +Ry
        |                                    |
        |                                    |<---- h ----->
        |                      ==============|==============
      Wo*-----> +Wx         Rl*            Ro*-----> +Rx    *Rr
                               =============================
       World frame                      Rod R, θ=0
</pre>
θ is the angle between Rx and Wx, measured using the right hand rule about
Wz (out of the screen), that is, counterclockwise. The rod has half-length
h, and "left" and "right" endpoints `Rl=Ro-h*Rx` and `Rr=Ro+h*Rx` at which
it can contact the halfspace whose surface is at Wy=0.

This system can be simulated using one of three models:

- continuously, using a compliant contact model (the rod is rigid, but contact
  between the rod and the half-space is modeled as compliant) simulated using
  ordinary differential equations (ODEs),
- a fully rigid model simulated with piecewise differential algebraic
  equations (DAEs), and
- a fully rigid model simulated as a discrete system using a first-order
  discretization approach.

The rod state is initialized to the configuration that corresponds to the
Painlevé Paradox problem, described in [Stewart 2000]. The paradox consists
of a rod contacting a planar surface *without impact* and subject to sliding
Coulomb friction. The problem is well known to correspond to an
*inconsistent rigid contact configuration*, where impulsive forces are
necessary to resolve the problem.

Inputs: planar force (two-dimensional) and torque (scalar), which are
        arbitrary "external" forces (expressed in the world frame) applied
        at the center-of-mass of the rod.

States: planar position (state indices 0 and 1) and orientation (state
        index 2), and planar linear velocity (state indices 3 and 4) and
        scalar angular velocity (state index 5) in units of m, radians,
        m/s, and rad/s, respectively. Orientation is measured counter-
        clockwise with respect to the x-axis.

Outputs: Output Port 0 corresponds to the state vector.

- [Stewart, 2000]  D. Stewart, "Rigid-Body Dynamics with Friction and
                   Impact". SIAM Rev., 42(1), 3-39, 2000.

@system
name: Rod2D
input_ports:
- u0
output_ports:
- state_output
@endsystem

@tparam_double_only
*/
template <typename T>
class Rod2D : public systems::LeafSystem<T> {
 public:
  ~Rod2D() override {}

  /// System model and approach for simulating the system.
  enum class SystemType {
    /// For modeling the system using rigid contact, Coulomb friction, and
    /// hybrid mode variables and simulating the system through piecewise
    /// solutions of differential algebraic equations.
    kPiecewiseDAE,

    /// For modeling the system using either rigid or compliant contact,
    /// Coulomb friction, and a first-order time discretization (which can
    /// be applied to simulating the system without an integrator).
    kDiscretized,

    /// For modeling the system using compliant contact, Coulomb friction,
    /// and ordinary differential equations and simulating the system
    /// through standard algorithms for solving initial value problems.
    kContinuous
  };

  /// Constructor for the 2D rod system using the piecewise DAE (differential
  /// algebraic equation) based approach, the discretization approach, or the
  /// continuous ordinary differential equation based approach.
  /// @param dt The integration step size. This step size cannot be reset
  ///           after construction.
  /// @throws std::exception if @p dt is not positive and system_type is
  ///         kDiscretized or @p dt is not zero and system_type is
  ///         kPiecewiseDAE or kContinuous.
  explicit Rod2D(SystemType system_type, double dt);

  static const Rod2dStateVector<T>& get_state(
      const systems::ContinuousState<T>& cstate) {
    return dynamic_cast<const Rod2dStateVector<T>&>(cstate.get_vector());
  }

  static Rod2dStateVector<T>& get_mutable_state(
      systems::ContinuousState<T>* cstate) {
    return dynamic_cast<Rod2dStateVector<T>&>(cstate->get_mutable_vector());
  }

  static const Rod2dStateVector<T>& get_state(
      const systems::Context<T>& context) {
    return dynamic_cast<const Rod2dStateVector<T>&>(
        context.get_continuous_state_vector());
  }

  static Rod2dStateVector<T>& get_mutable_state(
      systems::Context<T>* context) {
    return dynamic_cast<Rod2dStateVector<T>&>(
        context->get_mutable_continuous_state_vector());
  }

  /// Transforms dissipation (α) to damping, given a characteristic
  // deformation.
  double TransformDissipationToDampingAboutDeformation(
      double characteristic_deformation) const {
    // Equation (16) from [Hunt 1975], yields b = 3/2 * α * k * x. We can
    // assume that the stiffness and dissipation are determined for a small
    // deformation (x). Put another way, we determine the damping coefficient
    // for a harmonic oscillator from linearizing the dissipation factor about
    // the characteristic deformation: the system will behave like a harmonic
    // oscillator oscillating about x = `characteristic_deformation` meters.
    return dissipation_ * 1.5 * stiffness_ * characteristic_deformation *
        half_length_;
  }

  /// Transforms damping (b) to dissipation (α) , given a characteristic
  /// deformation.
  double TransformDampingToDissipationAboutDeformation(
      double characteristic_deformation, double b) const {
    // See documentation for TransformDissipationToDampingAboutDeformation()
    // for explanation of this formula.
    return b / (1.5 * stiffness_ * characteristic_deformation *
        half_length_);
  }

  /// Gets the constraint force mixing parameter (CFM, used for discretized
  /// systems only), which should lie in the interval [0, infinity].
  double get_cfm() const {
    return 1.0 /
        (stiffness_ * dt_ + TransformDissipationToDampingAboutDeformation(
        kCharacteristicDeformation));
  }

  /// Gets the error reduction parameter (ERP, used for discretized
  /// systems only), which should lie in the interval [0, 1].
  double get_erp() const {
    return dt_ * stiffness_ / (stiffness_ * dt_ +
        TransformDissipationToDampingAboutDeformation(
        kCharacteristicDeformation));
  }

  /// Gets the generalized position of the rod, given a Context. The first two
  /// components represent the location of the rod's center-of-mass, expressed
  /// in the global frame. The third component represents the orientation of
  /// the rod, measured counter-clockwise with respect to the x-axis.
  Vector3<T> GetRodConfig(const systems::Context<T>& context) const {
    return context.get_state().
        get_continuous_state().get_generalized_position().CopyToVector();
  }

  /// Gets the generalized velocity of the rod, given a Context. The first
  /// two components represent the translational velocities of the
  /// center-of-mass. The third component represents the angular velocity of
  /// the rod.
  Vector3<T> GetRodVelocity(const systems::Context<T>& context) const {
    return context.get_state().
        get_continuous_state().get_generalized_velocity().CopyToVector();
  }

  /// Gets the acceleration (with respect to the positive y-axis) due to
  /// gravity (i.e., this number should generally be negative).
  double get_gravitational_acceleration() const { return g_; }

  /// Sets the acceleration (with respect to the positive y-axis) due to
  /// gravity (i.e., this number should generally be negative).
  void set_gravitational_acceleration(double g) { g_ = g; }

  /// Gets the coefficient of dynamic (sliding) Coulomb friction.
  double get_mu_coulomb() const { return mu_; }

  /// Sets the coefficient of dynamic (sliding) Coulomb friction.
  void set_mu_coulomb(double mu) { mu_ = mu; }

  /// Gets the mass of the rod.
  double get_rod_mass() const { return mass_; }

  /// Sets the mass of the rod.
  void set_rod_mass(double mass) { mass_ = mass; }

  /// Gets the half-length h of the rod.
  double get_rod_half_length() const { return half_length_; }

  /// Sets the half-length h of the rod.
  void set_rod_half_length(double half_length) { half_length_ = half_length; }

  /// Gets the rod moment of inertia.
  double get_rod_moment_of_inertia() const { return J_; }

  /// Sets the rod moment of inertia.
  void set_rod_moment_of_inertia(double J) { J_ = J; }

  /// Get compliant contact normal stiffness in N/m.
  double get_stiffness() const { return stiffness_; }

  /// Set compliant contact normal stiffness in N/m (>= 0).
  void set_stiffness(double stiffness) {
    DRAKE_DEMAND(stiffness >= 0);
    stiffness_ = stiffness;
  }

  /// Get compliant contact normal dissipation in 1/velocity (s/m).
  double get_dissipation() const { return dissipation_; }

  /// Set compliant contact normal dissipation in 1/velocity (s/m, >= 0).
  void set_dissipation(double dissipation) {
    DRAKE_DEMAND(dissipation >= 0);
    dissipation_ = dissipation;
  }

  /// Sets stiffness and dissipation for the rod from cfm and erp values (used
  /// for discretized system implementations).
  void SetStiffnessAndDissipation(double cfm, double erp) {
    // These values were determined by solving the equations:
    // cfm = 1 / (dt * stiffness + damping)
    // erp = dt * stiffness / (dt * stiffness + damping)
    // for k and b.
    const double k = erp / (cfm * dt_);
    const double b = (1 - erp) / cfm;
    set_stiffness(k);
    set_dissipation(
        TransformDampingToDissipationAboutDeformation(
        kCharacteristicDeformation, b));
  }

  /// Get compliant contact static friction (stiction) coefficient `μ_s`.
  double get_mu_static() const { return mu_s_; }

  /// Set contact stiction coefficient (>= mu_coulomb). This has no
  /// effect if the rod model is discretized.
  void set_mu_static(double mu_static) {
    DRAKE_DEMAND(mu_static >= mu_);
    mu_s_ = mu_static;
  }

  /// Get the stiction speed tolerance (m/s).
  double get_stiction_speed_tolerance() const {return v_stick_tol_;}

  /// Set the stiction speed tolerance (m/s). This is the maximum slip
  /// speed that we are willing to consider as sticking. For a given normal
  /// force N this is the speed at which the friction force will be largest,
  /// at `μ_s*N` where `μ_s` is the static coefficient of friction. This has no
  /// effect if the rod model is not compliant.
  void set_stiction_speed_tolerance(double v_stick_tol) {
    DRAKE_DEMAND(v_stick_tol > 0);
    v_stick_tol_ = v_stick_tol;
  }

  /// Gets the rotation matrix that transforms velocities from a sliding
  /// contact frame to the global frame.
  /// @param xaxis_velocity The velocity of the rod at the point of contact,
  ///        projected along the +x-axis.
  /// @returns a 2x2 orthogonal matrix with first column set to the contact
  ///          normal, which is +y ([0 1]) and second column set to the
  ///          direction of sliding motion, ±x (±[1 0]). Both directions are
  ///          expressed in the global frame.
  /// @note Aborts if @p xaxis_velocity is zero.
  Matrix2<T> GetSlidingContactFrameToWorldTransform(
      const T& xaxis_velocity) const;

  /// Gets the rotation matrix that transforms velocities from a non-sliding
  /// contact frame to the global frame. Note: all such non-sliding frames are
  /// identical for this example.
  /// @returns a 2x2 orthogonal matrix with first column set to the contact
  ///          normal, which is +y ([0 1]) and second column set to the
  ///          contact tangent +x ([1 0]). Both directions are expressed in
  ///          the global frame.
  Matrix2<T> GetNonSlidingContactFrameToWorldTransform() const;

  /// Checks whether the system is in an impacting state, meaning that the
  /// relative velocity along the contact normal between the rod and the
  /// halfspace is such that the rod will begin interpenetrating the halfspace
  /// at any time Δt in the future (i.e., Δt > 0). If the context does not
  /// correspond to a configuration where the rod and halfspace are contacting,
  /// this method returns `false`.
  bool IsImpacting(const systems::Context<T>& context) const;

  /// Gets the integration step size for the discretized system.
  /// @returns 0 if this is a DAE-based system.
  double get_integration_step_size() const { return dt_; }

  /// Gets the model and simulation type for this system.
  SystemType get_system_type() const { return system_type_; }

  /// Return net contact forces as a spatial force F_Ro_W=(fx,fy,τ) where
  /// translational force f_Ro_W=(fx,fy) is applied at the rod origin Ro,
  /// and torque t_R=τ is the moment due to the contact forces actually being
  /// applied elsewhere. The returned spatial force may be the resultant of
  /// multiple active contact points. Only valid for simulation type
  /// kContinuous.
  Vector3<T> CalcCompliantContactForces(
      const systems::Context<T>& context) const;

  /// Gets the number of witness functions for the system active in the system
  /// for a given state (using @p context).
  int DetermineNumWitnessFunctions(const systems::Context<T>& context) const;

  /// Returns the state of this rod.
  const systems::OutputPort<T>& state_output() const {
    return *state_output_port_;
  }

  /// Utility method for determining the World frame location of one of three
  /// points on the rod whose origin is Ro. Let r be the half-length of the rod.
  /// Define point P = Ro+k*r where k = { -1, 0, 1 }. This returns p_WP.
  /// @param x The horizontal location of the rod center of mass (expressed in
  ///        the world frame).
  /// @param y The vertical location of the rod center of mass (expressed in
  ///        the world frame).
  /// @param k The rod endpoint (k=+1 indicates the rod "right" endpoint,
  ///          k=-1 indicates the rod "left" endpoint, and k=0 indicates the
  ///          rod origin; each of these are described in the primary class
  ///          documentation.
  /// @param ctheta cos(theta), where θ is the orientation of the rod (as
  ///        described in the primary class documentation).
  /// @param stheta sin(theta), where θ is the orientation of the rod (as
  ///        described in the class documentation).
  /// @param half_rod_len Half the length of the rod.
  /// @returns p_WP, the designated point on the rod, expressed in the world
  ///          frame.
  static Vector2<T> CalcRodEndpoint(const T& x, const T& y, int k,
                                    const T& ctheta, const T& stheta,
                                    double half_rod_len);

  /// Given a location p_WC of a point C in the World frame, define the point Rc
  /// on the rod that is coincident with C, and report Rc's World frame velocity
  /// v_WRc. We're given p_WRo=(x,y) and V_WRo = (v_WRo,w_WR) =
  /// (xdot,ydot,thetadot).
  /// @param p_WRo The center-of-mass of the rod, expressed in the world frame.
  /// @param v_WRo The translational velocity of the rod, expressed in the
  ///              world frame.
  /// @param w_WR The angular velocity of the rod.
  /// @param p_WC The location of a point on the rod.
  /// @returns The translational velocity of p_WC, expressed in the world frame.
  static Vector2<T> CalcCoincidentRodPointVelocity(
      const Vector2<T>& p_WRo, const Vector2<T>& v_WRo,
      const T& w_WR,  // aka thetadot
      const Vector2<T>& p_WC);

  /// Gets the point(s) of contact for the 2D rod.
  /// @p context The context storing the current configuration and velocity of
  ///            the rod.
  /// @p points Contains the contact points (those rod endpoints touching or
  ///           lying within the ground halfspace) on return. This function
  ///           aborts if @p points is null or @p points is non-empty.
  void GetContactPoints(const systems::Context<T>& context,
                        std::vector<Vector2<T>>* points) const;

  /// Gets the tangent velocities for all contact points.
  /// @p context The context storing the current configuration and velocity of
  ///            the rod.
  /// @p points The set of context points.
  /// @p vels Contains the velocities (measured along the x-axis) on return.
  ///         This function aborts if @p vels is null. @p vels will be resized
  ///         appropriately (to the same number of elements as @p points) on
  ///         return.
  void GetContactPointsTangentVelocities(
      const systems::Context<T>& context,
      const std::vector<Vector2<T>>& points, std::vector<T>* vels) const;

  /// Initializes the contact data for the rod, given a set of contact points.
  /// Aborts if data is null or if `points.size() != tangent_vels.size()`.
  /// @param points a vector of contact points, expressed in the world frame.
  /// @param tangent_vels a vector of tangent velocities at the contact points,
  ///        measured along the positive x-axis.
  /// @param[out] data the rigid contact problem data.
  void CalcConstraintProblemData(const systems::Context<T>& context,
                                   const std::vector<Vector2<T>>& points,
                                   const std::vector<T>& tangent_vels,
    multibody::constraint::ConstraintAccelProblemData<T>* data) const;

  /// Initializes the impacting contact data for the rod, given a set of contact
  /// points. Aborts if data is null.
  /// @param points a vector of contact points, expressed in the world frame.
  /// @param[out] data the rigid impact problem data.
  void CalcImpactProblemData(
      const systems::Context<T>& context,
      const std::vector<Vector2<T>>& points,
      multibody::constraint::ConstraintVelProblemData<T>* data) const;

 private:
  friend class Rod2DDAETest;
  friend class Rod2DDAETest_RigidContactProblemDataBallistic_Test;

  Vector3<T> GetJacobianRow(const systems::Context<T>& context,
                            const Vector2<T>& p,
                            const Vector2<T>& dir) const;
  Vector3<T> GetJacobianDotRow(const systems::Context<T>& context,
                               const Vector2<T>& p,
                               const Vector2<T>& dir) const;
  static Matrix2<T> GetRotationMatrixDerivative(T theta, T thetadot);
  Matrix3<T> GetInertiaMatrix() const;
  T GetSlidingVelocityTolerance() const;
  MatrixX<T> solve_inertia(const MatrixX<T>& B) const;
  int get_k(const systems::Context<T>& context) const;
  void DoCalcTimeDerivatives(const systems::Context<T>& context,
                             systems::ContinuousState<T>* derivatives)
                               const override;
  void DoCalcDiscreteVariableUpdates(
      const systems::Context<T>& context,
      const std::vector<const systems::DiscreteUpdateEvent<T>*>& events,
      systems::DiscreteValues<T>* discrete_state) const override;
  void SetDefaultState(const systems::Context<T>& context,
                       systems::State<T>* state) const override;

  Vector3<T> ComputeExternalForces(const systems::Context<T>& context) const;
  Matrix3<T> GetInverseInertiaMatrix() const;
  void CalcTwoContactNoSlidingForces(const systems::Context<T>& context,
                                    Vector2<T>* fN, Vector2<T>* fF) const;
  void CalcTwoContactSlidingForces(const systems::Context<T>& context,
                                    Vector2<T>* fN, Vector2<T>* fF) const;
  Vector2<T> CalcStickingImpactImpulse(const systems::Context<T>& context)
    const;
  Vector2<T> CalcFConeImpactImpulse(const systems::Context<T>& context) const;
  void CalcAccelerationsCompliantContactAndBallistic(
                                  const systems::Context<T>& context,
                                  systems::ContinuousState<T>* derivatives)
                                    const;

  // 2D cross product returns a scalar. This is the z component of the 3D
  // cross product [ax ay 0] × [bx by 0]; the x,y components are zero.
  static T cross2(const Vector2<T>& a, const Vector2<T>& b) {
    return a[0]*b[1] - b[0]*a[1];
  }

  // Calculates the cross product [0 0 ω] × [rx ry 0] and returns the first
  // two elements of the result (the z component is zero).
  static Vector2<T> w_cross_r(const T& w, const Vector2<T>& r) {
    return w * Vector2<T>(-r[1], r[0]);
  }

  // Quintic step function approximation used by Stribeck friction model.
  static T step5(const T& x);

  // Friction model used in the continuous model.
  static T CalcMuStribeck(const T& us, const T& ud, const T& v);

  // The constraint solver.
  multibody::constraint::ConstraintSolver<T> solver_;

  // Solves linear complementarity problems for the discretized system.
  solvers::MobyLCPSolver<T> lcp_;

  // The system type, unable to be changed after object construction.
  const SystemType system_type_;

  // TODO(edrumwri,sherm1) Document these defaults once they stabilize.

  double dt_{0.};           // Step-size for the discretization approach.
  double mass_{1.};         // The mass of the rod (kg).
  double half_length_{1.};  // The length of the rod (m).
  double mu_{1000.};        // The (dynamic) coefficient of friction.
  double g_{-9.81};         // The acceleration due to gravity (in y direction).
  double J_{1.};            // The moment of the inertia of the rod.

  // Compliant contact parameters.
  double stiffness_{10000};   // Normal stiffness of the ground plane (N/m).
  double dissipation_{1};     // Dissipation factor in 1/velocity (s/m).
  double mu_s_{mu_};          // Static coefficient of friction (>= mu).
  double v_stick_tol_{1e-5};  // Slip speed below which the compliant model
                              //   considers the rod to be in stiction.

  // Characteristic deformation is 1mm for a 1m (unit length) rod half-length.
  double kCharacteristicDeformation{1e-3};

  // Output ports.
  const systems::OutputPort<T>* state_output_port_{nullptr};
};

}  // namespace rod2d
}  // namespace examples
}  // namespace drake