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

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

Loading...
Searching...
No Matches
implicit.hh
Go to the documentation of this file.
1// Copyright (c) 2015 - 2018 CNRS
2// Authors: Joseph Mirabel (joseph.mirabel@laas.fr), Florent Lamiraux
3//
4
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_CONSTRAINTS_IMPLICIT_HH
30#define HPP_CONSTRAINTS_IMPLICIT_HH
31
32#include <hpp/constraints/comparison-types.hh>
33#include <hpp/constraints/config.hh>
34#include <hpp/constraints/fwd.hh>
35#include <hpp/constraints/matrix-view.hh>
36#include <hpp/pinocchio/liegroup-element.hh>
37#include <hpp/util/serialization-fwd.hh>
38
39 namespace hpp {
40 namespace constraints {
119 class HPP_CONSTRAINTS_DLLAPI Implicit {
120 public:
122 bool operator==(const Implicit& other) const;
124 Implicit& operator=(const Implicit& other) {
125 comparison_ = other.comparison_;
126 rhs_ = other.rhs_;
127 parameterSize_ = other.parameterSize_;
128 function_ = other.function_;
129 rhsFunction_ = other.rhsFunction_;
130 mask_ = other.mask_;
131 activeRows_ = other.activeRows_;
132 inactiveRows_ = other.inactiveRows_;
133 inequalityIndices_ = other.inequalityIndices_;
134 equalityIndices_ = other.equalityIndices_;
135 return *this;
136 }
138 virtual ImplicitPtr_t copy() const;
141 static ImplicitPtr_t create(const DifferentiableFunctionPtr_t& func,
142 ComparisonTypes_t comp,
143 std::vector<bool> mask = std::vector<bool>());
144
146 static ImplicitPtr_t createCopy(const ImplicitPtr_t& other);
147
148 virtual ~Implicit() {};
149
152
166 void rightHandSideFromConfig(ConfigurationIn_t config,
167 LiegroupElementRef rhs);
168
176 bool checkRightHandSide(LiegroupElementConstRef rhs) const;
177
181 size_type parameterSize() const;
182
186 size_type rightHandSideSize() const;
187
192 void rightHandSideFunction(const DifferentiableFunctionPtr_t& rhsF);
193
198 const DifferentiableFunctionPtr_t& rightHandSideFunction() const {
199 return rhsFunction_;
200 }
201
204 vectorIn_t rightHandSideAt(const value_type& s);
205
207
209 const ComparisonTypes_t& comparisonType() const;
210
212 void comparisonType(const ComparisonTypes_t& comp);
213
216 const segments_t& activeRows() const { return activeRows_; }
217
219 bool checkAllRowsActive() const { return inactiveRows_.nbRows() == 0; }
220
222 const Eigen::RowBlockIndices& equalityIndices() const {
223 return equalityIndices_;
224 }
225
229 void setInactiveRowsToZero(vectorOut_t error) const;
230
232 DifferentiableFunction& function() const { return *function_; }
233
235 const DifferentiableFunctionPtr_t& functionPtr() const { return function_; }
236
249 virtual std::pair<JointConstPtr_t, JointConstPtr_t>
250 doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const;
251
252 protected:
262 Implicit(const DifferentiableFunctionPtr_t& function, ComparisonTypes_t comp,
263 std::vector<bool> mask);
264
266 Implicit(const Implicit& other);
267
271 virtual bool isEqual(const Implicit& other, bool swapAndTest) const;
272
273 // Store weak pointer to itself
274 void init(const ImplicitWkPtr_t& weak) { weak_ = weak; }
275
276 friend class ImplicitConstraintSet;
277
278 private:
279 void computeIndices();
280 void computeActiveRows();
281 ComparisonTypes_t comparison_;
282 vector_t rhs_;
283 size_type parameterSize_;
284 DifferentiableFunctionPtr_t function_;
285 DifferentiableFunctionPtr_t rhsFunction_;
286 std::vector<bool> mask_;
287 segments_t activeRows_;
288 Eigen::RowBlockIndices inactiveRows_;
289 std::vector<std::size_t> inequalityIndices_;
290 Eigen::RowBlockIndices equalityIndices_;
291 ImplicitWkPtr_t weak_;
292 // To avoid dynamic memory allocation
293 mutable LiegroupElement output_;
294 mutable vector_t logOutput_;
295
296 protected:
297 Implicit() {}
298
299 private:
300 HPP_SERIALIZABLE();
301}; // class Implicit
303} // namespace constraints
304} // namespace hpp
305
306#endif // HPP_CONSTRAINTS_IMPLICIT_HH
Eigen::MatrixBlocks< false, true >
hpp::constraints::DifferentiableFunction
Definition differentiable-function.hh:63
hpp::constraints::ImplicitConstraintSet
Definition implicit-constraint-set.hh:45
hpp::constraints::Implicit
Definition implicit.hh:119
hpp::constraints::Implicit::create
static ImplicitPtr_t create(const DifferentiableFunctionPtr_t &func, ComparisonTypes_t comp, std::vector< bool > mask=std::vector< bool >())
hpp::constraints::Implicit::checkRightHandSide
bool checkRightHandSide(LiegroupElementConstRef rhs) const
hpp::constraints::Implicit::rightHandSideFromConfig
void rightHandSideFromConfig(ConfigurationIn_t config, LiegroupElementRef rhs)
hpp::constraints::Implicit::setInactiveRowsToZero
void setInactiveRowsToZero(vectorOut_t error) const
hpp::constraints::Implicit::doesConstrainRelPoseBetween
virtual std::pair< JointConstPtr_t, JointConstPtr_t > doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const
hpp::constraints::Implicit::~Implicit
virtual ~Implicit()
Definition implicit.hh:148
hpp::constraints::Implicit::function
DifferentiableFunction & function() const
Return a reference to function .
Definition implicit.hh:232
hpp::constraints::Implicit::rightHandSideFunction
const DifferentiableFunctionPtr_t & rightHandSideFunction() const
Definition implicit.hh:198
hpp::constraints::Implicit::rightHandSideFunction
void rightHandSideFunction(const DifferentiableFunctionPtr_t &rhsF)
hpp::constraints::Implicit::isEqual
virtual bool isEqual(const Implicit &other, bool swapAndTest) const
hpp::constraints::Implicit::operator==
bool operator==(const Implicit &other) const
Operator equality.
hpp::constraints::Implicit::copy
virtual ImplicitPtr_t copy() const
Copy object and return shared pointer to copy.
hpp::constraints::Implicit::Implicit
Implicit(const Implicit &other)
Copy constructor.
hpp::constraints::Implicit::functionPtr
const DifferentiableFunctionPtr_t & functionPtr() const
Return a reference to function .
Definition implicit.hh:235
hpp::constraints::Implicit::parameterSize
size_type parameterSize() const
hpp::constraints::Implicit::activeRows
const segments_t & activeRows() const
Definition implicit.hh:216
hpp::constraints::Implicit::equalityIndices
const Eigen::RowBlockIndices & equalityIndices() const
Get indices of constraint coordinates that are equality.
Definition implicit.hh:222
hpp::constraints::Implicit::comparisonType
void comparisonType(const ComparisonTypes_t &comp)
Set the comparison type.
hpp::constraints::Implicit::operator=
Implicit & operator=(const Implicit &other)
Operator assignation.
Definition implicit.hh:124
hpp::constraints::Implicit::rightHandSideAt
vectorIn_t rightHandSideAt(const value_type &s)
hpp::constraints::Implicit::rightHandSideSize
size_type rightHandSideSize() const
hpp::constraints::Implicit::Implicit
Implicit()
Definition implicit.hh:297
hpp::constraints::Implicit::Implicit
Implicit(const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, std::vector< bool > mask)
hpp::constraints::Implicit::init
void init(const ImplicitWkPtr_t &weak)
Definition implicit.hh:274
hpp::constraints::Implicit::checkAllRowsActive
bool checkAllRowsActive() const
Check if all rows are active (no inactive rows)
Definition implicit.hh:219
hpp::constraints::Implicit::comparisonType
const ComparisonTypes_t & comparisonType() const
Return the ComparisonType.
hpp::constraints::Implicit::createCopy
static ImplicitPtr_t createCopy(const ImplicitPtr_t &other)
Create a copy and return shared pointer.
hpp::pinocchio::LiegroupElementBase
hpp::pinocchio::LiegroupElementConstBase
comparison-types.hh
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
fwd.hh
liegroup-element.hh
matrix-view.hh
hpp::constraints::ComparisonTypes_t
std::vector< ComparisonType > ComparisonTypes_t
Definition fwd.hh:182
hpp::constraints::DeviceConstPtr_t
pinocchio::DeviceConstPtr_t DeviceConstPtr_t
Definition fwd.hh:112
hpp::constraints::DifferentiableFunctionPtr_t
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition fwd.hh:115
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
Definition fwd.hh:175
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::value_type
pinocchio::value_type value_type
Definition fwd.hh:49
hpp::constraints::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:61
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::constraints::segments_t
std::vector< segment_t > segments_t
Definition fwd.hh:86
hpp::constraints::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:62
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:60
hpp
serialization-fwd.hh
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()