hpp-constraints: include/hpp/constraints/differentiable-function-set.hh Source File

hpp-constraints 9.0.2 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
33#include <hpp/constraints/differentiable-function.hh>
34#include <hpp/constraints/fwd.hh>
35
36 namespace hpp {
37 namespace constraints {
40
44 class HPP_CONSTRAINTS_DLLAPI DifferentiableFunctionSet
45 : public DifferentiableFunction {
46 public:
47 typedef std::vector<DifferentiableFunctionPtr_t> Functions_t;
48
52 static DifferentiableFunctionSetPtr_t create(const std::string& name) {
53 return DifferentiableFunctionSetPtr_t(new DifferentiableFunctionSet(name));
54 }
55
56 virtual ~DifferentiableFunctionSet() {}
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
84 void merge(const DifferentiableFunctionSetPtr_t& other) {
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
101 DifferentiableFunctionSet() : DifferentiableFunction(0, 0, 0, "Stack") {}
102
103 protected:
104 void impl_compute(LiegroupElementRef result, ConfigurationIn_t arg) const {
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
hpp::constraints::DifferentiableFunctionSet
Definition differentiable-function-set.hh:45
hpp::constraints::DifferentiableFunctionSet::impl_jacobian
void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const
Definition differentiable-function-set.hh:118
hpp::constraints::DifferentiableFunctionSet::create
static DifferentiableFunctionSetPtr_t create(const std::string &name)
Definition differentiable-function-set.hh:52
hpp::constraints::DifferentiableFunctionSet::DifferentiableFunctionSet
DifferentiableFunctionSet()
Definition differentiable-function-set.hh:101
hpp::constraints::DifferentiableFunctionSet::Functions_t
std::vector< DifferentiableFunctionPtr_t > Functions_t
Definition differentiable-function-set.hh:47
hpp::constraints::DifferentiableFunctionSet::add
void add(const DifferentiableFunctionPtr_t &func)
Definition differentiable-function-set.hh:64
hpp::constraints::DifferentiableFunctionSet::impl_compute
void impl_compute(LiegroupElementRef result, ConfigurationIn_t arg) const
Definition differentiable-function-set.hh:104
hpp::constraints::DifferentiableFunctionSet::merge
void merge(const DifferentiableFunctionSetPtr_t &other)
The output columns selection of other is not taken into account.
Definition differentiable-function-set.hh:84
hpp::constraints::DifferentiableFunctionSet::~DifferentiableFunctionSet
virtual ~DifferentiableFunctionSet()
Definition differentiable-function-set.hh:56
hpp::constraints::DifferentiableFunctionSet::DifferentiableFunctionSet
DifferentiableFunctionSet(const std::string &name)
Definition differentiable-function-set.hh:98
hpp::constraints::DifferentiableFunctionSet::functions
const Functions_t & functions() const
Get the stack of functions.
Definition differentiable-function-set.hh:62
hpp::constraints::DifferentiableFunctionSet::isEqual
bool isEqual(const DifferentiableFunction &other) const
Definition differentiable-function-set.hh:128
hpp::constraints::DifferentiableFunctionSet::print
std::ostream & print(std::ostream &os) const
Display object in a stream.
hpp::constraints::DifferentiableFunction
Definition differentiable-function.hh:63
hpp::constraints::DifferentiableFunction::outputSize
size_type outputSize() const
Get dimension of output vector.
Definition differentiable-function.hh:116
hpp::constraints::DifferentiableFunction::impl_jacobian
virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const =0
hpp::constraints::DifferentiableFunction::outputDerivativeSize
size_type outputDerivativeSize() const
Get dimension of output derivative vector.
Definition differentiable-function.hh:118
hpp::constraints::DifferentiableFunction::impl_compute
virtual void impl_compute(LiegroupElementRef result, vectorIn_t argument) const =0
User implementation of function evaluation.
hpp::pinocchio::LiegroupElementBase
hpp::pinocchio::LiegroupElementBase::vector
const vector_type & vector() const
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
differentiable-function.hh
fwd.hh
hpp::pinocchio::checkNormalized
bool checkNormalized(const LiegroupElementConstBase< vector_type > &e1, const value_type &eps=PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)
hpp::constraints::DifferentiableFunctionPtr_t
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition fwd.hh:115
hpp::constraints::DifferentiableFunctionSetPtr_t
shared_ptr< DifferentiableFunctionSet > DifferentiableFunctionSetPtr_t
Definition fwd.hh:116
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:59
hpp