hpp-core: include/hpp/core/constraint-set.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
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
36namespace hpp {
37namespace core {
40
47 class HPP_CORE_DLLAPI ConstraintSet : public Constraint {
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
59 static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t& cs) {
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
146 ConstraintSet() {}
147
148 private:
149 ConfigProjectorPtr_t _configProj() const;
150
151 Constraints_t constraints_;
152 int configProjI_;
153 ConstraintSetWkPtr_t weak_;
154
155 friend class constraints::LockedJoint;
156 friend class Constraint;
157 friend class ConfigProjector;
158
159 HPP_SERIALIZABLE();
160}; // class ConstraintSet
162} // namespace core
163} // namespace hpp
164
165BOOST_CLASS_EXPORT_KEY(hpp::core::ConstraintSet)
166
167#endif // HPP_CORE_CONSTRAINT_SET_HH
hpp::constraints::LockedJoint
hpp::core::ConfigProjector
Definition config-projector.hh:66
hpp::core::ConstraintSet
Definition constraint-set.hh:47
hpp::core::ConstraintSet::ConstraintSet
ConstraintSet(const DevicePtr_t &robot, const std::string &name)
hpp::core::ConstraintSet::isSatisfied
virtual bool isSatisfied(ConfigurationIn_t config, vector_t &error)
hpp::core::ConstraintSet::addConstraint
void addConstraint(const ConstraintPtr_t &constraint)
Add a constraint to the set.
hpp::core::ConstraintSet::compressMatrix
void compressMatrix(matrixIn_t normal, matrixOut_t small, bool rows=true) const
hpp::core::ConstraintSet::ConstraintSet
ConstraintSet(const ConstraintSet &other)
Copy constructor.
hpp::core::ConstraintSet::begin
Constraints_t::iterator begin()
Iterator over the constraints.
Definition constraint-set.hh:76
hpp::core::ConstraintSet::print
virtual std::ostream & print(std::ostream &os) const
hpp::core::ConstraintSet::numberNonLockedDof
size_type numberNonLockedDof() const
Get number of non-locked degrees of freedom.
hpp::core::ConstraintSet::compressVector
void compressVector(vectorIn_t normal, vectorOut_t small) const
hpp::core::ConstraintSet::uncompressVector
void uncompressVector(vectorIn_t small, vectorOut_t normal) const
hpp::core::ConstraintSet::configProjector
ConfigProjectorPtr_t configProjector() const
Return pointer to config projector if any.
hpp::core::ConstraintSet::ConstraintSet
ConstraintSet()
Constructor for serialization only.
Definition constraint-set.hh:146
hpp::core::ConstraintSet::init
void init(const ConstraintSetPtr_t &self)
Store weak pointer to itself.
Definition constraint-set.hh:137
hpp::core::ConstraintSet::end
Constraints_t::iterator end()
Iterator over the constraints.
Definition constraint-set.hh:78
hpp::core::ConstraintSet::isSatisfied
virtual bool isSatisfied(ConfigurationIn_t config)
Check whether a configuration statisfies the constraint.
hpp::core::ConstraintSet::impl_compute
virtual bool impl_compute(ConfigurationOut_t configuration)
User defined implementation of the constraint.
hpp::core::ConstraintSet::create
static ConstraintSetPtr_t create(const DevicePtr_t &robot, const std::string &name)
Return shared pointer to new object.
Definition constraint-set.hh:50
hpp::core::ConstraintSet::copy
virtual ConstraintPtr_t copy() const
return shared pointer to copy
hpp::core::ConstraintSet::createCopy
static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t &cs)
Return shared pointer to new object.
Definition constraint-set.hh:59
hpp::core::ConstraintSet::uncompressMatrix
void uncompressMatrix(matrixIn_t small, matrixOut_t normal, bool rows=true) const
hpp::core::Constraint
Definition constraint.hh:49
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
constraint.hh
hpp
hpp::core::ConstraintPtr_t
shared_ptr< Constraint > ConstraintPtr_t
Definition fwd.hh:129
hpp::core::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:221
hpp::core::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:222
hpp::core::ConfigurationOut_t
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition fwd.hh:109
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::core::Constraints_t
std::vector< ConstraintPtr_t > Constraints_t
Definition fwd.hh:132
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::ConfigProjectorPtr_t
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition fwd.hh:114
hpp::core::matrixOut_t
constraints::matrixOut_t matrixOut_t
Definition fwd.hh:167
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::matrixIn_t
constraints::matrixIn_t matrixIn_t
Definition fwd.hh:166
hpp::core::ConstraintSetPtr_t
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition fwd.hh:130
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()