hpp-constraints  4.13.0
Definition of basic geometric constraints for motion planning
implicit.hh
Go to the documentation of this file.
1 // Copyright (c) 2015 - 2018 CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr), Florent Lamiraux
3 //
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 
29 #ifndef HPP_CONSTRAINTS_IMPLICIT_HH
30 #define HPP_CONSTRAINTS_IMPLICIT_HH
31 
34 #include <hpp/constraints/fwd.hh>
36 #include <hpp/pinocchio/liegroup-element.hh>
37 #include <hpp/util/serialization-fwd.hh>
38 
39 namespace hpp {
40 namespace constraints {
125  public:
127  bool operator==(const Implicit& other) const;
129  virtual ImplicitPtr_t copy() const;
133  ComparisonTypes_t comp,
134  std::vector<bool> mask = std::vector<bool>());
135 
137  static ImplicitPtr_t createCopy(const ImplicitPtr_t& other);
138 
139  virtual ~Implicit(){};
140 
143 
158  LiegroupElementRef rhs);
159 
168 
185 
192 
198 
202  size_type parameterSize() const;
203 
207  size_type rightHandSideSize() const;
208 
214 
219  void rightHandSideFunction(const DifferentiableFunctionPtr_t& rhsF);
220 
225  const DifferentiableFunctionPtr_t& rightHandSideFunction() const {
226  return rhsFunction_;
227  }
228 
232 
234 
237 
240 
241  const segments_t& activeRows() const { return activeRows_; }
242 
245  return equalityIndices_;
246  }
247 
252 
254  DifferentiableFunction& function() const { return *function_; }
255 
257  const DifferentiableFunctionPtr_t& functionPtr() const { return function_; }
258 
259  protected:
270  std::vector<bool> mask);
271 
273  Implicit(const Implicit& other);
274 
278  virtual bool isEqual(const Implicit& other, bool swapAndTest) const;
279 
280  // Store weak pointer to itself
281  void init(const ImplicitWkPtr_t& weak) { weak_ = weak; }
282 
283  friend class ImplicitConstraintSet;
284 
285  private:
286  void computeIndices();
287  void computeActiveRows();
288  ComparisonTypes_t comparison_;
289  vector_t rhs_;
290  size_type parameterSize_;
291  DifferentiableFunctionPtr_t function_;
292  DifferentiableFunctionPtr_t rhsFunction_;
293  std::vector<bool> mask_;
294  segments_t activeRows_;
295  Eigen::RowBlockIndices inactiveRows_;
296  std::vector<std::size_t> inequalityIndices_;
297  Eigen::RowBlockIndices equalityIndices_;
298  ImplicitWkPtr_t weak_;
299  // To avoid dynamic memory allocation
300  mutable LiegroupElement output_;
301  mutable vector_t logOutput_;
302 
303  protected:
304  Implicit() {}
305 
306  private:
307  HPP_SERIALIZABLE();
308 }; // class Implicit
310 } // namespace constraints
311 } // namespace hpp
312 
313 #endif // HPP_CONSTRAINTS_IMPLICIT_HH
Definition: differentiable-function.hh:63
Definition: implicit-constraint-set.hh:45
Definition: implicit.hh:124
static ImplicitPtr_t create(const DifferentiableFunctionPtr_t &func, ComparisonTypes_t comp, std::vector< bool > mask=std::vector< bool >())
bool checkRightHandSide(LiegroupElementConstRef rhs) const
const DifferentiableFunctionPtr_t & functionPtr() const
Return a reference to function .
Definition: implicit.hh:257
void rightHandSideFromConfig(ConfigurationIn_t config) HPP_CONSTRAINTS_DEPRECATED
void rightHandSideFromConfig(ConfigurationIn_t config, LiegroupElementRef rhs)
void setInactiveRowsToZero(vectorOut_t error) const
virtual ~Implicit()
Definition: implicit.hh:139
const ComparisonTypes_t & comparisonType() const
Return the ComparisonType.
void rightHandSide(vectorIn_t rhs) HPP_CONSTRAINTS_DEPRECATED
virtual bool isEqual(const Implicit &other, bool swapAndTest) const
bool operator==(const Implicit &other) const
Operator equality.
virtual ImplicitPtr_t copy() const
Copy object and return shared pointer to copy.
const Eigen::RowBlockIndices & equalityIndices() const
Get indices of constraint coordinates that are equality.
Definition: implicit.hh:244
Implicit(const Implicit &other)
Copy constructor.
void comparisonType(const ComparisonTypes_t &comp)
Set the comparison type.
vectorIn_t rightHandSideAt(const value_type &s)
vectorIn_t rightHandSide() const HPP_CONSTRAINTS_DEPRECATED
Implicit()
Definition: implicit.hh:304
Implicit(const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, std::vector< bool > mask)
void init(const ImplicitWkPtr_t &weak)
Definition: implicit.hh:281
const segments_t & activeRows() const
Definition: implicit.hh:241
static ImplicitPtr_t createCopy(const ImplicitPtr_t &other)
Create a copy and return shared pointer.
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:64
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:180
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:111
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:173
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:114
pinocchio::size_type size_type
Definition: fwd.hh:47
pinocchio::value_type value_type
Definition: fwd.hh:48
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:59
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:105
std::vector< segment_t > segments_t
Definition: fwd.hh:83
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:60
pinocchio::vector_t vector_t
Definition: fwd.hh:58
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:66
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:65
Definition: active-set-differentiable-function.hh:36