hpp-constraints: include/hpp/constraints/implicit-constraint-set.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
implicit-constraint-set.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2016 - 2018 CNRS
3// Authors: Joseph Mirabel, Florent Lamiraux
4//
5
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are
8// met:
9//
10// 1. Redistributions of source code must retain the above copyright
11// notice, this list of conditions and the following disclaimer.
12//
13// 2. Redistributions in binary form must reproduce the above copyright
14// notice, this list of conditions and the following disclaimer in the
15// documentation and/or other materials provided with the distribution.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28// DAMAGE.
29
30#ifndef HPP_CONSTRAINTS_IMPLICIT_CONSTRAINT_SET_HH
31#define HPP_CONSTRAINTS_IMPLICIT_CONSTRAINT_SET_HH
32
33#include <hpp/constraints/differentiable-function-set.hh>
34#include <hpp/constraints/fwd.hh>
35#include <hpp/constraints/implicit.hh>
36
37 namespace hpp {
38 namespace constraints {
41
45 class HPP_CONSTRAINTS_DLLAPI ImplicitConstraintSet : public Implicit {
46 public:
47 typedef std::vector<ImplicitPtr_t> Implicits_t;
48
52 static ImplicitConstraintSetPtr_t create(const std::string& name) {
53 return ImplicitConstraintSetPtr_t(new ImplicitConstraintSet(name));
54 }
55
56 virtual ~ImplicitConstraintSet() {}
57
60
61 void add(const ImplicitPtr_t& constraint) {
62 assert(HPP_DYNAMIC_PTR_CAST(DifferentiableFunctionSet, function_));
63 DifferentiableFunctionSetPtr_t functions(
64 HPP_STATIC_PTR_CAST(DifferentiableFunctionSet, function_));
65 functions->add(constraint->functionPtr());
66 constraints_.push_back(constraint);
67 // Handle comparison types
68 const ComparisonTypes_t& comp(constraint->comparisonType());
69 for (std::size_t i = 0; i < comp.size(); ++i) {
70 comparison_.push_back(comp[i]);
71 }
72 // Handle mask
73 mask_.insert(mask_.end(), constraint->mask_.begin(),
74 constraint->mask_.end());
75 // Recompute active rows
76 computeActiveRows();
77 computeIndices();
78 // Resize temporary variables
79 output_ = LiegroupElement(functions->outputSpace());
80 logOutput_.resize(functions->outputSpace()->nv());
81 }
82
84 const Implicits_t& constraints() const { return constraints_; }
85
87 void merge(const ImplicitConstraintSetPtr_t& other) {
88 const Implicits_t& constraints = other->constraints();
89 for (Implicits_t::const_iterator constraint = constraints.begin();
90 constraint != constraints.end(); ++constraint)
91 add(*constraint);
92 }
93
95
96 std::ostream& print(std::ostream& os) const {
97 function_->print(os);
98 return os;
99 }
100
104 ImplicitConstraintSet(const std::string& name)
105 : Implicit(DifferentiableFunctionSet::create(name), ComparisonTypes_t(),
106 std::vector<bool>()) {}
107
108 ImplicitConstraintSet()
109 : Implicit(DifferentiableFunctionSet::create("Stack"),
110 ComparisonTypes_t(), std::vector<bool>()) {}
111
112 ImplicitConstraintSet(const ImplicitConstraintSet& o)
113 : Implicit(DifferentiableFunctionSet::create("Stack"),
114 ComparisonTypes_t(), std::vector<bool>()) {
115 const Implicits_t& constraints = o.constraints();
116 for (Implicits_t::const_iterator constraint = constraints.begin();
117 constraint != constraints.end(); ++constraint)
118 add(*constraint);
119 }
120
121 private:
124 void replace(const ConstraintReplacement_t& oldToNew) {
125 for (auto& c : constraints_) {
126 auto it = oldToNew.find(c);
127 if (it != oldToNew.end()) {
128 c = it->second;
129 }
130 }
131 }
132
133 Implicits_t constraints_;
134
135 HPP_SERIALIZABLE();
136 friend class solver::HierarchicalIterative;
137}; // class ImplicitConstraintSet
139} // namespace constraints
140} // namespace hpp
141
142#endif // HPP_CONSTRAINTS_IMPLICIT_CONSTRAINT_SET_HH
hpp::constraints::DifferentiableFunctionSet
Definition differentiable-function-set.hh:45
hpp::constraints::ImplicitConstraintSet
Definition implicit-constraint-set.hh:45
hpp::constraints::ImplicitConstraintSet::ImplicitConstraintSet
ImplicitConstraintSet(const ImplicitConstraintSet &o)
Definition implicit-constraint-set.hh:112
hpp::constraints::ImplicitConstraintSet::ImplicitConstraintSet
ImplicitConstraintSet()
Definition implicit-constraint-set.hh:108
hpp::constraints::ImplicitConstraintSet::print
std::ostream & print(std::ostream &os) const
Definition implicit-constraint-set.hh:96
hpp::constraints::ImplicitConstraintSet::constraints
const Implicits_t & constraints() const
Get constraints.
Definition implicit-constraint-set.hh:84
hpp::constraints::ImplicitConstraintSet::create
static ImplicitConstraintSetPtr_t create(const std::string &name)
Definition implicit-constraint-set.hh:52
hpp::constraints::ImplicitConstraintSet::Implicits_t
std::vector< ImplicitPtr_t > Implicits_t
Definition implicit-constraint-set.hh:47
hpp::constraints::ImplicitConstraintSet::add
void add(const ImplicitPtr_t &constraint)
Definition implicit-constraint-set.hh:61
hpp::constraints::ImplicitConstraintSet::merge
void merge(const ImplicitConstraintSetPtr_t &other)
The output columns selection of other is not taken into account.
Definition implicit-constraint-set.hh:87
hpp::constraints::ImplicitConstraintSet::ImplicitConstraintSet
ImplicitConstraintSet(const std::string &name)
Definition implicit-constraint-set.hh:104
hpp::constraints::ImplicitConstraintSet::~ImplicitConstraintSet
virtual ~ImplicitConstraintSet()
Definition implicit-constraint-set.hh:56
hpp::constraints::Implicit
Definition implicit.hh:119
hpp::constraints::solver::HierarchicalIterative
Definition hierarchical-iterative.hh:240
hpp::pinocchio::LiegroupElementBase
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
differentiable-function-set.hh
fwd.hh
implicit.hh
hpp::constraints::ComparisonTypes_t
std::vector< ComparisonType > ComparisonTypes_t
Definition fwd.hh:182
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
Definition fwd.hh:175
hpp::constraints::DifferentiableFunctionSetPtr_t
shared_ptr< DifferentiableFunctionSet > DifferentiableFunctionSetPtr_t
Definition fwd.hh:116
hpp::constraints::ImplicitConstraintSetPtr_t
shared_ptr< ImplicitConstraintSet > ImplicitConstraintSetPtr_t
Definition fwd.hh:179
hpp::constraints::ConstraintReplacement_t
std::map< ImplicitPtr_t, ImplicitPtr_t > ConstraintReplacement_t
Definition fwd.hh:189
hpp
std
HPP_DYNAMIC_PTR_CAST
#define HPP_DYNAMIC_PTR_CAST(t, x)
HPP_STATIC_PTR_CAST
#define HPP_STATIC_PTR_CAST(t, x)
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()