hpp-constraints
4.9.1
Definition of basic geometric constraints for motion planning
|
#include <hpp/constraints/differentiable-function.hh>
Public Member Functions | |
virtual | ~DifferentiableFunction () |
LiegroupElement | operator() (vectorIn_t argument) const |
void | value (LiegroupElementRef result, vectorIn_t argument) const |
void | jacobian (matrixOut_t jacobian, vectorIn_t argument) const |
const ArrayXb & | activeParameters () const |
const ArrayXb & | activeDerivativeParameters () const |
size_type | inputSize () const |
Get dimension of input vector. More... | |
size_type | inputDerivativeSize () const |
LiegroupSpacePtr_t | outputSpace () const |
Get output space. More... | |
size_type | outputSize () const |
Get dimension of output vector. More... | |
size_type | outputDerivativeSize () const |
Get dimension of output derivative vector. More... | |
const std::string & | name () const |
Get function name. More... | |
virtual std::ostream & | print (std::ostream &o) const |
Display object in a stream. More... | |
std::string | context () const |
void | context (const std::string &c) |
void | finiteDifferenceForward (matrixOut_t jacobian, vectorIn_t arg, DevicePtr_t robot=DevicePtr_t(), value_type eps=std::sqrt(Eigen::NumTraits< value_type >::epsilon())) const |
void | finiteDifferenceCentral (matrixOut_t jacobian, vectorIn_t arg, DevicePtr_t robot=DevicePtr_t(), value_type eps=std::sqrt(Eigen::NumTraits< value_type >::epsilon())) const |
Protected Member Functions | |
DifferentiableFunction (size_type sizeInput, size_type sizeInputDerivative, size_type sizeOutput, std::string name=std::string()) | |
Concrete class constructor should call this constructor. More... | |
DifferentiableFunction (size_type sizeInput, size_type sizeInputDerivative, const LiegroupSpacePtr_t &outputSpace, std::string name=std::string()) | |
Concrete class constructor should call this constructor. More... | |
virtual void | impl_compute (LiegroupElementRef result, vectorIn_t argument) const =0 |
User implementation of function evaluation. More... | |
virtual void | impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const =0 |
Protected Attributes | |
size_type | inputSize_ |
Dimension of input vector. More... | |
size_type | inputDerivativeSize_ |
Dimension of input derivative. More... | |
LiegroupSpacePtr_t | outputSpace_ |
Dimension of output vector. More... | |
ArrayXb | activeParameters_ |
ArrayXb | activeDerivativeParameters_ |
Friends | |
class | DifferentiableFunctionSet |
Differentiable function from a Lie group, for instance the configuration space of a robot (hpp::pinocchio::Device) to a another Lie group.
Note that the input Lie group is only represented by the sizes of the elements and of the velocities: methods inputSize and inputDerivativeSize
The output space can be accessed by method outputSpace.
The value of the function for a given input can be accessed by method value . The Jacobian of the function for a given input can be accessed by method jacobian .
|
inlinevirtual |
|
protected |
Concrete class constructor should call this constructor.
sizeInput | dimension of the function input |
sizeInputDerivative | dimension of the function input derivative, |
sizeOutput | dimension of the output, |
name | function's name |
|
protected |
Concrete class constructor should call this constructor.
sizeInput | dimension of the function input |
sizeInputDerivative | dimension of the function input derivative, |
outputSpace | output space of the function. |
name | function name |
|
inline |
Returns a vector of booleans that indicates whether the corresponding velocity parameter influences this constraints.
|
inline |
Returns a vector of booleans that indicates whether the corresponding configuration parameter influences this constraints.
|
inline |
|
inline |
void hpp::constraints::DifferentiableFunction::finiteDifferenceCentral | ( | matrixOut_t | jacobian, |
vectorIn_t | arg, | ||
DevicePtr_t | robot = DevicePtr_t() , |
||
value_type | eps = std::sqrt(Eigen::NumTraits< value_type >::epsilon()) |
||
) | const |
Approximate the jacobian using forward finite difference.
jacobian | jacobian will be stored in this argument |
arg | point at which the jacobian will be computed |
robot | use to add configuration and velocities. If set to NULL, the configuration space is considered a vector space. |
eps | refers to \(\epsilon\) in http://en.wikipedia.org/wiki/Numerical_differentiation Evaluate the function 2*x.size() times but more precise the finiteDifferenceForward |
void hpp::constraints::DifferentiableFunction::finiteDifferenceForward | ( | matrixOut_t | jacobian, |
vectorIn_t | arg, | ||
DevicePtr_t | robot = DevicePtr_t() , |
||
value_type | eps = std::sqrt(Eigen::NumTraits< value_type >::epsilon()) |
||
) | const |
Approximate the jacobian using forward finite difference.
jacobian | jacobian will be stored in this argument |
arg | point at which the jacobian will be computed |
robot | use to add configuration and velocities. If set to NULL, the configuration space is considered a vector space. |
eps | refers to \(\epsilon\) in http://en.wikipedia.org/wiki/Numerical_differentiation Evaluate the function (x.size() + 1) times but less precise the finiteDifferenceCentral |
|
protectedpure virtual |
User implementation of function evaluation.
Implemented in hpp::constraints::ConstantFunction, hpp::constraints::explicit_::RelativeTransformation, hpp::constraints::explicit_::ImplicitFunction, hpp::constraints::ActiveSetDifferentiableFunction, hpp::constraints::function::OfParameterSubset, hpp::constraints::Manipulability, hpp::constraints::Identity, and hpp::constraints::function::Difference.
|
protectedpure virtual |
Implemented in hpp::constraints::ConstantFunction, hpp::constraints::explicit_::RelativeTransformation, hpp::constraints::explicit_::ImplicitFunction, hpp::constraints::ActiveSetDifferentiableFunction, hpp::constraints::function::OfParameterSubset, hpp::constraints::Manipulability, hpp::constraints::Identity, and hpp::constraints::function::Difference.
|
inline |
Get dimension of input derivative vector
The dimension of configuration vectors might differ from the dimension of velocity vectors since some joints are represented by non minimal size vectors: e.g. quaternion for SO(3)
|
inline |
Get dimension of input vector.
|
inline |
Computes the jacobian.
jacobian | jacobian will be stored in this argument |
argument | point at which the jacobian will be computed |
|
inline |
Get function name.
|
inline |
Evaluate the function at a given parameter.
|
inline |
Get dimension of output derivative vector.
|
inline |
Get dimension of output vector.
|
inline |
Get output space.
|
virtual |
Display object in a stream.
Reimplemented in hpp::constraints::GenericTransformation< _Options >, hpp::constraints::ConvexShapeContact, hpp::constraints::DifferentiableFunctionSet, hpp::constraints::RelativeCom, hpp::constraints::function::OfParameterSubset, and hpp::constraints::function::Difference.
|
inline |
Evaluate the function at a given parameter.
|
friend |
|
protected |
Initialized to true by this class. Child class are responsible for updating it.
|
protected |
Initialized to true by this class. Child class are responsible for updating it.
|
protected |
Dimension of input derivative.
|
protected |
Dimension of input vector.
|
protected |
Dimension of output vector.