Drake
binding.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 #include <utility>
5 
6 #include "drake/common/drake_copyable.h"
7 #include "drake/common/drake_deprecated.h"
8 #include "drake/solvers/decision_variable.h"
9 
10 namespace drake {
11 namespace solvers {
12 /**
13  * A binding on constraint type C is a mapping of the decision
14  * variables onto the inputs of C. This allows the constraint to operate
15  * on a vector made up of different elements of the decision variables.
16  */
17 template <typename C>
18 class Binding {
19  public:
21 
22  Binding(const std::shared_ptr<C>& c,
23  const Eigen::Ref<const VectorXDecisionVariable>& v)
24  : evaluator_(c), vars_(v) {
25  DRAKE_DEMAND(c->num_vars() == v.rows() || c->num_vars() == Eigen::Dynamic);
26  }
27 
28  /**
29  * Concatenates each VectorDecisionVariable object in @p v into a single
30  * column vector, binds this column vector of decision variables with
31  * the constraint @p c.
32  */
33  Binding(const std::shared_ptr<C>& c, const VariableRefList& v)
34  : evaluator_(c) {
36  DRAKE_DEMAND(c->num_vars() == vars_.rows() ||
37  c->num_vars() == Eigen::Dynamic);
38  }
39 
40  template <typename U>
41  Binding(const Binding<U>& b,
42  typename std::enable_if<std::is_convertible<
43  std::shared_ptr<U>, std::shared_ptr<C>>::value>::type* = nullptr)
44  : Binding(b.evaluator(), b.variables()) {}
45 
46  DRAKE_DEPRECATED("Please use evaluator() instead of constraint()")
47  const std::shared_ptr<C>& constraint() const { return evaluator_; }
48 
49  const std::shared_ptr<C>& evaluator() const { return evaluator_; }
50 
51  const VectorXDecisionVariable& variables() const { return vars_; }
52 
53  /**
54  * Returns true iff the given @p var is included in this Binding.*/
55  bool ContainsVariable(const symbolic::Variable& var) const {
56  for (int i = 0; i < vars_.rows(); ++i) {
57  if (vars_(i).equal_to(var)) {
58  return true;
59  }
60  }
61  return false;
62  }
63 
64  size_t GetNumElements() const {
65  // TODO(ggould-tri) assumes that no index appears more than once in the
66  // view, which is nowhere asserted (but seems assumed elsewhere).
67  return vars_.size();
68  }
69 
70  private:
71  std::shared_ptr<C> evaluator_;
73 };
74 
75 namespace internal {
76 
77 /*
78  * Create binding, inferring the type from the provided pointer.
79  * @tparam C Cost or Constraint type to be bound.
80  * @note Since this forwards arguments, this will not be usable with
81  * `std::intializer_list`.
82  */
83 template <typename C, typename... Args>
84 Binding<C> CreateBinding(const std::shared_ptr<C>& c, Args&&... args) {
85  return Binding<C>(c, std::forward<Args>(args)...);
86 }
87 
88 template <typename To, typename From>
90  auto constraint = std::dynamic_pointer_cast<To>(binding.evaluator());
91  DRAKE_DEMAND(constraint != nullptr);
92  return Binding<To>(constraint, binding.variables());
93 }
94 
95 } // namespace internal
96 
97 } // namespace solvers
98 } // namespace drake
Binding< C > CreateBinding(const std::shared_ptr< C > &c, Args &&...args)
Definition: binding.h:84
Binding(const std::shared_ptr< C > &c, const VariableRefList &v)
Concatenates each VectorDecisionVariable object in v into a single column vector, binds this column v...
Definition: binding.h:33
int i
Definition: reset_after_move_test.cc:51
Represents a symbolic variable.
Definition: symbolic_variable.h:24
double value
Definition: wrap_test_util_py.cc:12
Definition: automotive_demo.cc:90
STL namespace.
Definition: autodiff_overloads.h:34
A binding on constraint type C is a mapping of the decision variables onto the inputs of C...
Definition: binding.h:18
const Constraint * constraint
Definition: nlopt_solver.cc:104
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:57
#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
Binding(const Binding< U > &b, typename std::enable_if< std::is_convertible< std::shared_ptr< U >, std::shared_ptr< C >>::value >::type *=nullptr)
Definition: binding.h:41
VectorDecisionVariable< Eigen::Dynamic > VectorXDecisionVariable
Definition: decision_variable.h:17
DRAKE_DEPRECATED("Please use evaluator() instead of constraint()") const std VectorXDecisionVariable vars_
Returns true iff the given var is included in this Binding.
Definition: binding.h:46
#define DRAKE_DEPRECATED(message)
Use DRAKE_DEPRECATED("message") to discourage use of particular classes, typedefs, variables, non-static data members, functions, arguments, enumerations, and template specializations.
Definition: drake_deprecated.h:33
std::list< Eigen::Ref< const VectorXDecisionVariable >> VariableRefList
Definition: decision_variable.h:19
VectorXDecisionVariable ConcatenateVariableRefList(const VariableRefList &var_list)
Concatenates each element in var_list into a single Eigen vector of decision variables, returns this concatenated vector.
Definition: decision_variable.cc:5
Binding< To > BindingDynamicCast(const Binding< From > &binding)
Definition: binding.h:89