Drake
system_constraint.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <functional>
4 #include <string>
5 #include <utility>
6 
12 #include "drake/common/unused.h"
13 
14 namespace drake {
15 namespace systems {
16 
17 template <typename T>
18 class Context;
19 
21 
23  kEquality = 0, ///< The constraint is of the form f(x)=0.
24  kInequality = 1, ///< The constraint is of the form f(x)≥0.
25 };
26 
27 /// A SystemConstraint is a generic base-class for constraints on Systems.
28 ///
29 /// A SystemConstraint is a means to inform our algorithms *about*
30 /// the implemented system behavior -- declaring the constraint does not
31 /// *cause* the system behavior to change. It is meant to improve analysis
32 /// by telling our algorithms that "all valid solutions of this dynamical
33 /// system will satisfy the following (in)equalities". Examples could
34 /// include conserved quantities or joint limits on a mechanism.
35 ///
36 /// This class is intentionally similar to, but (so far) independent from
37 /// solvers::Constraint. This is primarily because there is no notion of
38 /// decision variables in the system classes (yet); rather each individual
39 /// algorithm (e.g. trajectory optimization, or system identification)
40 /// constructs decision variables for the particular mathematical program that
41 /// is being formulated, and must bind the system constraint to those variables
42 /// (e.g. by populating the Context with the decision variables and calling
43 /// Calc).
44 ///
45 /// @see LeafSystem<T>::DeclareEqualityConstraint and
46 /// LeafSystem<T>::DeclareInequalityConstraint for use cases.
47 /// @tparam T The vector element type, which must be a valid Eigen scalar.
48 ///
49 /// Instantiated templates for the following kinds of T's are provided:
50 /// - double
51 /// - AutoDiffXd
52 /// - symbolic::Expression
53 ///
54 /// They are already available to link against in the containing library.
55 template <typename T>
57  public:
59 
60  /// This is the signature of a stateless function that evaluates the value of
61  /// the constraint function f:
62  /// value = f(context),
63  /// where value has the dimension specified in the constructor.
64  // TODO(russt): replace the argument VectorX<T>* with an Eigen::Ref* using
65  // whatever magic jwnimmer and soonho figured out a few weeks back.
66  using CalcCallback =
67  std::function<void(const Context<T>& context, VectorX<T>* value)>;
68 
69  /// Constructs the SystemConstraint.
70  ///
71  /// @param count the number of constraints (size of the value vector).
72  /// @param type the SystemConstraintType.
73  /// @param description a human-readable description useful for debugging.
74  SystemConstraint(CalcCallback calc_function, int count,
75  SystemConstraintType type, const std::string& description)
76  : calc_function_(std::move(calc_function)),
77  count_(count),
78  type_(type),
79  description_(description) {
80  DRAKE_DEMAND(count_ >= 0);
81  }
82 
83  /// Evaluates the function pointer passed in through the constructor,
84  /// writing the output to @p value. @p value will be (non-conservatively)
85  /// resized to match the constraint function output.
86  void Calc(const Context<T>& context, VectorX<T>* value) const {
87  value->resize(count_);
88  calc_function_(context, value);
89  DRAKE_DEMAND(value->size() == count_);
90  }
91 
92  /// Evaluates the function pointer, and check if all of the outputs
93  /// are within the desired bounds.
94  // TODO(russt): Resolve names differences across the codebase. The vector
95  // gen scripts call this IsValid, but Constraint calls it CheckSatisfied.
96  template <typename T1 = T>
98  const Context<T1>& context, double tol) const {
99  DRAKE_DEMAND(tol >= 0.0);
100  VectorX<T> value(count_);
101  Calc(context, &value);
102  if (type_ == SystemConstraintType::kEquality) {
103  return (value.template lpNorm<Eigen::Infinity>() <= tol);
104  } else {
105  return (value.array() >= -tol).all();
106  }
107  }
108 
109  /// Supports CheckSatisfied calls for non-numeric scalar types by simply
110  /// returning true.
111  template <typename T1 = T>
113  const Context<T1>& context, double tol) const {
114  DRAKE_DEMAND(tol >= 0.0);
115  unused(context);
116  return true;
117  }
118 
119  // Accessor methods.
120  int size() const { return count_; }
121  SystemConstraintType type() const { return type_; }
122  bool is_equality_constraint() const {
123  return (type_ == SystemConstraintType::kEquality);
124  }
125  const std::string& description() const { return description_; }
126 
127  private:
128  const CalcCallback calc_function_;
129  const int count_{0};
130  const SystemConstraintType type_;
131  const std::string description_;
132 };
133 
134 } // namespace systems
135 } // namespace drake
std::enable_if< is_numeric< T1 >::value, bool >::type CheckSatisfied(const Context< T1 > &context, double tol) const
Evaluates the function pointer, and check if all of the outputs are within the desired bounds...
Definition: system_constraint.h:97
bool is_equality_constraint() const
Definition: system_constraint.h:122
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
SystemConstraintType type() const
Definition: system_constraint.h:121
Definition: automotive_demo.cc:88
This file contains traits for number (scalar) types.
The constraint is of the form f(x)=0.
STL namespace.
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
The constraint is of the form f(x)≥0.
int value
Definition: copyable_unique_ptr_test.cc:61
Provides Drake&#39;s assertion implementation.
#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
SystemConstraintType
Definition: system_constraint.h:22
SystemConstraint(CalcCallback calc_function, int count, SystemConstraintType type, const std::string &description)
Constructs the SystemConstraint.
Definition: system_constraint.h:74
std::enable_if<!is_numeric< T1 >::value, bool >::type CheckSatisfied(const Context< T1 > &context, double tol) const
Supports CheckSatisfied calls for non-numeric scalar types by simply returning true.
Definition: system_constraint.h:112
A SystemConstraint is a generic base-class for constraints on Systems.
Definition: system_constraint.h:56
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
void Calc(const Context< T > &context, VectorX< T > *value) const
Evaluates the function pointer passed in through the constructor, writing the output to value...
Definition: system_constraint.h:86
const std::string & description() const
Definition: system_constraint.h:125
int size() const
Definition: system_constraint.h:120
A type-safe non-negative index class.
Definition: type_safe_index.h:144
std::function< void(const Context< T > &context, VectorX< T > *value)> CalcCallback
This is the signature of a stateless function that evaluates the value of the constraint function f: ...
Definition: system_constraint.h:67
void unused(const Args &...)
Documents the argument(s) as unused, placating GCC&#39;s -Wunused-parameter warning.
Definition: unused.h:51