hpp-constraints  4.12.0
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  comparison_.push_back (comp[i]);
66  }
67  // Handle mask
68  mask_.insert(mask_.end(), constraint->mask_.begin(),
69  constraint->mask_.end());
70  // Recompute active rows
71  computeActiveRows();
72  computeIndices();
73  // Resize temporary variables
74  output_ = LiegroupElement(functions->outputSpace());
75  logOutput_.resize(functions->outputSpace()->nv());
76  }
77 
79  const Implicits_t& constraints () const
80  {
81  return constraints_;
82  }
83 
85  void merge (const ImplicitConstraintSetPtr_t& other)
86  {
87  const Implicits_t& constraints = other->constraints();
88  for (Implicits_t::const_iterator constraint = constraints.begin();
89  constraint != constraints.end(); ++constraint)
90  add (*constraint);
91  }
92 
94 
95  std::ostream& print (std::ostream& os) const
96  {
97  function_->print (os);
98  return os;
99  }
100 
104  ImplicitConstraintSet (const std::string& name)
105  : Implicit (DifferentiableFunctionSet::create (name),
106  ComparisonTypes_t (), std::vector<bool>())
107  {
108  }
109 
111  : Implicit (DifferentiableFunctionSet::create ("Stack"),
112  ComparisonTypes_t (), std::vector<bool>())
113  {}
114 
116  : Implicit (DifferentiableFunctionSet::create ("Stack"),
117  ComparisonTypes_t (), std::vector<bool>())
118  {
119  const Implicits_t& constraints = o.constraints();
120  for (Implicits_t::const_iterator constraint = constraints.begin();
121  constraint != constraints.end(); ++constraint)
122  add (*constraint);
123  }
124 
125  private:
126  Implicits_t constraints_;
127 
128  HPP_SERIALIZABLE();
129  }; // class ImplicitConstraintSet
131  } // namespace constraints
132 } // namespace hpp
133 
134 #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:104
void merge(const ImplicitConstraintSetPtr_t &other)
The output columns selection of other is not taken into account.
Definition: implicit-constraint-set.hh:85
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
ImplicitConstraintSet(const ImplicitConstraintSet &o)
Definition: implicit-constraint-set.hh:115
Definition: differentiable-function-set.hh:33
const Implicits_t & constraints() const
Get constraints.
Definition: implicit-constraint-set.hh:79
ImplicitConstraintSet()
Definition: implicit-constraint-set.hh:110
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:176
assert(d.lhs()._blocks()==d.rhs()._blocks())
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:95
virtual ~ImplicitConstraintSet()
Definition: implicit-constraint-set.hh:49
void add(const ImplicitPtr_t &constraint)
Definition: implicit-constraint-set.hh:54
shared_ptr< DifferentiableFunctionSet > DifferentiableFunctionSetPtr_t
Definition: fwd.hh:103
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
shared_ptr< ImplicitConstraintSet > ImplicitConstraintSetPtr_t
Definition: fwd.hh:168
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
Definition: implicit.hh:114