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

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

Loading...
Searching...
No Matches
differentiable-function.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_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
31#define HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
32
33#include <hpp/constraints/config.hh>
34#include <hpp/constraints/fwd.hh>
35#include <hpp/pinocchio/liegroup-element.hh>
36#include <hpp/util/debug.hh>
37#include <hpp/util/serialization-fwd.hh>
38
39 namespace hpp {
40 namespace constraints {
41
44
63 class HPP_CONSTRAINTS_DLLAPI DifferentiableFunction {
64 public:
65 virtual ~DifferentiableFunction() {}
66
70 LiegroupElement operator()(vectorIn_t argument) const {
71 assert(argument.size() == inputSize());
72 LiegroupElement result(outputSpace_);
73 impl_compute(result, argument);
74 return result;
75 }
79 void value(LiegroupElementRef result, vectorIn_t argument) const {
80 assert(result.space()->nq() == outputSize());
81 assert(argument.size() == inputSize());
82 impl_compute(result, argument);
83 }
88 void jacobian(matrixOut_t jacobian, vectorIn_t argument) const {
89 assert(argument.size() == inputSize());
90 assert(jacobian.rows() == outputDerivativeSize());
91 assert(jacobian.cols() == inputDerivativeSize());
92 impl_jacobian(jacobian, argument);
93 }
94
97 const ArrayXb& activeParameters() const { return activeParameters_; }
98
101 const ArrayXb& activeDerivativeParameters() const {
102 return activeDerivativeParameters_;
103 }
104
106 size_type inputSize() const { return inputSize_; }
112 size_type inputDerivativeSize() const { return inputDerivativeSize_; }
114 LiegroupSpacePtr_t outputSpace() const { return outputSpace_; }
116 size_type outputSize() const { return outputSpace_->nq(); }
118 size_type outputDerivativeSize() const { return outputSpace_->nv(); }
122 const std::string& name() const { return name_; }
123
125 virtual std::ostream& print(std::ostream& o) const;
126
127 std::string context() const { return context_; }
128
129 void context(const std::string& c) { context_ = c; }
130
140 void finiteDifferenceForward(
141 matrixOut_t jacobian, vectorIn_t arg, DevicePtr_t robot = DevicePtr_t(),
142 value_type eps =
143 std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;
144
154 void finiteDifferenceCentral(
155 matrixOut_t jacobian, vectorIn_t arg, DevicePtr_t robot = DevicePtr_t(),
156 value_type eps =
157 std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;
158
159 bool operator==(DifferentiableFunction const& other) const;
160 bool operator!=(DifferentiableFunction const& b) const;
161
174 virtual std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
175 DeviceConstPtr_t /*robot*/) const {
176 return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
177 };
178
193 static DifferentiableFunctionPtr_t extract(
194 DifferentiableFunctionPtr_t original, interval_t interval);
195
196 protected:
203 DifferentiableFunction(size_type sizeInput, size_type sizeInputDerivative,
204 size_type sizeOutput,
205 std::string name = std::string());
206
213 DifferentiableFunction(size_type sizeInput, size_type sizeInputDerivative,
214 const LiegroupSpacePtr_t& outputSpace,
215 std::string name = std::string());
216
218 virtual void impl_compute(LiegroupElementRef result,
219 vectorIn_t argument) const = 0;
220
221 virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const = 0;
222
223 virtual bool isEqual(const DifferentiableFunction& other) const {
224 if (name_ != other.name_) return false;
225 if (inputSize_ != other.inputSize_) return false;
226 if (inputDerivativeSize_ != other.inputDerivativeSize_) return false;
227 if (*outputSpace_ != *(other.outputSpace_)) return false;
228
229 return true;
230 }
231
233 size_type inputSize_;
235 size_type inputDerivativeSize_;
237 LiegroupSpacePtr_t outputSpace_;
238
242 ArrayXb activeParameters_;
246 ArrayXb activeDerivativeParameters_;
247
248 private:
249 std::string name_;
251 std::string context_;
252
253 friend class DifferentiableFunctionSet;
254
255 protected:
256 DifferentiableFunction() {}
257
258 private:
259 HPP_SERIALIZABLE();
260}; // class DifferentiableFunction
261 inline std::ostream& operator<<(std::ostream& os,
262 const DifferentiableFunction& f) {
263 return f.print(os);
264}
266} // namespace constraints
267} // namespace hpp
268
269/*
270// This will override boost::shared_ptr 's equality operator
271// between 2 DifferentiableFunctionPtr_t
272 namespace boost {
273 using namespace hpp::constraints;
274 typedef DifferentiableFunction T;
275 typedef shared_ptr<T> TPtr;
276
277 template<> inline bool operator==<T, T> (TPtr const & a, TPtr const & b)
278 BOOST_SP_NOEXCEPT
279 {
280 return *a == *b;
281 }
282
283 template<> inline bool operator!=<T, T> (TPtr const & a, TPtr const & b)
284 BOOST_SP_NOEXCEPT
285 {
286 return !(a == b);
287 }
288}
289*/
290
291#endif // HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
hpp::constraints::DifferentiableFunctionSet
Definition differentiable-function-set.hh:45
hpp::constraints::DifferentiableFunction
Definition differentiable-function.hh:63
hpp::constraints::DifferentiableFunction::outputSpace_
LiegroupSpacePtr_t outputSpace_
Dimension of output vector.
Definition differentiable-function.hh:237
hpp::constraints::DifferentiableFunction::DifferentiableFunction
DifferentiableFunction(size_type sizeInput, size_type sizeInputDerivative, const LiegroupSpacePtr_t &outputSpace, std::string name=std::string())
Concrete class constructor should call this constructor.
hpp::constraints::DifferentiableFunction::inputDerivativeSize_
size_type inputDerivativeSize_
Dimension of input derivative.
Definition differentiable-function.hh:235
hpp::constraints::DifferentiableFunction::isEqual
virtual bool isEqual(const DifferentiableFunction &other) const
Definition differentiable-function.hh:223
hpp::constraints::DifferentiableFunction::operator==
bool operator==(DifferentiableFunction const &other) const
hpp::constraints::DifferentiableFunction::context
std::string context() const
Definition differentiable-function.hh:127
hpp::constraints::DifferentiableFunction::operator!=
bool operator!=(DifferentiableFunction const &b) const
hpp::constraints::DifferentiableFunction::name
const std::string & name() const
Get function name.
Definition differentiable-function.hh:122
hpp::constraints::DifferentiableFunction::inputSize_
size_type inputSize_
Dimension of input vector.
Definition differentiable-function.hh:233
hpp::constraints::DifferentiableFunction::finiteDifferenceCentral
void finiteDifferenceCentral(matrixOut_t jacobian, vectorIn_t arg, DevicePtr_t robot=DevicePtr_t(), value_type eps=std::sqrt(Eigen::NumTraits< value_type >::epsilon())) const
hpp::constraints::DifferentiableFunction::context
void context(const std::string &c)
Definition differentiable-function.hh:129
hpp::constraints::DifferentiableFunction::~DifferentiableFunction
virtual ~DifferentiableFunction()
Definition differentiable-function.hh:65
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::DifferentiableFunction
DifferentiableFunction(size_type sizeInput, size_type sizeInputDerivative, size_type sizeOutput, std::string name=std::string())
Concrete class constructor should call this constructor.
hpp::constraints::DifferentiableFunction::inputDerivativeSize
size_type inputDerivativeSize() const
Definition differentiable-function.hh:112
hpp::constraints::DifferentiableFunction::outputDerivativeSize
size_type outputDerivativeSize() const
Get dimension of output derivative vector.
Definition differentiable-function.hh:118
hpp::constraints::DifferentiableFunction::dependsOnRelPoseBetween
virtual std::pair< JointConstPtr_t, JointConstPtr_t > dependsOnRelPoseBetween(DeviceConstPtr_t) const
Definition differentiable-function.hh:174
hpp::constraints::DifferentiableFunction::activeParameters
const ArrayXb & activeParameters() const
Definition differentiable-function.hh:97
hpp::constraints::DifferentiableFunction::impl_compute
virtual void impl_compute(LiegroupElementRef result, vectorIn_t argument) const =0
User implementation of function evaluation.
hpp::constraints::DifferentiableFunction::inputSize
size_type inputSize() const
Get dimension of input vector.
Definition differentiable-function.hh:106
hpp::constraints::DifferentiableFunction::print
virtual std::ostream & print(std::ostream &o) const
Display object in a stream.
hpp::constraints::DifferentiableFunction::activeParameters_
ArrayXb activeParameters_
Definition differentiable-function.hh:242
hpp::constraints::DifferentiableFunction::extract
static DifferentiableFunctionPtr_t extract(DifferentiableFunctionPtr_t original, interval_t interval)
hpp::constraints::DifferentiableFunction::operator()
LiegroupElement operator()(vectorIn_t argument) const
Definition differentiable-function.hh:70
hpp::constraints::DifferentiableFunction::jacobian
void jacobian(matrixOut_t jacobian, vectorIn_t argument) const
Definition differentiable-function.hh:88
hpp::constraints::DifferentiableFunction::outputSpace
LiegroupSpacePtr_t outputSpace() const
Get output space.
Definition differentiable-function.hh:114
hpp::constraints::DifferentiableFunction::DifferentiableFunction
DifferentiableFunction()
Definition differentiable-function.hh:256
hpp::constraints::DifferentiableFunction::activeDerivativeParameters_
ArrayXb activeDerivativeParameters_
Definition differentiable-function.hh:246
hpp::constraints::DifferentiableFunction::value
void value(LiegroupElementRef result, vectorIn_t argument) const
Definition differentiable-function.hh:79
hpp::constraints::DifferentiableFunction::activeDerivativeParameters
const ArrayXb & activeDerivativeParameters() const
Definition differentiable-function.hh:101
hpp::constraints::DifferentiableFunction::finiteDifferenceForward
void finiteDifferenceForward(matrixOut_t jacobian, vectorIn_t arg, DevicePtr_t robot=DevicePtr_t(), value_type eps=std::sqrt(Eigen::NumTraits< value_type >::epsilon())) const
hpp::pinocchio::LiegroupElementBase
hpp::pinocchio::LiegroupElementConstBase::space
const LiegroupSpacePtr_t & space() const
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
debug.hh
fwd.hh
liegroup-element.hh
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:111
hpp::constraints::DeviceConstPtr_t
pinocchio::DeviceConstPtr_t DeviceConstPtr_t
Definition fwd.hh:112
hpp::constraints::LiegroupSpacePtr_t
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition fwd.hh:70
hpp::constraints::DifferentiableFunctionPtr_t
shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition fwd.hh:115
hpp::constraints::operator<<
ComparisonTypes_t operator<<(const ComparisonType &a, const ComparisonType &b)
Definition comparison-types.hh:61
hpp::constraints::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:85
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::ArrayXb
pinocchio::ArrayXb ArrayXb
Definition fwd.hh:81
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::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:59
hpp
serialization-fwd.hh
HPP_SERIALIZABLE
#define HPP_SERIALIZABLE()