state.h

Go to the documentation of this file.

#pragma once

#include <cstdint>
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>

#include "drake/systems/framework/state_subvector.h"
#include "drake/systems/framework/state_vector.h"
#include "drake/systems/framework/vector_interface.h"

namespace drake {
namespace systems {

template <typename T>
class ContinuousState {
 public:
  explicit ContinuousState(std::unique_ptr<StateVector<T>> state) {
    state_ = std::move(state);
    generalized_position_.reset(new StateSubvector<T>(state_.get()));
    generalized_velocity_.reset(new StateSubvector<T>(state_.get()));
    misc_continuous_state_.reset(
        new StateSubvector<T>(state_.get(), 0, state_.size()));
  }

  ContinuousState(std::unique_ptr<StateVector<T>> state, int num_q,
                  int num_v, int num_z) {
    state_ = std::move(state);
    if (state_->size() != num_q + num_v + num_z) {
      throw std::out_of_range(
          "Continuous state of size " + std::to_string(state_->size()) +
          "cannot be partitioned as" + " q " + std::to_string(num_q) + " v " +
          std::to_string(num_v) + " z " + std::to_string(num_z));
    }
    if (num_v > num_q) {
      throw std::logic_error("Number of velocity variables " +
                             std::to_string(num_v) +
                             " must not exceed number of position variables " +
                             std::to_string(num_q));
    }
    generalized_position_.reset(new StateSubvector<T>(state_.get(), 0, num_q));
    generalized_velocity_.reset(
        new StateSubvector<T>(state_.get(), num_q, num_v));
    misc_continuous_state_.reset(
        new StateSubvector<T>(state_.get(), num_q + num_v, num_z));
  }

  // TODO(david-german-tri): Add a suitable constructor for the continuous
  // state of a Diagram, using StateSupervectors.

  const StateVector<T>& get_state() const { return *state_; }

  StateVector<T>* get_mutable_state() { return state_.get(); }

  const StateVector<T>& get_generalized_position() const {
    return *generalized_position_;
  }

  StateVector<T>* get_mutable_generalized_position() {
    return generalized_position_.get();
  }

  const StateVector<T>& get_generalized_velocity() const {
    return *generalized_velocity_;
  }

  StateVector<T>* get_mutable_generalized_velocity() {
    return generalized_velocity_.get();
  }

  const StateVector<T>& get_misc_continuous_state() const {
    return *misc_continuous_state_;
  }

  StateVector<T>* get_mutable_misc_continuous_state() {
    return misc_continuous_state_.get();
  }

 private:
  // The entire state vector.  May or may not own the underlying data.
  std::unique_ptr<StateVector<T>> state_;

  // Generalized coordinates representing System configuration, conventionally
  // denoted `q`. These are second-order state variables.
  // This is a subset of state_ and does not own the underlying data.
  std::unique_ptr<StateVector<T>> generalized_position_;

  // Generalized speeds representing System velocity. Conventionally denoted
  // `v`. These are first-order state variables that the System can linearly
  // map to time derivatives `qdot` of `q` above.
  // This is a subset of state_ and does not own the underlying data.
  std::unique_ptr<StateVector<T>> generalized_velocity_;

  // Additional continuous, first-order state variables not representing
  // multibody system motion.  Conventionally denoted `z`.
  // This is a subset of state_ and does not own the underlying data.
  std::unique_ptr<StateVector<T>> misc_continuous_state_;
};

template <typename T>
struct State {
  std::unique_ptr<ContinuousState<T>> continuous_state;
  // TODO(david-german-tri): Add discrete state variables.
};

}  // namespace systems
}  // namespace drake