bicycle_car.h

#pragma once

#include <memory>

#include <Eigen/Geometry>

#include "drake/automotive/gen/bicycle_car_parameters.h"
#include "drake/automotive/gen/bicycle_car_state.h"
#include "drake/common/drake_copyable.h"
#include "drake/systems/framework/leaf_system.h"

namespace drake {
namespace automotive {

/// BicycleCar implements a nonlinear rigid body bicycle model from Althoff &
/// Dolan (2014) [1].  The three-DOF model captures the rigid-body dynamics in
/// the lateral, longitudinal, and yaw directions but not in the roll and pitch
/// directions.  The model assumes a vehicle that has two wheels: one at the
/// front and one at the rear.  It has been demonstrated (e.g. [2]) that the
/// representation reasonably approximates the dynamics of a four-wheeled
/// vehicle; hence the model is useful as a simplified abstraction of car
/// dynamics.
///
/// The states of the model are:
///
/// - yaw angle Ψ [rad]
/// - yaw rate Ψ_dot [rad/s]
/// - slip angle at the center of mass β [rad]
/// - velocity magnitude (vector magnitude at the slip angle) vel [m/s]
/// - x-position of the center of mass sx [m]
/// - y-position of the center of mass sy [m]
///
/// @note "slip angle" (β) is the angle made between the body and the velocity
/// vector.  Thus, the velocity vector can be resolved into the body-relative
/// componenets `vx_body = cos(β)` and `vy_body = sin(β)`.  `β = 0` means the
/// velocity vector is pointing along the bicycle's longitudinal axis.
///
/// Inputs:
///
/// - Angle of the front wheel of the bicycle δ [rad]
///   (InputPort getter: get_steering_input_port())
/// - Force acting on the rigid body F_in [N]
///   (InputPort getter: get_force_input_port())
///
/// Output:
///
/// - A BicycleCarState containing the 6-dimensional state vector of the
///   bicycle.
///   (OutputPort getter: get_state_output_port())
///
/// Instantiated templates for the following kinds of T's are provided:
///
/// - double
/// - drake::AutoDiffXd
/// - drake::symbolic::Expression
///
/// They are already available to link against in libdrakeAutomotive.
///
/// [1] M. Althoff and J.M. Dolan, Online verification of automated road
///     vehicles using reachability analysis, IEEE Transactions on Robotics,
///     30(4), 2014, pp. 903-908.  DOI: 10.1109/TRO.2014.2312453.
///
/// [2] M. Althoff and J. M. Dolan, Reachability computation of low-order
///     models for the safety verification of high-order road vehicle models,
///     in Proc. of the American Control Conference, 2012, pp. 3559–3566.
///
/// @ingroup automotive_plants
template <typename T>
class BicycleCar final : public systems::LeafSystem<T> {
 public:
  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BicycleCar)

  /// Default constructor.
  BicycleCar();

  /// Scalar-converting copy constructor.  See @ref system_scalar_conversion.
  template <typename U>
  explicit BicycleCar(const BicycleCar<U>&);

  ~BicycleCar() override;

  /// Returns the input port that contains the steering angle.
  const systems::InputPort<T>& get_steering_input_port() const;

  /// Returns the input port that contains the applied powertrain force.
  const systems::InputPort<T>& get_force_input_port() const;

  /// Returns the output port that contains the bicycle states.
  const systems::OutputPort<T>& get_state_output_port() const;

 private:
  void CopyOutState(const systems::Context<T>& context,
                    BicycleCarState<T>* output) const;

  void DoCalcTimeDerivatives(
      const systems::Context<T>& context,
      systems::ContinuousState<T>* derivatives) const override;

  void ImplCalcTimeDerivatives(const BicycleCarParameters<T>& params,
                               const BicycleCarState<T>& state,
                               const systems::BasicVector<T>& steering,
                               const systems::BasicVector<T>& force,
                               BicycleCarState<T>* derivatives) const;

  int steering_input_port_{};
  int force_input_port_{};
  int state_output_port_{};
};

}  // namespace automotive
}  // namespace drake