hpp-constraints  4.10.1
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 <boost/function.hpp>
23 
24 #include <hpp/constraints/fwd.hh>
26 
29 
30 namespace hpp {
31  namespace constraints {
34 
94  {
95  public:
99 
102 
109  bool solve (vectorOut_t arg) const;
110 
113  bool isSatisfied (vectorIn_t arg) const;
114 
119  {
120  return errorSize_;
121  }
122 
126  bool isSatisfied (vectorIn_t arg, vectorOut_t error) const;
127 
137  bool isConstraintSatisfied (const ImplicitPtr_t& constraint,
138  vectorIn_t arg, vectorOut_t error,
139  bool& constraintFound) const;
141 
144 
152  size_type add (const ExplicitPtr_t& constraint);
153 
160  bool contains (const ExplicitPtr_t& numericalConstraint) const;
161 
166  : configSpace_ (space)
167  , inArgs_ (), notOutArgs_ ()
168  , inDers_ (), notOutDers_ ()
169  , outArgs_ (), outDers_ ()
170  , argFunction_ (Eigen::VectorXi::Constant(space->nq (), -1))
171  , derFunction_ (Eigen::VectorXi::Constant(space->nv (), -1))
172  , squaredErrorThreshold_ (Eigen::NumTraits<value_type>::epsilon())
173  , errorSize_(0)
174  // , Jg (nv, nv)
175  , arg_ (space->nq ()), diff_(space->nv ()), diffSmall_()
176  {
177  notOutArgs_.addRow(0, space->nq ());
178  notOutDers_.addCol(0, space->nv ());
179  }
180 
182 
185 
187  void errorThreshold (const value_type& threshold)
188  {
189  squaredErrorThreshold_ = threshold * threshold;
190  }
193  {
194  return sqrt (squaredErrorThreshold_);
195  }
198  {
199  return squaredErrorThreshold_;
200  }
201 
203 
206 
208  const RowBlockIndices& inArgs () const
209  {
210  return inArgs_;
211  }
212 
214  const ColBlockIndices& inDers () const
215  {
216  return inDers_;
217  }
218 
222  const RowBlockIndices& notOutArgs () const
223  {
224  return notOutArgs_;
225  }
226 
230  const ColBlockIndices& notOutDers () const
231  {
232  return notOutDers_;
233  }
234 
236  ColBlockIndices activeParameters () const;
237 
239  const ColBlockIndices& activeDerivativeParameters () const;
240 
246  const Eigen::MatrixXi& inOutDependencies () const
247  {
248  return inOutDependencies_;
249  }
250 
252  Eigen::MatrixXi inOutDofDependencies () const;
253 
254  const Eigen::VectorXi& derFunction () const
255  {
256  return derFunction_;
257  }
258 
263  const RowBlockIndices& outArgs () const
264  {
265  return outArgs_;
266  }
267 
272  const RowBlockIndices& outDers () const
273  {
274  return outDers_;
275  }
276 
279  {
280  return configSpace_;
281  }
282 
284  std::size_t nq () const
285  {
286  return configSpace_->nq ();
287  }
288 
290  std::size_t nv () const
291  {
292  return configSpace_->nv ();
293  }
294 
296 
310  {
311  return MatrixBlockView(jacobian,
312  outDers_.nbIndices() , outDers_.indices(),
313  notOutDers_.nbIndices(), notOutDers_.indices());
314  }
315 
332  void jacobian(matrixOut_t jacobian, vectorIn_t q) const;
333 
336 
356  vector_t rightHandSideFromInput (vectorIn_t p);
357 
378  bool rightHandSideFromInput (const ExplicitPtr_t& constraint,
379  vectorIn_t p);
380 
401  void rightHandSideFromInput (const size_type& i, vectorIn_t p);
402 
409  void rightHandSide (vectorIn_t rhs);
410 
415  bool rightHandSide (const ExplicitPtr_t& constraint, vectorIn_t rhs);
416 
421  bool getRightHandSide (const ExplicitPtr_t& constraint, vectorOut_t rhs) const ;
422 
427  void rightHandSide (const size_type& i, vectorIn_t rhs);
428 
432  vector_t rightHandSide () const;
433 
437  size_type rightHandSideSize () const;
438 
440 
441  std::ostream& print (std::ostream& os) const;
442 
443  private:
444  typedef std::vector<bool> Computed_t;
445 
450  void solveExplicitConstraint(const std::size_t& i, vectorOut_t arg)
451  const;
466  void computeJacobian(const std::size_t& i, matrixOut_t J) const;
467  void computeOrder(const std::size_t& iF, std::size_t& iOrder, Computed_t& computed);
468 
469  LiegroupSpacePtr_t configSpace_;
470 
471  struct Data {
472  Data (const ExplicitPtr_t& constraint);
473  ExplicitPtr_t constraint;
474  RowBlockIndices equalityIndices;
475  vector_t rhs_implicit;
476  // implicit formulation
477  mutable LiegroupElement h_value;
478  // explicit formulation
479  mutable vector_t qin, qout;
480  mutable LiegroupElement f_value, res_qout;
481  // jacobian of f
482  mutable matrix_t jacobian;
483  }; // struct Data
484 
485  RowBlockIndices inArgs_, notOutArgs_;
486  ColBlockIndices inDers_, notOutDers_;
488  RowBlockIndices outArgs_, outDers_;
489 
490  Eigen::MatrixXi inOutDependencies_;
491 
492  std::vector<Data> data_;
493  std::vector<std::size_t> computationOrder_;
499  Eigen::VectorXi argFunction_, derFunction_;
500  value_type squaredErrorThreshold_;
501  size_type errorSize_;
502  // mutable matrix_t Jg;
503  mutable vector_t arg_, diff_, diffSmall_;
504 
506  ExplicitConstraintSet()
507  : inArgs_ (), notOutArgs_ ()
508  , inDers_ (), notOutDers_ ()
509  , outArgs_ (), outDers_ ()
510  , squaredErrorThreshold_ (Eigen::NumTraits<value_type>::epsilon())
511  , errorSize_(0)
512  {}
514  void init(const LiegroupSpacePtr_t& space)
515  {
516  configSpace_ = space;
517  argFunction_ = Eigen::VectorXi::Constant(space->nq (), -1);
518  derFunction_ = Eigen::VectorXi::Constant(space->nv (), -1);
519  arg_.resize(space->nq ());
520  diff_.resize(space->nv ());
521 
522  notOutArgs_.addRow(0, space->nq ());
523  notOutDers_.addCol(0, space->nv ());
524  }
525 
527  }; // class ExplicitConstraintSet
529  } // namespace constraints
530 } // namespace hpp
531 
532 #endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
hpp::constraints::ExplicitConstraintSet::outArgs
const RowBlockIndices & outArgs() const
Definition: explicit-constraint-set.hh:263
Eigen
Definition: fwd.hh:27
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition: fwd.hh:47
hpp::constraints::ExplicitConstraintSet::errorThreshold
value_type errorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:192
hpp::constraints::ExplicitConstraintSet::MatrixBlockView
Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > MatrixBlockView
Definition: explicit-constraint-set.hh:98
hpp::constraints::ExplicitConstraintSet::nq
std::size_t nq() const
The number of variables.
Definition: explicit-constraint-set.hh:284
hpp::constraints::ExplicitConstraintSet
Definition: explicit-constraint-set.hh:93
hpp::constraints::ExplicitConstraintSet::notOutDers
const ColBlockIndices & notOutDers() const
Definition: explicit-constraint-set.hh:230
hpp::constraints::ExplicitConstraintSet::errorThreshold
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: explicit-constraint-set.hh:187
hpp::constraints::LiegroupSpacePtr_t
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
Eigen::MatrixBlocks< false, true >
differentiable-function-set.hh
hpp::constraints::ExplicitConstraintSet::configSpace
LiegroupSpacePtr_t configSpace() const
The Lie group on which constraints are defined.
Definition: explicit-constraint-set.hh:278
hpp::constraints::LiegroupElement
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
matrix-view.hh
hpp::constraints::ExplicitConstraintSet::jacobianNotOutToOut
MatrixBlockView jacobianNotOutToOut(matrix_t &jacobian) const
Definition: explicit-constraint-set.hh:309
hpp::constraints::ExplicitConstraintSet::ExplicitConstraintSet
ExplicitConstraintSet(const LiegroupSpacePtr_t &space)
Definition: explicit-constraint-set.hh:165
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
fwd.hh
Eigen::MatrixBlockView
Definition: matrix-view.hh:111
hpp::constraints::ExplicitConstraintSet::inOutDependencies
const Eigen::MatrixXi & inOutDependencies() const
Definition: explicit-constraint-set.hh:246
hpp::constraints::ExplicitConstraintSet::ColBlockIndices
Eigen::ColBlockIndices ColBlockIndices
Definition: explicit-constraint-set.hh:97
hpp::constraints::ExplicitConstraintSet::derFunction
const Eigen::VectorXi & derFunction() const
Definition: explicit-constraint-set.hh:254
Eigen::RowBlockIndices
Eigen::MatrixBlocks< false, true > RowBlockIndices
Definition: matrix-view.hh:782
hpp::constraints::ExplicitConstraintSet::nv
std::size_t nv() const
The number of derivative variables.
Definition: explicit-constraint-set.hh:290
hpp::constraints::solver::NumTraits
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:24
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
hpp::constraints::solver::BySubstitution
Definition: by-substitution.hh:62
hpp
Definition: active-set-differentiable-function.hh:24
Eigen::ColBlockIndices
Eigen::MatrixBlocks< true, false > ColBlockIndices
Definition: matrix-view.hh:783
hpp::constraints::ExplicitPtr_t
boost::shared_ptr< Explicit > ExplicitPtr_t
Definition: fwd.hh:179
hpp::constraints::size_type
pinocchio::size_type size_type
Definition: fwd.hh:36
hpp::constraints::ExplicitConstraintSet::squaredErrorThreshold
value_type squaredErrorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:197
hpp::constraints::ExplicitConstraintSet::outDers
const RowBlockIndices & outDers() const
Definition: explicit-constraint-set.hh:272
hpp::constraints::ExplicitConstraintSet::errorSize
size_type errorSize() const
Definition: explicit-constraint-set.hh:118
hpp::constraints::value_type
pinocchio::value_type value_type
Definition: fwd.hh:37
hpp::constraints::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
hpp::constraints::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
hpp::constraints::ExplicitConstraintSet::RowBlockIndices
Eigen::RowBlockIndices RowBlockIndices
Definition: explicit-constraint-set.hh:96
hpp::constraints::ExplicitConstraintSet::notOutArgs
const RowBlockIndices & notOutArgs() const
Definition: explicit-constraint-set.hh:222
hpp::constraints::ImplicitPtr_t
boost::shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
hpp::constraints::ExplicitConstraintSet::inArgs
const RowBlockIndices & inArgs() const
Set of input configuration variables.
Definition: explicit-constraint-set.hh:208
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
config.hh
hpp::constraints::ExplicitConstraintSet::inDers
const ColBlockIndices & inDers() const
Set of input velocity variables.
Definition: explicit-constraint-set.hh:214