hpp-constraints  4.15.1
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  HPP_CONSTRAINTS_DEPRECATED void rightHandSideFromConfig(
184  ConfigurationIn_t config);
185 
191  HPP_CONSTRAINTS_DEPRECATED void rightHandSide(vectorIn_t rhs);
192 
197  HPP_CONSTRAINTS_DEPRECATED vectorIn_t rightHandSide() const;
198 
202  size_type parameterSize() const;
203 
207  size_type rightHandSideSize() const;
208 
213  HPP_CONSTRAINTS_DEPRECATED vectorOut_t rightHandSide();
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 
243  const segments_t& activeRows() const { return activeRows_; }
244 
246  bool checkAllRowsActive() const { return inactiveRows_.nbRows() == 0; }
247 
250  return equalityIndices_;
251  }
252 
256  void setInactiveRowsToZero(vectorOut_t error) const;
257 
259  DifferentiableFunction& function() const { return *function_; }
260 
262  const DifferentiableFunctionPtr_t& functionPtr() const { return function_; }
263 
276  virtual std::pair<JointConstPtr_t, JointConstPtr_t>
277  doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const;
278 
279  protected:
290  std::vector<bool> mask);
291 
293  Implicit(const Implicit& other);
294 
298  virtual bool isEqual(const Implicit& other, bool swapAndTest) const;
299 
300  // Store weak pointer to itself
301  void init(const ImplicitWkPtr_t& weak) { weak_ = weak; }
302 
303  friend class ImplicitConstraintSet;
304 
305  private:
306  void computeIndices();
307  void computeActiveRows();
308  ComparisonTypes_t comparison_;
309  vector_t rhs_;
310  size_type parameterSize_;
311  DifferentiableFunctionPtr_t function_;
312  DifferentiableFunctionPtr_t rhsFunction_;
313  std::vector<bool> mask_;
314  segments_t activeRows_;
315  Eigen::RowBlockIndices inactiveRows_;
316  std::vector<std::size_t> inequalityIndices_;
317  Eigen::RowBlockIndices equalityIndices_;
318  ImplicitWkPtr_t weak_;
319  // To avoid dynamic memory allocation
320  mutable LiegroupElement output_;
321  mutable vector_t logOutput_;
322 
323  protected:
324  Implicit() {}
325 
326  private:
327  HPP_SERIALIZABLE();
328 }; // class Implicit
330 } // namespace constraints
331 } // namespace hpp
332 
333 #endif // HPP_CONSTRAINTS_IMPLICIT_HH
std::vector< segment_t > segments_t
Definition: fwd.hh:84
pinocchio::vector_t vector_t
Definition: fwd.hh:59
const segments_t & activeRows() const
Definition: implicit.hh:243
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
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:65
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:67
#define HPP_CONSTRAINTS_DEPRECATED
Definition: deprecated.hh:37
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:249
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:182
bool checkAllRowsActive() const
Check if all rows are active (no inactive rows)
Definition: implicit.hh:246
Definition: differentiable-function.hh:63
void init(const ImplicitWkPtr_t &weak)
Definition: implicit.hh:301
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:113
const DifferentiableFunctionPtr_t & functionPtr() const
Return a reference to function .
Definition: implicit.hh:262
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:66
pinocchio::size_type size_type
Definition: fwd.hh:47
pinocchio::DeviceConstPtr_t DeviceConstPtr_t
Definition: fwd.hh:110
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:175
pinocchio::value_type value_type
Definition: fwd.hh:48
Definition: implicit.hh:124
Implicit()
Definition: implicit.hh:324