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  boolean<T> CheckSatisfied(const Context<T>& context, double tol) const {
97  DRAKE_DEMAND(tol >= 0.0);
98  VectorX<T> value(count_);
99  Calc(context, &value);
100  // Special-case (tol == 0.0) cases both so that the symbolic form is
101  // elegant, and so that double evaluation is as fast as possible.
102  if (type_ == SystemConstraintType::kEquality) {
103  if (tol == 0.0) {
104  return all(value.array() == 0.0);
105  } else {
106  return all(value.cwiseAbs().array() <= tol);
107  }
108  } else {
110  if (tol == 0.0) {
111  return all(value.array() >= 0.0); // N.B. Not -tol, which is -0.0.
112  } else {
113  return all(value.array() >= -tol);
114  }
115  }
116  }
117 
118  // Accessor methods.
119  int size() const { return count_; }
120  SystemConstraintType type() const { return type_; }
121  bool is_equality_constraint() const {
122  return (type_ == SystemConstraintType::kEquality);
123  }
124  const std::string& description() const { return description_; }
125 
126  private:
127  const CalcCallback calc_function_;
128  const int count_{0};
129  const SystemConstraintType type_;
130  const std::string description_;
131 };
132 
133 } // namespace systems
134 } // namespace drake
bool is_equality_constraint() const
Definition: system_constraint.h:121
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
double value
Definition: wrap_test_util_py.cc:12
SystemConstraintType type() const
Definition: system_constraint.h:120
Definition: bullet_model.cc:22
The constraint is of the form f(x)=0.
STL namespace.
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
#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
The constraint is of the form f(x)≥0.
Provides Drake&#39;s assertion implementation.
typename scalar_predicate< T >::type boolean
An alias for a boolean-like value, conditioned on the scalar type T.
Definition: drake_bool.h:29
#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
ReferenceType type
Definition: loader.cc:34
boolean< T > CheckSatisfied(const Context< T > &context, double tol) const
Evaluates the function pointer, and check if all of the outputs are within the desired bounds...
Definition: system_constraint.h:96
Derived::Scalar all(const Eigen::DenseBase< Derived > &m)
Checks truth for all elements in matrix m.
Definition: drake_bool.h:36
SystemConstraint(CalcCallback calc_function, int count, SystemConstraintType type, const std::string &description)
Constructs the SystemConstraint.
Definition: system_constraint.h:74
const std::string description
Definition: proximity_engine_test.cc:721
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:124
int size() const
Definition: system_constraint.h:119
A type-safe non-negative index class.
Definition: type_safe_index.h:129
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