hpp-constraints  5.0.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 {
120  public:
122  bool operator==(const Implicit& other) const;
124  virtual ImplicitPtr_t copy() const;
127  static ImplicitPtr_t create(const DifferentiableFunctionPtr_t& func,
128  ComparisonTypes_t comp,
129  std::vector<bool> mask = std::vector<bool>());
130 
132  static ImplicitPtr_t createCopy(const ImplicitPtr_t& other);
133 
134  virtual ~Implicit(){};
135 
138 
152  void rightHandSideFromConfig(ConfigurationIn_t config,
153  LiegroupElementRef rhs);
154 
162  bool checkRightHandSide(LiegroupElementConstRef rhs) const;
163 
167  size_type parameterSize() const;
168 
172  size_type rightHandSideSize() const;
173 
178  void rightHandSideFunction(const DifferentiableFunctionPtr_t& rhsF);
179 
185  return rhsFunction_;
186  }
187 
190  vectorIn_t rightHandSideAt(const value_type& s);
191 
193 
195  const ComparisonTypes_t& comparisonType() const;
196 
198  void comparisonType(const ComparisonTypes_t& comp);
199 
202  const segments_t& activeRows() const { return activeRows_; }
203 
205  bool checkAllRowsActive() const { return inactiveRows_.nbRows() == 0; }
206 
209  return equalityIndices_;
210  }
211 
215  void setInactiveRowsToZero(vectorOut_t error) const;
216 
218  DifferentiableFunction& function() const { return *function_; }
219 
221  const DifferentiableFunctionPtr_t& functionPtr() const { return function_; }
222 
235  virtual std::pair<JointConstPtr_t, JointConstPtr_t>
236  doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const;
237 
238  protected:
249  std::vector<bool> mask);
250 
252  Implicit(const Implicit& other);
253 
257  virtual bool isEqual(const Implicit& other, bool swapAndTest) const;
258 
259  // Store weak pointer to itself
260  void init(const ImplicitWkPtr_t& weak) { weak_ = weak; }
261 
262  friend class ImplicitConstraintSet;
263 
264  private:
265  void computeIndices();
266  void computeActiveRows();
267  ComparisonTypes_t comparison_;
268  vector_t rhs_;
269  size_type parameterSize_;
270  DifferentiableFunctionPtr_t function_;
271  DifferentiableFunctionPtr_t rhsFunction_;
272  std::vector<bool> mask_;
273  segments_t activeRows_;
274  Eigen::RowBlockIndices inactiveRows_;
275  std::vector<std::size_t> inequalityIndices_;
276  Eigen::RowBlockIndices equalityIndices_;
277  ImplicitWkPtr_t weak_;
278  // To avoid dynamic memory allocation
279  mutable LiegroupElement output_;
280  mutable vector_t logOutput_;
281 
282  protected:
283  Implicit() {}
284 
285  private:
286  HPP_SERIALIZABLE();
287 }; // class Implicit
289 } // namespace constraints
290 } // namespace hpp
291 
292 #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:202
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
Definition: implicit-constraint-set.hh:45
virtual ~Implicit()
Definition: implicit.hh:134
Definition: active-set-differentiable-function.hh:36
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:65
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:67
bool operator==(const ComparisonTypes_t &v, const internal::ReplicateCompType &r)
Definition: comparison-types.hh:117
const DifferentiableFunctionPtr_t & rightHandSideFunction() const
Definition: implicit.hh:184
const Eigen::RowBlockIndices & equalityIndices() const
Get indices of constraint coordinates that are equality.
Definition: implicit.hh:208
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:180
bool checkAllRowsActive() const
Check if all rows are active (no inactive rows)
Definition: implicit.hh:205
Definition: differentiable-function.hh:63
void init(const ImplicitWkPtr_t &weak)
Definition: implicit.hh:260
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:113
const DifferentiableFunctionPtr_t & functionPtr() const
Return a reference to function .
Definition: implicit.hh:221
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:88
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:173
pinocchio::value_type value_type
Definition: fwd.hh:48
Definition: implicit.hh:119
Implicit()
Definition: implicit.hh:283