hpp-core  4.11.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 
65  ConfigProjectorPtr_t configProjector () const;
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 
94  size_type numberNonLockedDof () const;
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 
124  void uncompressMatrix (matrixIn_t small, matrixOut_t normal,
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
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:97
Definition: bi-rrt-planner.hh:24
static ConstraintSetPtr_t create(const DevicePtr_t &robot, const std::string &name)
Return shared pointer to new object.
Definition: constraint-set.hh:40
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::size_type size_type
Definition: fwd.hh:156
std::vector< ConstraintPtr_t > Constraints_t
Definition: fwd.hh:112
ConstraintSet()
Constructor for serialization only.
Definition: constraint-set.hh:143
Constraints_t::iterator end()
Iterator over the constraints.
Definition: constraint-set.hh:72
Definition: config-projector.hh:57
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition: fwd.hh:104
Definition: constraint.hh:38
void init(const ConstraintPtr_t &self)
Store shared pointer to itself.
Definition: constraint.hh:80
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:203
constraints::matrixIn_t matrixIn_t
Definition: fwd.hh:149
static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t &cs)
Return shared pointer to new object.
Definition: constraint-set.hh:50
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
pinocchio::vector_t vector_t
Definition: fwd.hh:202
void init(const ConstraintSetPtr_t &self)
Store weak pointer to itself.
Definition: constraint-set.hh:133
constraints::matrixOut_t matrixOut_t
Definition: fwd.hh:150
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:204
Constraints_t::iterator begin()
Iterator over the constraints.
Definition: constraint-set.hh:68
Definition: constraint-set.hh:36
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:109
constraints::LockedJoint LockedJoint
Definition: fwd.hh:141