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> 49 template <
int SplineType,
int Degree>
50 struct spline_basis_function;
51 template <
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;
87 template <
int _PolynomeBasis,
int _Order>
91 PolynomeBasis = _PolynomeBasis,
93 NbCoeffs = _Order + 1,
94 NbPowerOfT = 2 * NbCoeffs + 1
97 typedef internal::sbf_traits<PolynomeBasis, Order>
sbf_traits;
103 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor>
122 assert(res.size() == NbCoeffs);
123 impl_paramDerivative(res, t);
128 assert(dParam.size() == NbCoeffs * parameterSize_);
129 impl_paramIntegrate(dParam);
140 void squaredNormIntegralDerivative(
const size_type order,
148 static void timeFreeBasisFunctionDerivative(
const size_type order,
150 BasisFunctionVector_t& res);
155 assert(res.size() == NbCoeffs);
157 timeFreeBasisFunctionDerivative(order, u, tmp);
167 BasisFunctionVector_t& res)
const;
171 assert(res.size() == NbCoeffs);
173 basisFunctionDerivative(order, u, tmp);
186 void squaredNormBasisFunctionIntegral(
187 const size_type order, BasisFunctionIntegralMatrix_t& res)
const;
193 squaredNormBasisFunctionIntegral(order, tmp);
199 bool res = operator()(q, timeRange().first);
207 bool res = operator()(q, timeRange().second);
254 Ptr_t shPtr(
new Spline(robot, interval, constraints));
267 Eigen::Ref<const ParameterMatrix_t> params,
274 :
Path(interval, robot->configSize(), robot->numberDof(), constraints),
275 parameterSize_(robot->numberDof()),
277 base_(robot->RnxSOnConfigSpace()->vectorSpacesMerged()),
278 parameters_((int)NbCoeffs, parameterSize_),
279 velocity_(parameterSize_) {
281 for (
size_type i = 1; i < NbPowerOfT; ++i)
282 powersOfT_(i) = powersOfT_(i - 1) * length();
294 std::ostream& print(std::ostream& os)
const;
336 #endif // HPP_CORE_PATH_SPLINE_HH internal::sbf_traits< PolynomeBasis, Order > sbf_traits
Definition: spline.hh:97
sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t
Definition: spline.hh:101
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
Definition: bi-rrt-planner.hh:35
Eigen::Map< vector_t, Eigen::Aligned > ParameterVector_t
Definition: spline.hh:106
PolynomeBasisType
Definition: spline.hh:45
sbf_traits::Coeffs_t BasisFunctionVector_t
Definition: spline.hh:100
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:175
virtual Configuration_t initial() const
Get the initial configuration.
Definition: spline.hh:197
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:108
pinocchio::size_type size_type
Definition: fwd.hh:173
PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition: spline.hh:244
const ParameterMatrix_t & parameters() const
Each row corresponds to a velocity of the robot.
Definition: spline.hh:222
Spline(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints)
Definition: spline.hh:272
DevicePtr_t robot_
Definition: spline.hh:310
void parameterIntegrate(vectorIn_t dParam)
Adds dParam to the parameters.
Definition: spline.hh:127
void squaredNormBasisFunctionIntegral(const size_type order, matrixOut_t res) const
Definition: spline.hh:189
constraints::LiegroupElement LiegroupElement
Definition: fwd.hh:168
internal::spline_basis_function< PolynomeBasis, Order > BasisFunction_t
Definition: spline.hh:98
virtual ~Spline()
Definition: spline.hh:250
ConstParameterVector_t rowParameters() const
Concatenate the parameters as one vector (P_0^T, ..., P_n^T).
Definition: spline.hh:229
const Configuration_t & base() const
Definition: spline.hh:216
PathPtr_t copy() const
Return a shared pointer to a copy of this.
Definition: spline.hh:238
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:221
void init(const PathWkPtr_t &self)
pinocchio::LiegroupElementConstRef LiegroupElementConstRef
Definition: fwd.hh:170
static void timeFreeBasisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res)
Definition: spline.hh:152
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
pinocchio::vector_t vector_t
Definition: fwd.hh:220
LiegroupElement base_
Definition: spline.hh:314
pinocchio::value_type value_type
Definition: fwd.hh:174
ParameterMatrix_t parameters_
Definition: spline.hh:318
void parameterDerivativeCoefficients(vectorOut_t res, const value_type &t) const
Definition: spline.hh:120
constraints::matrixOut_t matrixOut_t
Definition: fwd.hh:167
void init(const Ptr_t &self)
Definition: spline.hh:289
size_type parameterSize() const
Definition: spline.hh:111
shared_ptr< Spline > Ptr_t
Definition: spline.hh:108
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:222
void base(const Configuration_t &q)
Definition: spline.hh:219
void basisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res) const
Definition: spline.hh:169
void rowParameters(vectorIn_t p)
Set the parameters.
Definition: spline.hh:234
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor > ParameterMatrix_t
Definition: spline.hh:104
weak_ptr< Spline > WkPtr_t
Definition: spline.hh:109
Eigen::Map< const vector_t, Eigen::Aligned > ConstParameterVector_t
Definition: spline.hh:105
size_type parameterSize_
Robot number of degrees of freedom.
Definition: spline.hh:309
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:106
Eigen::Matrix< value_type, NbPowerOfT, 1 > PowersOfT_t
Definition: spline.hh:99
virtual Configuration_t end() const
Get the final configuration.
Definition: spline.hh:205
static Ptr_t create(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints)
Definition: spline.hh:252
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
void parameters(const ParameterMatrix_t &m)
Definition: spline.hh:226