hpp-constraints  4.10.1
Definition of basic geometric constraints for motion planning
implicit-constraint-set.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016 - 2018 CNRS
3 // Authors: Joseph Mirabel, Florent Lamiraux
4 //
5 // This file is part of hpp-constraints.
6 // hpp-constraints is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-constraints is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-constraints. If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CONSTRAINTS_IMPLICIT_CONSTRAINT_SET_HH
20 # define HPP_CONSTRAINTS_IMPLICIT_CONSTRAINT_SET_HH
21 
22 # include <hpp/constraints/fwd.hh>
25 
26 namespace hpp {
27  namespace constraints {
30 
35  public Implicit
36  {
37  public:
38  typedef std::vector<ImplicitPtr_t> Implicits_t;
39 
43  static ImplicitConstraintSetPtr_t create (const std::string& name)
44  {
46  (new ImplicitConstraintSet(name));
47  }
48 
49  virtual ~ImplicitConstraintSet () {}
50 
53 
54  void add (const ImplicitPtr_t& constraint)
55  {
56  assert (HPP_DYNAMIC_PTR_CAST (DifferentiableFunctionSet,
57  function_));
59  (HPP_STATIC_PTR_CAST (DifferentiableFunctionSet, function_));
60  functions->add (constraint->functionPtr ());
61  constraints_.push_back(constraint);
62  // Handle comparison types
63  const ComparisonTypes_t& comp (constraint->comparisonType ());
64  for (std::size_t i = 0; i < comp.size(); ++i) {
65  if ((comp[i] == Superior) && (comp[i] == Inferior))
66  {
67  inequalityIndices_.push_back (comparison_.size());
68  }
69  else if (comp[i] == Equality)
70  {
71  equalityIndices_.addRow(comparison_.size(), 1);
72  }
73  comparison_.push_back (comp[i]);
74  }
75  equalityIndices_.updateRows<true, true, true>();
76  }
77 
79  const std::vector<std::size_t>& inequalityIndices () const
80  {
81  return inequalityIndices_;
82  }
85  {
86  return equalityIndices_;
87  }
89  const Implicits_t& constraints () const
90  {
91  return constraints_;
92  }
93 
95  void merge (const ImplicitConstraintSetPtr_t& other)
96  {
97  const Implicits_t& constraints = other->constraints();
98  for (Implicits_t::const_iterator constraint = constraints.begin();
99  constraint != constraints.end(); ++constraint)
100  add (*constraint);
101  }
102 
104 
105  std::ostream& print (std::ostream& os) const
106  {
107  function_->print (os);
108  return os;
109  }
110 
114  ImplicitConstraintSet (const std::string& name)
115  : Implicit (DifferentiableFunctionSet::create (name),
117  {
118  }
119 
121  : Implicit (DifferentiableFunctionSet::create ("Stack"),
123  {}
124 
126  : Implicit (DifferentiableFunctionSet::create ("Stack"),
128  {
129  const Implicits_t& constraints = o.constraints();
130  for (Implicits_t::const_iterator constraint = constraints.begin();
131  constraint != constraints.end(); ++constraint)
132  add (*constraint);
133  }
134 
135  private:
136  Implicits_t constraints_;
137  ComparisonTypes_t comparison_;
138  std::vector<std::size_t> inequalityIndices_;
139  Eigen::RowBlockIndices equalityIndices_;
140 
141  HPP_SERIALIZABLE();
142  }; // class ImplicitConstraintSet
144  } // namespace constraints
145 } // namespace hpp
146 
147 #endif // HPP_CONSTRAINTS_IMPLICIT_CONSTRAINT_SET_HH
std::vector< ImplicitPtr_t > Implicits_t
Definition: implicit-constraint-set.hh:38
Definition: implicit-constraint-set.hh:34
ImplicitConstraintSet(const std::string &name)
Definition: implicit-constraint-set.hh:114
void merge(const ImplicitConstraintSetPtr_t &other)
The output columns selection of other is not taken into account.
Definition: implicit-constraint-set.hh:95
Definition: active-set-differentiable-function.hh:24
ImplicitConstraintSet(const ImplicitConstraintSet &o)
Definition: implicit-constraint-set.hh:125
Definition: fwd.hh:174
const Eigen::RowBlockIndices & equalityIndices() const
Get indices of constraint coordinates that are equality.
Definition: implicit-constraint-set.hh:84
Definition: fwd.hh:173
Definition: differentiable-function-set.hh:33
const Implicits_t & constraints() const
Get constraints.
Definition: implicit-constraint-set.hh:89
ImplicitConstraintSet()
Definition: implicit-constraint-set.hh:120
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:176
assert(d.lhs()._blocks()==d.rhs()._blocks())
boost::shared_ptr< ImplicitConstraintSet > ImplicitConstraintSetPtr_t
Definition: fwd.hh:168
Definition: fwd.hh:171
static ImplicitConstraintSetPtr_t create(const std::string &name)
Definition: implicit-constraint-set.hh:43
std::ostream & print(std::ostream &os) const
Definition: implicit-constraint-set.hh:105
virtual ~ImplicitConstraintSet()
Definition: implicit-constraint-set.hh:49
void add(const ImplicitPtr_t &constraint)
Definition: implicit-constraint-set.hh:54
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
const std::vector< std::size_t > & inequalityIndices() const
Get indices of constraint coordinates that are inequality.
Definition: implicit-constraint-set.hh:79
Definition: implicit.hh:99
boost::shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
boost::shared_ptr< DifferentiableFunctionSet > DifferentiableFunctionSetPtr_t
Definition: fwd.hh:103