Drake
context.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <utility>
5 
13 
14 namespace drake {
15 namespace systems {
16 
17 /// Contains information about the independent variable including time and
18 /// step number.
19 // TODO(sherm1) Add step information.
20 template <typename T>
21 struct StepInfo {
22  /// The time, in seconds. For typical T implementations based on
23  /// doubles, time resolution will gradually degrade as time increases.
24  // TODO(sherm1): Consider whether this is sufficiently robust.
25  T time_sec{0.0};
26 };
27 
28 /// %Context is an abstract class template that represents all the typed values
29 /// that are used in a System's computations: time, numeric-valued input ports,
30 /// numerical state, and numerical parameters. There are also type-erased
31 /// abstract state variables, abstract-valued input ports, abstract parameters,
32 /// and a double accuracy setting. The framework provides two concrete
33 /// subclasses of %Context: LeafContext (for leaf Systems) and DiagramContext
34 /// (for composite System Diagrams). Users are forbidden to extend
35 /// DiagramContext and are discouraged from subclassing LeafContext.
36 ///
37 /// @tparam T The mathematical type of the context, which must be a valid Eigen
38 /// scalar.
39 template <typename T>
40 class Context : public ContextBase {
41  public:
42  /// @name Does not allow copy, move, or assignment.
43  //@{
44  // Copy constructor is protected for use in implementing Clone().
45  Context(Context&&) = delete;
46  Context& operator=(const Context&) = delete;
47  Context& operator=(Context&&) = delete;
48  //@}
49 
50  /// @name Accessors for locally-stored values
51  /// Methods in this group provide `const` access to values stored locally in
52  /// this %Context. The available values are:
53  /// - time
54  /// - state
55  /// - parameters
56  /// - accuracy
57  ///
58  /// Fixed input port values and cached values (including output port values)
59  /// are also stored in the %Context but are accessed indirectly via methods
60  /// like SystemBase::EvalInputValue(), through CacheEntry and OutputPort
61  /// objects, or via the FixedInputPortValue object that was returned when
62  /// an input port value was set.
63  /// @see FixInputPort()
64  //@{
65 
66  /// Returns the current time in seconds.
67  const T& get_time() const { return get_step_info().time_sec; }
68 
69  /// Returns a const reference to the whole State.
70  const State<T>& get_state() const {
71  return do_access_state();
72  }
73 
74  /// Returns true if the Context has no state.
75  bool is_stateless() const {
76  const int nxc = get_continuous_state().size();
77  const int nxd = get_num_discrete_state_groups();
78  const int nxa = get_num_abstract_states();
79  return nxc == 0 && nxd == 0 && nxa == 0;
80  }
81 
82  /// Returns true if the Context has continuous state, but no discrete or
83  /// abstract state.
85  const int nxc = get_continuous_state().size();
86  const int nxd = get_num_discrete_state_groups();
87  const int nxa = get_num_abstract_states();
88  return nxc > 0 && nxd == 0 && nxa == 0;
89  }
90 
91  /// Returns true if the Context has discrete state, but no continuous or
92  /// abstract state.
93  bool has_only_discrete_state() const {
94  const int nxc = get_continuous_state().size();
95  const int nxd = get_num_discrete_state_groups();
96  const int nxa = get_num_abstract_states();
97  return nxd > 0 && nxc == 0 && nxa == 0;
98  }
99 
100  /// Returns the total dimension of all of the basic vector states (as if they
101  /// were muxed).
102  /// @throws std::runtime_error if the system contains any abstract state.
103  int get_num_total_states() const {
104  DRAKE_THROW_UNLESS(get_num_abstract_states() == 0);
105  int count = get_continuous_state().size();
106  for (int i = 0; i < get_num_discrete_state_groups(); i++)
107  count += get_discrete_state(i).size();
108  return count;
109  }
110 
111  /// Returns a const reference to the continuous component of the state,
112  /// which may be of size zero.
114  return get_state().get_continuous_state();
115  }
116 
117  /// Returns a reference to the continuous state vector, devoid of second-order
118  /// structure. The vector may be of size zero.
120  return get_continuous_state().get_vector();
121  }
122 
123  /// Returns the number of vectors (groups) in the discrete state.
125  return get_state().get_discrete_state().num_groups();
126  }
127 
128  /// Returns a reference to the entire discrete state, which may consist of
129  /// multiple discrete state vectors (groups).
131  return get_state().get_discrete_state();
132  }
133 
134  /// Returns a reference to the _only_ discrete state vector. The vector may be
135  /// of size zero.
136  /// @pre There is only one discrete state group.
138  return get_discrete_state().get_vector();
139  }
140 
141  /// Returns a const reference to group (vector) @p index of the discrete
142  /// state.
143  /// @pre @p index must identify an existing group.
144  const BasicVector<T>& get_discrete_state(int index) const {
145  const DiscreteValues<T>& xd = get_state().get_discrete_state();
146  return xd.get_vector(index);
147  }
148 
149  /// Returns the number of elements in the abstract state.
151  return get_state().get_abstract_state().size();
152  }
153 
154  /// Returns a const reference to the abstract component of the state, which
155  /// may be of size zero.
157  return get_state().get_abstract_state();
158  }
159 
160  /// Returns a const reference to the abstract component of the
161  /// state at @p index.
162  /// @pre @p index must identify an existing element.
163  /// @pre the abstract state's type must match the template argument.
164  template <typename U>
165  const U& get_abstract_state(int index) const {
166  const AbstractValues& xa = get_state().get_abstract_state();
167  return xa.get_value(index).GetValue<U>();
168  }
169 
170  /// Returns the accuracy setting (if any). Note that the return type is
171  /// `optional<double>` rather than the double value itself.
172  /// @see set_accuracy() for details.
173  const optional<double>& get_accuracy() const { return accuracy_; }
174 
175  /// Returns a const reference to this %Context's parameters.
176  const Parameters<T>& get_parameters() const { return *parameters_; }
177 
178  /// Returns the number of vector-valued parameters.
180  return parameters_->num_numeric_parameters();
181  }
182 
183  /// Returns a const reference to the vector-valued parameter at @p index.
184  /// @pre @p index must identify an existing parameter.
185  const BasicVector<T>& get_numeric_parameter(int index) const {
186  return parameters_->get_numeric_parameter(index);
187  }
188 
189  /// Returns the number of abstract-valued parameters.
191  return get_parameters().num_abstract_parameters();
192  }
193 
194  /// Returns a const reference to the abstract-valued parameter at @p index.
195  /// @pre @p index must identify an existing parameter.
196  const AbstractValue& get_abstract_parameter(int index) const {
197  return get_parameters().get_abstract_parameter(index);
198  }
199  //@}
200 
201  /// @name Methods for changing locally-stored values
202  /// Methods in this group allow changes to the values of quantities stored
203  /// locally in this %Context. The changeable quantities are:
204  /// - time
205  /// - state
206  /// - parameters
207  /// - accuracy
208  /// - fixed input port values
209  ///
210  /// Expensive computations may be performed that depend on the current values
211  /// of some or all of the above quantities. For efficiency, we save the
212  /// results of such computations in the %Context so that we can reuse
213  /// those results without unnecessary recomputation.
214  ///
215  /// <h3>Terminology</h3>
216  /// We call a quantity whose value is needed in order to perform a particular
217  /// computation a _prerequisite_ of that computation, and we say that the
218  /// computation is a _dependent_ of that prerequisite. If a prerequisite's
219  /// value changes, a result computed using an earlier value is _invalid_; we
220  /// say that the prerequisite change _invalidates_ that result, and that the
221  /// result is _out of date_ with respect to its prerequisites. It is important
222  /// to note that the result of one computation can serve as a prerequisite to
223  /// another computation; we call the dependent computation a _downstream_
224  /// computation and the prerequisite an _upstream_ computation.
225  ///
226  /// <h3>Caching</h3>
227  /// Drake provides a caching system that is responsible for
228  /// - storing computed results in the %Context's _cache_, and
229  /// - ensuring that any cached result that _may_ be invalid is marked
230  /// "out of date".
231  ///
232  /// The correctness of results reported by Drake depends critically on _every_
233  /// cached result being correctly flagged as up to date or out of date with
234  /// respect to its prerequisites. Only when it is known _for certain_ that
235  /// a result is valid can it be marked up to date. Access to cached results
236  /// is performed through `Eval()` methods that return up to date results
237  /// immediately but initiate recomputation first for results marked out of
238  /// date. The methods in the group below are responsible for ensuring that
239  /// cached results are marked out of date whenever a prerequisite value may
240  /// have changed. These methods _do not_ initiate such recomputation
241  /// themselves.
242  ///
243  /// <h3>Invalidation and "out of date" notification</h3>
244  /// Each method in this group provides the ability to change a particular
245  /// subset of the available quantities listed above. This triggers "out of
246  /// date" notifications to the cached results for all computations for which
247  /// any element of that subset is a prerequisite. Such notifications
248  /// propagate to downstream computations, whose cached results may reside
249  /// anywhere in the full Diagram context tree of which this %Context is a
250  /// part. That ensures that "out of date" flags are set correctly for the
251  /// cached results of all computations that could be made invalid by a value
252  /// change to any of the affected subset of quantities. We call this
253  /// process a _notification sweep_.
254  ///
255  /// <h3>Which method to use</h3>
256  /// Choose the most-specific method in this group that permits you to perform
257  /// the modifications you need. For example, if you need to modify only
258  /// continuous state variables, don't use a method that provides mutable
259  /// access to the whole state. That provides two performance advantages:
260  /// - fewer downstream computations need to be marked out of date, making the
261  /// notification sweep faster, and
262  /// - fewer results will need to be recomputed later.
263  ///
264  /// The methods below may be grouped into "safe" methods that begin with
265  /// `set` or `Set` and "dangerous" methods that begin with `get_mutable`
266  /// and return a mutable reference. In addition the `FixInputPort` methods
267  /// return an object that has `get_mutable` methods with the same dangers
268  /// (see FixedInputPortValue). Prefer the safe methods when possible.
269  ///
270  /// <h4>Safe "set" methods</h4>
271  /// The `set` and `Set` methods are safe in the sense that they perform both
272  /// the "mark as out of date" notification sweep through dependent cached
273  /// results and the update to the local quantity's value. They do not return
274  /// a reference to the value object. Using these methods ensures that no value
275  /// modification can occur without an appropriate notification sweep. Also,
276  /// these methods can be used to perform multiple changes at once (say time
277  /// and state), requiring only a single notification sweep, and _may_ perform
278  /// optimizations to avoid notifications in case some of the new values are
279  /// the same as the old ones.
280  ///
281  /// <h4>Dangerous "get_mutable" methods</h4>
282  /// The `get_mutable` methods return a mutable reference to the local value
283  /// object within this %Context. The notification sweep is done prior to
284  /// returning that reference. You can then use the reference to make the
285  /// desired change. Note that with these methods we do not actually know
286  /// whether dependent computations are invalid; that depends what you do with
287  /// the reference once you have it. Nevertheless you will pay the cost of
288  /// the notifications sweep immediately and the cost of recomputation later
289  /// when you ask for the value. So don't call one of these methods unless
290  /// you are certain you will be writing through the returned reference.
291  ///
292  /// You _must not_ hold on to the returned reference expecting
293  /// to be able to make subsequent changes, because those changes can't be
294  /// seen by the framework and thus will not cause the necessary notification
295  /// sweep to occur. Instead, request the mutable reference again when
296  /// you need it so that the necessary notification sweep can be performed.
297  ///
298  /// <h3>Implementation note</h3>
299  /// Each method in the group below guarantees to mark as out of date any
300  /// dependents of the quantities to which it permits modification, including
301  /// all downstream dependents. However, the current implementations may also
302  /// perform some unnecessary notifications. If so, that is noted in the method
303  /// documentation. You should still use the most-specific available method so
304  /// that you will benefit from later improvements that result in fewer
305  /// notifications.
306  //@{
307 
308  // TODO(sherm1) All these methods perform invalidation sweeps so aren't
309  // entitled to lower_case_names. Deprecate and replace (see #9205).
310 
311  /// Sets the current time in seconds. Sends out of date notifications for all
312  /// time-dependent computations (at least if the time has actually changed).
313  /// Time must have the same value in every subcontext within the same Diagram
314  /// context tree so may only be modified at the root context of the tree.
315  /// @throws std::logic_error if this is not the root context.
316  // TODO(sherm1) Consider whether this should avoid the notification sweep
317  // if the new time is the same as the old time.
318  void set_time(const T& time_sec) {
319  ThrowIfNotRootContext(__func__, "Time");
320  const int64_t change_event = this->start_new_change_event();
321  PropagateTimeChange(this, time_sec, change_event);
322  }
323 
324  /// Returns a mutable reference to the whole State, potentially invalidating
325  /// _all_ state-dependent computations so requiring out of date notifications
326  /// to be made for all such computations. If you don't mean to change the
327  /// whole state, use more focused methods to modify only a portion of the
328  /// state. See class documentation for more information.
330  const int64_t change_event = this->start_new_change_event();
331  PropagateBulkChange(change_event, &Context<T>::NoteAllStateChanged);
332  return do_access_mutable_state();
333  }
334 
335  /// Returns a mutable reference to the continuous component of the state,
336  /// which may be of size zero. Sends out of date notifications for all
337  /// continuous-state-dependent computations.
339  const int64_t change_event = this->start_new_change_event();
340  PropagateBulkChange(change_event,
342  return do_access_mutable_state().get_mutable_continuous_state();
343  }
344 
345  /// Returns a mutable reference to the continuous state vector, devoid
346  /// of second-order structure. The vector may be of size zero. Sends out of
347  /// date notifications for all continuous-state-dependent computations.
349  return get_mutable_continuous_state().get_mutable_vector();
350  }
351 
352  // TODO(sherm1) Add more-specific state "set" methods for smaller
353  // state groupings (issue #9205).
354 
355  /// Sets the continuous state to @p xc, including q, v, and z partitions.
356  /// The supplied vector must be the same size as the existing continuous
357  /// state. Sends out of date notifications for all continuous-state-dependent
358  /// computations.
359  void SetContinuousState(const Eigen::Ref<const VectorX<T>>& xc) {
360  get_mutable_continuous_state().SetFromVector(xc);
361  }
362 
363  /// Sets time to @p time_sec and continuous state to @p xc. Performs a single
364  /// notification sweep to avoid duplicate notifications for computations that
365  /// depend on both time and state.
366  /// @throws std::logic_error if this is not the root context.
367  // TODO(sherm1) Consider whether this should avoid invalidation of
368  // time-dependent quantities if the new time is the same as the old time.
370  const Eigen::Ref<const VectorX<T>>& xc) {
371  ThrowIfNotRootContext(__func__, "Time");
372  const int64_t change_event = this->start_new_change_event();
373  PropagateTimeChange(this, time_sec, change_event);
374  PropagateBulkChange(change_event,
376  do_access_mutable_state().get_mutable_continuous_state().SetFromVector(xc);
377  }
378 
379  /// Returns a mutable reference to the discrete component of the state,
380  /// which may be of size zero. Sends out of date notifications for all
381  /// discrete-state-dependent computations.
383  const int64_t change_event = this->start_new_change_event();
384  PropagateBulkChange(change_event,
386  return do_access_mutable_state().get_mutable_discrete_state();
387  }
388 
389  /// Returns a mutable reference to the _only_ discrete state vector.
390  /// Sends out of date notifications for all discrete-state-dependent
391  /// computations.
392  /// @sa get_discrete_state_vector().
393  /// @pre There is only one discrete state group.
395  return get_mutable_discrete_state().get_mutable_vector();
396  }
397 
398  /// Returns a mutable reference to group (vector) @p index of the discrete
399  /// state. Sends out of date notifications for all computations that depend
400  /// on this discrete state group.
401  /// @pre @p index must identify an existing group.
402  /// @bug Currently notifies dependents of _all_ groups.
403  // TODO(sherm1) Invalidate only dependents of this one discrete group.
405  DiscreteValues<T>& xd = get_mutable_discrete_state();
406  return xd.get_mutable_vector(index);
407  }
408 
409  /// Returns a mutable reference to the abstract component of the state,
410  /// which may be of size zero. Sends out of date notifications for all
411  /// abstract-state-dependent computations.
413  const int64_t change_event = this->start_new_change_event();
414  PropagateBulkChange(change_event,
416  return do_access_mutable_state().get_mutable_abstract_state();
417  }
418 
419  /// Returns a mutable reference to element @p index of the abstract state.
420  /// Sends out of date notifications for all computations that depend on this
421  /// abstract state variable.
422  /// @pre @p index must identify an existing element.
423  /// @pre the abstract state's type must match the template argument.
424  /// @bug Currently notifies dependents of _any_ abstract state variable.
425  // TODO(sherm1) Invalidate only dependents of this one abstract variable.
426  template <typename U>
428  AbstractValues& xa = get_mutable_abstract_state();
429  return xa.get_mutable_value(index).GetMutableValue<U>();
430  }
431 
432  /// Returns a mutable reference to this %Context's parameters. Sends out of
433  /// date notifications for all parameter-dependent computations. If you don't
434  /// mean to change all the parameters, use the indexed methods to modify only
435  /// some of the parameters so that fewer computations are invalidated and
436  /// fewer notifications need be sent.
438  const int64_t change_event = this->start_new_change_event();
439  PropagateBulkChange(change_event, &Context<T>::NoteAllParametersChanged);
440  return *parameters_;
441  }
442 
443  /// Returns a mutable reference to element @p index of the vector-valued
444  /// (numeric) parameters. Sends out of date notifications for all computations
445  /// dependent on this parameter.
446  /// @pre @p index must identify an existing numeric parameter.
447  /// @bug Currently notifies dependents of _all_ numeric parameters.
448  // TODO(sherm1) Invalidate only dependents of this one parameter.
450  const int64_t change_event = this->start_new_change_event();
451  PropagateBulkChange(change_event,
453  return parameters_->get_mutable_numeric_parameter(index);
454  }
455 
456  /// Returns a mutable reference to element @p index of the abstract-valued
457  /// parameters. Sends out of date notifications for all computations dependent
458  /// on this parameter.
459  /// @pre @p index must identify an existing abstract parameter.
460  /// @bug Currently notifies dependents of _all_ abstract parameters.
461  // TODO(sherm1) Invalidate only dependents of this one parameter.
463  const int64_t change_event = this->start_new_change_event();
464  PropagateBulkChange(change_event,
466  return parameters_->get_mutable_abstract_parameter(index);
467  }
468 
469  /// Sets this context's time, accuracy, state, and parameters from the
470  /// `double` values in @p source, regardless of this context's scalar type.
471  /// Sends out of date notifications for all dependent computations in this
472  /// context.
473  /// @throws std::logic_error if this is not the root context.
474  /// @bug Currently does not copy fixed input port values from `source`.
475  /// See System::FixInputPortsFrom() if you want to copy those.
476  // TODO(sherm1) Should treat fixed input port values same as parameters.
477  // TODO(sherm1) Change the name of this method to be more inclusive since it
478  // also copies accuracy (now) and fixed input port values
479  // (pending above TODO).
481  ThrowIfNotRootContext(__func__, "Time");
482  // A single change event for all these changes is faster than doing
483  // each separately.
484  const int64_t change_event = this->start_new_change_event();
485 
486  // These two both set the value and perform notifications.
487  PropagateTimeChange(this, T(source.get_time()), change_event);
488  PropagateAccuracyChange(this, source.get_accuracy(), change_event);
489 
490  // Notification is separate from the actual value change for bulk changes.
491  PropagateBulkChange(change_event, &Context<T>::NoteAllStateChanged);
492  do_access_mutable_state().SetFrom(source.get_state());
493 
494  PropagateBulkChange(change_event, &Context<T>::NoteAllParametersChanged);
495  parameters_->SetFrom(source.get_parameters());
496 
497  // TODO(sherm1) Fixed input copying goes here.
498  }
499 
500  // Allow access to the base class method (takes an AbstractValue).
502 
503  /// Connects the input port at @p index to a FixedInputPortValue with
504  /// the given vector @p vec. Aborts if @p index is out of range.
505  /// Returns a reference to the allocated FixedInputPortValue. The
506  /// reference will remain valid until this input port's value source is
507  /// replaced or the %Context is destroyed. You may use that reference to
508  /// modify the input port's value using the appropriate
509  /// FixedInputPortValue method, which will ensure that invalidation
510  /// notifications are delivered.
513  index, std::make_unique<Value<BasicVector<T>>>(vec.Clone()));
514  }
515 
516  /// Same as above method but starts with an Eigen vector whose contents are
517  /// used to initialize a BasicVector in the FixedInputPortValue.
519  int index, const Eigen::Ref<const VectorX<T>>& data) {
520  return FixInputPort(index, BasicVector<T>(data));
521  }
522 
523  /// Same as the above method that takes a `const BasicVector<T>&`, but here
524  /// the vector is passed by unique_ptr instead of by const reference. The
525  /// caller must not retain any aliases to `vec`; within this method, `vec`
526  /// is cloned and then deleted.
527  /// @note This overload will become deprecated in the future, because it can
528  /// mislead users to believe that they can retain an alias of `vec` to mutate
529  /// the fixed value during a simulation. Callers should prefer to use one of
530  /// the other overloads instead.
532  int index, std::unique_ptr<BasicVector<T>> vec) {
533  DRAKE_THROW_UNLESS(vec.get() != nullptr);
534  return FixInputPort(index, *vec);
535  }
536 
537  /// Records the user's requested accuracy. If no accuracy is requested,
538  /// computations are free to choose suitable defaults, or to refuse to
539  /// proceed without an explicit accuracy setting. Any accuracy-dependent
540  /// computation in this Context and its subcontexts may be invalidated
541  /// by a change to the accuracy setting, so out of date notifications are
542  /// sent to all such computations (at least if the accuracy setting has
543  /// actually changed). Accuracy must have the same value in every subcontext
544  /// within the same context tree so may only be modified at the root context
545  /// of a tree.
546  ///
547  /// @throws std::logic_error if this is not the root context.
548  ///
549  /// Requested accuracy is stored in the %Context for two reasons:
550  /// - It permits all computations performed over a System to see the _same_
551  /// accuracy request since accuracy is stored in one shared place, and
552  /// - it allows us to notify accuracy-dependent cached results that they are
553  /// out of date when the accuracy setting changes.
554  ///
555  /// The accuracy of a complete simulation or other numerical study depends on
556  /// the accuracy of _all_ contributing computations, so it is important that
557  /// each computation is done in accordance with the overall requested
558  /// accuracy. Some examples of where this is needed:
559  /// - Error-controlled numerical integrators use the accuracy setting to
560  /// decide what step sizes to take.
561  /// - The Simulator employs a numerical integrator, but also uses accuracy to
562  /// decide how precisely to isolate witness function zero crossings.
563  /// - Iterative calculations reported as results or cached internally depend
564  /// on accuracy to decide how strictly to converge the results. Examples of
565  /// these are: constraint projection, calculation of distances between
566  /// smooth shapes, and deformation calculations for soft contact.
567  ///
568  /// The common thread among these examples is that they all share the
569  /// same %Context, so by keeping accuracy here it can be used effectively to
570  /// control all accuracy-dependent computations.
571  // TODO(sherm1) Consider whether to avoid invalidation if the new value is
572  // the same as the old one.
573  void set_accuracy(const optional<double>& accuracy) {
574  ThrowIfNotRootContext(__func__, "Accuracy");
575  const int64_t change_event = this->start_new_change_event();
576  PropagateAccuracyChange(this, accuracy, change_event);
577  }
578  //@}
579 
580  /// @name Miscellaneous public methods
581  //@{
582 
583  /// Returns a deep copy of this Context.
584  // This is just an intentional shadowing of the base class method to return
585  // a more convenient type.
586  std::unique_ptr<Context<T>> Clone() const {
587  return dynamic_pointer_cast_or_throw<Context<T>>(ContextBase::Clone());
588  }
589 
590  /// Returns a deep copy of this Context's State.
591  std::unique_ptr<State<T>> CloneState() const {
592  return DoCloneState();
593  }
594  //@}
595 
596  protected:
597  Context() = default;
598 
599  /// Copy constructor takes care of base class and `Context<T>` data members.
600  /// Derived classes must implement copy constructors that delegate to this
601  /// one for use in their DoCloneWithoutPointers() implementations.
602  // Default implementation invokes the base class copy constructor and then
603  // the local member copy constructors.
604  Context(const Context<T>&) = default;
605 
606  // Structuring these methods as statics permits a DiagramContext to invoke
607  // the protected functionality on its children.
608 
609  /// (Internal use only) Sets a new time and notifies time-dependent
610  /// quantities that they are now invalid, as part of a given change event.
611  static void PropagateTimeChange(Context<T>* context, const T& time_sec,
612  int64_t change_event) {
613  DRAKE_ASSERT(context != nullptr);
614  context->NoteTimeChanged(change_event);
615  context->step_info_.time_sec = time_sec;
616  context->DoPropagateTimeChange(time_sec, change_event);
617  }
618 
619  /// (Internal use only) Sets a new accuracy and notifies accuracy-dependent
620  /// quantities that they are now invalid, as part of a given change event.
621  static void PropagateAccuracyChange(Context<T>* context,
622  const optional<double>& accuracy,
623  int64_t change_event) {
624  DRAKE_ASSERT(context != nullptr);
625  context->NoteAccuracyChanged(change_event);
626  context->accuracy_ = accuracy;
627  context->DoPropagateAccuracyChange(accuracy, change_event);
628  }
629 
630  /// (Internal use only) Returns a reference to mutable parameters _without_
631  /// invalidation notifications. Use get_mutable_parameters() instead for
632  /// normal access.
634  DRAKE_ASSERT(context);
635  return *context->parameters_;
636  }
637 
638  /// (Internal use only) Returns a reference to a mutable state _without_
639  /// invalidation notifications. Use get_mutable_state() instead for normal
640  /// access.
642  DRAKE_ASSERT(context);
643  return context->do_access_mutable_state();
644  }
645 
646  /// (Internal use only) Clones a context but without any of its internal
647  /// pointers.
648  // This is just an intentional shadowing of the base class method to return a
649  // more convenient type.
650  static std::unique_ptr<Context<T>> CloneWithoutPointers(
651  const Context<T>& source) {
652  return dynamic_pointer_cast_or_throw<Context<T>>(
654  }
655 
656  /// Returns a const reference to its concrete State object.
657  virtual const State<T>& do_access_state() const = 0;
658 
659  /// Returns a mutable reference to its concrete State object _without_ any
660  /// invalidation. We promise not to allow user access to this object without
661  /// invalidation.
662  virtual State<T>& do_access_mutable_state() = 0;
663 
664  /// Returns the appropriate concrete State object to be returned by
665  /// CloneState().
666  virtual std::unique_ptr<State<T>> DoCloneState() const = 0;
667 
668  /// Invokes PropagateTimeChange() on all subcontexts of this Context. The
669  /// default implementation does nothing, which is suitable for leaf contexts.
670  /// Diagram contexts must override.
671  virtual void DoPropagateTimeChange(const T& time_sec, int64_t change_event) {
672  unused(time_sec, change_event);
673  }
674 
675  /// Invokes PropagateAccuracyChange() on all subcontexts of this Context. The
676  /// default implementation does nothing, which is suitable for leaf contexts.
677  /// Diagram contexts must override.
678  virtual void DoPropagateAccuracyChange(const optional<double>& accuracy,
679  int64_t change_event) {
680  unused(accuracy, change_event);
681  }
682 
683  /// Returns a const reference to current time and step information.
684  const StepInfo<T>& get_step_info() const { return step_info_; }
685 
686  /// (Internal use only) Sets the continuous state to @p xc, deleting whatever
687  /// was there before.
688  /// @warning Does _not_ invalidate state-dependent computations.
689  void init_continuous_state(std::unique_ptr<ContinuousState<T>> xc) {
690  do_access_mutable_state().set_continuous_state(std::move(xc));
691  }
692 
693  /// (Internal use only) Sets the discrete state to @p xd, deleting whatever
694  /// was there before.
695  /// @warning Does _not_ invalidate state-dependent computations.
696  void init_discrete_state(std::unique_ptr<DiscreteValues<T>> xd) {
697  do_access_mutable_state().set_discrete_state(std::move(xd));
698  }
699 
700  /// (Internal use only) Sets the abstract state to @p xa, deleting whatever
701  /// was there before.
702  /// @warning Does _not_ invalidate state-dependent computations.
703  void init_abstract_state(std::unique_ptr<AbstractValues> xa) {
704  do_access_mutable_state().set_abstract_state(std::move(xa));
705  }
706 
707  /// (Internal use only) Sets the parameters to @p params, deleting whatever
708  /// was there before. You must supply a Parameters object; null is not
709  /// acceptable.
710  /// @warning Does _not_ invalidate parameter-dependent computations.
711  void init_parameters(std::unique_ptr<Parameters<T>> params) {
712  DRAKE_DEMAND(params != nullptr);
713  parameters_ = std::move(params);
714  }
715 
716  private:
717  // Call with arguments like (__func__, "Time"), capitalized as shown.
718  void ThrowIfNotRootContext(const char* func_name,
719  const char* quantity) const {
720  if (!is_root_context()) {
721  throw std::logic_error(
722  fmt::format("{}(): {} change allowed only in the root Context.",
723  func_name, quantity));
724  }
725  }
726 
727  // Current time and step information.
728  StepInfo<T> step_info_;
729 
730  // Accuracy setting.
731  optional<double> accuracy_;
732 
733  // The parameter values (p) for this Context; this is never null.
735  std::make_unique<Parameters<T>>()};
736 };
737 
738 } // namespace systems
739 } // namespace drake
void init_parameters(std::unique_ptr< Parameters< T >> params)
(Internal use only) Sets the parameters to params, deleting whatever was there before.
Definition: context.h:711
void init_discrete_state(std::unique_ptr< DiscreteValues< T >> xd)
(Internal use only) Sets the discrete state to xd, deleting whatever was there before.
Definition: context.h:696
AbstractValues is a container for non-numerical state and parameters.
Definition: abstract_values.h:18
const AbstractValue & get_abstract_parameter(int index) const
Returns a const reference to the abstract-valued parameter at index.
Definition: context.h:196
DiscreteValues< T > & get_mutable_discrete_state()
Returns a mutable reference to the discrete component of the state, which may be of size zero...
Definition: context.h:382
DiscreteValues is a container for numerical but non-continuous state and parameters.
Definition: discrete_values.h:33
const State< T > & get_state() const
Returns a const reference to the whole State.
Definition: context.h:70
const AbstractValues & get_abstract_state() const
Returns a const reference to the abstract component of the state, which may be of size zero...
Definition: context.h:156
std::unique_ptr< Context< T > > Clone() const
Returns a deep copy of this Context.
Definition: context.h:586
FixedInputPortValue & FixInputPort(int index, std::unique_ptr< BasicVector< T >> vec)
Same as the above method that takes a const BasicVector<T>&, but here the vector is passed by unique_...
Definition: context.h:531
int i
Definition: reset_after_move_test.cc:51
const BasicVector< T > & get_numeric_parameter(int index) const
Returns a const reference to the vector-valued parameter at index.
Definition: context.h:185
bool has_only_discrete_state() const
Returns true if the Context has discrete state, but no continuous or abstract state.
Definition: context.h:93
Provides a convenient wrapper to throw an exception when a condition is unmet.
Definition: automotive_demo.cc:105
const BasicVector< T > & get_discrete_state(int index) const
Returns a const reference to group (vector) index of the discrete state.
Definition: context.h:144
void set_time(const T &time_sec)
Sets the current time in seconds.
Definition: context.h:318
int num_abstract_parameters() const
Returns the number of abstract-valued parameters.
Definition: context.h:190
const VectorBase< T > & get_continuous_state_vector() const
Returns a reference to the continuous state vector, devoid of second-order structure.
Definition: context.h:119
Context is an abstract class template that represents all the typed values that are used in a System&#39;...
Definition: context.h:40
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
static std::unique_ptr< ContextBase > CloneWithoutPointers(const ContextBase &source)
(Internal use only) Clones a context but without copying any of its internal pointers; the clone&#39;s po...
Definition: context_base.h:403
static void PropagateAccuracyChange(Context< T > *context, const optional< double > &accuracy, int64_t change_event)
(Internal use only) Sets a new accuracy and notifies accuracy-dependent quantities that they are now ...
Definition: context.h:621
BasicVector< T > & get_mutable_discrete_state_vector()
Returns a mutable reference to the only discrete state vector.
Definition: context.h:394
VectorBase is an abstract base class that real-valued signals between Systems and real-valued System ...
Definition: vector_base.h:27
void set_accuracy(const optional< double > &accuracy)
Records the user&#39;s requested accuracy.
Definition: context.h:573
const AcrobotParameters parameters_
Definition: multibody_plant_test.cc:428
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
std::unique_ptr< BasicVector< T > > Clone() const
Copies the entire vector to a new BasicVector, with the same concrete implementation type...
Definition: basic_vector.h:130
FixedInputPortValue & FixInputPort(int index, std::unique_ptr< AbstractValue > value)
Connects the input port at index to a FixedInputPortValue with the given abstract value...
Definition: context_base.cc:39
virtual void DoPropagateTimeChange(const T &time_sec, int64_t change_event)
Invokes PropagateTimeChange() on all subcontexts of this Context.
Definition: context.h:671
void NoteTimeChanged(int64_t change_event)
Notifies the local time tracker that time may have changed.
Definition: context_base.h:292
static std::unique_ptr< Context< T > > CloneWithoutPointers(const Context< T > &source)
(Internal use only) Clones a context but without any of its internal pointers.
Definition: context.h:650
stx::optional< T > optional
Definition: drake_optional.h:22
std::unique_ptr< ContextBase > Clone() const
Creates an identical copy of the concrete context object.
Definition: context_base.cc:11
const T & GetValue() const
Returns the value wrapped in this AbstractValue, which must be of exactly type T. ...
Definition: value.h:142
Provides non-templatized Context functionality shared by the templatized derived classes.
Definition: context_base.h:38
const BasicVector< T > & get_discrete_state_vector() const
Returns a reference to the only discrete state vector.
Definition: context.h:137
static void PropagateTimeChange(Context< T > *context, const T &time_sec, int64_t change_event)
(Internal use only) Sets a new time and notifies time-dependent quantities that they are now invalid...
Definition: context.h:611
U & get_mutable_abstract_state(int index)
Returns a mutable reference to element index of the abstract state.
Definition: context.h:427
T time_sec
The time, in seconds.
Definition: context.h:25
BasicVector< T > & get_mutable_vector()
Returns a mutable reference to the BasicVector containing the values for the only group...
Definition: discrete_values.h:112
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
Parameters< T > & get_mutable_parameters()
Returns a mutable reference to this Context&#39;s parameters.
Definition: context.h:437
FixedInputPortValue & FixInputPort(int index, const Eigen::Ref< const VectorX< T >> &data)
Same as above method but starts with an Eigen vector whose contents are used to initialize a BasicVec...
Definition: context.h:518
void SetTimeStateAndParametersFrom(const Context< double > &source)
Sets this context&#39;s time, accuracy, state, and parameters from the double values in source...
Definition: context.h:480
virtual State< T > & do_access_mutable_state()=0
Returns a mutable reference to its concrete State object without any invalidation.
const U & get_abstract_state(int index) const
Returns a const reference to the abstract component of the state at index.
Definition: context.h:165
static State< T > & access_mutable_state(Context< T > *context)
(Internal use only) Returns a reference to a mutable state without invalidation notifications.
Definition: context.h:641
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
int get_num_discrete_state_groups() const
Returns the number of vectors (groups) in the discrete state.
Definition: context.h:124
bool has_only_continuous_state() const
Returns true if the Context has continuous state, but no discrete or abstract state.
Definition: context.h:84
Provides drake::optional as an alias for the appropriate implementation of std::optional or std::expe...
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
int num_numeric_parameters() const
Returns the number of vector-valued parameters.
Definition: context.h:179
const StepInfo< T > & get_step_info() const
Returns a const reference to current time and step information.
Definition: context.h:684
void SetTimeAndContinuousState(const T &time_sec, const Eigen::Ref< const VectorX< T >> &xc)
Sets time to time_sec and continuous state to xc.
Definition: context.h:369
static Parameters< T > & access_mutable_parameters(Context< T > *context)
(Internal use only) Returns a reference to mutable parameters without invalidation notifications...
Definition: context.h:633
const T & get_time() const
Returns the current time in seconds.
Definition: context.h:67
const Parameters< T > & get_parameters() const
Returns a const reference to this Context&#39;s parameters.
Definition: context.h:176
VectorBase< T > & get_mutable_continuous_state_vector()
Returns a mutable reference to the continuous state vector, devoid of second-order structure...
Definition: context.h:348
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
void init_abstract_state(std::unique_ptr< AbstractValues > xa)
(Internal use only) Sets the abstract state to xa, deleting whatever was there before.
Definition: context.h:703
AbstractValue & get_mutable_abstract_parameter(int index)
Returns a mutable reference to element index of the abstract-valued parameters.
Definition: context.h:462
A fully type-erased container class.
Definition: value.h:101
AbstractValues & get_mutable_abstract_state()
Returns a mutable reference to the abstract component of the state, which may be of size zero...
Definition: context.h:412
const AbstractValue & get_value(int index) const
Returns the element of AbstractValues at the given index, or aborts if the index is out-of-bounds...
Definition: abstract_values.cc:33
BasicVector< T > & get_mutable_numeric_parameter(int index)
Returns a mutable reference to element index of the vector-valued (numeric) parameters.
Definition: context.h:449
ContinuousState< T > & get_mutable_continuous_state()
Returns a mutable reference to the continuous component of the state, which may be of size zero...
Definition: context.h:338
Parameters is a container for variables that parameterize a System so that it can represent a family ...
Definition: parameters.h:26
int get_num_total_states() const
Returns the total dimension of all of the basic vector states (as if they were muxed).
Definition: context.h:103
BasicVector< T > & get_mutable_discrete_state(int index)
Returns a mutable reference to group (vector) index of the discrete state.
Definition: context.h:404
virtual void DoPropagateAccuracyChange(const optional< double > &accuracy, int64_t change_event)
Invokes PropagateAccuracyChange() on all subcontexts of this Context.
Definition: context.h:678
const optional< double > & get_accuracy() const
Returns the accuracy setting (if any).
Definition: context.h:173
FixedInputPortValue & FixInputPort(int index, const BasicVector< T > &vec)
Connects the input port at index to a FixedInputPortValue with the given vector vec.
Definition: context.h:511
Contains information about the independent variable including time and step number.
Definition: context.h:21
State< T > & get_mutable_state()
Returns a mutable reference to the whole State, potentially invalidating all state-dependent computat...
Definition: context.h:329
int get_num_abstract_states() const
Returns the number of elements in the abstract state.
Definition: context.h:150
ContinuousState is a view of, and optionally a container for, all the continuous state variables xc o...
Definition: continuous_state.h:76
const DiscreteValues< T > & get_discrete_state() const
Returns a reference to the entire discrete state, which may consist of multiple discrete state vector...
Definition: context.h:130
void init_continuous_state(std::unique_ptr< ContinuousState< T >> xc)
(Internal use only) Sets the continuous state to xc, deleting whatever was there before.
Definition: context.h:689
A container class for an arbitrary type T.
Definition: value.h:252
AbstractValue & get_mutable_value(int index)
Returns the element of AbstractValues at the given index, or aborts if the index is out-of-bounds...
Definition: abstract_values.cc:39
A FixedInputPortValue encapsulates a vector or abstract value for use as an internal value source for...
Definition: fixed_input_port_value.h:35
void SetContinuousState(const Eigen::Ref< const VectorX< T >> &xc)
Sets the continuous state to xc, including q, v, and z partitions.
Definition: context.h:359
void NoteAccuracyChanged(int64_t change_event)
Notifies the local accuracy tracker that the accuracy setting may have changed.
Definition: context_base.h:299
const BasicVector< T > & get_vector() const
Returns a const reference to the BasicVector containing the values for the only group.
Definition: discrete_values.h:105
const ContinuousState< T > & get_continuous_state() const
Returns a const reference to the continuous component of the state, which may be of size zero...
Definition: context.h:113
std::unique_ptr< State< T > > CloneState() const
Returns a deep copy of this Context&#39;s State.
Definition: context.h:591
A smart pointer with deep copy semantics.
Definition: copyable_unique_ptr.h:107
bool is_stateless() const
Returns true if the Context has no state.
Definition: context.h:75
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:51
int data
Definition: value_test.cc:20
T & GetMutableValue()
Returns the value wrapped in this AbstractValue, which must be of exactly type T. ...
Definition: value.h:171