hpp-constraints 4.15.1
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
36#include <hpp/pinocchio/liegroup-element.hh>
37#include <hpp/util/serialization-fwd.hh>
38
39namespace hpp {
40namespace constraints {
125 public:
127 bool operator==(const Implicit& other) const;
129 virtual ImplicitPtr_t copy() const;
134 std::vector<bool> mask = std::vector<bool>());
135
138
139 virtual ~Implicit(){};
140
143
159
168
184 ConfigurationIn_t config);
185
192
198
203
208
214
220
226 return rhsFunction_;
227 }
228
232
234
237
240
243 const segments_t& activeRows() const { return activeRows_; }
244
246 bool checkAllRowsActive() const { return inactiveRows_.nbRows() == 0; }
247
250 return equalityIndices_;
251 }
252
257
259 DifferentiableFunction& function() const { return *function_; }
260
262 const DifferentiableFunctionPtr_t& functionPtr() const { return function_; }
263
276 virtual std::pair<JointConstPtr_t, JointConstPtr_t>
278
279 protected:
290 std::vector<bool> mask);
291
293 Implicit(const Implicit& other);
294
298 virtual bool isEqual(const Implicit& other, bool swapAndTest) const;
299
300 // Store weak pointer to itself
301 void init(const ImplicitWkPtr_t& weak) { weak_ = weak; }
302
304
305 private:
306 void computeIndices();
307 void computeActiveRows();
308 ComparisonTypes_t comparison_;
309 vector_t rhs_;
310 size_type parameterSize_;
312 DifferentiableFunctionPtr_t rhsFunction_;
313 std::vector<bool> mask_;
314 segments_t activeRows_;
315 Eigen::RowBlockIndices inactiveRows_;
316 std::vector<std::size_t> inequalityIndices_;
317 Eigen::RowBlockIndices equalityIndices_;
318 ImplicitWkPtr_t weak_;
319 // To avoid dynamic memory allocation
320 mutable LiegroupElement output_;
321 mutable vector_t logOutput_;
322
323 protected:
325
326 private:
327 HPP_SERIALIZABLE();
328}; // class Implicit
330} // namespace constraints
331} // namespace hpp
332
333#endif // HPP_CONSTRAINTS_IMPLICIT_HH
Definition: differentiable-function.hh:63
Definition: implicit-constraint-set.hh:45
Definition: implicit.hh:124
static ImplicitPtr_t create(const DifferentiableFunctionPtr_t &func, ComparisonTypes_t comp, std::vector< bool > mask=std::vector< bool >())
bool checkRightHandSide(LiegroupElementConstRef rhs) const
HPP_CONSTRAINTS_DEPRECATED void rightHandSideFromConfig(ConfigurationIn_t config)
HPP_CONSTRAINTS_DEPRECATED vectorOut_t rightHandSide()
HPP_CONSTRAINTS_DEPRECATED vectorIn_t rightHandSide() const
void rightHandSideFromConfig(ConfigurationIn_t config, LiegroupElementRef rhs)
void setInactiveRowsToZero(vectorOut_t error) const
virtual std::pair< JointConstPtr_t, JointConstPtr_t > doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const
virtual ~Implicit()
Definition: implicit.hh:139
DifferentiableFunction & function() const
Return a reference to function .
Definition: implicit.hh:259
const DifferentiableFunctionPtr_t & rightHandSideFunction() const
Definition: implicit.hh:225
void rightHandSideFunction(const DifferentiableFunctionPtr_t &rhsF)
virtual bool isEqual(const Implicit &other, bool swapAndTest) const
HPP_CONSTRAINTS_DEPRECATED void rightHandSide(vectorIn_t rhs)
bool operator==(const Implicit &other) const
Operator equality.
virtual ImplicitPtr_t copy() const
Copy object and return shared pointer to copy.
Implicit(const Implicit &other)
Copy constructor.
const DifferentiableFunctionPtr_t & functionPtr() const
Return a reference to function .
Definition: implicit.hh:262
size_type parameterSize() const
const segments_t & activeRows() const
Definition: implicit.hh:243
const Eigen::RowBlockIndices & equalityIndices() const
Get indices of constraint coordinates that are equality.
Definition: implicit.hh:249
void comparisonType(const ComparisonTypes_t &comp)
Set the comparison type.
vectorIn_t rightHandSideAt(const value_type &s)
size_type rightHandSideSize() const
Implicit()
Definition: implicit.hh:324
Implicit(const DifferentiableFunctionPtr_t &function, ComparisonTypes_t comp, std::vector< bool > mask)
void init(const ImplicitWkPtr_t &weak)
Definition: implicit.hh:301
bool checkAllRowsActive() const
Check if all rows are active (no inactive rows)
Definition: implicit.hh:246
const ComparisonTypes_t & comparisonType() const
Return the ComparisonType.
static ImplicitPtr_t createCopy(const ImplicitPtr_t &other)
Create a copy and return shared pointer.
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
#define HPP_CONSTRAINTS_DEPRECATED
Definition: deprecated.hh:37
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:65
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:182
pinocchio::DeviceConstPtr_t DeviceConstPtr_t
Definition: fwd.hh:110
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:113
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:175
pinocchio::size_type size_type
Definition: fwd.hh:47
pinocchio::value_type value_type
Definition: fwd.hh:48
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
std::vector< segment_t > segments_t
Definition: fwd.hh:84
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
pinocchio::vector_t vector_t
Definition: fwd.hh:59
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:67
pinocchio::LiegroupElementRef LiegroupElementRef
Definition: fwd.hh:66
Definition: active-set-differentiable-function.hh:36