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;
90 template <
int _PolynomeBasis,
int _Order>
94 PolynomeBasis = _PolynomeBasis,
96 NbCoeffs = _Order + 1,
97 NbPowerOfT = 2 * NbCoeffs + 1
100 typedef internal::sbf_traits<PolynomeBasis, Order>
sbf_traits;
106 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor>
125 assert(res.size() == NbCoeffs);
126 impl_paramDerivative(res, t);
131 assert(dParam.size() == NbCoeffs * parameterSize_);
132 impl_paramIntegrate(dParam);
143 void squaredNormIntegralDerivative(
const size_type order,
151 static void timeFreeBasisFunctionDerivative(
const size_type order,
153 BasisFunctionVector_t& res);
158 assert(res.size() == NbCoeffs);
159 BasisFunctionVector_t tmp;
160 timeFreeBasisFunctionDerivative(order, u, tmp);
170 BasisFunctionVector_t& res)
const;
174 assert(res.size() == NbCoeffs);
175 BasisFunctionVector_t tmp;
176 basisFunctionDerivative(order, u, tmp);
189 void squaredNormBasisFunctionIntegral(
190 const size_type order, BasisFunctionIntegralMatrix_t& res)
const;
195 BasisFunctionIntegralMatrix_t tmp;
196 squaredNormBasisFunctionIntegral(order, tmp);
202 bool res = operator()(q, timeRange().first);
210 bool res = operator()(q, timeRange().second);
233 return ConstParameterVector_t(parameters_.data(), parameters_.size());
238 ParameterVector_t(parameters_.data(), parameters_.size()) = p;
242 Ptr_t other(
new Spline(*
this));
248 Ptr_t other(
new Spline(*
this, constraints));
257 Ptr_t shPtr(
new Spline(robot, interval, constraints));
270 Eigen::Ref<const ParameterMatrix_t> params,
277 :
Path(interval, robot->configSize(), robot->numberDof(), constraints),
278 parameterSize_(robot->numberDof()),
280 base_(robot->RnxSOnConfigSpace()->vectorSpacesMerged()),
281 parameters_((int)NbCoeffs, parameterSize_),
282 velocity_(parameterSize_) {
284 for (
size_type i = 1; i < NbPowerOfT; ++i)
285 powersOfT_(i) = powersOfT_(i - 1) * length();
288 Spline(
const Spline& path);
297 std::ostream& print(std::ostream& os)
const;
327 mutable PowersOfT_t powersOfT_;
339 #endif // HPP_CORE_PATH_SPLINE_HH internal::sbf_traits< PolynomeBasis, Order > sbf_traits
Definition: spline.hh:100
sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t
Definition: spline.hh:104
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:109
PolynomeBasisType
Definition: spline.hh:45
sbf_traits::Coeffs_t BasisFunctionVector_t
Definition: spline.hh:103
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:175
virtual Configuration_t initial() const
Get the initial configuration.
Definition: spline.hh:200
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:247
const ParameterMatrix_t & parameters() const
Each row corresponds to a velocity of the robot.
Definition: spline.hh:225
Spline(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints)
Definition: spline.hh:275
DevicePtr_t robot_
Definition: spline.hh:313
void parameterIntegrate(vectorIn_t dParam)
Adds dParam to the parameters.
Definition: spline.hh:130
void squaredNormBasisFunctionIntegral(const size_type order, matrixOut_t res) const
Definition: spline.hh:192
constraints::LiegroupElement LiegroupElement
Definition: fwd.hh:168
internal::spline_basis_function< PolynomeBasis, Order > BasisFunction_t
Definition: spline.hh:101
virtual ~Spline()
Definition: spline.hh:253
ConstParameterVector_t rowParameters() const
Concatenate the parameters as one vector (P_0^T, ..., P_n^T).
Definition: spline.hh:232
const Configuration_t & base() const
Definition: spline.hh:219
PathPtr_t copy() const
Return a shared pointer to a copy of this.
Definition: spline.hh:241
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:155
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
pinocchio::vector_t vector_t
Definition: fwd.hh:220
LiegroupElement base_
Definition: spline.hh:317
pinocchio::value_type value_type
Definition: fwd.hh:174
ParameterMatrix_t parameters_
Definition: spline.hh:321
void parameterDerivativeCoefficients(vectorOut_t res, const value_type &t) const
Definition: spline.hh:123
constraints::matrixOut_t matrixOut_t
Definition: fwd.hh:167
void init(const Ptr_t &self)
Definition: spline.hh:292
size_type parameterSize() const
Definition: spline.hh:114
shared_ptr< Spline > Ptr_t
Definition: spline.hh:111
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:222
void base(const Configuration_t &q)
Definition: spline.hh:222
void basisFunctionDerivative(const size_type order, const value_type &u, vectorOut_t res) const
Definition: spline.hh:172
void rowParameters(vectorIn_t p)
Set the parameters.
Definition: spline.hh:237
Eigen::Matrix< value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor > ParameterMatrix_t
Definition: spline.hh:107
weak_ptr< Spline > WkPtr_t
Definition: spline.hh:112
Eigen::Map< const vector_t, Eigen::Aligned > ConstParameterVector_t
Definition: spline.hh:108
size_type parameterSize_
Robot number of degrees of freedom.
Definition: spline.hh:312
#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:102
virtual Configuration_t end() const
Get the final configuration.
Definition: spline.hh:208
static Ptr_t create(const DevicePtr_t &robot, const interval_t &interval, const ConstraintSetPtr_t &constraints)
Definition: spline.hh:255
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
void parameters(const ParameterMatrix_t &m)
Definition: spline.hh:229