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;
132  static ImplicitPtr_t create(const DifferentiableFunctionPtr_t& func,
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 
157  void rightHandSideFromConfig(ConfigurationIn_t config,
158  LiegroupElementRef rhs);
159 
167  bool checkRightHandSide(LiegroupElementConstRef rhs) const;
168 
183  void rightHandSideFromConfig(ConfigurationIn_t config)
185 
191  void rightHandSide(vectorIn_t rhs) HPP_CONSTRAINTS_DEPRECATED;
192 
197  vectorIn_t rightHandSide() const HPP_CONSTRAINTS_DEPRECATED;
198 
202  size_type parameterSize() const;
203 
207  size_type rightHandSideSize() const;
208 
213  vectorOut_t rightHandSide() HPP_CONSTRAINTS_DEPRECATED;
214 
219  void rightHandSideFunction(const DifferentiableFunctionPtr_t& rhsF);
220 
226  return rhsFunction_;
227  }
228 
231  vectorIn_t rightHandSideAt(const value_type& s);
232 
234 
236  const ComparisonTypes_t& comparisonType() const;
237 
239  void comparisonType(const ComparisonTypes_t& comp);
240 
241  const segments_t& activeRows() const { return activeRows_; }
242 
245  return equalityIndices_;
246  }
247 
251  void setInactiveRowsToZero(vectorOut_t error) const;
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
std::vector< segment_t > segments_t
Definition: fwd.hh:83
pinocchio::vector_t vector_t
Definition: fwd.hh:58
const segments_t & activeRows() const
Definition: implicit.hh:241
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:59
Definition: implicit-constraint-set.hh:45
virtual ~Implicit()
Definition: implicit.hh:139
Definition: active-set-differentiable-function.hh:36
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:64
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:66
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:114
bool operator==(const ComparisonTypes_t &v, const internal::ReplicateCompType &r)
Definition: comparison-types.hh:117
const DifferentiableFunctionPtr_t & rightHandSideFunction() const
Definition: implicit.hh:225
const Eigen::RowBlockIndices & equalityIndices() const
Get indices of constraint coordinates that are equality.
Definition: implicit.hh:244
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:105
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:180
Definition: differentiable-function.hh:63
void init(const ImplicitWkPtr_t &weak)
Definition: implicit.hh:281
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:111
const DifferentiableFunctionPtr_t & functionPtr() const
Return a reference to function .
Definition: implicit.hh:257
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:60
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:65
pinocchio::size_type size_type
Definition: fwd.hh:47
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:173
pinocchio::value_type value_type
Definition: fwd.hh:48
Definition: implicit.hh:124
Implicit()
Definition: implicit.hh:304