hpp-constraints  4.12.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 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 
18 #ifndef HPP_CONSTRAINTS_IMPLICIT_HH
19 # define HPP_CONSTRAINTS_IMPLICIT_HH
20 
21 # include <hpp/constraints/fwd.hh>
22 # include <hpp/constraints/config.hh>
25 
26 # include <hpp/util/serialization-fwd.hh>
27 
28 # include <hpp/pinocchio/liegroup-element.hh>
29 
30 namespace hpp {
31  namespace constraints {
115  public:
117  bool operator== (const Implicit& other) const
118  {
119  return isEqual (other, true);
120  }
122  virtual ImplicitPtr_t copy () const;
125  static ImplicitPtr_t create
127  std::vector<bool> mask = std::vector<bool>());
128 
130  static ImplicitPtr_t createCopy (const ImplicitPtr_t& other);
131 
132  virtual ~Implicit () {};
133 
136 
150  void rightHandSideFromConfig (ConfigurationIn_t config,
151  LiegroupElementRef rhs);
152 
160  bool checkRightHandSide(LiegroupElementConstRef rhs) const;
161 
176  void rightHandSideFromConfig (ConfigurationIn_t config)
178 
184  void rightHandSide (vectorIn_t rhs) HPP_CONSTRAINTS_DEPRECATED;
185 
190  vectorIn_t rightHandSide () const HPP_CONSTRAINTS_DEPRECATED;
191 
195  size_type parameterSize () const;
196 
200  size_type rightHandSideSize () const;
201 
206  vectorOut_t rightHandSide () HPP_CONSTRAINTS_DEPRECATED;
207 
212  void rightHandSideFunction (const DifferentiableFunctionPtr_t& rhsF);
213 
218  const DifferentiableFunctionPtr_t& rightHandSideFunction () const
219  {
220  return rhsFunction_;
221  }
222 
225  vectorIn_t rightHandSideAt (const value_type& s);
226 
228 
230  const ComparisonTypes_t& comparisonType () const;
231 
233  void comparisonType (const ComparisonTypes_t& comp);
234 
235  const segments_t& activeRows() const
236  {
237  return activeRows_;
238  }
239 
242  {
243  return equalityIndices_;
244  }
245 
249  void setInactiveRowsToZero(vectorOut_t error) const;
250 
252  DifferentiableFunction& function () const
253  {
254  return *function_;
255  }
256 
259  {
260  return function_;
261  }
262 
263  protected:
273  Implicit (const DifferentiableFunctionPtr_t& function,
274  ComparisonTypes_t comp, std::vector<bool> mask);
275 
277  Implicit (const Implicit& other);
278 
282  virtual bool isEqual (const Implicit& other, bool swapAndTest) const;
283 
284  // Store weak pointer to itself
285  void init (const ImplicitWkPtr_t& weak)
286  {
287  weak_ = weak;
288  }
289 
290  friend class ImplicitConstraintSet;
291  private:
292  void computeIndices();
293  void computeActiveRows();
294  ComparisonTypes_t comparison_;
295  vector_t rhs_;
296  size_type parameterSize_;
297  DifferentiableFunctionPtr_t function_;
298  DifferentiableFunctionPtr_t rhsFunction_;
299  std::vector<bool> mask_;
300  segments_t activeRows_;
301  Eigen::RowBlockIndices inactiveRows_;
302  std::vector<std::size_t> inequalityIndices_;
303  Eigen::RowBlockIndices equalityIndices_;
304  ImplicitWkPtr_t weak_;
305  // To avoid dynamic memory allocation
306  mutable LiegroupElement output_;
307  mutable vector_t logOutput_;
308  protected:
309  Implicit () {}
310  private:
311  HPP_SERIALIZABLE();
312  }; // class Implicit
314  } // namespace constraints
315 } // namespace hpp
316 
317 #endif // HPP_CONSTRAINTS_IMPLICIT_HH
pinocchio::vector_t vector_t
Definition: fwd.hh:47
const segments_t & activeRows() const
Definition: implicit.hh:235
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
Definition: implicit-constraint-set.hh:34
virtual ~Implicit()
Definition: implicit.hh:132
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:55
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:105
bool operator==(const ComparisonTypes_t &v, const internal::ReplicateCompType &r)
Definition: comparison-types.hh:117
std::vector< segment_t > segments_t
Definition: fwd.hh:72
const Eigen::RowBlockIndices & equalityIndices() const
Get indices of constraint coordinates that are equality.
Definition: implicit.hh:241
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:176
Definition: differentiable-function.hh:52
void init(const ImplicitWkPtr_t &weak)
Definition: implicit.hh:285
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:101
const DifferentiableFunctionPtr_t & functionPtr() const
Return a reference to function .
Definition: implicit.hh:258
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:54
pinocchio::size_type size_type
Definition: fwd.hh:36
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
pinocchio::value_type value_type
Definition: fwd.hh:37
Definition: implicit.hh:114
Implicit()
Definition: implicit.hh:309