hpp-constraints  4.14.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 
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_EXPLICIT_CONSTRAINT_SET_HH
30 #define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
31 
34 #include <hpp/constraints/fwd.hh>
36 #include <vector>
37 
38 namespace hpp {
39 namespace constraints {
42 
100  public:
103  typedef Eigen::MatrixBlockView<matrix_t, Eigen::Dynamic, Eigen::Dynamic,
104  false, false>
106 
109 
116  bool solve(vectorOut_t arg) const;
117 
121  size_type errorSize() const { return errorSize_; }
122 
128  bool isSatisfied(vectorIn_t arg, value_type errorThreshold = -1) const;
129 
136  bool isSatisfied(vectorIn_t arg, vectorOut_t error,
137  value_type errorThreshold = -1) const;
138 
148  bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
149  vectorOut_t error, bool& constraintFound) const;
151 
154 
162  size_type add(const ExplicitPtr_t& constraint);
163 
170  bool contains(const ExplicitPtr_t& numericalConstraint) const;
171 
176  : configSpace_(space),
177  inArgs_(),
178  notOutArgs_(),
179  inDers_(),
180  notOutDers_(),
181  outArgs_(),
182  outDers_(),
183  argFunction_(Eigen::VectorXi::Constant(space->nq(), -1)),
184  derFunction_(Eigen::VectorXi::Constant(space->nv(), -1)),
185  errorThreshold_(Eigen::NumTraits<value_type>::epsilon()),
186  errorSize_(0)
187  // , Jg (nv, nv)
188  ,
189  arg_(space->nq()),
190  diff_(space->nv()),
191  diffSmall_() {
192  notOutArgs_.addRow(0, space->nq());
193  notOutDers_.addCol(0, space->nv());
194  }
195 
197 
200 
202  void errorThreshold(const value_type& threshold) {
203  errorThreshold_ = threshold;
204  }
206  value_type errorThreshold() const { return errorThreshold_; }
209  return errorThreshold_ * errorThreshold_;
210  }
211 
213 
216 
218  const RowBlockIndices& inArgs() const { return inArgs_; }
219 
221  const ColBlockIndices& inDers() const { return inDers_; }
222 
226  const RowBlockIndices& notOutArgs() const { return notOutArgs_; }
227 
231  const ColBlockIndices& notOutDers() const { return notOutDers_; }
232 
234  ColBlockIndices activeParameters() const;
235 
237  const ColBlockIndices& activeDerivativeParameters() const;
238 
244  const Eigen::MatrixXi& inOutDependencies() const {
245  return inOutDependencies_;
246  }
247 
249  Eigen::MatrixXi inOutDofDependencies() const;
250 
251  const Eigen::VectorXi& derFunction() const { return derFunction_; }
252 
257  const RowBlockIndices& outArgs() const { return outArgs_; }
258 
263  const RowBlockIndices& outDers() const { return outDers_; }
264 
266  LiegroupSpacePtr_t configSpace() const { return configSpace_; }
267 
269  std::size_t nq() const { return configSpace_->nq(); }
270 
272  std::size_t nv() const { return configSpace_->nv(); }
273 
275 
288  inline MatrixBlockView jacobianNotOutToOut(matrix_t& jacobian) const {
289  return MatrixBlockView(jacobian, outDers_.nbIndices(), outDers_.indices(),
290  notOutDers_.nbIndices(), notOutDers_.indices());
291  }
292 
309  void jacobian(matrixOut_t jacobian, vectorIn_t q) const;
310 
313 
333  vector_t rightHandSideFromInput(vectorIn_t p);
334 
355  bool rightHandSideFromInput(const ExplicitPtr_t& constraint, vectorIn_t p);
356 
377  void rightHandSideFromInput(const size_type& i, vectorIn_t p);
378 
385  void rightHandSide(vectorIn_t rhs);
386 
391  bool rightHandSide(const ExplicitPtr_t& constraint, vectorIn_t rhs);
392 
397  bool getRightHandSide(const ExplicitPtr_t& constraint, vectorOut_t rhs) const;
398 
403  void rightHandSide(const size_type& i, vectorIn_t rhs);
404 
408  vector_t rightHandSide() const;
409 
413  size_type rightHandSideSize() const;
414 
416 
417  std::ostream& print(std::ostream& os) const;
418 
419  private:
420  typedef std::vector<bool> Computed_t;
421 
426  void solveExplicitConstraint(const std::size_t& i, vectorOut_t arg) const;
441  void computeJacobian(const std::size_t& i, matrixOut_t J) const;
442  void computeOrder(const std::size_t& iF, std::size_t& iOrder,
443  Computed_t& computed);
444 
445  LiegroupSpacePtr_t configSpace_;
446 
447  struct Data {
448  Data(const ExplicitPtr_t& constraint);
449  ExplicitPtr_t constraint;
450  RowBlockIndices equalityIndices;
451  LiegroupElement rhs_implicit;
452  // implicit formulation
453  mutable LiegroupElement h_value;
454  // explicit formulation
455  mutable vector_t qin, qout;
456  mutable LiegroupElement f_value, res_qout;
457  // jacobian of f
458  mutable matrix_t jacobian;
459  }; // struct Data
460 
461  RowBlockIndices inArgs_, notOutArgs_;
462  ColBlockIndices inDers_, notOutDers_;
464  RowBlockIndices outArgs_, outDers_;
465 
466  Eigen::MatrixXi inOutDependencies_;
467 
468  std::vector<Data> data_;
469  std::vector<std::size_t> computationOrder_;
475  Eigen::VectorXi argFunction_, derFunction_;
476  value_type errorThreshold_;
477  size_type errorSize_;
478  // mutable matrix_t Jg;
479  mutable vector_t arg_, diff_, diffSmall_;
480 
483  : inArgs_(),
484  notOutArgs_(),
485  inDers_(),
486  notOutDers_(),
487  outArgs_(),
488  outDers_(),
489  errorThreshold_(Eigen::NumTraits<value_type>::epsilon()),
490  errorSize_(0) {}
492  void init(const LiegroupSpacePtr_t& space) {
493  configSpace_ = space;
494  argFunction_ = Eigen::VectorXi::Constant(space->nq(), -1);
495  derFunction_ = Eigen::VectorXi::Constant(space->nv(), -1);
496  arg_.resize(space->nq());
497  diff_.resize(space->nv());
498 
499  notOutArgs_.addRow(0, space->nq());
500  notOutDers_.addCol(0, space->nv());
501  }
502 
504 }; // class ExplicitConstraintSet
506 } // namespace constraints
507 } // namespace hpp
508 
509 #endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
void addCol(const size_type &col, const size_type size)
Definition: matrix-view.hh:648
pinocchio::vector_t vector_t
Definition: fwd.hh:59
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: explicit-constraint-set.hh:202
std::size_t nq() const
The number of variables.
Definition: explicit-constraint-set.hh:269
Definition: active-set-differentiable-function.hh:36
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:69
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:65
const RowBlockIndices & inArgs() const
Set of input configuration variables.
Definition: explicit-constraint-set.hh:218
Definition: fwd.hh:38
const RowBlockIndices & notOutArgs() const
Definition: explicit-constraint-set.hh:226
pinocchio::matrix_t matrix_t
Definition: fwd.hh:56
void addRow(const size_type &row, const size_type size)
Definition: matrix-view.hh:640
LiegroupSpacePtr_t configSpace() const
The Lie group on which constraints are defined.
Definition: explicit-constraint-set.hh:266
size_type errorSize() const
Definition: explicit-constraint-set.hh:121
std::size_t nv() const
The number of derivative variables.
Definition: explicit-constraint-set.hh:272
Eigen::MatrixBlockView< matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false > MatrixBlockView
Definition: explicit-constraint-set.hh:105
shared_ptr< Explicit > ExplicitPtr_t
Definition: fwd.hh:185
ExplicitConstraintSet(const LiegroupSpacePtr_t &space)
Definition: explicit-constraint-set.hh:175
Definition: matrix-view.hh:124
const Eigen::MatrixXi & inOutDependencies() const
Definition: explicit-constraint-set.hh:244
const Eigen::VectorXi & derFunction() const
Definition: explicit-constraint-set.hh:251
value_type errorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:206
Eigen::ColBlockIndices ColBlockIndices
Definition: explicit-constraint-set.hh:102
Definition: by-substitution.hh:72
const RowBlockIndices & outDers() const
Definition: explicit-constraint-set.hh:263
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
Eigen::RowBlockIndices RowBlockIndices
Definition: explicit-constraint-set.hh:101
pinocchio::size_type size_type
Definition: fwd.hh:47
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:58
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:36
MatrixBlockView jacobianNotOutToOut(matrix_t &jacobian) const
Definition: explicit-constraint-set.hh:288
const ColBlockIndices & notOutDers() const
Definition: explicit-constraint-set.hh:231
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:175
pinocchio::value_type value_type
Definition: fwd.hh:48
const ColBlockIndices & inDers() const
Set of input velocity variables.
Definition: explicit-constraint-set.hh:221
const RowBlockIndices & outArgs() const
Definition: explicit-constraint-set.hh:257
Definition: explicit-constraint-set.hh:99
value_type squaredErrorThreshold() const
Get error threshold.
Definition: explicit-constraint-set.hh:208