hpp-core  4.12.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 // This file is part of hpp-core
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CORE_CONSTRAINT_SET_HH
20 # define HPP_CORE_CONSTRAINT_SET_HH
21 
22 # include <deque>
23 # include <hpp/core/constraint.hh>
24 
25 namespace hpp {
26  namespace core {
29 
37  {
38  public:
40  static ConstraintSetPtr_t create (const DevicePtr_t& robot,
41  const std::string& name)
42  {
43  ConstraintSet* ptr = new ConstraintSet (robot, name);
44  ConstraintSetPtr_t shPtr (ptr);
45  ptr->init (shPtr);
46  return shPtr;
47  }
48 
51  {
52  ConstraintSet* ptr = new ConstraintSet (*cs);
53  ConstraintSetPtr_t shPtr (ptr);
54  ptr->init (shPtr);
55  return shPtr;
56  }
57 
59  virtual ConstraintPtr_t copy () const;
60 
62  void addConstraint (const ConstraintPtr_t& constraint);
63 
66 
68  Constraints_t::iterator begin () {
69  return constraints_.begin ();
70  }
72  Constraints_t::iterator end () {
73  return constraints_.end ();
74  }
75 
77  virtual bool isSatisfied (ConfigurationIn_t config);
78 
83  virtual bool isSatisfied (ConfigurationIn_t config, vector_t& error);
84 
92 
95 
100  void compressVector (vectorIn_t normal, vectorOut_t small) const;
101 
109  void uncompressVector (vectorIn_t small, vectorOut_t normal) const;
110 
116  void compressMatrix (matrixIn_t normal, matrixOut_t small,
117  bool rows = true) const;
118 
125  bool rows = true) const;
127 
128  protected:
129  ConstraintSet (const DevicePtr_t& robot, const std::string& name);
131  ConstraintSet (const ConstraintSet& other);
133  void init (const ConstraintSetPtr_t& self)
134  {
135  Constraint::init (self);
136  weak_ = self;
137  }
138  virtual bool impl_compute (ConfigurationOut_t configuration);
139 
140  virtual std::ostream& print (std::ostream& os) const;
141 
144 
145  private:
146  ConfigProjectorPtr_t _configProj () const;
147 
148  Constraints_t constraints_;
149  int configProjI_;
150  ConstraintSetWkPtr_t weak_;
151 
153  friend class Constraint;
154  friend class ConfigProjector;
155 
156  HPP_SERIALIZABLE();
157  }; // class ConstraintSet
159  } // namespace core
160 } // namespace hpp
161 
162 BOOST_CLASS_EXPORT_KEY(hpp::core::ConstraintSet)
163 
164 #endif // HPP_CORE_CONSTRAINT_SET_HH
Definition: config-projector.hh:58
Definition: constraint-set.hh:37
ConstraintSet(const DevicePtr_t &robot, const std::string &name)
virtual bool isSatisfied(ConfigurationIn_t config, vector_t &error)
void addConstraint(const ConstraintPtr_t &constraint)
Add a constraint to the set.
void compressMatrix(matrixIn_t normal, matrixOut_t small, bool rows=true) const
ConstraintSet(const ConstraintSet &other)
Copy constructor.
Constraints_t::iterator begin()
Iterator over the constraints.
Definition: constraint-set.hh:68
size_type numberNonLockedDof() const
Get number of non-locked degrees of freedom.
void compressVector(vectorIn_t normal, vectorOut_t small) const
void uncompressVector(vectorIn_t small, vectorOut_t normal) const
ConfigProjectorPtr_t configProjector() const
Return pointer to config projector if any.
ConstraintSet()
Constructor for serialization only.
Definition: constraint-set.hh:143
void init(const ConstraintSetPtr_t &self)
Store weak pointer to itself.
Definition: constraint-set.hh:133
Constraints_t::iterator end()
Iterator over the constraints.
Definition: constraint-set.hh:72
virtual bool isSatisfied(ConfigurationIn_t config)
Check whether a configuration statisfies the constraint.
virtual bool impl_compute(ConfigurationOut_t configuration)
User defined implementation of the constraint.
static ConstraintSetPtr_t create(const DevicePtr_t &robot, const std::string &name)
Return shared pointer to new object.
Definition: constraint-set.hh:40
virtual ConstraintPtr_t copy() const
return shared pointer to copy
static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t &cs)
Return shared pointer to new object.
Definition: constraint-set.hh:50
void uncompressMatrix(matrixIn_t small, matrixOut_t normal, bool rows=true) const
virtual std::ostream & print(std::ostream &os) const
Definition: constraint.hh:39
void init(const ConstraintPtr_t &self)
Store shared pointer to itself.
Definition: constraint.hh:80
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:109
constraints::LockedJoint LockedJoint
Definition: fwd.hh:141
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:203
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:204
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::vector_t vector_t
Definition: fwd.hh:202
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:97
std::vector< ConstraintPtr_t > Constraints_t
Definition: fwd.hh:112
pinocchio::size_type size_type
Definition: fwd.hh:156
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition: fwd.hh:104
constraints::matrixOut_t matrixOut_t
Definition: fwd.hh:150
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
constraints::matrixIn_t matrixIn_t
Definition: fwd.hh:149
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
Definition: bi-rrt-planner.hh:24