hpp-constraints  4.11.0
Definition of basic geometric constraints for motion planning
explicit-constraint-set.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
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 #ifndef HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
18 #define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
19 
20 #include <vector>
21 
22 #include <hpp/constraints/fwd.hh>
24 
27 
28 namespace hpp {
29  namespace constraints {
32 
92  {
93  public:
97 
100 
107  bool solve (vectorOut_t arg) const;
108 
113  {
114  return errorSize_;
115  }
116 
122  bool isSatisfied (vectorIn_t arg, value_type errorThreshold = -1) const;
123 
130  bool isSatisfied (vectorIn_t arg, vectorOut_t error,
131  value_type errorThreshold = -1) const;
132 
142  bool isConstraintSatisfied (const ImplicitPtr_t& constraint,
143  vectorIn_t arg, vectorOut_t error,
144  bool& constraintFound) const;
146 
149 
157  size_type add (const ExplicitPtr_t& constraint);
158 
165  bool contains (const ExplicitPtr_t& numericalConstraint) const;
166 
171  : configSpace_ (space)
172  , inArgs_ (), notOutArgs_ ()
173  , inDers_ (), notOutDers_ ()
174  , outArgs_ (), outDers_ ()
175  , argFunction_ (Eigen::VectorXi::Constant(space->nq (), -1))
176  , derFunction_ (Eigen::VectorXi::Constant(space->nv (), -1))
177  , errorThreshold_ (Eigen::NumTraits<value_type>::epsilon())
178  , errorSize_(0)
179  // , Jg (nv, nv)
180  , arg_ (space->nq ()), diff_(space->nv ()), diffSmall_()
181  {
182  notOutArgs_.addRow(0, space->nq ());
183  notOutDers_.addCol(0, space->nv ());
184  }
185 
187 
190 
192  void errorThreshold (const value_type& threshold)
193  {
194  errorThreshold_ = threshold;
195  }
198  {
199  return errorThreshold_;
200  }
203  {
204  return errorThreshold_*errorThreshold_;
205  }
206 
208 
211 
213  const RowBlockIndices& inArgs () const
214  {
215  return inArgs_;
216  }
217 
219  const ColBlockIndices& inDers () const
220  {
221  return inDers_;
222  }
223 
227  const RowBlockIndices& notOutArgs () const
228  {
229  return notOutArgs_;
230  }
231 
235  const ColBlockIndices& notOutDers () const
236  {
237  return notOutDers_;
238  }
239 
241  ColBlockIndices activeParameters () const;
242 
244  const ColBlockIndices& activeDerivativeParameters () const;
245 
251  const Eigen::MatrixXi& inOutDependencies () const
252  {
253  return inOutDependencies_;
254  }
255 
257  Eigen::MatrixXi inOutDofDependencies () const;
258 
259  const Eigen::VectorXi& derFunction () const
260  {
261  return derFunction_;
262  }
263 
268  const RowBlockIndices& outArgs () const
269  {
270  return outArgs_;
271  }
272 
277  const RowBlockIndices& outDers () const
278  {
279  return outDers_;
280  }
281 
284  {
285  return configSpace_;
286  }
287 
289  std::size_t nq () const
290  {
291  return configSpace_->nq ();
292  }
293 
295  std::size_t nv () const
296  {
297  return configSpace_->nv ();
298  }
299 
301 
314  inline MatrixBlockView jacobianNotOutToOut (matrix_t& jacobian) const
315  {
316  return MatrixBlockView(jacobian,
317  outDers_.nbIndices() , outDers_.indices(),
318  notOutDers_.nbIndices(), notOutDers_.indices());
319  }
320 
337  void jacobian(matrixOut_t jacobian, vectorIn_t q) const;
338 
341 
361  vector_t rightHandSideFromInput (vectorIn_t p);
362 
383  bool rightHandSideFromInput (const ExplicitPtr_t& constraint,
384  vectorIn_t p);
385 
406  void rightHandSideFromInput (const size_type& i, vectorIn_t p);
407 
414  void rightHandSide (vectorIn_t rhs);
415 
420  bool rightHandSide (const ExplicitPtr_t& constraint, vectorIn_t rhs);
421 
426  bool getRightHandSide (const ExplicitPtr_t& constraint, vectorOut_t rhs) const ;
427 
432  void rightHandSide (const size_type& i, vectorIn_t rhs);
433 
437  vector_t rightHandSide () const;
438 
442  size_type rightHandSideSize () const;
443 
445 
446  std::ostream& print (std::ostream& os) const;
447 
448  private:
449  typedef std::vector<bool> Computed_t;
450 
455  void solveExplicitConstraint(const std::size_t& i, vectorOut_t arg)
456  const;
471  void computeJacobian(const std::size_t& i, matrixOut_t J) const;
472  void computeOrder(const std::size_t& iF, std::size_t& iOrder, Computed_t& computed);
473 
474  LiegroupSpacePtr_t configSpace_;
475 
476  struct Data {
477  Data (const ExplicitPtr_t& constraint);
478  ExplicitPtr_t constraint;
479  RowBlockIndices equalityIndices;
480  LiegroupElement rhs_implicit;
481  // implicit formulation
482  mutable LiegroupElement h_value;
483  // explicit formulation
484  mutable vector_t qin, qout;
485  mutable LiegroupElement f_value, res_qout;
486  // jacobian of f
487  mutable matrix_t jacobian;
488  }; // struct Data
489 
490  RowBlockIndices inArgs_, notOutArgs_;
491  ColBlockIndices inDers_, notOutDers_;
493  RowBlockIndices outArgs_, outDers_;
494 
495  Eigen::MatrixXi inOutDependencies_;
496 
497  std::vector<Data> data_;
498  std::vector<std::size_t> computationOrder_;
504  Eigen::VectorXi argFunction_, derFunction_;
505  value_type errorThreshold_;
506  size_type errorSize_;
507  // mutable matrix_t Jg;
508  mutable vector_t arg_, diff_, diffSmall_;
509 
512  : inArgs_ (), notOutArgs_ ()
513  , inDers_ (), notOutDers_ ()
514  , outArgs_ (), outDers_ ()
515  , errorThreshold_ (Eigen::NumTraits<value_type>::epsilon())
516  , errorSize_(0)
517  {}
519  void init(const LiegroupSpacePtr_t& space)
520  {
521  configSpace_ = space;
522  argFunction_ = Eigen::VectorXi::Constant(space->nq (), -1);
523  derFunction_ = Eigen::VectorXi::Constant(space->nv (), -1);
524  arg_.resize(space->nq ());
525  diff_.resize(space->nv ());
526 
527  notOutArgs_.addRow(0, space->nq ());
528  notOutDers_.addCol(0, space->nv ());
529  }
530 
532  }; // class ExplicitConstraintSet
534  } // namespace constraints
535 } // namespace hpp
536 
537 #endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
void addCol(const size_type &col, const size_type size)
Definition: matrix-view.hh:609
pinocchio::vector_t vector_t
Definition: fwd.hh:47
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: explicit-constraint-set.hh:192
std::size_t nq() const
The number of variables.
Definition: explicit-constraint-set.hh:289
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
const RowBlockIndices & inArgs() const
Set of input configuration variables.
Definition: explicit-constraint-set.hh:213
Definition: fwd.hh:27
const RowBlockIndices & notOutArgs() const
Definition: explicit-constraint-set.hh:227
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
void addRow(const size_type &row, const size_type size)
Definition: matrix-view.hh:600
LiegroupSpacePtr_t configSpace() const
The Lie group on which constraints are defined.
Definition: explicit-constraint-set.hh:283
size_type errorSize() const
Definition: explicit-constraint-set.hh:112
std::size_t nv() const
The number of derivative variables.
Definition: explicit-constraint-set.hh:295
Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > MatrixBlockView
Definition: explicit-constraint-set.hh:96
shared_ptr< Explicit > ExplicitPtr_t
Definition: fwd.hh:179
ExplicitConstraintSet(const LiegroupSpacePtr_t &space)
Definition: explicit-constraint-set.hh:170
Definition: matrix-view.hh:111
const Eigen::MatrixXi & inOutDependencies() const
Definition: explicit-constraint-set.hh:251
const Eigen::VectorXi & derFunction() const
Definition: explicit-constraint-set.hh:259
value_type errorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:197
Eigen::ColBlockIndices ColBlockIndices
Definition: explicit-constraint-set.hh:95
Definition: by-substitution.hh:62
const RowBlockIndices & outDers() const
Definition: explicit-constraint-set.hh:277
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
Eigen::RowBlockIndices RowBlockIndices
Definition: explicit-constraint-set.hh:94
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:24
MatrixBlockView jacobianNotOutToOut(matrix_t &jacobian) const
Definition: explicit-constraint-set.hh:314
const ColBlockIndices & notOutDers() const
Definition: explicit-constraint-set.hh:235
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
pinocchio::value_type value_type
Definition: fwd.hh:37
const ColBlockIndices & inDers() const
Set of input velocity variables.
Definition: explicit-constraint-set.hh:219
const RowBlockIndices & outArgs() const
Definition: explicit-constraint-set.hh:268
Definition: explicit-constraint-set.hh:91
value_type squaredErrorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:202