hpp-constraints 6.0.0
Definition of basic geometric constraints for motion planning
Loading...
Searching...
No Matches
differentiable-function-set.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2016 CNRS
3// Authors: Joseph Mirabel
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_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_SET_HH
31#define HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_SET_HH
32
35
36namespace hpp {
37namespace constraints {
40
45 : public DifferentiableFunction {
46 public:
47 typedef std::vector<DifferentiableFunctionPtr_t> Functions_t;
48
52 static DifferentiableFunctionSetPtr_t create(const std::string& name) {
54 }
55
57
60
62 const Functions_t& functions() const { return functions_; }
63
64 void add(const DifferentiableFunctionPtr_t& func) {
65 if (functions_.empty()) {
66 inputSize_ = func->inputSize();
67 inputDerivativeSize_ = func->inputDerivativeSize();
68 activeParameters_ = func->activeParameters();
69 activeDerivativeParameters_ = func->activeDerivativeParameters();
70 } else {
71 assert(inputSize_ == func->inputSize());
72 assert(inputDerivativeSize_ == func->inputDerivativeSize());
73
74 activeParameters_ = activeParameters_ || func->activeParameters();
75 activeDerivativeParameters_ =
76 activeDerivativeParameters_ || func->activeDerivativeParameters();
77 }
78 functions_.push_back(func);
79 result_.push_back(LiegroupElement(func->outputSpace()));
80 *outputSpace_ *= func->outputSpace();
81 }
82
85 const Functions_t& functions = other->functions();
86 for (Functions_t::const_iterator _f = functions.begin();
87 _f != functions.end(); ++_f)
88 add(*_f);
89 }
90
92
93 std::ostream& print(std::ostream& os) const;
94
98 DifferentiableFunctionSet(const std::string& name)
99 : DifferentiableFunction(0, 0, 0, name) {}
100
102
103 protected:
105 size_type row = 0;
106 std::size_t i = 0;
107 for (Functions_t::const_iterator _f = functions_.begin();
108 _f != functions_.end(); ++_f) {
109 const DifferentiableFunction& f = **_f;
110 f.impl_compute(result_[i], arg);
111 assert(hpp::pinocchio::checkNormalized(result_[i]));
112 result.vector().segment(row, f.outputSize()) = result_[i].vector();
113 row += f.outputSize();
114 ++i;
115 }
116 assert(hpp::pinocchio::checkNormalized(result));
117 }
118 void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const {
119 size_type row = 0;
120 for (Functions_t::const_iterator _f = functions_.begin();
121 _f != functions_.end(); ++_f) {
122 const DifferentiableFunction& f = **_f;
123 f.impl_jacobian(jacobian.middleRows(row, f.outputDerivativeSize()), arg);
124 row += f.outputDerivativeSize();
125 }
126 }
127
128 bool isEqual(const DifferentiableFunction& other) const {
129 const DifferentiableFunctionSet& castother =
130 dynamic_cast<const DifferentiableFunctionSet&>(other);
131 if (!DifferentiableFunction::isEqual(other)) return false;
132
133 if (functions_ != castother.functions_) return false;
134 if (result_.size() != castother.result_.size()) return false;
135 for (std::size_t i = 0; i < result_.size(); i++)
136 if (result_[i] != castother.result_[i]) return false;
137
138 return true;
139 }
140
141 private:
142 Functions_t functions_;
143 mutable std::vector<LiegroupElement> result_;
144}; // class DifferentiableFunctionSet
146} // namespace constraints
147} // namespace hpp
148
149#endif // HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_SET_HH
Definition differentiable-function-set.hh:45
void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
Definition differentiable-function-set.hh:118
static DifferentiableFunctionSetPtr_t create(const std::string &name)
Definition differentiable-function-set.hh:52
DifferentiableFunctionSet()
Definition differentiable-function-set.hh:101
std::vector< DifferentiableFunctionPtr_t > Functions_t
Definition differentiable-function-set.hh:47
void add(const DifferentiableFunctionPtr_t &func)
Definition differentiable-function-set.hh:64
void impl_compute(LiegroupElementRef result, ConfigurationIn_t arg) const
Definition differentiable-function-set.hh:104
void merge(const DifferentiableFunctionSetPtr_t &other)
The output columns selection of other is not taken into account.
Definition differentiable-function-set.hh:84
virtual ~DifferentiableFunctionSet()
Definition differentiable-function-set.hh:56
DifferentiableFunctionSet(const std::string &name)
Definition differentiable-function-set.hh:98
const Functions_t & functions() const
Get the stack of functions.
Definition differentiable-function-set.hh:62
bool isEqual(const DifferentiableFunction &other) const
Definition differentiable-function-set.hh:128
std::ostream & print(std::ostream &os) const
Display object in a stream.
Definition differentiable-function.hh:63
size_type outputSize() const
Get dimension of output vector.
Definition differentiable-function.hh:116
virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const =0
size_type outputDerivativeSize() const
Get dimension of output derivative vector.
Definition differentiable-function.hh:118
virtual void impl_compute(LiegroupElementRef result, vectorIn_t argument) const =0
User implementation of function evaluation.
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
pinocchio::LiegroupElement LiegroupElement
Definition fwd.hh:65
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition fwd.hh:113
shared_ptr< DifferentiableFunctionSet > DifferentiableFunctionSetPtr_t
Definition fwd.hh:114
pinocchio::size_type size_type
Definition fwd.hh:47
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:106
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:58
pinocchio::LiegroupElementRef LiegroupElementRef
Definition fwd.hh:66
Definition active-set-differentiable-function.hh:36