hpp-core  4.14.0
Implement basic classes for canonical path planning for kinematic chains.
constraint-set.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: 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_CORE_CONSTRAINT_SET_HH
31 #define HPP_CORE_CONSTRAINT_SET_HH
32 
33 #include <deque>
34 #include <hpp/core/constraint.hh>
35 
36 namespace hpp {
37 namespace core {
40 
48  public:
50  static ConstraintSetPtr_t create(const DevicePtr_t& robot,
51  const std::string& name) {
52  ConstraintSet* ptr = new ConstraintSet(robot, name);
53  ConstraintSetPtr_t shPtr(ptr);
54  ptr->init(shPtr);
55  return shPtr;
56  }
57 
60  ConstraintSet* ptr = new ConstraintSet(*cs);
61  ConstraintSetPtr_t shPtr(ptr);
62  ptr->init(shPtr);
63  return shPtr;
64  }
65 
67  virtual ConstraintPtr_t copy() const;
68 
70  void addConstraint(const ConstraintPtr_t& constraint);
71 
73  ConfigProjectorPtr_t configProjector() const;
74 
76  Constraints_t::iterator begin() { return constraints_.begin(); }
78  Constraints_t::iterator end() { return constraints_.end(); }
79 
81  virtual bool isSatisfied(ConfigurationIn_t config);
82 
87  virtual bool isSatisfied(ConfigurationIn_t config, vector_t& error);
88 
96 
98  size_type numberNonLockedDof() const;
99 
104  void compressVector(vectorIn_t normal, vectorOut_t small) const;
105 
113  void uncompressVector(vectorIn_t small, vectorOut_t normal) const;
114 
120  void compressMatrix(matrixIn_t normal, matrixOut_t small,
121  bool rows = true) const;
122 
128  void uncompressMatrix(matrixIn_t small, matrixOut_t normal,
129  bool rows = true) const;
131 
132  protected:
133  ConstraintSet(const DevicePtr_t& robot, const std::string& name);
135  ConstraintSet(const ConstraintSet& other);
137  void init(const ConstraintSetPtr_t& self) {
138  Constraint::init(self);
139  weak_ = self;
140  }
141  virtual bool impl_compute(ConfigurationOut_t configuration);
142 
143  virtual std::ostream& print(std::ostream& os) const;
144 
147 
148  private:
149  ConfigProjectorPtr_t _configProj() const;
150 
151  Constraints_t constraints_;
152  int configProjI_;
153  ConstraintSetWkPtr_t weak_;
154 
156  friend class Constraint;
157  friend class ConfigProjector;
158 
159  HPP_SERIALIZABLE();
160 }; // class ConstraintSet
162 } // namespace core
163 } // namespace hpp
164 
165 BOOST_CLASS_EXPORT_KEY(hpp::core::ConstraintSet)
166 
167 #endif // HPP_CORE_CONSTRAINT_SET_HH
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:133
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
Definition: bi-rrt-planner.hh:35
static ConstraintSetPtr_t create(const DevicePtr_t &robot, const std::string &name)
Return shared pointer to new object.
Definition: constraint-set.hh:50
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:107
pinocchio::size_type size_type
Definition: fwd.hh:172
std::vector< ConstraintPtr_t > Constraints_t
Definition: fwd.hh:131
ConstraintSet()
Constructor for serialization only.
Definition: constraint-set.hh:146
Constraints_t::iterator end()
Iterator over the constraints.
Definition: constraint-set.hh:78
Definition: config-projector.hh:66
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition: fwd.hh:113
Definition: constraint.hh:49
void init(const ConstraintPtr_t &self)
Store shared pointer to itself.
Definition: constraint.hh:82
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:220
constraints::matrixIn_t matrixIn_t
Definition: fwd.hh:165
static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t &cs)
Return shared pointer to new object.
Definition: constraint-set.hh:59
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:129
pinocchio::vector_t vector_t
Definition: fwd.hh:219
void init(const ConstraintSetPtr_t &self)
Store weak pointer to itself.
Definition: constraint-set.hh:137
constraints::matrixOut_t matrixOut_t
Definition: fwd.hh:166
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:221
Constraints_t::iterator begin()
Iterator over the constraints.
Definition: constraint-set.hh:76
Definition: constraint-set.hh:47
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:128
constraints::LockedJoint LockedJoint
Definition: fwd.hh:157