29#ifndef HPP_CORE_PATH_SPLINE_HH
30#define HPP_CORE_PATH_SPLINE_HH
36#include <hpp/pinocchio/device.hh>
37#include <hpp/pinocchio/liegroup-element.hh>
49template <
int SplineType,
int Degree>
50struct spline_basis_function;
51template <
int SplineType,
int Degree>
53 enum { NbCoeffs = Degree + 1 };
54 typedef Eigen::Matrix<value_type, NbCoeffs, 1> Coeffs_t;
55 typedef Eigen::Matrix<value_type, NbCoeffs, NbCoeffs> IntegralCoeffs_t;
90template <
int _PolynomeBasis,
int _Order>
100 typedef internal::sbf_traits<PolynomeBasis, Order>
sbf_traits;
106 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor>
269 static void value(pinocchio::LiegroupElementConstRef
base,
270 Eigen::Ref<const ParameterMatrix_t> params,
277 :
Path(interval, robot->configSize(), robot->numberDof(),
constraints),
280 base_(robot->RnxSOnConfigSpace()->vectorSpacesMerged()),
285 powersOfT_(i) = powersOfT_(i - 1) *
length();
297 std::ostream&
print(std::ostream& os)
const;
Configuration_t eval(const value_type &time, bool &success) const
Configuration at time.
Definition path.hh:130
size_type outputSize() const
Get size of configuration space.
Definition path.hh:184
void init(const PathWkPtr_t &self)
const interval_t & timeRange() const
Get interval of definition.
Definition path.hh:190
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition path.hh:204
virtual value_type length() const
Get length of definition interval.
Definition path.hh:193
Path(const interval_t &interval, size_type outputSize, size_type outputDerivativeSize, const ConstraintSetPtr_t &constraints)
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor > ParameterMatrix_t
Definition spline.hh:107
void base(const Configuration_t &q)
Definition spline.hh:222
const Configuration_t & base() const
Definition spline.hh:219
void parameterDerivativeCoefficients(vectorOut_t res, const value_type &t) const
Definition spline.hh:123
std::ostream & print(std::ostream &os) const
void parameterIntegrate(vectorIn_t dParam)
Adds dParam to the parameters.
Definition spline.hh:130
virtual Configuration_t initial() const
Get the initial configuration.
Definition spline.hh:200
Eigen::Matrix< value_type, NbPowerOfT, 1 > PowersOfT_t
Definition spline.hh:102
Spline(const Spline &path)
static Ptr_t create(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints)
Definition spline.hh:255
@ NbPowerOfT
Definition spline.hh:97
@ NbCoeffs
Definition spline.hh:96
@ PolynomeBasis
Definition spline.hh:94
@ Order
Definition spline.hh:95
weak_ptr< Spline > WkPtr_t
Definition spline.hh:112
virtual ~Spline()
Definition spline.hh:253
void impl_velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
ParameterMatrix_t parameters_
Definition spline.hh:321
void impl_paramIntegrate(vectorIn_t dParam)
sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t
Definition spline.hh:104
virtual Configuration_t end() const
Get the final configuration.
Definition spline.hh:208
static void timeFreeBasisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res)
Definition spline.hh:155
shared_ptr< Spline > Ptr_t
Definition spline.hh:111
Eigen::Map< const vector_t, Eigen::Aligned > ConstParameterVector_t
Definition spline.hh:108
Eigen::Map< vector_t, Eigen::Aligned > ParameterVector_t
Definition spline.hh:109
void parameters(const ParameterMatrix_t &m)
Definition spline.hh:229
void squaredNormIntegralDerivative(const size_type order, vectorOut_t res) const
void rowParameters(vectorIn_t p)
Set the parameters.
Definition spline.hh:237
void basisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res) const
Definition spline.hh:172
void impl_derivative(vectorOut_t res, const value_type &t, size_type order) const
void squaredNormBasisFunctionIntegral(const size_type order, matrixOut_t res) const
Definition spline.hh:192
void maxVelocity(vectorOut_t res) const
sbf_traits::Coeffs_t BasisFunctionVector_t
Definition spline.hh:103
bool impl_compute(ConfigurationOut_t configuration, value_type t) const
Function evaluation without applying constraints.
LiegroupElement base_
Definition spline.hh:317
PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition spline.hh:247
DevicePtr_t robot_
Definition spline.hh:313
value_type squaredNormIntegral(const size_type order) const
Spline(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints)
Definition spline.hh:275
internal::spline_basis_function< PolynomeBasis, Order > BasisFunction_t
Definition spline.hh:101
static void value(pinocchio::LiegroupElementConstRef base, Eigen::Ref< const ParameterMatrix_t > params, const value_type &u, ConfigurationOut_t config, vectorOut_t velocity)
const ParameterMatrix_t & parameters() const
Each row corresponds to a velocity of the robot.
Definition spline.hh:225
void impl_paramDerivative(vectorOut_t res, const value_type &t) const
static void timeFreeBasisFunctionDerivative(const size_type order, const value_type &u, BasisFunctionVector_t &res)
ConstParameterVector_t rowParameters() const
Concatenate the parameters as one vector (P_0^T, ..., P_n^T).
Definition spline.hh:232
PathPtr_t copy() const
Return a shared pointer to a copy of this.
Definition spline.hh:241
size_type parameterSize() const
Definition spline.hh:114
void init(const Ptr_t &self)
Definition spline.hh:292
void basisFunctionDerivative(const size_type order, const value_type &u, BasisFunctionVector_t &res) const
void squaredNormBasisFunctionIntegral(const size_type order, BasisFunctionIntegralMatrix_t &res) const
Spline(const Spline &path, const ConstraintSetPtr_t &constraints)
internal::sbf_traits< PolynomeBasis, Order > sbf_traits
Definition spline.hh:100
size_type parameterSize_
Definition spline.hh:312
#define HPP_CORE_DLLAPI
Definition config.hh:88
PolynomeBasisType
Definition spline.hh:45
@ BernsteinBasis
Definition spline.hh:45
@ CanonicalPolynomeBasis
Definition spline.hh:45
Definition bi-rrt-planner.hh:36
pinocchio::value_type value_type
Definition fwd.hh:174
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:221
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:222
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition fwd.hh:109
pinocchio::vector_t vector_t
Definition fwd.hh:220
std::pair< value_type, value_type > interval_t
Definition fwd.hh:175
pinocchio::size_type size_type
Definition fwd.hh:173
constraints::LiegroupElement LiegroupElement
Definition fwd.hh:168
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
constraints::matrixOut_t matrixOut_t
Definition fwd.hh:167
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition fwd.hh:130
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
Definition bi-rrt-planner.hh:35