system3.h

Go to the documentation of this file.

#pragma once

#include <functional>
#include <string>

#include "drake/common/drake_assert.h"
#include "drake/drakeSystemFramework_export.h"
#include "drake/systems/framework/context3.h"
#include "drake/systems/framework/system3_input.h"
#include "drake/systems/framework/system3_output.h"
#include "drake/systems/framework/vector_interface.h"

namespace drake {
namespace systems {

class AbstractContext3;
template <typename T>
class Context3;

// TODO(sherm1) These should probably be local classes so API users don't
// have to look at them.

struct InputPortFinder {
  InputPortFinder(int subsystem_num, int port_num, InputPort3* port)
      : subsystem_num(subsystem_num), port_num(port_num), port(port) {}
  int subsystem_num{-1};  
  int port_num{-1};       
  InputPort3* port{};      
};

struct OutputPortFinder {
  OutputPortFinder(int subsystem_num, int port_num, OutputPort3* port)
      : subsystem_num(subsystem_num), port_num(port_num), port(port) {}
  int subsystem_num{-1};  
  int port_num{-1};       
  OutputPort3* port{};     
};

class AbstractSystem3 {
 public:
  virtual ~AbstractSystem3() {}

  // TODO(sherm1) Consider requiring this to be unique among sibling subsystems
  // so that we can construct a unique pathname to any subsystem.
  std::string get_name() const {return name_;}

  //----------------------------------------------------------------------------
  int AddInputPort(std::unique_ptr<InputPort3> port) {
    const int my_port_num = (int)input_ports_.size();
    port->set_owner(this, my_port_num);
    input_ports_.emplace_back(-1, my_port_num, port.get());
    owned_input_ports_.push_back(std::move(port));
    return my_port_num;
  }

  int AddOutputPort(std::unique_ptr<OutputPort3> port) {
    const int my_port_num = (int)output_ports_.size();
    port->set_owner(this, my_port_num);
    output_ports_.emplace_back(-1, my_port_num, port.get());
    owned_output_ports_.push_back(std::move(port));
    return my_port_num;
  }

  int get_num_input_ports() const { return (int)input_ports_.size(); }

  const InputPort3& get_input_port(int port_num) const {
    return *get_input_port_finder(port_num).port;
  }

  InputPort3* get_mutable_input_port(int port_num) {
    return get_input_port_finder(port_num).port;
  }

  int get_num_output_ports() const { return (int)output_ports_.size(); }

  const OutputPort3& get_output_port(int port_num) const {
    return *get_output_port_finder(port_num).port;
  }

  OutputPort3* get_mutable_output_port(int port_num) {
    return get_output_port_finder(port_num).port;
  }
  DRAKESYSTEMFRAMEWORK_EXPORT std::unique_ptr<AbstractContext3>
  CreateDefaultContext() const;

  DRAKESYSTEMFRAMEWORK_EXPORT const AbstractValue& EvalOutputPort(
      const AbstractContext3& context, int port_num) const;

  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------
  template <class ConcreteSystem>
  ConcreteSystem* AddSubsystem(std::unique_ptr<ConcreteSystem> subsystem) {
    ConcreteSystem* concrete = subsystem.get();
    subsystem->set_owner(this, get_num_subsystems());
    subsystems_.push_back(std::move(subsystem));
    return concrete;
  }

  int InheritInputPort(AbstractSystem3* child_subsystem, int input_port_num) {
    DRAKE_ABORT_UNLESS(child_subsystem->get_parent_system() == this);
    InputPort3* sub_port =
        child_subsystem->get_mutable_input_port(input_port_num);
    const int my_port_num = (int)input_ports_.size();
    input_ports_.emplace_back(child_subsystem->get_subsystem_num(),
                              input_port_num, sub_port);
    return my_port_num;
  }

  int InheritOutputPort(AbstractSystem3* child_subsystem, int output_port_num) {
    DRAKE_ABORT_UNLESS(child_subsystem->get_parent_system() == this);
    OutputPort3* sub_port =
        child_subsystem->get_mutable_output_port(output_port_num);
    const int my_port_num = (int)output_ports_.size();
    output_ports_.emplace_back(child_subsystem->get_subsystem_num(),
                               output_port_num, sub_port);
    return my_port_num;
  }

  void Connect(AbstractSystem3* source_subsystem, int output_port_num,
               AbstractSystem3* sink_subsystem, int input_port_num) {
    DRAKE_ABORT_UNLESS(source_subsystem->get_parent_system() == this &&
                       sink_subsystem->get_parent_system() == this);
    const OutputPort3& out = source_subsystem->get_output_port(output_port_num);
    InputPort3* in = sink_subsystem->get_mutable_input_port(input_port_num);
    in->ConnectTo(&out);
  }

  int get_num_subsystems() const { return (int)subsystems_.size(); }

  const AbstractSystem3& get_subsystem(int index) const {
    const AbstractSystem3* subsystem = subsystems_[index].get();
    DRAKE_ASSERT(subsystem && subsystem->get_parent_system() == this &&
                 subsystem->get_subsystem_num() == index);
    return *subsystem;
  }

  AbstractSystem3* get_mutable_subsystem(int index) {
    return const_cast<AbstractSystem3*>(&get_subsystem(index));
  }

  const AbstractSystem3* get_parent_system() const { return owner_; }

  AbstractSystem3* get_mutable_parent_system() { return owner_; }

  int get_subsystem_num() const { return subsystem_num_; }

  std::string GetSubsystemPathName() const {
    std::string path_name;
    if (get_parent_system())
      path_name = get_parent_system()->GetSubsystemPathName();
    path_name += ("/" + get_name());
    return path_name;
  }

  const AbstractContext3& find_my_subcontext(
      const AbstractContext3& some_subcontext) const {
    // TODO(sherm1) This should use a prebuilt index rather than searching.
    std::vector<int> path = get_path_from_root_system();
    const AbstractContext3& my_subcontext =
        some_subcontext.get_root_context().find_subcontext(path);
    return my_subcontext;
  }

  AbstractContext3* find_my_mutable_subcontext(
      AbstractContext3* some_subcontext) const {
    return const_cast<AbstractContext3*>(&find_my_subcontext(*some_subcontext));
  }

  const AbstractSystem3& get_root_system() const {
    if (!get_parent_system()) return *this;
    return get_parent_system()->get_root_system();
  }

  AbstractSystem3* get_mutable_root_system() {
    return const_cast<AbstractSystem3*>(&get_root_system());
  }

  const InputPortFinder& get_input_port_finder(int port_num) const {
    DRAKE_ASSERT(0 <= port_num && port_num < (int)input_ports_.size());
    return input_ports_[port_num];
  }

  const OutputPortFinder& get_output_port_finder(int port_num) const {
    DRAKE_ASSERT(0 <= port_num && port_num < (int)output_ports_.size());
    return output_ports_[port_num];
  }

  CacheEntry::Calculator get_output_port_calculator(int port_num) const {
    using namespace std::placeholders;  // for _1, _2, _3...
    return std::bind(&AbstractSystem3::CalcOutputPort, _1, _2, port_num, _3);
  }

  void CalcOutputPort(const AbstractContext3& context, int port_num,
                      AbstractValue* result) const {
    const int n = get_num_output_ports();
    if (!(0 <= port_num && port_num < n))
      throw std::out_of_range("AbstractSystem3::CalcOutputPort(): port " +
                              std::to_string(port_num) +
                              " is out of range. There are " +
                              std::to_string(n) + " output ports.");
    DoCalcOutputPort(context, port_num, result);
  }

 protected:
  explicit AbstractSystem3(const std::string& name) : name_(name) {}

  DRAKESYSTEMFRAMEWORK_EXPORT const AbstractValue& EvalInputPort(
      const AbstractContext3& context, int port_num) const;

  virtual std::unique_ptr<AbstractContext3> DoCreateEmptyContext() const = 0;

  virtual void DoAcquireContextResources(AbstractContext3* context) const {}

  virtual void DoCalcOutputPort(const AbstractContext3& context, int port_num,
                                AbstractValue* value) const = 0;

 private:
  friend class AbstractContext3;

  // Create a Context that has no content but whose tree structure matches
  // that of this system diagram, with the right kind of Context in each node
  // as provided by DoCreateEmptyContext().
  DRAKESYSTEMFRAMEWORK_EXPORT std::unique_ptr<AbstractContext3>
  CreateEmptyContext() const;

  // Given an empty Context with appropriate tree structure, allocate and
  // connect input and output ports in the context to match the wiring of this
  // system diagram.
  DRAKESYSTEMFRAMEWORK_EXPORT void AllocateOutputPorts(
      AbstractContext3* context) const;

  // Given an empty Context with appropriate tree structure, allocate and
  // connect input and output ports in the context to match the wiring of this
  // system diagram.
  DRAKESYSTEMFRAMEWORK_EXPORT void AllocateAndConnectInputPorts(
      AbstractContext3* context) const;

  const AbstractSystem3& find_subsystem(const std::vector<int>& path,
                                       int start = 0) const {
    if (start == (int)path.size()) return *this;
    return get_subsystem(path[start]).find_subsystem(path, start + 1);
  }

  AbstractSystem3* find_mutable_subsystem(const std::vector<int>& path) {
    return const_cast<AbstractSystem3*>(&find_subsystem(path));
  }

  std::vector<int> get_path_from_root_system() const {
    std::vector<int> path;
    if (get_parent_system()) {
      path = get_parent_system()->get_path_from_root_system();
      DRAKE_ASSERT(get_subsystem_num() >= 0);
      path.push_back(get_subsystem_num());
    }
    return path;
  }

  // AbstractSystem3 objects are neither copyable nor moveable.
  AbstractSystem3(const AbstractSystem3& other) = delete;
  AbstractSystem3& operator=(const AbstractSystem3& other) = delete;
  AbstractSystem3(AbstractSystem3&& other) = delete;
  AbstractSystem3& operator=(AbstractSystem3&& other) = delete;

  // Set backpointer to parent system and subsystem number by which we are
  // known there.
  void set_owner(AbstractSystem3* parent, int subsystem_num) {
    owner_ = parent;
    subsystem_num_ = subsystem_num;
  }

  std::string name_;

  // These are the actual ports indexed by port number. For leaf systems these
  // simply point to the corresponding entries in the "owned" lists. For
  // system diagrams they may point to inherited ports owned by an interior
  // subsystem, and the port numbers will be different than the indices of
  // the owned ports. If you need to know the difference, ask the port for
  // the System that owns it.
  std::vector<InputPortFinder> input_ports_;
  std::vector<OutputPortFinder> output_ports_;

  // These ports are owned by this system object.
  std::vector<std::unique_ptr<InputPort3>> owned_input_ports_;
  std::vector<std::unique_ptr<OutputPort3>> owned_output_ports_;

  // These are the immediate children owned by this system, indexed by
  // "subsystem number", in order of AddSubsystem calls.
  std::vector<std::unique_ptr<AbstractSystem3>> subsystems_;

  // If this system is a subsystem in a system diagram, this is set to the
  // owning parent system, with the subsystem number as known to the parent.
  AbstractSystem3* owner_{};
  int subsystem_num_{-1};
};

template <typename T>
class System3 : public AbstractSystem3 {
  // TODO(david-german-tri): Add static_asserts on T.
 public:
  //----------------------------------------------------------------------------
  std::unique_ptr<Context3<T>> CreateDefaultContext() const {
    auto abstract_context = AbstractSystem3::CreateDefaultContext();
    std::unique_ptr<Context3<T>> context(
        dynamic_cast<Context3<T>*>(abstract_context.release()));
    DRAKE_ABORT_UNLESS(context != nullptr);
    return context;
  }

  const VectorInterface<T>& EvalVectorOutputPort(const Context3<T>& context,
                                                 int port_num) const {
    const VectorInterface<T>& vector =
        to_vector_interface<T>(EvalOutputPort(context, port_num));
    return vector;
  }
  //----------------------------------------------------------------------------
  const T& EvalPotentialEnergy(const Context3<T>& context) const {
    // TODO(sherm1) Validate the context at least in Debug.
    return DoEvalPotentialEnergy(context);
  }

  const T& EvalKineticEnergy(const Context3<T>& context) const {
    // TODO(sherm1) Validate the context at least in Debug.
    return DoEvalKineticEnergy(context);
  }
  //----------------------------------------------------------------------------
  enum DerivativeBlock { kQdot = 0, kVdot = 1, kZdot = 2, kQdotDot = 3 };

  Eigen::VectorBlock<const VectorX<T>> EvalTimeDerivatives(
      const Context3<T>& context, DerivativeBlock block) const;

  const T& EvalConservativePower(const Context3<T>& context) const {
    return DoEvalConservativePower(context);
  }

  const T& EvalNonConservativePower(const Context3<T>& context) const {
    return DoEvalNonConservativePower(context);
  }

  void MapVelocityToConfigurationDerivatives(
      const Context3<T>& context,
      const Eigen::Ref<const VectorX<T>>& generalized_velocity,
      Eigen::Ref<VectorX<T>>* configuration_derivatives) const {
    // TODO(sherm1) Validate arguments.
    DoMapVelocityToConfigurationDerivatives(context, generalized_velocity,
                                            configuration_derivatives);
  }

  void MapAccelerationToConfigurationSecondDerivatives(
      const Context3<T>& context,
      const Eigen::Ref<const VectorX<T>>& generalized_acceleration,
      Eigen::Ref<VectorX<T>>* configuration_second_derivatives) const {
    // TODO(sherm1) Validate arguments.
    DoMapAccelerationToConfigurationSecondDerivatives(
        context, generalized_acceleration, configuration_second_derivatives);
  }
  //----------------------------------------------------------------------------
  // TODO(sherm1) Only a subset needs to be updated at any given sampling
  // time. Probably makes more sense to pass a writable Context so the
  // %System can perform the limited update.
  void UpdateDiscreteVariables(Context3<T>& context, int sample_key) const;

  // TODO(sherm1) This is half-baked and need to be thought through better.
  // Sampling doesn't just apply to discrete variables; many internal
  // computations need sampling too.
  std::pair<double,int> GetNextSampleTime(const Context3<T>& context) const;
 protected:
  explicit System3(const std::string& name) : AbstractSystem3(name) {}

  const VectorInterface<T>& EvalVectorInputPort(const Context3<T>& context,
                                                int port_num) const {
    const VectorInterface<T>& vector =
        to_vector_interface<T>(EvalInputPort(context, port_num));
    return vector;
  }

  virtual const T& DoEvalPotentialEnergy(const Context3<T>& context) const {
    static const T zero(0);
    return zero;
  }

  virtual const T& DoEvalKineticEnergy(const Context3<T>& context) const {
    static const T zero(0);
    return zero;
  }

  void DoMapVelocityToConfigurationDerivatives(
      const Context3<T>& context,
      const Eigen::Ref<const VectorX<T>>& generalized_velocity,
      Eigen::Ref<VectorX<T>>* configuration_derivatives) const;

  void DoMapAccelerationToConfigurationSecondDerivatives(
      const Context3<T>& context,
      const Eigen::Ref<const VectorX<T>>& generalized_acceleration,
      Eigen::Ref<VectorX<T>>* configuration_second_derivatives) const;

  // These two power methods should be present only for continuous
  // systems that represent some kind of physical system that can inject or
  // dissipate energy into the simulation.

  virtual const T& DoEvalConservativePower(const Context3<T>& context) const {
    static const T zero(0);
    return zero;
  }

  virtual const T& DoEvalNonConservativePower(const Context3<T>& context) const {
    static const T zero(0);
    return zero;
  }

 private:
  // System3 objects are neither copyable nor moveable.
  System3(const System3<T>& other) = delete;
  System3& operator=(const System3<T>& other) = delete;
  System3(System3<T>&& other) = delete;
  System3& operator=(System3<T>&& other) = delete;
};

}  // namespace systems
}  // namespace drake