context3.h

Go to the documentation of this file.

#pragma once

#include <memory>
#include <vector>

#include "drake/common/drake_assert.h"
#include "drake/systems/framework/cache3.h"
#include "drake/systems/framework/vector_interface.h"

namespace drake {
namespace systems {

class AbstractSystem3;
class InputPort3;
class OutputPort3;

struct InputEntryFinder {
  InputEntryFinder() = default;
  InputEntryFinder(int subcontext_num, int port_num, CacheEntry* entry)
      : subcontext_num(subcontext_num), port_num(port_num), entry(entry) {}
  InputEntryFinder(const InputEntryFinder& source) { *this = source; }
  InputEntryFinder& operator=(const InputEntryFinder& source) {
    subcontext_num = source.subcontext_num;
    port_num = source.port_num;
    entry = nullptr;
    return *this;
  }
  InputEntryFinder(InputEntryFinder&& source) { *this = std::move(source); }
  InputEntryFinder& operator=(InputEntryFinder&& source) {
    subcontext_num = source.subcontext_num;
    port_num = source.port_num;
    entry = source.entry;
    new (&source) InputEntryFinder(); // default re-construct source
    return *this;
  }
  int subcontext_num{-1};  
  int port_num{-1};        
  CacheEntry* entry{};     
};

struct OutputEntryFinder {
  OutputEntryFinder() = default;
  OutputEntryFinder(int subcontext_num, int location, CacheEntry* entry)
      : subcontext_num(subcontext_num), location(location), entry(entry) {
  
  }
  OutputEntryFinder(const OutputEntryFinder& source) { *this = source; }
  OutputEntryFinder& operator=(const OutputEntryFinder& source) {
    subcontext_num = source.subcontext_num;
    location = source.location;
    entry = nullptr;
    return *this;
  }
  OutputEntryFinder(OutputEntryFinder&& source) { *this = std::move(source); }
  OutputEntryFinder& operator=(OutputEntryFinder&& source) {
    subcontext_num = source.subcontext_num;
    location = source.location;
    entry = source.entry;
    new (&source) OutputEntryFinder(); // default re-construct source
    return *this;
  }
  int subcontext_num{-1};  
  int location{-1};        
  CacheEntry* entry{};     
};

class AbstractContext3 {
 public:
  virtual ~AbstractContext3() {};

  std::unique_ptr<AbstractContext3> Clone() const {
    return std::unique_ptr<AbstractContext3>(DoClone());
  }

  void SetNumInputPorts(int num_ports) {
    DRAKE_ABORT_UNLESS(num_ports >= 0);
    ClearInputPorts();
    inputs_.resize(num_ports);
  }

  void SetNumOutputPorts(int num_ports) {
    DRAKE_ABORT_UNLESS(num_ports >= 0);
    ClearOutputPorts();
    outputs_.resize(num_ports);
  }

  DRAKESYSTEMFRAMEWORK_EXPORT
  void SetInputPort(const AbstractSystem3& system, int port_num);

  DRAKESYSTEMFRAMEWORK_EXPORT 
  void SetOutputPort(const AbstractSystem3& system, int port_num);

  int get_num_input_ports() const { return (int)inputs_.size(); }
  int get_num_output_ports() const { return (int)outputs_.size(); }
  int get_num_cache_entries() const { return (int)cache_.size(); }

  const CacheEntry* get_input_entry(int port_num) const {
    return get_input_entry_finder(port_num).entry;
  }

  CacheEntry* get_mutable_input_entry(int port_num) const {
    return get_input_entry_finder(port_num).entry;
  }

  const CacheEntry& get_output_entry(int port_num) const {
    return *get_output_entry_finder(port_num).entry;
  }

  CacheEntry* get_mutable_output_entry(int port_num) const {
    return get_output_entry_finder(port_num).entry;
  }

  int AddSubcontext(std::unique_ptr<AbstractContext3> subcontext) {
    const int subcontext_num = get_num_subcontexts();
    subcontext->set_owner(this, subcontext_num);
    subcontexts_.push_back(std::move(subcontext));
    return subcontext_num;
  }

  int get_num_subcontexts() const { return (int)subcontexts_.size(); }

  const AbstractContext3& get_subcontext(int index) const {
    return *subcontexts_[index];
  }

  AbstractContext3* get_mutable_subcontext(int index) {
    return subcontexts_[index].get();
  }

  const AbstractContext3* get_parent_context() const { return owner_; }

  AbstractContext3* get_mutable_parent_context() { return owner_; }

  int get_subcontext_num() const { return subcontext_num_; }


  const AbstractSystem3& find_my_subsystem(
      const AbstractSystem3& some_subsystem) const;

  AbstractSystem3* find_my_mutable_subsystem(
      AbstractSystem3* some_subsystem) const {
    return const_cast<AbstractSystem3*>(&find_my_subsystem(*some_subsystem));
  }


  const AbstractContext3& get_root_context() const {
    if (!get_parent_context()) return *this;
    return get_parent_context()->get_root_context();
  }

  AbstractContext3* get_mutable_root_context() {
    return const_cast<AbstractContext3*>(&get_root_context());
  }

 protected:
   AbstractContext3() = default;

   AbstractContext3(const AbstractContext3& source)
       : inputs_(source.inputs_),
         outputs_(source.outputs_),
         cache_(source.cache_) {
     // TODO(sherm1) input and output pointers are null now an need fixup.

     // TODO(sherm1) copy subcontexts
     //for (const auto& sub : source.subcontexts_)
     //  subcontexts_.emplace_back(sub->Clone());
   }

   virtual AbstractContext3* DoClone() const = 0;

 private:
  friend class AbstractSystem3;

  const InputEntryFinder& get_input_entry_finder(int port_num) const {
    return inputs_[port_num];
  }

  const OutputEntryFinder& get_output_entry_finder(int port_num) const {
    return outputs_[port_num];
  }

  InputEntryFinder* get_mutable_input_entry_finder(int port_num) {
    return &inputs_[port_num];
  }

  OutputEntryFinder* get_mutable_output_entry_finder(int port_num) {
    return &outputs_[port_num];
  }

  // Removes all the input ports, and deregisters them from the output ports
  // on which they depend.
  // TODO(sherm1) Actually do that.
  void ClearInputPorts() { inputs_.clear(); }

  // Removes all the output ports, and disconnects any input ports that might
  // have been plugged in to them.
  // TODO(sherm1) Actually do that.
  void ClearOutputPorts() { outputs_.clear(); }

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

  AbstractContext3* find_mutable_subcontext(const std::vector<int>& path) {
    return const_cast<AbstractContext3*>(&find_subcontext(path));
  }

  std::vector<int> get_path_from_root_context() const {
    std::vector<int> path;
    if (get_parent_context()) {
      path = get_parent_context()->get_path_from_root_context();
      DRAKE_ASSERT(get_subcontext_num() >= 0);
      path.push_back(get_subcontext_num());
    }
    return path;
  }

  // Set backpointer to parent context and subcontext number by which we are
  // known there.
  void set_owner(AbstractContext3* parent, int subcontext_num) {
    owner_ = parent;
    subcontext_num_ = subcontext_num;
  }

  std::vector<InputEntryFinder> inputs_;
  // TODO(sherm1) Add parameter entries.

  std::vector<OutputEntryFinder> outputs_;


  // The cache, consisting of a pool of cache entries that are individually
  // validated and invalidated, and upon which other calculations may depend.
  mutable std::vector<CacheEntry> cache_;


  // Entries here correspond directly to System subsystems, using the
  // same index. Any of these may be context diagrams recursively.
  std::vector<std::unique_ptr<AbstractContext3>> subcontexts_;

  // If this context is a subcontext in a context diagram, this is set to the
  // owning parent context, with the subcontext number as known to the parent.
  AbstractContext3* owner_{};
  int subcontext_num_{-1};
};

// TODO(sherm1) Add step information.
template <typename T>
struct StepInfo {
  // TODO(sherm1): Consider whether this is sufficiently robust.
  T time_sec{};
};

// TODO(sherm1): Manage cache invalidation.
template <typename T>
class Context3 : public AbstractContext3 {
 public:
  Context3() {}
  ~Context3() override {}

  const T& get_time() const { return get_step_info().time_sec; }

  void set_time(const T& time_sec) {
    get_mutable_step_info()->time_sec = time_sec;
  }

  //const State<T>& get_state() const { return state_; }

  //State<T>* get_mutable_state() { return &state_; }

  std::unique_ptr<Context3<T>> Clone() const {
    return std::unique_ptr<Context3<T>>(DoClone());
  }

  Context3(const Context3& source) : AbstractContext3(source) {

  }

 protected:
  // TODO(sherm1) Fix this.

  Context3<T>* DoClone() const override {
    Context3<T>* context = new Context3<T>();

    // TODO(sherm1)

    //const ContinuousState<T>& xc = *this->get_state().continuous_state;
    //const int num_q = xc.get_generalized_position().size();
    //const int num_v = xc.get_generalized_velocity().size();
    //const int num_z = xc.get_misc_continuous_state().size();
    //const LeafStateVector<T>& xc_vector =
    //    dynamic_cast<const LeafStateVector<T>&>(xc.get_state());
    //context->get_mutable_state()->continuous_state.reset(
    //    new ContinuousState<T>(xc_vector.Clone(), num_q, num_v, num_z));

    //for (const auto& port : this->inputs_) {
    //  context->inputs_.emplace_back(new FreestandingInputPort<T>(
    //      port->get_vector_data()->Clone(), port->get_sample_time_sec()));
    //}

    //*context->get_mutable_step_info() = this->get_step_info();
    //*context->get_mutable_cache() = this->get_cache();
    return context;
  }

 private:
  // Returns a const reference to current time and step information.
  const StepInfo<T>& get_step_info() const { return step_info_; }

  // Provides writable access to time and step information, with the side
  // effect of invaliding any computation that is dependent on them.
  // TODO(david-german-tri) Invalidate all cached time- and step-dependent
  // computations.
  StepInfo<T>* get_mutable_step_info() { return &step_info_; }

  // Context3 objects are neither assignable nor moveable, but they are
  // copy constructible.
  Context3& operator=(const Context3& other) = delete;
  Context3(Context3&& other) = delete;
  Context3& operator=(Context3&& other) = delete;

  // Typed independent quantities: time and state.

  // Current time and step information. Think of this as a "time port".
  // TODO(sherm1) Consider literally making this a port.
  StepInfo<T> step_info_;

  // The internal state of the System.
  //State<T> state_;

  // Typed dependent quantities: state derivatives, discrete state updates.
  // TODO(sherm1) These should be VectorCacheEntry<T>.

  // Continuous state time derivative values (numerical vectors). Together
  // these are xc_dot.
  mutable CacheEntry qdot_, udot_, zdot_;

  // Second time-derivative of the configuration states q (numerical vector).
  mutable CacheEntry qdotdot_;

  // Discrete state updates (numerical vector). This is xd_hat.
  mutable CacheEntry updates_;
};

}  // namespace systems
}  // namespace drake